IMU进行普通的3D SLAM能得到什么样的结果呢？

1

背景

此次的优化针对的场景是室内小场景、室内大场景和室外场景下运行3D SLAM时，z方向的误差累积、以及十万平米（≈400*400）建图时回环检测效率低下的问题，机器上使用的主要传感器3D LiDAR有velodyne16线（或ouster16线），IMU使用的是阿路比-LPMS-IG1。那么在进行优化前，进行普通的3D SLAM能得到什么样“离谱“的结果呢？有如此一幅点云地图（见图1上）在俯瞰视角下彷佛一切”岁月静好“，但在正视视角下其尾巴都翘上了天（见图1下）：

图1：开环室内20x260米走廊场景（俯视图：上，正视图：下）

由图1，我们可以看到在SLAM过程中x、y、yaw的状态量估计还是基本准确的，在点云地图上的边界没有出现明显的结构性问题，但是其在建图开始（始于地图最右侧）后，随着roll、pitch、z的误差严重积累，在跑完这260+米的走廊后终点位置（地图最左侧）的z坐标与起始点位置的z坐标偏差了8.57米（实际偏差<2m）。再看另一个现象在一个厂区室外130x240的回环区域下进行3D SLAM，得出了如图2所示的点云地图：

图2：闭环室外130x240米厂区场景（俯视图：上，正视图：下）

由图2，还是可以得出SLAM过程中状态量估计基本准确的状态为x、y、yaw，而roll、pitch、z的误差积累问题严重，在上图中长边（240米）左、右关键帧的位姿上z坐标间的差值有9.88米（实际偏差<2m）。

针对上面两个例子中发现的问题，本人将简单分析其误差的产生的原因，并分享根据误差产生原因的物理依据进行优化设计和实现思路。

2

轨迹z方向飘移原因分析

在SLAM建图过程中轨迹某个状态量的不准确、在某个状态量上的误差积累本质上都是算法设计时的假设与实际情况不符，且在进行SLAM问题建模和解决方案设计时没有考虑到假设与真实情况的不一致性而导致的解算误差。而在点云配准的阶段，经典点云配准算法ICP及其变体针对了各种环境、场景上出现的问题进行了各种各样的算法设计以想要高效且鲁棒的解决所有使用上的问题，但是z轴方向的飘移其实是由于z轴、roll和pitch这三个机器人状态量在一定程度上不可观测且难以评价和消除，虽然机器上使用了IMU和LiDAR传感器进行惯性信息与环境信息的输入，但轨迹中z方向的误差来源还是主要集中在LiDAR的数据。

2.1、LiDAR静态tf标定引入的误差

一般来说机器人出厂后都需要进行一次详细地标定工作，但即使再详尽的标定工作多多少少都还是会留有一些误差存在，另外随着机器的使用机器上实际的LiDAR静态tf也会因为机器的损耗逐渐与最初标定的数值发生偏差。以上图1中场景点云数据为例，在获取到一帧激光点云数据后直接进行静态tf的变换，将其从LiDAR坐标系转移到车体坐标系后可获得俯视图和正视图如图3所示：

图3：室内工厂单帧点云数据（俯视图：上，正视图：下）

由于该机器是刚出厂且完成了标定，可以由图3看到当机器人静止水平时LiDAR返回的点云数据经过tf变换后的roll、pitch误差还是较小的。但是在另外一台使用年限较长、经过多次维修的机器人上做相同的实验，roll、pitch的误差就变得肉眼可见的大了（见图4）。将上述两帧点云分别与对应的以LiDAR坐标系的点云进行ICP匹配以获取静态tf的真值，可以发现图3中roll、pitch的误差分别是0.011和0.008弧度，而图4中的roll、pitch误差分别是0.035和0.017弧度。

在上述分析的基础上，若是进行纯LiDAR的SLAM，一般情况下第一帧关键帧的位姿就是（0，0，0，0，0，0），而由于静态tf的误差存在其真实情况的关键帧并不完全水平，即使在后续的SLAM轨迹构件中没有其他问题发生，在开环情况下z轴的累积误差就是：

z轴的累积误差=(实际行走距离)×sin⁡(初始俯仰角误差)

图4：室外单帧点云数据（俯视图：上，正视图：下）

2.2、LiDAR竖直方向分辨率及入射角引入的误差

首先从3D激光雷达的竖直方向分辨率开始，以常见的机械式激光雷达为例，即使是128线的3D激光雷达，其竖直方向上的分辨率也是小于水平方向的分辨率的，更何况16线的3D激光雷达。以Velodyne-VLP-16为例，

传感器: Velodyne-VLP-16

16 通道 测量范围: 高达100米 范围精度: 高达 ±3cm (典型的) 垂直视场: + 15.0° 至 -15.0°(30°) 角分辨率(垂直): 2.0° 水平视场: 360° 角分辨率 (水平/方位角): 0.1°- 0.4° 旋转频率: 5 Hz – 20 Hz 集成网页服务器，便于监控和配置

由于较低的竖直方向分辨率和竖直视场，以至于每一帧激光数据能检测到地面的点云十分有限，且地图特征远远不如水平视场探测到的角点、平面特征，例子可见图3和图4中的俯视图，其中蓝色标记的点云即为激光打到地面上的点云。

其次，由于机器人上3D激光雷达普遍的安装方式（雷达水平安装在机器人正上方）导致在SLAM构建轨迹的过程中激光打到地面上的入射角很大（远大于水平视场的垂直入射角），雷达入射角越大其产生的光斑也就越大，绝对精度和重复精度就越差。

上述的结论，除了可以通过各个雷达技术论文和技术参数手册获取，还有一个广而知之的论证途径就是LOAM中针对激光点云数据的预处理中定义了两类异常点并进行了剔除，其中一类就是平行点（定义图示见图5），平行点指的就是图5中的B点，就是激光的射线几乎和物体的平面平行了，而LOAM中剔除这类点的原因有两个：

1、 激光的数据会不准，射线被拉长

2、这种点被视为特征点后会非常不稳定，下一帧可能就没了，无法进行下一步的点云配准，例如图5中的LiDAR原点再向左移一点，那么B点就消失了，与之形成对比的就是A点，极短时间内不会消失。

图5：LOAM中平行点定义图示

由此，若是在SLAM系统中的配准使用的是点云层面的处理，而不是基于特征的匹配，则入射角很大处的地面点云由于绝对精度和重复精度差，就很有可能影响点云配准时roll、pitch的观测估算了。

综上，LiDAR竖直方向分辨率及入射角对SLAM过程中轨迹z轴的影响在于两个方面，第一个就是由于竖直分辨率低导致地面点云及其特征少，特别是降采样后可能都没剩下多少可以用来点云配准了，第二个就是激光雷达打到地面上的点入射角通常较大，其点云精度差且不稳定导致在点云配准时出现误差。

3

SLAM轨迹z方向飘移问题优化思路

以上的分析都是常规过程中可量化且较为统一的误差来源分析，而其他的原因比如退化场景下配准算法失效引起的误差、配准算法本身性能导致的误差还有机器运算资源高压条件下SLAM运算阻塞引发的位姿滞后误差等不在本文中进行优化的讨论。下文中针对SLAM过程中轨迹z轴飘移的优化思路还是针对上文中的静态tf标定误差、LiDAR竖直方向低分辨率及大入射角带来的误差。

3.1、静态tf标定引入的误差优化

激光雷达坐标系与车体中心坐标系的静态tf，首先会在SLAM开始阶段引入一个初始位姿误差，因为机器人在每一次开始SLAM时不一定是完全水平的，那么为了解决这个问题就需要利用IMU的数据进行重力方向估计和对每一个关键帧位姿进行重力方向校准，这部分的工作可以参考LIO-SAM（局部因子图紧耦合的LIO）和VINS-Fusion（滑窗优化下的VIO）中针对IMU的高频惯性数据的处理。

图6：VINS中的滑窗优化示意图

以LIO-SAM为例，可以考虑将SLAM前端模块进行基于IMU的紧耦合局部因子图的联合优化，与此同时完成针对IMU数据的初始化（外参及bias的初始估计）、IMU重力估计模块和IMU预积分功能，局部因子图的构建主要节点还是基于点云配准后的关键帧，但此时关键帧之间会再多上一个重力方向校准的约束和IMU预积分计算出的惯性变换约束，以尽可能地减少静态tf和点云配准时带来的误差，特别是IMU重力估计模块，可以避免纯LiDAR SLAM在roll、pitch的完全不可观测问题，在很大程度上优化z轴的飘移问题。

图7：LIO-SAM的紧耦合LIO系统框图

3.2、竖直方向低分辨率及大入射角带来的误差优化

竖直方向低分辨率及大入射角带来的误差来源根据上文中的分析，可以归结为两个：第一，由于竖直分辨率低导致地面点云及其特征少，第二，激光雷达打到地面上的点入射角通常较大，其点云精度差且不稳定。一言以蔽之，3D激光雷达获取到的地面信息较少且获取到的点云信息较差导致的观测校正不够理想，误差由此被引入到SLAM轨迹中且难以在后续构建中被消除。

既然点云的信息又少又差，我们避免直接使用点云信息进行配准操作不就行了嘛。还是先以LIO-SAM为例，在其系统的激光回调中完成了以下关键步骤：

（1）将当前帧位姿初始化并随后提取当前帧的局部角点、平面点云集合，加入到局部map；

（2）将当前激光帧角点、平面点集合降采样后进行scan-to-map匹配，以优化当前帧的位姿；

（3）若当前帧为关键帧则执行因子图优化，更新因子图中所有变量节点的位姿由此获得优化后的SLAM轨迹。可以看到，LIO-SAM的系统中没有进行基于点云的配准，而是通过提取平面点进行基于特征的匹配优化计算，而且特征匹配时要求当前帧特征点数量需要足够多且reference匹配的点数也足够多才执行优化。

我们不妨根据上文中平面特征的特征匹配过程进一步扩展，平面点云特征不再单单作为用以前端关键帧位姿观测更新的一部分，而是将其扩展成一个地面的约束（Ground Constrain）以避免z轴的飘移问题，而添加了地面约束后即是尝试利用地面特征作为假设条件，在优化时考虑地面约束带来的残差以影响优化结构，为了避免添加地面约束对上下坡场景的不友好，可以对地面约束进行打分计算，只要地面特征足够“明显“和地面约束足够可靠，才在对应的关键帧处添加地面约束的残差块。

另一个延申就是可以针对每一帧点云数据进行地面点的分割和标记，而这个分割后的结果可以不再像上文一样单独使用在前端的配准模块中，而是进一步地被利用在回环检测模块中以保证地面特征被充分利用。在这里分享两篇对我启发较大的有关地面点云分割的技术论文和开源代码，

3.3、万能误差消除术

万能误差消除术其一：添加和利用好GNSS、Landmarks等外部的绝对位置信息，唯一需要注意的就是当GNSS数据或者Landmark信息被机器人获取时，需要自动地判断出该绝对位置信息是否可靠，避免因为使用垃圾、离群点位置信息而带来更严重的误差。可参考文章链接：https://zhuanlan.zhihu.com/p/603099637。

万能误差消除术其二：回环检测。回环检测是所有SLAM问题的解决方案里必不可少关键模块，比如经典的Scan-Context，基于多分辨率地图的分支定界+相关性匹配，以及利用语义信息的各种回环检测算法，只要在SLAM过程中找到正确的回环，基本上就可以消除90%的误差了，但是怎么保证回环检测的效率、准确度又是一个研究方向了。

4

优化结果展示

使用优化后的算法模块在相同的数据集和硬件环境下获得了与图1和图2例子进行对比的图10开环室内工厂环境和图8闭环室外厂区环境结果，其中优化后的结果（见图8）SLAM轨迹图中长边（240米）中最左和最右关键帧的位姿上z坐标间的差值从之前的9.88米优化成1.82m，图10的轨迹结果则从原先260米长度下累积8.57米误差，到现在300米累积0.89米的误差，以上两个对比实例都说明了上文中的优化思路是可行的。

图8：闭环室外厂区130x240米（俯视图：上，正视图：下）

图9：闭环室内+室外厂区205x240米（俯视图：上，正视图：下）

图10：室内厂区120x300米（俯视图：上，正视图：下）

除开大场景、长轨迹的SLAM构建，还使用优化后的SLAM算法模块在小范围的地库及室内房间场景进行了鲁棒性测试，地库场景的有效高度区间相较于工厂与室外环境会更加小，同时存在更多的动态障碍物（移动的车辆和行人），室内房间场景由于空间更加狭窄，3D激光雷达获取到的地面特征几乎没有，由此也就更加依赖IMU的重力估计和校准功能，同时还需要避免错误的地面约束影响SLAM轨迹发生偏移。图11和图12分别对应优化后的SLAM算法模块下地库、室内房间场景下的结果。

图11：室内地库50x70米（俯视图：上，正视图：下）

图12：室内房间22x35米（俯视图：上，正视图：下）

5

总结

在纯LiDAR SLAM的算法模块中添加IMU惯性里程计约束、IMU重力估计及校准，并针对点云数据进行地面点云的分割和针对地面点云进行平面特征匹配，可在一定程度上解决LO在z、roll、pitch这几个机器人状态量轻微不可观的问题。值得说明的一点是，在本人的优化工作中，先是尝试在SLAM算法模块的前端使用局部因子图框架，利用LiDAR观测（地面点云在此阶段也被利用到点云配准算法中以进一步提升z、roll、pitch的观测精度）得到的关键帧及帧间IMU惯性里程计约束，获取一个优化后LIO关键帧并输入到后端中，这样的处理在回环检测触发之前，前端LIO中还是会积累一定程度的z轴飘移（但比没有添加地面点云优化配准和imu约束前要好一些）。而最有效的优化处理则是在后端的优化问题构造时，针对每一个关键帧添加必要的IMU重力校准残差项和地面约束残差项，如此一来才能真正意义上的解决z轴飘移的问题。

在后端优化的改造工作中，难点在于如何保证添加的IMU重力校准残差项和地面约束残差项是精准和可靠的，在改造初期由于经常引入较差的IMU重力校准约束和地面约束而出现关键帧位姿出现大跳变和SLAM轨迹失败的问题，地面约束的约束质量确认可以通过地面点云分割质量和地面特征匹配得分的高低来进行把控，而要保证IMU重力校准精度，一方面需要加强针对IMU数据的使用问题建模和解决方案设计，添加重力校准约束前再进一步进行完整性分析以最后把控约束质量（这部分的内容后续再加更输出文章）。

审核编辑：刘清