退化环境中多线激光雷达融合IMU的里程计改进方法

2022-10-10张旭,孔令云,赖冠宇

价值工程 2022年27期

0 引言

近年来，我们在生活中越来越多地看到自主移动机器人出现在生活中，如家庭中使用的扫地机器人可以根据室内场景环境，规划轨迹路径，将全屋无死角打扫；饿了么和美团研发的无人自动售货车，在现实环境的马路上可以自主运动与避障；百度小米等公司研发的完全自动驾驶汽车在智能网联汽车管理法实施之后，已经开始在实际环境中完全脱离人工控制自主驾驶测试。这一切的发展都依赖于同步定位与建图（Simultaneous Localization and Mapping，SLAM）技术近些年的快速发展，SLAM技术是指机器人处于未知环境中，通过传感器在运动过程中估计自身位姿，同时完成环境地图的构建。

由于机器人承担的任务与工作的环境越来越复杂，当前室外情况下SLAM技术主流的传感器使用的是视觉传感器或者多线激光雷达，由于视觉传感器极易受环境与天气影响，如暗光环境中感光元件宽容度不够无法获取充足信息等不稳定情况，多线激光雷达是更加稳定的选择。2014年，卡耐基梅隆大学的Zhang Ji等人提出了LOAM算法，LOAM算法是三维SLAM中表现十分亮眼的算法，在使用多线激光雷达的三维SLAM中是里程碑的存在，该算法最有贡献的地方是提出了一种通过特征点提取构成约束的前端高频低精准度的激光里程计和后端低频高精准的扫描帧与地图匹配的地图构建，通过这种方法对扫描匹配的方法进行了拆分，前端帧帧匹配的激光里程计有着很好的实时性，后端帧与地图的匹配提高了精度保证建图的准确性，实现了系统资源的合理分配，系统框架如图1。

图1 LOAM算法系统流程图

1 研究背景与内容

由于LOAM算法仅依赖多线激光雷达进行定位与建图，前端激光里程计使用的是特征提取的方法，在城市道路环境中，道路两边建筑物结特征明显且丰富，激光里程计定位较为准确，当处于一些结构特征不明显，不丰富的情况场景中会产生退化现象，使得机器人无法正确地估计自身的位姿，导致定位与建图的结果与真实场景产生较大的偏差，当定位不准确，地图构建错误，机器人也就丧失了自主移动的能力。

退化现象的产生原因是因为机器人所处的环境中结构几何信息缺乏方向约束，导致机器人在相应方向上移动定位与建图的准确性明显降低。退化场景可以分为单方向结构特征稀疏场景，如高速公路、乡村道路、桥梁、长直走廊、单侧或两侧由灌木丛或者墙体构成的道路等。在此类场景中，使用多线激光雷达构成的激光里程计中对于前后方向上不容易提取出有特征性的集合约束，所以在建图过程中，单方向结构特征稀疏场景容易造成激光里程计定位轨迹估计过长或过短；多方向结构特征稀疏场景，如空旷的操场，广场等，这时使用多线激光雷达的机器人在SLAM过程中在前后左右方向均缺少约束，容易造成激光里程计运动估计轨迹左右偏移或着长短发生变化。

退化现象是由于多线激光雷达对此类场景难以处理，是由于传感器本体原理所限制的，所以处理此类问题必须引入新的传感器与多线激光雷达形成互补。本文在使用LOAM算法的基础上改进激光里程计，引入IMU传感器，通过使用IMU预积分为激光里程计帧间约束在退化环境中约束不足提供正确的运动估计，保证建图与定位的准确性，改进激光里程计框图如图2所示。

图2 改进激光里程计框图

2 激光IMU里程计构成

2.1 激光特征残差函数构建

由于激光点云信息量较为丰富，匹配消耗资源，本文使用LOAM算法提出的根据点的曲率大小进行特征点的提取，计算公式如式（1）：

其中，S代表在t时刻以点i为中心构成的点云集，p、p分别是S中的点k与点l，且不是同一个点。

通过计算曲率后将曲率大的作为角特征点，曲率小的作为面特征点，角特征点在不同线束的点云中寻找最近一点构成线特征，面特征点从相同线束和不同线束的点云中各选取最近的一点构成面特征。示意图如图3。

图3 线特征与面特征

通过计算当前点云中的角特征点i点到上一帧点云的线特征直线ab的距离构成线约束，距离计算公式为式（2）。

计算当前点云中的平面特征点i到上一帧点云的面特征平面mjl的距离构成面约束，计算公式如式（3）。

2.2 IMU预计分残差函数构成

将当前帧j与上一帧i之间IMU的状态进行积分，可以获得在世界坐标系下的当前帧j的姿态（q），速度（v），位置q，从i帧到j帧的状态转移公式如式（4）。

至此，可以对两帧之间状态进行约束，IMU预计分残差如式（7）

其中，e、e、e、e、e分别表示位置、姿态、速度、加速度偏置、角速度偏置的残差。

2.3 整体误差函数构建

通过激光点云信息和IMU预计分信息共同构建里程计求解位姿变化，需要求估计解量的i时刻状态量如式（8）。

在上文中构建的点云残差函数和IMU预计分残差函数的基础上构建整体的误差函数如式（9）。

需要求解使得误差函数最小时X的取值，此时这个问题已经是一个非线性最小二乘的优化问题，本文使用Google开源的非线性优化库Ceres Solver对本问题进行迭代求解，最终得出最优位姿估计，当运动状态由这些优化后的结果不断累加，就构成了激光IMU里程计。可以在雷达处于退化环境中缺少约束时，由IMU提供约束，保证定位的准确。

3 实验验证

实验部分使用开源数据集KITTI与真实退化中实验验证平台对本文构建的激光IMU里程计与LOAM方法的激光里程计进行对比分析。

3.1 KITTI数据集验证

KITTI数据集01轨迹采集于高速公路，使用此轨迹数据集运行LOAM方法与本文提出方法记录轨迹，通过evo工具将轨迹画出，与数据集提供真实轨迹对比，轨迹对比图如图4。

图4 轨迹图对比

其中，LOAM方法估计的轨迹长度是2006.542米，本文提出的Lidar-imu方法估计的轨迹是2451.735米，真实轨迹的长度是2453.259米，从输出轨迹长度来看，相比于LOAM的方法，本文提出的方法在高速公路退化环境中对于轨迹长度的轨迹要远好于LOAM方法。从图4的轨迹图中可以看出，本文提出的Lidar-imu在轨迹上更接近于真值，角度的估计上也好于LOAM方法。

3.2 真实环境测试

图5是本实验中使用的验证平台，由一个16线的速腾创的多线激光雷达和一个IMU，以及一台笔记本电脑构成，实验环境运行在Ubuntu18.04+ROS Melodic环境之中，通过实验平台采集真实数据记录成bag数据包，通过真实环境数据包运行LOAM方法和本文提出方法。

图5 实验平台

实验场景如图6为一被树木包围的空旷广场上进行。实验中按照顺时针方向在广场上移动。在移动过程中，实验平台的左侧一直面向广场缺少约束的环境，右侧一直是树木这种几何结构缺少约束的环境。最终结果如图7，左侧LOAM算法在此退化环境中获取不到足够约束，定位建图失败，右侧是本文提出方法，通过添加了IMU对位姿进行了估计，很好地完成了定位与建图的任务。