激光SLAM中Z轴漂移优化

现象：

下面两张图分别是建图过程中的俯视图和正视图。从俯视图看建图基本是正确的，但是从正视图来看，在前进了四五百米之后Z轴方向产生了比较大的偏差。

原因分析：

1. Z轴方向分辨率低（以16线激光雷达为例，水平方向分辨率为0.2°，但是竖直方向分辨率为2°），观测信息（地面点云及特征）较少。

2. 激光雷达打到地面上的点入射角通常较大（想象激光雷达打到墙上与打到地面的入射角差异），点云精度差且不稳定。

3. 点云配准过程中的误差累积问题。

4. lidar和IMU之间的外参及IMU本身误差及同样会造成影响。

解决思路：

1. 增加地面约束。在hdl-graph-slam和livox-mapping中均有用到地面约束，在优化时考虑地面约束残差。不同SLAM方案对比可参考开源3D激光（视觉）SLAM算法汇总（持续更新）。地面约束也有不同思路：一种思路是，在已知应用场景为平面场景时，可以第一帧地面作为基准，后面的所有地面都与第一帧地面添加约束边；另一种思路是当前帧与local_map分别提取地面，来做地面约束。当然，地面约束本质上也是帧间约束，仍存在累计误差，为了避免添加地面约束对上下坡场景的不友好，可以对地面约束进行打分计算，只要地面特征足够“明显“和地面约束足够可靠，才在对应的关键帧处添加地面约束的残差块。

2. 利用IMU提供roll和pitch观测信息，但是要避免引入较差的IMU重力约束。

3. 回环检测。场景不是特别大的话还是十分有效的，回环检测方案可参考开源3D激光雷达闭环检测方法。

4. 利用GNSS或Landmarks等外部的绝对位置信息。但是要避免因为使用垃圾、离群点位置信息而带来更严重的误差。

参考：

最新｜3D SLAM关于z轴精度的优化思路整理 (qq.com)