误差融合技术的移动机器人SLAM算法研究

2023-11-21 07:52程广伟樊顺涛郭占正徐立友

洛阳理工学院学报(自然科学版) 2023年4期

李洋，程广伟，樊顺涛，郭占正，徐立友

(1.河南科技大学车辆与交通工程学院，河南洛阳 471003；2.洛阳理工学院智能制造学院，河南洛阳 471023)

移动机器人在物流、教育、医疗、服务、餐饮等行业的推广与应用，对即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)提出了更高的精度要求[1-2]。激光雷达、深度相机、毫米波雷达以及惯性测量单元(Inertial Measurement Unit，IMU)等是获取周围环境信息和实现SLAM技术的主要传感器[3]。Zhang等采用高频低精度的里程计进行运动估计，利用低频特征点云进行运动匹配去矫正定位构建地图，建立了3D激光SLAM方案LOAM[4]。Deschaud针对采集点云数据量大小和稀疏性的问题，提出了单帧激光点云匹配模型的IMLS-SLAM方案[5]。Shan等针对小型无人车辆在多变地形实时运行采集的数据畸变问题，对地面点进行分类和提取，使用点云聚类算法剔除噪声点，两步L-M优化定位精度，提出了LeGO-LOAM方案[6]。Shan等针对单一传感器在大场景同步定位与建图时高漂移的问题，通过因子图耦合雷达惯性里程计，提出了LIO-SAM方案[7]，确保了系统的实时性，对复杂多变和快速旋转等大场景扫描有更好的鲁棒性。

综上所述，单一激光雷达提取的原始特征点云去求解帧间运动时会产生位姿估计失准现象，运用多传感器信息融合提高SLAM精度的研究很有必要。本文拟以激光雷达点云为主，IMU信息为辅，开展激光雷达与IMU信息融合的研究与检验。

1 回环检测插值算法概述

回环检测插值算法框架如图1所示。IMU数据用于状态估计和预积分，利用IMU状态估计的结果对激光雷达失真点云进行运动补偿，更新矫正自身位姿，对运动补偿后的点云进行特征提取与匹配。在激光里程计中，通过选取关键帧减少系统计算压力，采用基于关键帧的建图方法，保证系统实时运行。在后端增加回环约束，降低系统长期运行的累计误差。利用预积分误差约束、特征点匹配约束和回环约束构建整体代价函数，并进行非线性优化求解，优化整体误差，保持全局一致性，输出准确的定位与建图信息。

2 回环检测插值算法的构建

2.1 IMU状态估计和预积分

IMU用于测量移动机器人的三轴加速度与角速度信息[8]：

(1)

图1 回环检测插值算法框架

在一段连续时间[i，j]内，若要得到j时刻状态量(PVQ，位置、速度、姿态)，可对i时刻的状态量进行积分。积分结果作为j时刻状态量估计的初始值，用于下一时刻的状态估计。由于IMU更新频率较快，一段时间内得到的数据很多，每次位姿优化更新后需要重新积分，通过预积分的方法减少计算时间，将IMU的积分模型转化为预积分模型[9]：

(2)

式(2)中的积分部分是预积分量，为i到j时刻IMU数据直接积分的结果。利用式(3)表示预积分量，得到的预积分量只与IMU的测量值有关，是j时刻相对于i时刻的状态。

(3)

式中：αbibj、βbibj和γbibj分别为i时刻到j时刻的位置、速度和姿态预积分量。

为约束IMU相邻时刻之间的运动，将上一时刻IMU预积分量作为下一时刻测量值，对这两相邻时刻做预积分可得IMU预积分误差模型：

(4)

式中：φp、φq和φv分别为IMU预积分约束的位置、姿态和速度残差；φba和φbg分别为加速度和角速度偏差。

构建IMU预积分误差约束如下：

(5)

2.2 运动补偿

移动机器人运动过程中受环境约束，不能保持匀速行驶状态，通过激光雷达获取周围环境的点云信息，常常存在三维点云失真的现象[10]，可利用IMU的状态信息对失真点云进行补偿。综合考虑激光雷达和IMU接收数据时间的不同，进行时间配准后，使用线性插值的方法进行补偿，线性插值原理如图2所示。

图2 线性插值原理图

在线性插值过程中，利用式(6)进行IMU数据的位姿解算，得到两帧点云之间的位姿转换矩阵，利用式(7)对失真点云进行补偿。

(6)

(7)

2.3 特征提取与匹配

提取特征点云的过程中，使用粗糙度c作为提取的评价指标。遍历每个运动补偿后的点云及其周围点的坐标，使用式(8)来计算点云粗糙度c。c值按从小到大排序，选择c值最大的点作为边缘点，该类点云在三维空间发生尖锐拐角变化；选择c值最小的点作为平面点，该类点云在三维空间变化平缓。假设S是激光雷达在同一扫描中返回的连续点A的集合，则粗糙度c表达式为：

(8)

特征点提取完成后，对特征点建立匹配关系。边缘特征点匹配时，选取当前帧中边缘特征点A，查找上一帧中与点A距离最近的点B，查找与点B同一帧中相邻扫描线上的点G，点A与直线BG建立匹配关系如图3所示。

(a)匹配示意 (b)匹配关系

根据点到直线的特征匹配关系，通过点到直线的特征距离对匹配过程进行约束：

(9)

平面特征点匹配时，选取当前帧中平面特征点A，查找上一帧中与点A距离最近的点B，分别查找与点B同一帧中的同一扫描线和相邻扫描线上距离最近的点G和点H，点A与平面BGH建立的匹配关系如图4所示。

(a)匹配示意 (b)匹配关系

根据点到平面的特征匹配关系，通过点到平面的特征距离对匹配过程进行约束：

(10)

2.4 关键帧选取与建图

移动机器人实时构建较大环境地图对系统计算能力有较高要求，通过采用选取关键帧的策略，降低计算压力。选取关键帧过多，不能达到减少计算时间的目的；选取关键帧过于稀疏，会增大帧间匹配的误差。针对以上问题，采用如下关键帧选取方法：① 第一帧点云作为关键帧；② 相邻两关键帧平移距离ΔX超过设定阈值；③ 相邻两关键帧时间间隔Δt超过设定阈值；④ 相邻关键帧旋转ΔR超过设定阈值。选取关键帧的标准方程：

(11)

式中：Δx(a，a+1)，Δy(a，a+1)，Δz(a，a+1)分别为a关键帧的x，y，z轴到a+1关键帧的x，y，z轴的距离。

(12)

2.5 回环检测

回环检测可以减小传感器长时间运行对系统产生的累计误差，在系统中增加回环检测以提升系统的全局定位精度。回环场景中，当前关键帧和历史关键帧的相同环境特征较多，使用ICP算法对其进行匹配并建立位姿约束。判定存在回环的条件为：① 当前关键帧序列与历史关键帧序列之差大于设定阈值；② 当前关键帧与历史关键帧之间的轨迹距离ΔL小于设定阈值；③ 当前关键帧与历史关键帧的时间间隔ΔtR大于设定阈值。回环判定方程如式(13)所示。使用式(14)对存在回环的两关键帧进行位姿约束，建立回环约束模型，更新当前运动位姿并进行建图。

(13)

式中：ΔO为当前关键帧序列与历史关键帧序列之差；Op，a为第a个关键帧序列；vab为a关键帧到b关键帧的行驶速度。

(14)

2.6 误差优化

为了有效解决大规模同步定位与建图，减小因系统误差对建图带来的影响，构建整体代价函数：

F=argmin∑‖rX‖2+∑‖rDe+Dp‖2+∑‖rf‖2

(15)

式中：rX为IMU预积分误差约束；rDe+Dp为特征点匹配约束；rf为回环约束。

整体代价函数为非线性函数，使用L-M(Levenberg-Marquard)算法[12]进行非线性优化并求解，使得优化后的误差结果最小，输出的定位保持全局一致性。

3 实验验证与结果分析

3.1 实验流程

(1)典型场景选取：选取国际上自动驾驶场景KITTI数据集[13]的00、05、09三个序列，其中序列00和05的场景为道路转换很多、少量动态物体和树木、大量住宅和停泊汽车；序列09的场景为上下坡、弯道和少量树木。

(2)算法验证：使用KITTI官方提供的kitti2bag工具将序列00、05、09转换为bag文件，运行SLAM算法并跑通转化过的bag文件。

(3)结果分析：获取KITTI数据集官网提供的GNSS行驶轨迹真值，输出回环检测插值算法运行3个序列的位姿数据，从行驶轨迹对比和绝对轨迹误差对回环检测插值算法做定量分析。通过可视化工具Rviz对KITTI数据集环境进行三维点云重现，并做定性分析。

3.2 定量实验结果与分析

回环检测插值算法和LOAM分别运行序列00、05和09得到的运动轨迹结果与轨迹真值对比如图5所示，GT_00灰色线为KITTI的运动轨迹真值，OURS蓝色线为回环检测插值算法运行KITTI的运动轨迹，LOAM橙色线为LOAM运行KITTI的运动轨迹。回环检测插值算法运行的轨迹更接近轨迹真值。在转换很多的大型场景00和05中，回环检测插值算法表现更加稳定，误差更小。在变化较少的序列09中，回环检测插值算法和LOAM表现整体相当，急转弯处本算法略有优势。

(a)序列00轨迹对比 (b)序列05轨迹对比 (c)序列09轨迹对比

利用绝对轨迹误差评测不同算法运行KITTI数据集的全局地图一致性。采集回环检测插值算法和LOAM运行序列00、05、09位姿估计值，利用evo工具与真值位姿相比，得到不同算法运行序列00、05、09的绝对轨迹误差，分别如图6～图8所示，APE灰色线为绝对轨迹误差曲线，mean橙色线为误差均值，median绿色线为中值误差，rmse红色线为均方根误差，std紫色区域为标准差。统计了不同算法运行不同序列得出的绝对轨迹误差各项结果，如表1所示。

由图6～图8和表1得出：序列00中，回环检测插值算法的均方根误差比LOAM减少了5.08 m，相比下降了23.5%；序列05中，回环检测插值算法的均方根误差比LOAM减少了5.98 m，相比下降了56.5%；序列09中，回环检测插值算法的均方根误差比LOAM减少了2.43 m，相比下降了12%。在绝对轨迹误差中其他各项统计均有所下降。由此可见有回环检测的回环检测插值算法比无回环检测的LOAM更有优势，减少了系统的累计误差，对构建全局一致地图有正向作用。

表1 不同方法运行KITTI的绝对轨迹误差结果统计

(a)LOAM (b)回环检测插值算法

3.3 定性实验结果与分析

为了评测算法还原三维真实场景，将回环检测插值算法和LOAM运行序列00、05、09，利用Rviz对不同算法构建的环境地图进行三维点云重现。不同算法构建序列00、05、09的三维点云地图分别如图9、图10和图11所示。

图9和图10中LOAM构建的点云地图出现了明显漂移，地图重影较大，回环检测插值算法很少有重影，十字路口和和转弯处的地图依旧清晰，构建地图更接近真实环境。图11中道路转弯角度大，LOAM算法构建的地图有明显残影，转弯位置的处理较差，与LOAM算法相比，回环检测插值算法在转弯处构建地图更清晰。回环检测插值算法增加IMU约束和回环约束减小了地图漂移，转弯细节处理更好，周围环境的三维重现效果更优。