融合距离残差和概率残差的激光SLAM前端匹配方法

技术领域

本发明属于SLAM技术领域，更具体地，本发明涉及一种融合距离残差和概率残差的激光SLAM前端匹配方法。

背景技术

随着移动机器人在服务业和仓储物流领域有着越来越广泛的应用，其自主定位导航技术也越发重要。现主要的技术有磁导航、二维码导航和激光导航，其中磁导航工作轨迹固定，路径需要铺设电磁导线或磁条。二维码导航轨迹相对自由，但是环境中多使用二维码或特殊图形码等标记物，还需要定期维护。激光导航定位精准，路径灵活多变，适应多种现场环境，因此成为了移动机器人主流的定位导航方式。其中激光导航包括反光板定位导航和不依赖反光板的激光SLAM自然导航，基于激光雷达的反光板定位导航必须在环境中布设反光板，然而很多环境无法布设，或者影响机器人的运行路径。

激光SLAM可分为前端匹配和后端优化，后端图优化步骤和原理基本是固定的，即求解最小二乘解。激光SLAM如果要获得较高的精度，那么前端配准必须更加精准。现有的激光SLAM采用基于激光束末端与已知地图的匹配优化实现定位，读取激光数据并滤波转换,得到激光预估位姿；再利用无迹卡尔曼滤波算法，对非线性系统,融合激光预估位姿数据和里程计推算位姿数据,作为实际估计位姿；最后计算自适应向量,更新测量噪声协方差矩阵，使用自适应无迹卡尔曼滤波算法，对非线性系统进行滤波处理。该方案存在如下问题：

1)该方案使用里程计，里程计最大的问题就是会存在机器人打滑，里程计数据和激光定位数据产生较大偏差，直接使用无迹卡尔曼融合匹配结果和里程计数据就会出现较大的偏差和定位不确定性，使得机器人定位结果出现不连续的跳动。

2)该方案直接使用原始hector算法的结果进行融合，原始hector的缺点就是匹配容易陷入局部极值，那么再融合结果依然会陷入局部极值，出现误匹配和定位结果跳动。

发明内容

本发明提供了一种融合距离残差和概率残差的激光SLAM前端匹配方法，旨在改善上述问题。

本发明是这样实现的，一种融合距离残差和概率残差的激光SLAM前端匹配方法，所述方法具体包括如下步骤：

S1、计算T时刻激光帧相对于(T-1)时刻激光帧的距离残差；

S2、对距离残差与栅格概率地图在T时刻的概率残差进行非线性优化，得到T时刻的最优位姿；

S3、将T时刻激光帧投影至栅格概率地图的最优位姿处，进行栅格概率地图的更新。

进一步的，所述距离残差的计算方法具体如下：

S11、将T时刻的激光帧投影到(T-1)时刻的激光帧上，查找T时刻激光帧中各点p

S12、获取该两最近点所在直线的法向量

S13、将T时刻激光帧中的所有点放入点集中，若点p

其中，p

S14、计算T时刻激光帧相对于(T-1)时刻激光帧的距离残差项plicp_err，表示如下：

其中，N为激光帧中的点云点数量。

进一步的，栅格概率地图的概率残差项p_err如下：

M(S(R*p

进一步的，所述步骤S2具体包括如下步骤：

S21、基于距离残与栅格概率地图在T时刻的概率残差构建最小二乘目标函数F，其表示具体如下：

其中，其w

S22、基于高斯牛顿优化方法对最小二乘目标函数F进行优化，获取T时刻的最优位姿X

其中，

本发明不依赖其它传感器和安装环境，降低了多传感器和改装环境的成本，提高了移动机器人定位的稳定性和精度。

本发明通过PL-ICP算法获得点云的初始位姿，并融合相邻点云匹配的距离残差和点云与地图匹配的概率残差，构建统一的最小二乘目标函数，并求解得到移动机器人的最优位姿。PL-ICP算法能在不依赖里程计的情况下得到移动机器人的初始位姿，避免了里程计打滑带来的定位错误，又可以在运动速度较快的情况下，得到机器人较为准确的初始位姿；采用融合距离残差与概率残差构建统一的最小二乘目标函数，即考虑了当前帧与以往所有帧生成的地图之间的误差，又反映了相邻帧之间的匹配误差，能有效去除意外物体或运动物体对匹配结果的影响，提高了匹配的稳定性及精度。

附图说明

图1为本发明实施例提供的融合距离残差和概率残差的激光SLAM前端匹配方法流程图。

具体实施方式

下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。

图1为本发明实施例提供的融合距离残差和概率残差的激光SLAM前端匹配方法流程图，该方法具体包括如下步骤：

S1、启动雷达获取点云并滤波；

雷达一帧数据的激光帧中的点云数很高，一帧激光上的点云有数千上万个点，在激光帧的匹配过程中很耗计算时间，使用体素滤波器，在保证点云微观特征的基础上降低激光帧中的点云密度，提高匹配效率。

S2、通过点云初始化栅格概率地图

栅格概率地图的栅格有三种状态：未知、占据和自由。常用栅格概率地图未知概率为0.5，大于0.5表示栅格为占据状态，小于0.5表示为自由状态。初始化未知栅格概率地图值全为0.5，通过经验设置占据和自由更新的概率p

将第一个激光帧中的点云投影到栅格概率地图原点(即将第一个激光帧的中心投影至栅格地图的原点)，激光点打到的栅格更新为占据，激光线掠过的栅格更新为自由空间，其中栅格的占据和自由概率值更新如下：

栅格概率地图的地图概率的更新通过优势函数odds进行更新，函数如下：

优势得分:

将第一帧激光点云投影到栅格概率地图原点，激光点打到的栅格更新为占据更新，激光线掠过的栅格更新为自由更新，因为odds累乘更新会发生数据指数级增长，使用log(odds)函数进行压缩，在此称为log(odds)概率。栅格的占据和自由更新如下：

占据更新:

自由更新：

t-1时刻经过自由及占据更新后，概率统称为log(odds)，最终得到t时刻栅格的概率值为：

其中，p

S3、计算T时刻激光帧相对于(T-1)时刻激光帧的距离残差，具体包括如下步骤：

S31、将T时刻的激光帧投影到(T-1)时刻的激光帧上，查找T时刻激光帧中各点p

S32、获取该两最近点所在直线的法向量

S33、将T时刻激光帧中的所有点放入点集中，若点p

其中，p

那么雷达在任意t时刻的位姿表示如下：

pose

(x

S34、计算T时刻激光帧相对于(T-1)时刻激光帧的距离残差项plicp_err，表示如下：

其中，N为激光帧中的点云点数量。

在本发明实施例中，栅格概率地图的概率残差项p_err表示如下：

S6、对距离残与栅格概率地图在T时刻的概率残差进行非线性优化，非线性优化初值为R&t，最小二乘目标函数如下：

其中，w

将R&t转为位姿矩阵

S7、T时刻的最优位姿X

不依赖其它传感器和安装环境，降低了多传感器和改装环境的成本，提高了移动机器人定位的稳定性和精度。

上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。