点击上方“计算机视觉life”，选择“星标”
快速获得最新干货
MetaCam EDU 机器人定位导航、实景三维重建等，让研发之路，畅通无阻！
作者：Catalina，计算机视觉工坊整理
链接：https://www.zhihu.com/question/533056547
Z轴漂移的原因是因z轴观测量相对较少。下面是针对LiDAR SLAM的一些浅薄的看法，很多也是我认为可以做的一些点，不局限于纯LiDAR。
LIDAR SLAM中的漂移问题，一般是与退化分析放在一起的。LiDAR退化虽说是环境导致的，但很多时候可以通过分析这些有限的观测来避免匹配失败。但退化不等于匹配失败，肯定会降低精度的。简单理解，前者相当于在6自由度上都有一些约束但可能部分约束噪声过大，后者相当于有部分自由度约束不够导致这些维度上的退化。两者其实是一个问题，都是通过分析系统线性化后局部雅可比矩阵，分析每一对观测信息的约束强度，然后考虑解决办法。解决思路无非分为以下几种：
考虑闭环检测。也是引入相对的位姿约束可以显著消除累积误差，但分布不均匀的闭环检测约束会导致地图上下翘（roll），临近时间和近距离的频繁闭环反而会引入噪声导致精度下降。有一些去除闭环outlier的方法可以参考，比如Kimera-RPGO，一般都会结合PCM来做（PCM设置阈值略微麻烦点，但很实用）。
考虑倾斜45度安装雷达，增强z轴约束。
考虑引入其他传感器，增加在LIDAR退化维度上的约束，比如引入GNSS，IMU，磁力计，气压计等，这些大都是引入不完整维度上的约束（3DOF GNSS），可能导致部分维度（比如姿态）不连续。这种方法是大多数紧耦合LIO采用的思路。这种方法对引入的sensor的建模需要非常谨慎，否则可能会降低系统精度。比如不同sensor与雷达的误差尺度不一致。比如很多LIO系统对IMU bias非常敏感，对GNSS数据质量敏感。这其实本质上是一个权重调节的过程，设置不好，最终反而降低系统精度。
GNSS状态切换(退化->收敛)时导致的数据跳变
考虑Zero velocity update(ZUPT)约束（加速度计和陀螺仪）。也是引入其他传感器的观测信息，常见做法包括velocity ZUPT和angular rate ZUPT等。还有一些其他重力相关的约束。对于四自由度的地面机器人而言，ZUPT的效果显著。还可以利用gravity 来引入其他的一些约束，包括rotation-constrained yaw优化(VINS，LIO-Mapping)，gravity-constrianed 4DOF ICP[1]等.这种方法也常见于使用低成本的IMU进行行人定位的一些工作里面，通过抬脚落脚的ZUPT检测，选取不同的更新策略，比如不更新/局部更新(降低协方差)等。
大场景中闭环约束分布不均匀导致的地图上翘
考虑LiDAR BA[8]-[10]。map和位姿一起优化，在室内很多场景下精度优势明显。点云特征点数量较多且图的规模有限，导致其只能离线处理，效率低。并且部分场景下还容易陷入局部最优。
考虑筛选或者选取高质量的观测信息。是否需要增加重要的关联点的权重，或者过滤及筛选高质量的关联点进行位姿估计，类似GICP这种方法。
静止情况下ouster雷达的测量点云，可以发现点基本是呈球状或者线状的。
考虑对观测噪声或者地图的uncertainty精细建模。比如不同线束的入射角，range不确定性，和姿态估计误差以及它们在map中的传播(map uncertainty)。比如z轴观测点本身较少，在长廊场景下z方向的观测点较远，带来更大的噪声，导致更大的z轴漂移。这种方法在surfel-based（suma，wildcat，probalistic-xx）[6][7]的算法中用的较多，也有VoxelMap[4]这种基于voxel的。但这种方法大多不稳定，需要调参，整体场景测试效果可能一般，但在类似长廊这种室内环境下效果极好。实际场景来看，如果建模不够准确，反而不如像FAST-LIO这种设置测量噪声为零均值的高斯效果好(In practice, we set R_j to a constant value and found that it works very well)。
VoxelMap中对雷达观测噪声建模，range不确定性和入射角方向
至于pose uncertainty的建模，指的是相对变换的斜方差(odom factor)，我们不考虑古老的用多元高斯欧拉角参数化方法(Fig.4 SSC方法，橙色区域)，直接考虑在李代数上的高斯建模(fig.4中的两个香蕉分布)，其很多时候存在一个关联协方差的问题。简单说，同一个map点被多个位姿观测到，这多个位姿之间是关联的，独立同分布的假设并不成立。通常做法是在barfoot 2014 TRO(可查阅机器人的状态估计这本书)中提出的，假设了位姿独立，在李代数上做，这也意味着没有互斜方差了，我自己的论文里面也是这么实现的(下图PALoc)。但经过图优化均摊误差后，各位姿显然是高度关联的，这就破坏了位姿独立性的假设，导致相对位姿的cov过度估计。有同学可能对factor graph中uncertainty如何传播的不太了解，可以参考下我的论文中的Section VII.B 章节，结合因子图做了详细介绍，也有对应的退化场景实验效果。
那么真实情况下考虑相对位姿的互斜方差时，其估计的cov与真实cov差距多大呢？可以看Mangelson方法，下面这张图，去除位姿独立假设后，绿色部份的香蕉分布明显更大了一些。说明浅蓝色的香蕉分布存在过度估计，会导致odom factor权重过大，图优化均摊误差的时候自然不符合实际情况，会降低精度。
以上barfoot方法以及Mangelson方法，都是在李代数上建模，但后者考虑了互斜方差，因为都要用BCH公式近似，这使得始终无法精确估计cov。这个问题近期的重大进展就是，港科大吴荩博士2022年在TRO上的QPEP[11]方法了，使用了四元数方法做了全局最优的精确计算，刚好吴荩博士自己也在知乎上介绍了这部分工作。
还有一些其余应用类的论文港科大焦健浩博士的M-LOAM[14]，港科大蒋斌乾博士最近的ICRA论文[13]。
分析不同观测点约束强度，不同的关联点对6自由度的约束强度不同，特别是退化方向上，可以直接将求出在非退化方向的解分量，对于退化方向的解使用预测值分量或者直接抛弃。这种方法需要先检测出退化方向，典型是LOAM[5]. 检测退化一般用基于特征值的方法就能取得较好效果，也有基于匹配的残差分布等方法，后者需要设置区块数目来生成PDF，所以效率慢点。
X-ICP[5]中对每一对约束的强度分析，分类和过滤。
可以发现，后三种方法其实可以归结到一类，最终落脚点都是分析系统局部的雅可比矩阵。有很多同学提到了基于地面的优化，这是一种很好的办法。但是它的假设和约束过强，导致一旦引入错误的地面约束，会直接导致地图形变。位姿求解可以简单理解成如何从一个超定的带噪声的线性方程组中求解6维度的位姿。我们可以简单的将雅可比矩阵的每一行理解成对这几个维度的位姿的约束强度（参考X-ICP[4]），进而大致能够分析出哪些观测方程对这个维度的约束比较强。
路口的退化分析。绿色圆球表示三轴约束强度，越球形，各方向约束越均匀。
当在普通特征丰富的场景下，假设有2000对关联点，那么这个线性方程组可能就有2000个方程，可以很好的约束6个维度，进而保证迭代求解的精度。如果系统x退化（长廊），那么能够有效约束x的方程相比较其他维度的方程可能非常少，导致x维度的求解误差较大。
窄长廊的退化分析，x维度约束强度明显远小于yz，退化严重
从以上几点其实可以看出来，这部分还有很多点可以去深入做的，一时半会儿很难解释清楚，后面专门总结一篇文章来说明吧。
推荐教程
MetaCam EDU 正式发售！应用于机器人定位导航、实景三维重建等，让研发之路，畅通无阻！
隧道低纹理大摆锤，暴力SLAM！
暴力升级！暗黑隧道极限测试SLAM！
效果哇塞！3D高斯泼溅来实景重建公园！
MetaCam EDU 产品功能及细节详细介绍
MetaCam EDU 实机展示和操作
MetaCam EDU产品介绍、教程文档、询价等联系客服获取，备注EDU
