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

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

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

式中，海森矩阵近似为$H = {{J}}{(x)^{\rm{T}}}{{J}}(x)$，其中${{J}}(x)$是雅各比矩阵；$g = - {{J}}{(x)^{\rm{T}}}{{f}}(x)$，${{f}}(x)$表示优化过程中当前点的残差；$\Delta {{x}}$是需要求解的更新量。在迭代优化过程中，为了提升系统的鲁棒性，现有做法是，在某个固定的门限下面对优化过程中约束较弱的空间维度做最优估计，使这个维度上的信息不影响最终的更新结果。设矩阵变换B能将这些弱约束维度在更新结果中去除，则B为：

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

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

式中，V_p代表特征值较小所对应的特征向量，也是将要屏蔽的维度；V_u对应特征值较大，将用于特征值较小维度的最优估计；V_f包含了所有的特征向量。高斯牛顿迭代解出来的优化量为$\Delta {{x}}$。则之后的迭代优化的增量就变为了：

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

可以看出，$\Delta x$可以分为两种更新量，处理后的更新量$\Delta {x^{\rm{*}}}$中没有考虑弱约束方向带来的更新量。但是，由于决定弱约束方向的退化门限是通过统计及经验得出的固定值，并不能很好地反映各种环境下真实的退化门限，这可能会带来退化门限过大或者过小的问题。当退化门限过大时，较多的有用信息会由于相对不大的噪声而被完全抛弃，而这些方向上的最优估计可能并不准确；当退化门限过小时，迭代更新量会较多的受到噪声的影响，导致更新量异常地变大并偏离正确的更新方向。

为了弥补退化门限固定所带来的影响，本文设计出一种动态的自适应的更新退化门限的方法。对退化门限做以下改进：由两个退化门限共同决定是否执行退化时的最优估计操作，退化门限D₁为固定值，其值较大，用来初步判断迭代过程是否退化，通过统计分析得出^[11]；退化门限D₂为动态变化的值，主要为了消除原始退化门限过大时，有较好约束的有用信息损失的问题。当通过D₁判断出现退化的时候，将各维度特征值与动态门限D₂对比，若小于D₂则进一步对比，以D₂作为退化门限的两种更新量$\Delta {x_{p2}}$与$\Delta {x_{u2}}$。

若$\left| {\Delta {x_{p2,i}}} \right| > \left| {\Delta {x_{u2,i}}} \right|$，表示此时受噪声影响较大($ \Delta {x}_{p2,i} $与$ \Delta {x}_{u2,i} $分别表示i方向上，弱约束与强约束的更新量)，应该只保留有较好约束关系的更新量$\Delta {x_{u2}}$；否则表示噪声的影响其实不大，可不用改变更新量。当v_min位于D₁和D₂之间时，表示D₂决定的噪声并不是噪声的主体，其中还包括别的方向的噪声。如果此时在D₁决定的更新量中，噪声依旧有主要影响才按原始的方法屏蔽掉较小方向的噪声及更新量，从而避免出现“假退化”的情况。在单次迭代过程中，若动态门限D₂判断出现了退化，则表明当前动态门限较小，应适当增大，否则应该减小动态门限。综上，统计得出的静态门限D₁表示对噪声大小进行一次初步判断，其取值较大；D₂表示随环境噪声变化而变化的动态门限，其取值较小，表示对时变噪声是否对系统有较大影响的更稳健的判断。

通常算法采用了一个默认的假设：前端假设激光雷达是匀速运动的。因此这种假设在一般情况下表现较好，但是当位姿在短时间内变化较大时，就会失去精度。而这一步解算出来的位姿会作为运动畸变的矫正参数，由此导致畸变矫正出问题，是输入给后端优化的参数的进一步恶化，如图1所示。

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

这种未很好矫正畸变的点云在后端精细化的ICP中会引入较大噪声，使得迭代优化过程中即使有较好的位姿初始值，也会受到较大的噪声影响，带来的噪声又同时会对退化的判断造成影响。因此本文在输入后端进行全局优化之前，先根据已有局部地图对位姿做一遍修正，之后用修正的位姿送入后端进一步迭代优化。

退化算法流程为：D₁表示较大的固定退化门限，D₂表示动态变化的退化门限(其初值为D₁的一半)，v_min表示高斯牛顿迭代中海森矩阵的最小特征值，$\Delta {x_{p1,i}}$表示以D₁为退化门限所计算的${{V}}_f^{ - 1}{{{V}}_p}\Delta {{{x}}_p}$的第$ i $个更新量，$\Delta {x_{u1,i}}$表示以D₁为退化门限所计算的${{V}}_f^{ - 1}{{{V}}_u}\Delta {{{x}}_u}$的第i个更新量，$\Delta {x_{u2,i}}$与$\Delta {x_{p2,i}}$同理。count表示到目前积累的的退化次数，为动态变化的值，最小值为0，表示当前SLAM系统是否处于连续性的大幅度退化中：若处于连续性的大幅度退化中，则表示当前系统需要用局部地图来进行进一步的位姿迭代优化。

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

图  2  算法整体流程图

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

算法1：单次迭代中出现$ \exists {v}_{i}<{{{D}}}_{1}$时，执行以下步骤进一步确定退化情况

输入：A (迭代过程的海森矩阵)、$ \Delta {{x}}$(之前按没有退化算出来的更新量)、D₁、D₂

输出：通过进一步的判断更新或不更新$\Delta {{x}}$

开始：

　由A计算D₁下的$\Delta {x_{p1}}$与$\Delta {x_{u1}}$，D₂下的$\Delta {x_{p2}}$与$\Delta {x_{u2}}$

　　${\rm{if}} \;{v}_{\min} < {D}_{2}$

　　　${\rm{if}} \; \exists (\left|\Delta {x}_{p2,i}\right| > \left|\Delta {x}_{u2,i}\right|)$

　　　$\Delta {{x}}=\Delta {{{x}}}_{{{u}}2}$；更新动态门限：${D}_{2}= {D}_{2}+5; {\rm{count}}++;$

　　　else

　　　　更新动态门限：${D}_{2}={D}_{2}-5; {\rm{count}}--;$

　　　end

　　else if $\exists (\left| {\Delta {x_{p1,i}}} \right| > \left| {\Delta {x_{u1,i}}} \right|)$

　　　$\Delta {{x}}=\Delta {{{x}}}_{u1} ;{D}_{2}={D}_{2}+5;{\rm{count}}++;$

　　else count−−

　 end

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

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

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

图  3  室内轨迹误差对比图

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

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

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

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

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

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

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

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

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

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