CN112710312A - 融合距离残差和概率残差的激光slam前端匹配方法 - Google Patents

Abstract

本发明公开了一种融合距离残差和概率残差的激光SLAM前端匹配方法，具体包括如下步骤：S1、计算T时刻激光帧相对于(T‑1)时刻激光帧的距离残差；S2、对距离残与栅格概率地图在T时刻的概率残差进行非线性优化，得到T时刻的最优位姿；S3、基于T时刻最优位姿将T时刻激光帧投影至栅格概率地图，进行栅格概率地图的更新。本发明不依赖其它传感器和安装环境，降低了多传感器和改装环境的成本，提高了移动机器人定位的稳定性和精度。

Description

融合距离残差和概率残差的激光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_i在(T-1)时刻激光帧中的两个最近点

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

通过公式(1)计算点p_i到两最近点所在直线的距离，该距离值即表示误差值：

S13、将T时刻激光帧中的所有点放入点集中，若点p_i对应的距离dis_err大于设定的误差阈值，则在点集中删除该点p_i，遍历点集中的所有点，基于公式(2)来获取(T-1)时刻至T时刻雷达的初始位姿R&t；

其中，p_j表示点集中的第j个点，

表示点p_j在上一激光帧距其距离最近的一个点；

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

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

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

M(S(R*p_i+t))表示将第i个激光点打到各栅格的概率值。

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

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

其中，其w₁和w₂分别是距离残差项plicp_err和概率残差项p_err的权重值；

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

其中，

是基于初始位姿R&t形成位姿矩阵，J_F是目标函数F关于位姿的导数。

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

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

附图说明

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

具体实施方式

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

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

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

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

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

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

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

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

优势得分:

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

占据更新:

自由更新：

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

其中，p_occ、p_free为设定的经验参数值。

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

S31、将T时刻的激光帧投影到(T-1)时刻的激光帧上，查找T时刻激光帧中各点p_i在(T-1)时刻激光帧中的两个最近点

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

那么可以通过公式(1)计算点p_i到两最近点所在直线的距离，该距离值即表示误差值：

S33、将T时刻激光帧中的所有点放入点集中，若点p_i对应的距离dis_err大于设定的误差阈值，则在点集中删除该点p_i，遍历点集中的所有点，基于公式(2)来获取(T-1)时刻至T时刻雷达的初始位姿R&t；

其中，p_j表示点集中的第j个点，

表示点p_j在上一激光帧距其距离最近的一个点。从而得到，两个时刻的位姿矩阵Q_T，表示如下：

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

pose_t＝(x_t,y_t,θ_t)＝Q₀Q₁Q₂…Q_t

(x_i,y_i,θ_i)即雷达的实时位姿。

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

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

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

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

其中，w₁和w₂分别是距离残差项plicp_err和概率残差项p_err的权重值经验值。p_i表示第i个激光点的世界坐标，S(R*p_i+t)表示将第i个激光点的世界坐标转换成栅格坐标，M(S(R*p_i+t))表示将第i个激光点打到各栅格的概率值。

将R&t转为位姿矩阵

即为上文中的位姿矩阵Q_T，J_F是目标函数F关于位姿的导数，那么由高斯牛顿优化方法可得最优位姿X_T为：

S7、T时刻的最优位姿X_T，实现移动机器人SLAM的定位问题。将T时刻激光帧投影至栅格概率地图的最优位姿处(即将激光帧的中心投影至栅格地图的最优位姿)，进行第S2步的栅格概率地图更新，最终实现SLAM的同步建图和定位。

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

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

Claims (4)

1.一种融合距离残差和概率残差的激光SLAM前端匹配方法，其特征在于，所述方法具体包括如下步骤：

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

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

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

2.如权利要求1所述融合距离残差和概率残差的激光SLAM前端匹配方法，其特征在于，所述距离残差的计算方法具体如下：

S11、将T时刻的激光帧投影到(T-1)时刻的激光帧上，查找T时刻激光帧中各点p_i在(T-1)时刻激光帧中的两个最近点

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

通过公式(1)计算点p_i到两最近点所在直线的距离，该距离值即表示误差值：

S13、将T时刻激光帧中的所有点放入点集中，若点p_i对应的距离dis_err大于设定的误差阈值，则在点集中删除该点p_i，遍历点集中的所有点，基于公式(2)来获取(T-1)时刻至T时刻雷达的初始位姿R&t；

其中，p_j表示点集中的第j个点，

表示点p_j在上一激光帧距其距离最近的一个点；

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

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

3.如权利要求1所述融合距离残差和概率残差的激光SLAM前端匹配方法，其特征在于，栅格概率地图的概率残差项p_err如下：

M(S(R*p_i+t))表示将第i个激光点打到各栅格的概率值。

4.如权利要求1所述融合距离残差和概率残差的激光SLAM前端匹配方法，其特征在于，所述步骤S2具体包括如下步骤：

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

其中，其w₁和w₂分别是距离残差项plicp_err和概率残差项p_err的权重值；

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

其中，

是基于初始位姿R&t形成位姿矩阵，J_F是目标函数F关于位姿的导数。

Images