黄 瑞，张 轶

(四川大学计算机学院 成都 610065)

最近几年来，由于自动驾驶及自主机器人的迅速发展，作为其基础核心技术的时定位与地图构建技术 (simultaneous localization and mapping, SLAM)获得了较多的关注[1-4]。且由于硬件的发展，特别是以多线激光为代表的高精度空间传感器的出现，SLAM 系统在导航精度鲁棒性上都有了较大突破。得益于激光雷达能够获取高精度的距离信息、不受光照变化的影响，且能够提供丰富的结构化信息用于位姿估计，近年来涌现了众多的基于激光雷达的SLAM 算法[5-8]。这些算法的稳定性、定位精度、运算实时性相较基于视觉或视觉与惯性测量单元 (inertial measurement unit, IMU) 融合的方案都有了较大的提升。但基于激光雷达的算法也面临一些其他的问题，如：空间结构信息较少时系统稳定性及精度问题、运动畸变的矫正、动态场景问题等。

为了适应复杂多变的周围环境和无规律的自身运动，需要一种兼具鲁棒性和准确性的定位与建图的方法。在过去的十多年中，基于视觉和单线激光的定位方法取得了较多的成果[2,6-8]；而最近几年，基于多线激光雷达的SLAM 受到了广泛的关注[8-10]。但是现有的激光雷达SLAM 算法在平面结构较多且空间狭小的环境中定位时，会受到结构化约束信息较少而带来的位姿更新误差的影响。更重要的是，如果激光雷达本身进行剧烈运动，其将表现出与结构化信息较少相似的特点：定位误差较大且建图不精确。在剧烈运动过程中，相比处于结构化信息较少的环境，系统更容易积累，这就导致了其定位和建图都会产生更大的误差。

在应对激光雷达剧烈运动方面的文章较少，且都是关于多传感器融合的[11-15]。其通常做法是，使用激光雷达和惯性传感器(inertial measurement unit,IMU)紧耦合，相互之间提供约束，共同估计自身位姿。这种方法面对剧烈运动时误差累积小、精度高[16-17]，但是存在以下两个问题：

1)多传感器之间的时间同步较难[17-18]，只能在设备使用前，通过静态方法估计，然后在后续优化问题中作为常量添加；

2)由于多传感器紧耦合框架复杂，且严重依赖于传感器的自身特性[19-21]，最终会对系统稳定性造成较大影响。

本文只使用激光雷达，免去了时间同步误差及多传感器外参标定误差的影响，同时可以保证在剧烈运动场景中误差与多传感器紧耦合的误差处于同一量级。实验中发现单纯使用激光雷达在特殊场景中定位精度低，且主要是以下两方面的原因：1)迭代优化过程中，固定的退化处理门限不适应动态的场景变化；2)过大噪声导致前端里程计不准，从而使得点云畸变矫正出现问题。本文主要对这两方面进行改进。

1 算法描述

本文提出一种适应性强、在各种环境及运动状态下都有较高精度及鲁棒性的算法。主要从两方面改进：1)迭代优化过程中，不再固定退化门限，以动态门限和静态门限结合的方式，综合处理点云退化的情况；2)在最终执行ICP (就近点迭代)之前，使用之前已有的局部地图，进一步进行优化位姿后再一次进行畸变矫正，从而使得最终优化的时候引入更少的噪声，对应1)中较平缓的进行门限变化，保证普通场景中门限变化是由于所处环境变化而非偶发性的运动状态变化导致。

1.1 退化处理相关

文献[22]通过添加扰动找到退化方向，并通过统计分析，确定一个退化值的门限，小于这个退化门限的特征值都可以认为在这个方向上有退化。本文继承这种处理方式，但是采用了更加灵活的方式减小噪声的影响。

在非线性优化中，高斯牛顿法是最常用的优化算法，其思路是在函数的局部极值点附近，对函数进行二阶泰勒展开。由于泰勒展开的二次项中海森矩阵的二阶偏导计算复杂，所以一般用一阶雅可比矩阵计算得出。最终得到一个线性方程组：

式中，P为变换矩阵，是由n个特征值对应的n个线性无关的特征向量按照特征值的对应关系所组成的矩阵；A为对角线上存在零元素的对角矩阵，表示在基空间中对弱约束维度屏蔽的线性变换；B是A的相似矩阵，处在优化更新量的空间中，即B变换是更新量所在空间中对应的线性变换。A,B两个变换在不同基空间中描述了同一件事情：即对弱约束维度的屏蔽。

本文迭代过程中用到的3 种海森矩阵特征向量组成的矩阵：

而考虑弱约束的完整更新量为：

1.2 畸变矫正

图1 畸变矫正不好时，输入给后端ICP的点云会出现较大畸变

通常算法采用了一个默认的假设：前端假设激光雷达是匀速运动的。因此这种假设在一般情况下表现较好，但是当位姿在短时间内变化较大时，就会失去精度。而这一步解算出来的位姿会作为运动畸变的矫正参数，由此导致畸变矫正出问题，是输入给后端优化的参数的进一步恶化，如图1所示。这种未很好矫正畸变的点云在后端精细化的ICP 中会引入较大噪声，使得迭代优化过程中即使有较好的位姿初始值，也会受到较大的噪声影响，带来的噪声又同时会对退化的判断造成影响。因此本文在输入后端进行全局优化之前，先根据已有局部地图对位姿做一遍修正，之后用修正的位姿送入后端进一步迭代优化。

1.3 算法整体流程

在上述算法中，引入动态门限：当动态门限D2增大时，表示当前噪声影响较大，需要在下一次迭代过程中剔除更多的噪声影响；当动态门限D2减小时，表示当前噪声的影响并不大，可以在下一次迭代过程中剔除更少的噪声影响。同时，由于激光雷达测量的环境结构信息具有时间上的连续性，动态门限D2的引入能更好的反映当前时间段所处环境的结构信息丰富与否，同时在面对复杂多变的环境时有更好的适应性。整体的算法流程如图2 所示。

图2 算法整体流程图

该系统首先进行帧间匹配，此时并不考虑帧间迭代过程中的噪声与退化，因为基于帧间对应扫描线匹配的位姿迭代足够鲁棒。之后进入局部地图优化模块，若出现退化，则按照算法1 屏蔽特征向量，同时判断当前动态积累的退化帧数是否大于5(表示动态累计退化时间大于0.5 s)。若count 大于5，则需要局部优化的位姿进行第二次畸变矫正。将点云送入当前维护的局部地图，再次进行位姿的迭代优化。

该算法在多个层级上减小噪声对SLAM 系统的影响，保证系统在各种环境，各种运动状态中都有较好的定位与建图精度。同时，当设备并未处于持续性的剧烈运动中时，系统并不进行局部地图优化，保证了系统整体的计算量较低。

2 实验与分析

2.1 室内公开数据集测试

对比本文算法、LOAM 算法和LIO 算法的实现数据，数据集[17]为公开的室内无人机采集的数据集，表1 为数据集中fast1 数据的对比。

表1 3 种算法的定位精度统计

由上表可以看出，本文提出的算法相比于同样只使用激光雷达的LOAM 算法，在定位精度上有了很大的提升。同时相较于使用激光雷达和IMU紧耦合的LIO 算法，本文提出的算法在定位精度上也没有太大的差距。进一步的，本文提出的算法在室内激烈运动场景中，对平移量的相对误差控制得较好。在误差水平上，整体跟LIO 处于同一个量级。但本算法的旋转量相对误差优势不明显，整体误差水平和LOAM 接近。因为LIO 使用了IMU 与激光雷达的紧耦合，更容易获得较为精确的旋转信息，而本文算法旋转信息的估计本质上还是依靠帧间匹配，相对于同样使用帧间匹配约束的位移信息更加不好约束，因此，图3a 和图3b 中，本文算法轨迹相对于LIO 算法轨迹表现出更明显的在真值附近来回震荡的效果。

图3 室内轨迹误差对比图

2.2 自有设备数据采集测试

本实验采用自行录制的数据，硬件如图4a 所示，无人小车搭载rslidar-16、GPS、相机及毫米波雷达(用于后期自主导航等用途)，测试中故意制造较大的角度变化。在这种场景下，迭代优化过程将会出现退化，原始的退化控制算法将造成某些方向上的不可控。

可以看出，图4a 在优化过程中，角度优化量出现了较大的误差。为了分析以上过程，画出了各种情况下的海森矩阵的特征向量矩阵及相似变换矩阵。

图4 测试环境为楼道即狭长走廊的测试结果

各种情况下的特征向量矩阵及相似矩阵如图5 所示。其中，5a 表示受到较好约束的迭代过程，此时每行每列都有明显的极大值，只在平面运动(即F和L)上有少许噪声。但是对结果影响不大，因为平面运动中，平面上的两个轴会相互影响。5b 表示受到了严重的噪声影响，此时P,Y,F列与V1,V4,V5行都 没有 出 现明 显的极大值。此时红线上方代表固定门限判定的退化维度，此时为forward 方向。图5c 对应图5a效果，在方向维度出现了较大误差，即出现了伪退化的情况。

图5 各种情况下的特征向量矩阵及相似矩阵

图5d 表示真实退化情况下算法估计的结果。退化维度(即V4行)的更新量不是由其自身(即F列)主导的，而是通过所有更新维度对其进行估计。与图5d 不同的是，图5c 中最后更新量依旧取决于F的原始更新量(即[V4,F]依旧是所在行与列的极大值)，但此时的海森矩阵的特征向量矩阵已经屏蔽掉了F维度，这也造成了图5b 中V1维度在其他维度上的投影也被屏蔽了。此时5b 中第二大值[V1,P]也受到了较大影响，导致其在pitch 角度的更新量受到严重影响，即对应图4b 中所示的轨迹和建图误差。综上，该场景下，迭代优化不应该被判断为退化，这也是本文提出动态门限的作用，最终的结果如图4b 所示，此时更新量正常，没有出现严重的建图误差。

2.3 室外公开数据集测试

实验使用室外公开数据集[23]，该数据集使用20 Hz、16 线激光雷达，结果如图6 所示。该数据集中传感器处于长期的平面运动中，期间经过城市街景和树木较多的类森林路段，周围环境复杂多变，可以较好地评价算法性能。

在这个数据集中，LIO 的表现较为糟糕，其垂直方向的轨迹出现了较大误差。因为激光雷达和IMU 紧耦合的框架不适合长期的平面运动，IMU 某些轴的未充分激励很容易导致将随机误差引入系统中。LOAM 的表现稍好于LIO，但是依旧存在较大误差。由于该数据集设置20 Hz 的采集频率，造成单帧点云数量减少一半，有用约束减少，系统容易出现退化，特别是在后半程树木较多的场景下，结构化信息约束杂乱，噪声较多，容易出现退化。而本文算法计算的轨迹非常接近真值，表明本文算法在室外开阔且多变的场景中，依旧有较好的表现。

图6 室外公开数据集轨迹对比图

3 结 束 语

本文采用多层级多分支的数据处理流程，最终达到了在室内剧烈运动场景和室外开阔且又变化的场景都有较高精度的效果。改进的退化算法保证系统能尽可能减小多种场景下的噪声影响，同时有选择的进入局部地图优化，既保证了设备在剧烈运动时能通过更多的信息优化自身位姿，又使得在普通场景中，系统不会消耗太多计算资源。最后，实验数据表明本文提出的算法流程，能够兼顾多种运动场景和多种运动状态，在复杂的环境和运动中保持较高的定位和建图精度。