基于ESKF 与图优化的轻量级多传感器融合SLAM 系统

2021-04-24李文豪

科学技术创新 2021年8期

李文豪

（广东工业大学，广东广州510006）

1 概述

状态估计、定位和建图是移动机器人的关键技术，是反馈控制、避障和规划等任务的先决条件。目前主流的是基于视觉以及激光的建图定位方案，并且投入大量的研究在如何确保系统的稳定实时运行中。基于视觉方法通常使用单目或立体相机对连续图像中的特征进行分析以确定摄像机的运动。尽管基于视觉的方法是可行的，但它们对初始化、光照以及环境的纹理比较敏感，这使它们不可靠，但是基于视觉的方案特别适合位置识别。另一方面，激光雷达对环境不敏感，光照变化基本不会影响激光雷达。尤其是远程高分辨率三维激光雷达的应用，例如Velodyne VLS-128 和Outster OS1-128 激光雷达,使得激光更适合直接捕捉三维空间中的环境因此，本文的研究重点是基于激光雷达的状态估计和SLAM方法。我们提出的方法有如下几个特点：

1.1 结合点线、点面ICP 以及ndt 算法实现激光里程计的匹配，并采用了一种高频帧间匹配与低频Map 优化的策略。

1.2 采用一种以机器人坐标为中心的ESKF 对IMU 进行融合，并实现对先验观测的融合，可更好适配室内平面环境。

1.3 实现一种简单直接的去除激光匹配过程中动态物体影响的方法。

2 相关工作

对于激光SLAM任务，核心是根据传感器两帧的数据推断其运动，其最经典的方法即ICP 算法[1],ICP 算法衍生出了多种变体，如IMLS-ICP、NICP、GICP[2]等,近年，Zhang 提出了一种基于特征提取的ICP 变种方法[3]，它首先进行特征提取减少匹配点云量并实现点云的配准。由于机械式激光的旋转机制，当激光传感器运动时，产生的点云会产生较大的畸变，因此单独使用激光雷达姿态估计效果不理想，必须要另外的传感器提供信息去进行补偿，此外，单独的激光传感器无法适应剧烈的运动以及退化环境，多传感器融合提供了解决上述问题可靠的方法，多传感器融合通常分为松耦合与紧耦合，松耦合通常使用EKF，例如[4-6]采用EKF 去融合IMU、GPS、Lidar 等传感器，紧耦合由于可以获得更好的精度，所以获得了更大的关注，[7]实现了一种基于滑动窗口优化的紧耦合系统，[8]实现了一种基于IEKF 的紧耦合激光惯导里程计。

3 系统框架

系统主要由数据处理、激光惯导里程计、后端优化、回环检测等几个模块构成。系统采用ROS 作为通信框架，ROS 是一种适用于机器人的开源系统，可方便实现进程间的通信，以及进程的管理，并有丰富的数据分析工具如可视化工具RVIZ 等。

4 数据处理

数据处理的主要完成滤波与畸变去除，原始的激光雷达点云数据包含大量的噪声，并且过于稠密，因此需要通过滤波去处理，滤波方法如下：首先采用距离滤波提取出ROI 区域范围内的点云，然后采用基于角度的快速聚类算法[9]对点云进行聚类，剔除掉聚类后数量少于一定阈值的点，只保留一定体积的点云进行匹配，聚类滤波并且过滤掉地面后的效果如图1 所示。

图1 滤波提取后的点云

然后基于曲率提取特征点，特征点有两类平面点与曲面点，具体的实现方法参考LOAM[3]。经过优化，滤波以及特征提取的的任务可以在5ms 以内的时间内完成。

5 激光惯导里程计

激光惯导里程计输入IMU 数据以及提取的特征点数据，输出系统运动的状态信息。出于轻量化以及运行效率考虑，我们采用高频的scan-scan 匹配结合低频的scan-map 优化的前端思路，可以最大限度的平衡快速性以及精确性，同时在scan-scan匹配的阶段，我们采用eskf 融合IMU 与激光的估计结果，同样为了追求快速性以及高效率，我们采用松耦合的策略，激光惯导里程计模块的结构如图2。

图2 激光惯导里程计模块

激光惯导里程计主要由两个模块构成，LIO 模块- 融合scan-scan 的匹配结果与IMU 的运动预测结果，校正模块则负责对其结果进行优化。Scan-scan 匹配通过最小化点线、点面的残差从而求解最优变换T，即求解一个非线性最小二乘问题。点线，点面的残差构造如下：

上式可以采用高斯牛顿法迭代求解，首先求解出，如下式所示：

6 图优化融合

图优化信息融合主要负责融合激光惯导里程计、GNSS 信息、回环检测信息，结构如图3 所示，我们采用g2o 实现，执行图优化的策略如下：

(1)具备回环信息时，执行全局图优化。

(2)由于激光里程计的飘逸很慢，没有必要一直添加GNSS 因子，当激光里程计输出的状态协方差过大，且GNSS 协方差足够小时，添加GNSS 约束因子，并取前m 个关键帧执行局部图优化。

(3)当机器人出于平面场景下运动时，可以添加先验全局平面约束，由于LIO 模块融合了对于平面信息的观测，因此全局平面约束可以每间隔n 个关键帧添加一次。

图3 后端优化框架

7 实验

为了测试系统的实际性能，搭建了载人车辆并在校园环境下以及开源数据集下进行了测试，实验车辆如图4 所示。

图4 实验自动驾驶车辆

我们使用velodyne VLP16 激光雷达、MTI-300S 惯导以及差分式GPS 作为实验传感器，工控机的配置为i7-8550u, 8g 内存。在校园环境进行了测试，并将最终输出的点云地图与实际的卫星地图进行比较，可以看到点云地图可以与卫星地图很好的进行匹配，这说明该系统估计的位姿与GPS 真值之间具有很小的误差，另外，由于高频scan-scan 的匹配与低频scan-map 的匹配是通过两个独立的进程完成的，系统状态输出频率是由scan-scan 的频率决定的，而scan-scan 的匹配由于具有较少的约束，因此可以在很短的时间完成，经过实际的测试，对于16 线的激光雷达，scan-scan 的匹配可以在10ms 左右的时间完成，完全达到实时的需求。