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

技术领域

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

背景技术

定位导航与建图是获取时空信息的基础手段，在国民经济建设与国防安全领域发挥着十分重要的作用，已被美国列入技术封锁清单。高精度同步定位与建图(SLAM)技术是无人系统实现完全自主化控制所必须解决的瓶颈难题，具有重要的应用前景和经济价值。

激光SLAM比视觉SLAM具有更高的建图精度和稳定性，已经得到商业化运用。然而，对于精度要求较高的应用场合(如高精地图、三维测绘等)，当前激光SLAM方法还难以满足需求。主要原因之一在于主流激光SLAM算法通常利用激光扫描线的曲率信息提取特征，忽视了局部区域的几何结构特性，使得匹配点位置精度相对较低。同时，SLAM前端采用增量式匹配，匹配误差势必会沿着链条形成传递，最终导致轨迹漂移。尽管随着闭环检测与图优化技术的发展，该问题得到了有效改善，但仍然突出。图优化技术仅利用结点间的相对位姿构建几何约束，忽略了匹配连接点之间的约束，在本质上造成了平差系统的脆弱性，而SLAM的高效率需求又决定了经典区域网平差技术无法适用。在国际著名评测数据集KITTI上，激光SLAM的最优位置漂移误差仍大于0.5％，无法满足激光SLAM对长时高精度的需求。

发明内容

针对激光同步定位与建图(SLAM)中的局部误差传递速度快的问题，发展多点云帧联合的局部误差消除方法，研究参数空间降维与变换参数平均的数学思想，解决平差优化中精度与速度的矛盾性问题，实现局部地图构建中误差传递的有效抑制。本发明提出了一种基于特征点联合平差的激光SLAM局部地图构建方法。该发明借鉴摄影测量领域区域网平差的成功经验，将其应用于三维激光点云领域，利用多度重叠的匹配连接点构建位姿优化模型，可基本消除局部地图内的误差累积效应。

将局部地图作为后续位姿图优化的匹配基元能够有效降低误差累积速度和缩小位姿图规模，从而实现精度和效率的同步提升，已经在Google的Cartographer中得以验证。然而，其采用帧到模型的局部地图构建策略本质上还是一种增量式匹配方法，势必会造成误差沿着链条形成传递。借鉴摄影测量领域区域网平差的成功经验，利用多度重叠的匹配连接点构建位姿优化模型，可基本消除误差累积效应。尽管受限于SLAM效率需求，无法进行全局区域平差，但应用于局部地图构建则完全可行。同时，由于激光点云的稀疏特性，特征匹配点位置精度仍有待提升。基于上述分析，本发明主要提出了多点云帧相联合的局部地图误差消除方法。首先，提出对称点-面ICP精配准方法优化匹配点位置精度；其次，利用虚拟线概念进行参数空间分解与匹配连接点平差建模，降低求解复杂度；然后，构建旋转平均和平移平均模型，通过凸松弛思想求取联合平差的全局最优解，实现高精度局部地图生成。

为实现上述目的，本发明所设计的基于特征点联合平差的激光SLAM局部地图构建方法，主要包括以下步骤：

步骤1，采用由粗到精的方式进行三维激光点云特征匹配，获取高精度三维匹配点集合；

步骤2，由上述匹配点集合构建虚拟线并进行参数空间分解，利用虚拟线将6自由度刚体变换进行参数空间分解，得到两个三自由度子问题，即旋转子问题与平移子问题；

步骤3，基于匹配连接点的旋转子问题和平移子问题进行平差建模，得到每帧点云的位姿参数；

步骤4，基于旋转平均和平移平均得到每帧点云的最优位置和姿态参数；

步骤5，根据最终位置和姿态参数，将局部地图所包含的激光点云帧转换至统一的坐标系下，得到局部地图。

进一步的，步骤1的具体实现包括如下子步骤；

首先，对局部地图所包含的m个激光点云帧

其次，将上一步得到的初始匹配点集，采用RANSAC方法计算点云S

最后，对点云S

进一步的，对称点-面ICP算法的具体实现方式如下；

给定匹配点(x

式中，i表示匹配点的编号，(R,t)为旋转和平移参数，与点-面距离不同，对称距离同时考虑了两帧点云的法向信息；

基于对称点-面距离度量，并引入自适应尺度M-估计来应对噪声和粗差，构建对称点-面ICP的数学模型：

式中，ρ是稳健代价函数；w

进一步的，使用罗德里格斯旋转表示法进行称点-面ICP的数学模型线性化，并基于RANSAC获取的初值，采用高斯-牛顿法求解。

进一步的，步骤2的具体实现包括如下子步骤；

虚拟线由特征点两两随机连接得到，每两个匹配点对(x

其中，i和j表示匹配点的编号，(R,t)为旋转和平移参数，两方程相减可得

进一步的，步骤3的具体实现方式如下；

假设局部地图包含m个激光点云帧

式中，X

进一步的，对平差模型进行泰勒展开线性化，并采用高斯-牛顿法快速求解。

进一步的，步骤4中旋转平均具体实现方式如下；

给定一组相对旋转观测量{R

式中，||·||

进一步的，步骤4中平移平均具体实现方式如下；

给定一组相对平移观测量{t

/>

式中，

进一步的，步骤5的具体实现方式如下；

假设局部地图包含m个激光点云帧

本发明具有如下积极效果：

1)本发明提出了一种基于特征点联合平差的激光SLAM局部地图构建方法，通过联合位姿参数和世界坐标点共同作为未知数进行优化求解，实现局部地图构建中误差传递的有效抑制。

2)本发明提出了一种参数空间降维方法，通过虚拟性概念，在不损失解的精度的前提下，将6自由度刚体变换参数空间分解为两个3自由度子问题，大幅降低参数搜索空间与求解难度，解决了平差优化中精度与速度的矛盾性问题。

3)本发明提供了一种利用旋转平均和平移平均进行位姿平差优化的思路。

采用本发明可更高精度的、稳定的构建三维激光局部地图，为将局部地图作为后续位姿图优化的匹配基元奠定基础，从而有效降低SLAM系统误差累积速度和缩小位姿图规模，实现精度和效率的同步提升。本发明在室内测图、三维重建、自动驾驶、矿山测量等领域具有广阔的应用前景。

附图说明

图1为本发明的技术流程图。

图2为本发明中虚拟线示意图。

图3为本发明中基于旋转的无向图结构示意图。

具体实施方式

下面结合附图对本发明的方案做进一步详细的说明。

如图1所示，一种基于特征点联合平差的激光SLAM局部地图构建方法，包括以下步骤：

步骤1)，采用由粗到精的方式进行三维激光点云特征匹配，获取高精度三维匹配点集合；首先基于三维特征检测和特征描述方法进行点云粗配准，并利用对称点-面ICP精配准来优化匹配点位置精度；

进一步的，步骤1)的具体实现方式如下：

首先，对局部地图所包含的m个激光点云帧

其次，将上一步得到的初始匹配点集，采用RANSAC方法计算点云S

最后，对点云S

本发明通过构建对称点-面距离，提升ICP算法的收敛效率和收敛区间。给定匹配点(x

式中，(R,t)为旋转和平移参数。与点-面距离不同，对称距离同时考虑了两帧点云的法向信息。

由于点云具有稀疏性，特征法获取的匹配点位置精度不高，为了最大限度地抑制误差传递，采用精配准方法优化匹配点精度。因此，基于对称点-面距离度量，并引入自适应尺度M-估计来应对噪声和粗差，构建对称点-面ICP的数学模型：

式中，ρ是稳健代价函数；w

步骤2)，如图2所示，由上述匹配点集合构建虚拟线并进行参数空间分解，提出虚拟性概念，利用虚拟线将6自由度刚体变换进行参数空间分解，得到两个三自由度子问题，即旋转子问题与平移子问题；

首先，将匹配点两两随机配对，每两个匹配点对(x

其次，虚拟线

进一步的，步骤2)的具体实现方式如下：

虚拟线由特征点两两随机连接得到，虚拟线参数空间分解利用了其平移不变性。若(x

两方程相减可得

步骤3)，基于匹配连接点的旋转子问题和平移子问题平差优化，得到每帧点云的位姿参数

进一步的，步骤3)的具体实现方式如下：

假设局部地图包含m个激光点云帧

式中，X

首先，由公式(4)构建虚拟线匹配点的旋转子问题，对该非线性方程进行泰勒展开线性化，并采用高斯-牛顿法求解得到每帧点云在统一坐标系里的旋转变换参数R

然后，将R

步骤4)，如图3，基于旋转平均和平移平均得到每帧点云的最优位置和姿态参数。

进一步的，步骤4)的具体实现方式如下：

旋转平均模型：给定一组相对旋转观测量{R

式中，||·||

平移平均模型：给定一组相对平移观测量{t

式中，

首先，将步骤2)得到的相对旋转{R

然后，依据公式(5)构建旋转平均的数学模型，基于SDP进行松弛化处理，采用坐标下降法求解得到每帧点云的最终姿态参数。

最后，将上一步的最终姿态参数作为已知值，依据公式(6)构建平移平均的数学模型，采用最小二乘法求解得到每帧点云的最终位置参数。

步骤5)，根据最终位置和姿态参数，将局部地图所包含m个激光点云帧

进一步的，步骤5)的具体实现方式如下：

假设局部地图包含m个激光点云帧

以上所述仅为本发明的优选实施例而已，并不用以限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。