基于特征点联合平差的激光SLAM局部地图构建方法技术

本发明专利技术提出了一种基于特征点联合平差的激光SLAM局部地图构建方法，所提出的方法解决了无人驾驶领域中地图构建精度差、位姿求解效率低等问题。通过联合位姿参数和世界坐标点进行优化求解，有效抑制了构建局部地图时的误差传递。通过利用虚拟线的方式对变换参数进行降维，显著降低参数搜索空间和求解难度，提高位姿计算效率。提出利用旋转平均和位移平均的思想，为位姿优化提供新的思路。该发明专利技术能够创建鲁棒、高精度的局部地图，可以有效抑制SLAM系统的误差传递，在提升计算效率的同时，能够显著改善建图精度。著改善建图精度。著改善建图精度。

【技术实现步骤摘要】
基于特征点联合平差的激光SLAM局部地图构建方法


[0001]本专利技术属于测绘遥感和无人驾驶
，具体涉及一种基于特征点联合平差的激光SLAM局部地图构建方法。

技术介绍

[0002]定位导航与建图是获取时空信息的基础手段，在国民经济建设与国防安全领域发挥着十分重要的作用，已被美国列入技术封锁清单。高精度同步定位与建图(SLAM)技术是无人系统实现完全自主化控制所必须解决的瓶颈难题，具有重要的应用前景和经济价值。
[0003]激光SLAM比视觉SLAM具有更高的建图精度和稳定性，已经得到商业化运用。然而，对于精度要求较高的应用场合(如高精地图、三维测绘等)，当前激光SLAM方法还难以满足需求。主要原因之一在于主流激光SLAM算法通常利用激光扫描线的曲率信息提取特征，忽视了局部区域的几何结构特性，使得匹配点位置精度相对较低。同时，SLAM前端采用增量式匹配，匹配误差势必会沿着链条形成传递，最终导致轨迹漂移。尽管随着闭环检测与图优化技术的发展，该问题得到了有效改善，但仍然突出。图优化技术仅利用结点间的相对位姿构建几何约束，忽略了匹配连接点之间的约束，在本质上造成了平差系统的脆弱性，而SLAM的高效率需求又决定了经典区域网平差技术无法适用。在国际著名评测数据集KITTI上，激光SLAM的最优位置漂移误差仍大于0.5％，无法满足激光SLAM对长时高精度的需求。

技术实现思路

[0004]针对激光同步定位与建图(SLAM)中的局部误差传递速度快的问题，发展多点云帧联合的局部误差消除方法，研究参数空间降维与变换参数平均的数学思想，解决平差优化中精度与速度的矛盾性问题，实现局部地图构建中误差传递的有效抑制。本专利技术提出了一种基于特征点联合平差的激光SLAM局部地图构建方法。该专利技术借鉴摄影测量领域区域网平差的成功经验，将其应用于三维激光点云领域，利用多度重叠的匹配连接点构建位姿优化模型，可基本消除局部地图内的误差累积效应。
[0005]将局部地图作为后续位姿图优化的匹配基元能够有效降低误差累积速度和缩小位姿图规模，从而实现精度和效率的同步提升，已经在Google的Cartographer中得以验证。然而，其采用帧到模型的局部地图构建策略本质上还是一种增量式匹配方法，势必会造成误差沿着链条形成传递。借鉴摄影测量领域区域网平差的成功经验，利用多度重叠的匹配连接点构建位姿优化模型，可基本消除误差累积效应。尽管受限于SLAM效率需求，无法进行全局区域平差，但应用于局部地图构建则完全可行。同时，由于激光点云的稀疏特性，特征匹配点位置精度仍有待提升。基于上述分析，本专利技术主要提出了多点云帧相联合的局部地图误差消除方法。首先，提出对称点
‑
面ICP精配准方法优化匹配点位置精度；其次，利用虚拟线概念进行参数空间分解与匹配连接点平差建模，降低求解复杂度；然后，构建旋转平均和平移平均模型，通过凸松弛思想求取联合平差的全局最优解，实现高精度局部地图生成。
[0006]为实现上述目的，本专利技术所设计的基于特征点联合平差的激光SLAM局部地图构建
方法，主要包括以下步骤：
[0007]步骤1，采用由粗到精的方式进行三维激光点云特征匹配，获取高精度三维匹配点集合；
[0008]步骤2，由上述匹配点集合构建虚拟线并进行参数空间分解，利用虚拟线将6自由度刚体变换进行参数空间分解，得到两个三自由度子问题，即旋转子问题与平移子问题；
[0009]步骤3，基于匹配连接点的旋转子问题和平移子问题进行平差建模，得到每帧点云的位姿参数；
[0010]步骤4，基于旋转平均和平移平均得到每帧点云的最优位置和姿态参数；
[0011]步骤5，根据最终位置和姿态参数，将局部地图所包含的激光点云帧转换至统一的坐标系下，得到局部地图。
[0012]进一步的，步骤1的具体实现包括如下子步骤；
[0013]首先，对局部地图所包含的m个激光点云帧中的两帧点云S
i
和S
j
，使用ISS算法从每一帧点云中提取三维特征，并利用FPFH算法对这些特征进行描述，得到特征描述向量，对S
i
中的每一特征向量，在所有S
j
特征向量中搜索与之具有最近欧式距离的特征向量，形成一对一匹配对；
[0014]其次，将上一步得到的初始匹配点集，采用RANSAC方法计算点云S
i
和S
j
之间的相对位置和姿态参数，即刚体变换模型参数；将相对位置和姿态参数作为初始值，利用对称点
‑
面ICP算法进行精配准，得到精配准的相对位置和姿态参数；
[0015]最后，对点云S
i
的每一个特征点p
i
，由精配准相对位置和姿态参数预测其在点云S
j
中的最优位置搜索点云S
j
中离最优位置最近的点q
j
，若ε为距离阈值，则认为p
i
和q
j
为精优化后的匹配对，得到匹配点集合。
[0016]进一步的，对称点
‑
面ICP算法的具体实现方式如下；
[0017]给定匹配点(x
i
,y
i
)和法向量其对称点
‑
面距离d
i
(
·
)的数学定义为：
[0018][0019]式中，i表示匹配点的编号，(R,t)为旋转和平移参数，与点
‑
面距离不同，对称距离同时考虑了两帧点云的法向信息；
[0020]基于对称点
‑
面距离度量，并引入自适应尺度M
‑
估计来应对噪声和粗差，构建对称点
‑
面ICP的数学模型：
[0021][0022]式中，ρ是稳健代价函数；w
i
是权函数；χ
ρ
(w
i
)是权函数惩罚项。
[0023]进一步的，使用罗德里格斯旋转表示法进行称点
‑
面ICP的数学模型线性化，并基于RANSAC获取的初值，采用高斯
‑
牛顿法求解。
[0024]进一步的，步骤2的具体实现包括如下子步骤；
[0025]虚拟线由特征点两两随机连接得到，每两个匹配点对(x
i
,y
i
)和(x
j
,y
j
)可构成一个虚拟线匹配对虚拟线参数空间分解利用了其平移不变性，若(x
i
,y
i
)和(x
j
,y
j
)为正确匹配，则有，
[0026][0027]其中，i和j表示匹配点的编号，(R,t)为旋转和平移参数，两方程相减可得可见虚拟线之间只存在旋转关系，是平移无关量；因此将6自由度的点云匹配问题分解为2个3自由度的子问题进行分开解算，并且不影响求解精度，同时，虚拟线由本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于特征点联合平差的激光SLAM局部地图构建方法，其特征在于，包括以下步骤：步骤1，采用由粗到精的方式进行三维激光点云特征匹配，获取高精度三维匹配点集合；步骤2，由上述匹配点集合构建虚拟线并进行参数空间分解，利用虚拟线将6自由度刚体变换进行参数空间分解，得到两个三自由度子问题，即旋转子问题与平移子问题；步骤3，基于匹配连接点的旋转子问题和平移子问题进行平差建模，得到每帧点云的位姿参数；步骤4，基于旋转平均和平移平均得到每帧点云的最优位置和姿态参数；步骤5，根据最终位置和姿态参数，将局部地图所包含的激光点云帧转换至统一的坐标系下，得到局部地图。2.根据权利要求1所述的基于特征点联合平差的激光SLAM局部地图构建方法，其特征在于：步骤1的具体实现包括如下子步骤；首先，对局部地图所包含的m个激光点云帧中的两帧点云S
i
和S
j
，使用ISS算法从每一帧点云中提取三维特征，并利用FPFH算法对这些特征进行描述，得到特征描述向量，对S
i
中的每一特征向量，在所有S
j
特征向量中搜索与之具有最近欧式距离的特征向量，形成一对一匹配对；其次，将上一步得到的初始匹配点集，采用RANSAC方法计算点云S
i
和S
j
之间的相对位置和姿态参数，即刚体变换模型参数；将相对位置和姿态参数作为初始值，利用对称点
‑
面ICP算法进行精配准，得到精配准的相对位置和姿态参数；最后，对点云S
i
的每一个特征点p
i
，由精配准相对位置和姿态参数预测其在点云S
j
中的最优位置搜索点云S
j
中离最优位置最近的点q
j
，若ε为距离阈值，则认为p
i
和q
j
为精优化后的匹配对，得到匹配点集合。3.根据权利要求2所述的基于特征点联合平差的激光SLAM局部地图构建方法，其特征在于：对称点
‑
面ICP算法的具体实现方式如下；给定匹配点(x

【专利技术属性】
技术研发人员：李加元，史鹏程，张永军，胡庆武，
申请(专利权)人：武汉大学，
类型：发明
国别省市：