基于滑动窗口优化的激光雷达惯性测量单元紧耦合同时定位与建图算法

刘振宇， 惠泽宇*， 郭旭， 李刚

(1. 沈阳工业大学信息科学与工程学院， 沈阳 110870； 2. 沈阳新松机器人自动化股份有限公司， 沈阳 110169)

准确的位姿估计是移动机器人投入实际应用的关键，它确保机器人在运动中能够计算出自己的位置和姿态，为同时定位与建图(simultaneous localization and mapping，SLAM)和导航提供了数据支撑[1]。

在建立周围场景时，根据所使用的传感器不同，可分为激光SLAM和视觉SLAM[2]。三维激光传感器根据其硬件结构，在无需提前布置场景的情况下可以直接获取场景的三维点云信息，并且基于三维激光的SLAM技术在室外场景中不受到光照、尺度等因素影响，还可融合多传感器加强定位精度，为后续移动机器人导航环节提供更为精确的先验信息，因此成为当前定位方案中较为主流的新技术[3]。然而单独的三维激光雷达传感器的应用还不完善，从其本身来看，存在以下缺点：①垂直分辨率较低，获取的稀疏点云提供的特征有限，使帧间匹配环节变得困难[4]；②当雷达安装在移动机器人上，机器人运动时雷达获取的点云存在运动畸变，直接影响传感器数据精度；③更新频率低，大概仅在10 Hz左右[5]。因此仅使用激光雷达进行位姿估计，输出位姿速度低，且在范围较大的室外场景，累积误差造成的轨迹漂移无法满足机器人快速定位的要求[6]。而惯性测量单元(inertial measurement unit，IMU)有着高达100 Hz角速度和加速度的更新频率，可在激光雷达一个扫描周期内为其提供状态信息，对单一传感器定位的不足进行补偿，提高精确度[7]。因此，现研究激光雷达与IMU融合进行数据融合的紧耦合SLAM定位算法，以期提升移动机器人位姿估计精度。

国内外许多学者对激光雷达与里程计的融合方案进行了研究，Zhang等[8]提出的激光雷达里程计和地图绘制(lidar odometry and mapping，LOAM)方案，用IMU辅助激光雷达计算方向，并假设匀速运动，将激光雷达和惯性测量单元解耦，主要使用IMU作为整个系统的先验，因此无法利用IMU对系统进行进一步优化。Bavle等[9]通过对传感器得到的图像提取语义信息，提出一种轻量级和实时的语义SLAM框架，然而在面临大型的复杂场景，会受到过多语义信息的干扰。Shan等[10]对LOAM进行优化，提出Lego-LOAM方案，该方案对地面分割，并引入后端优化和回环检测模块，是完整的SLAM系统。Ye等[11]提出的LIO-Mapping方案使用的是图优化的方法对激光和IMU进行紧耦合的里程计，然而处理速度慢，在运行时无法满足里程计实时性的要求，所以对其进行了降采样处理，室内隔一帧采一次、室外隔两帧采一次。Qin等[12]提出的R-LINS方案选用了迭代误差状态卡尔曼滤波(error-state Kalman filter，ESKF)进行激光和IMU的数据融合，同样属于紧耦合方案。Yin等[13]融合神经网络对三维点云从旋转和平移两部分进行处理，提高整个SLAM系统的鲁棒性。Lin等[14]在LOAM的基础上针对小视场的不规则采样做了优化，提出了一种比LOAM更具鲁棒性的里程计框架。Zhou等[15]通过改进的SUPER4PC和传统ICP进行前端匹配，通过NDT匹配局部地图实现精匹配。

受上述里程计方法的启发，现选用以Lego-LOAM方案为基础进行改进，以期改善里程计定位精度低、鲁棒性差的问题。

1 系统框架及算法理论

在Lego-LOAM方案中，系统整体精度高，但里程计精度低，主要通过回环检测模块对全局位姿进行校正。但应用在大场景建图时，里程计部分漂移过大后，当再次回到原点时，系统无法识别此时是一个回环，因此无法对全局位姿进行校正，主要原因如下。

(1)去除点云畸变时，采用松耦合方式进行修正，需要提供精确的角度初值，但IMU高频噪声没有被预先处理，整体效果不理想。

(2)特征匹配时特征点较多，在相近区域内的特征点匹配时会产生错误匹配的现象，在相邻帧计算位姿变换时产生较大的累积误差。

(3)Lego-LOAM在里程计部分属于IMU和激光的松耦合，数据间缺少相互校正的环节，在大场景建图时，里程计误差较大，后端优化和回环检测部分无法校正全局位姿。

针对Lego-LOAM方案在里程计部分存在的问题，提出如下4种解决方案。

方案1：引入IMU进行线性插值，消除点云畸变。

方案2：引入地面分割模块，快速识别点云中的地面部分并提取。

方案3：以曲率为标准提取点云中的边和面特征，按特性对提取的特征点进行分类，分别进行储存；在匹配阶段对边和面特征按所分类别进行相应匹配，降低错误匹配的概率。

方案4：利用滑动窗口优化的框架融合激光数据和IMU预积分数据，提高里程计位姿估计的准确率。

改进系统框架如图1所示。

为加速度；为角速度；m为距离因子图1 改进算法系统框架图Fig.1 Improved algorithm odometer framework

2 里程计算法理论

2.1 数据预处理

(1)

(2)

(3)

式中：Δpij、Δvij、Δqij分别为从上一时刻i到当前时刻j的位置、速度、旋转变化量；pi、pj分别为上一时刻与当前时刻的位置；gW为重力系数；ba、bg为IMU读数的零偏与噪声；vi、vj分别为上一时刻与当前时刻的速度；符号⊗表示四元数相乘。

图2 线性插值Fig.2 Linear interpolation

定义当前激光点的时间戳为tL，curr，IMU在j帧时在世界坐标系的状态为

(4)

(5)

(6)

(7)

2.2 地面分割

对于激光雷达扫描的点云数众多，全部处理对于计算机资源消耗量大，因此将部分对于全局特征不明显的点优先提取出来，不参与后面的特征提取和相邻帧匹配环节，这里采用点云的几何特征进行地面分割，保证系统的实时性。

根据激光雷达的垂直分辨率为-15°～15°，选取0到7号线束所扫描的点云进行分类处理。

定义一个夹角β，已知激光雷达线数Rn、Rn-1对应的深度分别为OA、OB，垂直方向激光雷达线束与水平线的夹角为α1、α2，角度β的计算方法如式(8)所示。由于地面是一个水平面，根据它的特性，将所有β<5°且‖BM‖<5 cm所对应的点识别为地面点区域。地面分割算法设计示意图如图3所示。

(8)

图3 地面分割算法设计Fig.3 Ground segmentation algorithm design

2.3 特征提取与分类

为提高计算效率，需要进行激光雷达的特征提取。主要提取的是平面上和边上的点，因为这些点可以从激光的每次扫描中提取，通过曲率和距离变化来选择，即

(9)

为了从各个方向上都能均匀地提取特征，将一圈点云平均分成6份，在每个区域内对点的曲率进行大小排序，选取曲率最大和最小的点，分别对应最像边的点和最像平面的点。

需要说明的是，针对激光雷达一圈扫描的起始和结束分别对应的前后5个点是不参与运算的，因为这10个点没有与其对应的前或后5个点，无法完成平滑度的计算。

将每个点所对应的平滑度与设定好的平滑度阈值进行比较。将平滑度小于阈值的点设置为平面特征，如果该点前后5个点已经存在一个平面特征点，则跳过该点，并从上一平面特征点范围外继续选取；边特征点是在平滑度大于阈值的范围中选取，选取条件与平面特征点一致。

2.4 点云分割

在提取特征点的过程中，一些不稳定的点同样也会被提取，包括遮挡点和离群点，这类特征点在移动机器人运动过程中会消失，在特征匹配阶段导致误匹配，降低位姿估计的精度。

这里采用聚类的方法，将一部分有可能是同一物体的点云几何特征进行标记，并在后续匹配过程中使用该标记的特征点云进行处理，主要分为两部分，如图4所示。

对于同一条水平激光线上的特征，首先选取相邻且深度差在阈值以内的点，根据公式进行阈值判断。

Δd=‖OFi，k‖-‖OFi，k+1‖=d1-d2cosα

(10)

(11)

式中：‖OFi，k‖为特征点深度值大的点记为d1；‖OFi，k+1‖为特征点深度值小的点记为d2；α为两个相邻特征点之间的分辨率。

经过粗分割后，可将同类特征进行关联处理，同时也有效地去除激光雷达扫描时由于物体材质原因生成的无效点。

按照深度值较大的特征点所对应的激光线深度到下一个相邻特征点连线所对应的夹角β，对不同特征进行精分类，并设定一个阈值参数η用于判断相邻两点特征是分割成两个类别或者合并成一个类别。

d为特征点的深度；α为水平分辨率；β为相邻特征点连线夹角； a、b、c为同类型特征点图4 点云聚类Fig.4 Point Cloud Clustering

2.5 激光里程计

2.5.1 帧间匹配

图5 优化窗口Fig.5 Optimize window

通过建立局部地图，可以找到当前时刻特征点FLα与局部地图的对应关系，其中α∈{p+1，p+2，…，i}特征点包含之前特征提取部分的平面点和边点。

(12)

对其中的每个特征点xLp拟合成平面，通过求解线性方程组的形式可以得到平面系数为

ωTx+d=0

(13)

式(13)中：ω为平面法向量；d为p时刻坐标系下的截距。

定义一个距离因子m=[x，ω，d]，记录每个x∈FLα的特征点到面的相对测量值，其残差可以表示为

(14)

2.5.2 数据融合

滑动窗口有助于限制计算量，当新的位姿约束出现后，边缘化窗口中最早时刻的位姿约束，在窗口内从p时刻到i时刻有Na个IMU状态信息，整体待估计的变量有

(15)

构建代价函数求解状态变量X的全局位姿估计，表达式为

(16)

(17)

非线性最小二乘形式的代价函数可通过高斯牛顿法求解，这里通过谷歌的非线性优化库Ceres Solver进行求解。

3 实验结果与分析

3.1 软硬件平台

实验平台的软硬件配置如表1所示。其中，移动平台如图6所示。

表1 软硬件配置Table 1 Hardware and software configuration

图6 移动平台Fig.6 Mobile platform

3.2 预处理结果分析

在该阶段主要处理在移动机器人运动过程中造成的点云畸变，为后续点云处理提供一个良好的先验条件。

根据自行录制沈阳新松三期公司停车场数据集进行算法测试，根据图7可以看到，该预处理算法可以有效降低点云畸变。

图7 点云预处理Fig.7 Point cloud preprocessing

3.3 特征提取与地面分割结果分析

针对图8(a)中预处理后的点云进行特征提取，图8(b)～图8(e)均为点云分割后的不同特征的结果。

地面特征提取效果如图8(b)所示，其中不同颜色表示激光扫描物体的反射率；图8(c)为地面点提取的特征，通过对地面特征仅计算每个均分区域内平滑度最大的那些点，并进行分类；通过点云的聚类，将树干、路标和墙角这类边特征进行分类，如图8(d)中绿色的特征点为提取的边特征；分类后的面特征如图8(e)所示。

图8 点云特征提取与分类Fig.8 Feature extraction and classification of point cloud

3.4 里程计精度分析

3.4.1 KITTI数据

使用KITTI00号数据集进行测试，分别利用Lego-LOAM、LIO-Mapping和改进方案LIO-SWO(sliding window odometer， SWO)对里程计输出轨迹和真值进行精度评定，定位精度评价指标选取绝对位姿误差(absolute pose error， APE)和均方根误差(root mean square error， RMSE)，使用EVO插件对轨迹进行评定。

(18)

(19)

式中：Xi为算法估计出的位姿；Xt为GNSS的真值。

图9为改进方案所建的KITTI00数据集的地图，其中红色三角形为数据录制的起始点。

表2为三种方案在KITTI00数据集下建图的绝对位姿误差。

改进方案相比于两种比对方案，绝对误差的均值最小为1.14 m，相比于Lego-LOAM算法结果(2.86 m)和LIO-Mapping算法结果(1.53 m)分别提高60.1%和34.2%，但从均方根误差的对比来看，略低于同样使用紧耦合框架的LIO-Mapping方案。

图9 KITTI00建图Fig.9 KITTI00 mapping

表2 绝对位姿误差Table 2 Absolute pose error

3.4.2 厂区数据

对改进算法的滑动窗口优化位姿模块进行测试，数据集选用厂区办公楼，总长度为943 m，通过室外大场景测试改进算法的鲁棒性。厂区地图如图10所示。

通过记录优化后的位姿输出轨迹显示，可以看出，在进行大场景的SLAM作业时，Lego-LOAM算法的累积误差逐渐加大，如图11(a)所示，且原有的回环检测模块也由于漂移过大，未能在回环处进行闭环以优化位姿。

图11(b)和图11(c)分别对应LIO-Mapping轨迹和改进算法LIO-SWO轨迹，使用了紧耦合框架的两种算法，在建图距离较长时依旧可以维持较强的鲁棒性；对比起点和终点轨迹，改进算法在累积漂移部分的处理略优于LIO-Mapping。

图10 厂区地图Fig.10 Factory map

图12为改进算法的相对误差评估，通过计算里程计在相邻时间戳位姿变化量与真值在相邻时间戳位姿变化量，对得到的结果作差以获得相对位姿误差，在943 m的园区中，改进算法的漂移量误差的最大值不到1 m。可以看出，基于优化窗口的紧耦合里程计框架能够在大范围室外场景SLAM作业时，以较低漂移量完成定位建图工作。

3.5 运行时间评估

前端激光里程计需要首先满足快速性的要求，分别对Lego-LOAM、LIO-Mapping和改进方案进行逐个模块的处理时间进行比对，采用的数据集包括16线激光雷达和IMU的数据。具体步骤为在每个模块插入起始和结束的时间函数，用以记录从数据进入该模块到处理结束期间的时间戳。表3为上述方案的不同模块消耗时间。

图11 三种方案轨迹Fig.11 Trajectory of three schemes

在特征提取模块，改进方案除了提取了边面特征外，还融入了不同特征的聚类和分类存储，耗时方

图12 相对位姿误差Fig.12 Relative pose error

表3 各个模块数据处理用时Table 3 Data processing time of different schemes

面稍有增加。

在激光里程计模块Lego-LOAM方案属于轻量化，以稀疏的特征点云输入里程计模块进行处理，同时对优化函数直接进行分解处理，没有使用优化库，耗时为12.5 ms，改进方案采用特征分类匹配，同时引入IMU数据作为全局先验，耗时较Lego-LOAM略有增加，但优于采用多步优化的LIO-Mapping方案。

优化建图模块中，Lego-LOAM方案加入了回环检测，通过对当前扫描与全局地图进行匹配，判断是否回到原点，耗时281.1 ms，改进方案使用滑窗扫描多帧特征建立局部地图，通过帧与局部地图的匹配，并在窗口中完成优化，耗时较引入IMU数据优化的LIO-Mapping方案稍有增加。

在整体定位环节中改进方案需要282.3 ms计算每帧位姿，Lego-LOAM和LIO-Mapping分别需要308.3 ms和233.6 ms，由于改进的LIO-SWO方案为了实现紧耦合构建基于IMU和激光雷达的优化函数，提高整体定位精度，所以总体耗时较比对方案有所增加，但依旧小于激光雷达10 Hz的接收频率，即激光雷达每扫描一圈获取的数据时间为100 ms，满足里程计实时性的要求。

4 结论

面向室外大场景，针对现有里程计在该场景中定位精度低、鲁棒性不足的问题，提出一种三维激光和IMU惯导紧耦合的里程计方案，主要包括4个部分：数据预处理、地面点云提取、特征点云提取与分割、紧耦合激光里程计框架。

通过使用自行录制的停车场数据集和公开的KITTI数据集分别对各个模块处理效果和里程计精度进行评测，以及对运行时间进行分析得出以下结论。

(1)改进方案的数据预处理环节可以有效地去除激光雷达在运动时的点云畸变。

(2)通过对地面点的提取以及特征点的分割，对里程计部分的处理时间有小幅度提升，在帧间匹配环节也有效抑制了错误的匹配。

(3)建立局部地图与优化滑窗并且构建联合优化函数，提升了激光雷达和IMU数据的关联性和里程计定位的精度，同时依旧满足实时性的要求。

在未来的研究中，可从以下两个方面进行改进。一方面，针对动态物体所造成的点云拖影问题，加入一个识别动态物体的模块，并对其进行跟踪，精确检测和分割动态物体，优化拖影问题；另一方面，在室外场景，通过提取关键帧，并计算关键帧位姿与GNSS真值在优化窗口内协方差，当漂移过大时引入GNSS真值进行校正。