3D SLAM关于z轴精度的优化思路整理

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

作者丨SkyShaw

来源丨 深蓝AI

点击进入—>3D视觉工坊学习交流群

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

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

2

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轴的飘移问题，github链接：https://github.com/PJLab-ADG/Livox-Mapping。而添加了地面约束后即是尝试利用地面特征作为假设条件，在优化时考虑地面约束带来的残差以影响优化结构，为了避免添加地面约束对上下坡场景的不友好，可以对地面约束进行打分计算，只要地面特征足够“明显“和地面约束足够可靠，才在对应的关键帧处添加地面约束的残差块。

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

1、论文名称及链接：《Fast Segmentation of 3D Point Clouds for Ground Vehicles》，https://ieeexplore.ieee.org/document/5548059/figures#figures，开源代码：https://github.com/lorenwel/linefit_ground_segmentation；

2 、论文名称及链接：《GndNet: Fast Ground plane Estimation and Point Cloud Segmentation for Autonomous Vehicles.》，https://ieeexplore.ieee.org/abstract/document/9340979，开源代码：https://github.com/anshulpaigwar/GndNet；

3.3、万能误差消除术

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

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

2

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

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数据的使用问题建模和解决方案设计，添加重力校准约束前再进一步进行完整性分析以最后把控约束质量（这部分的内容后续再加更输出文章）。

本文仅做学术分享，如有侵权，请联系删文。
点击进入—>3D视觉工坊学习交流群

干货下载与学习

后台回复：巴塞罗那自治大学课件，即可下载国外大学沉淀数年3D Vison精品课件
后台回复：计算机视觉书籍，即可下载3D视觉领域经典书籍pdf
后台回复：3D视觉课程，即可学习3D视觉领域精品课程
3D视觉工坊精品课程官网：3dcver.com
1.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
2.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
3.国内首个面向工业级实战的点云处理课程
4.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
5.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
6.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
7.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)8.从零搭建一套结构光3D重建系统[理论+源码+实践]
9.单目深度估计方法：算法梳理与代码实现10.自动驾驶中的深度学习模型部署实战11.相机模型与标定(单目+双目+鱼眼）12.重磅！四旋翼飞行器：算法与实战13.ROS2从入门到精通：理论与实战14.国内首个3D缺陷检测教程：理论、源码与实战15.基于Open3D的点云处理入门与实战教程16.透彻理解视觉ORB-SLAM3：理论基础+代码解析+算法改进17.机械臂抓取从入门到实战
重磅！粉丝学习交流群已成立
交流群主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、ORB-SLAM系列源码交流、深度估计、TOF、求职交流等方向。
扫描以下二维码，添加小助理微信（dddvisiona）,一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿，微信号：dddvisiona
3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、源码分享、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答等进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，6000+星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看，3天内无条件退款

高质量教程资料、答疑解惑、助你高效解决问题
觉得有用，麻烦给个赞和在看~