SLAM中的退化问题

简介

最近在研究定位的退化方向的判断和处理，看了几篇关于SLAM退化问题中的论文，并做个小结。定位的退化主要是因为约束的减少，比如NDT需要三个正交方向的约束才能很好的匹配，但若在狭长的走廊上或者隧道环境，条件单一，即使人肉眼观看激光雷达数据，也很难判断机器人所处的位置。

相关论文

paper 1
On Degeneracy of Optimization-based State Estimation Problems
paper 2
Robust localization and localizability estimation with a rotating laser scanner
paper 3
Estimating the Localizability in Tunnel-like Environments using LiDAR and UWB

论文小结

Paper Ⅰ

论文1是由LOAM的作者Ji Zhang发表在2016年IROS上的一篇关于优化问题的退化处理，该思想在LOAM源码中也有体现(源码的话我看的是LOAM_NOTED的1026行开始是退化的处理)。
基于优化的状态估计问题都可以描述为一个最小二乘问题，对于非线性问题也可以线性化，所以优化问题可以描述为$$arg\underset{x}{\min }||Ax-b|{|}^2$$这其实可以看做为一个约束，一个状态估计问题是又多个约束一起构成的。然后定义了的degeneracy factor $D$，然后一波证明(见论文) $$D=\delta d/\delta x_c^{\ast }={\lambda }_{min}+1$$所以求$A^{T}A$的特征值和特征向量即可，特别小的特征值所对应的特征向量即为退化方向。
由于退化方向有噪声，所以在优化的过程中，会有误差，所以他舍去了特征方向上的信息，具体实施如下图：

我本来想在NDT的优化中加一个退化方向检测的功能，但是看了NDT的源码和相关论文后发现NDT不是一个最小二乘问题，而是一个最大似然问题，所以不能使用这种方法来求它的退化方向。

Paper Ⅱ

论文2是发表在ICRA2017的论文，它是用ESFK来进行定位，前半部分是用ESKF来推导，后半部分则如何估计定位的准确性，即定位的退化方向。
位姿约束可以描述为$$n_i^T(x+r_i-p_{i,0})=0\Rightarrow n_i^{T}x=n_i^{T}x(p_{i,0}-r_i)=d_i$$
$n_i$是激光束打到环境中的点云的平面法向量，$x$为机器人位姿，$r$为激光束的向量。所以一帧激光束的约束为
对N矩阵进行奇异值的分解(SVD)，最小的奇异值所对应的特征向量便是退化方向。

Paper Ⅲ

论文3是发表于ICRA2019的文章，这三篇文章都是CMU的。这篇文章是测量隧道环境中的退化方向，并使用UWB与激光雷达融合的方法解决隧道等几何环境单一环境的定位问题。这篇文章的工作是在论文2的工作下推进的。
衡量Lidar的定位的可信程度，他描述的约束为$$C(x,R,{\rho }_i)=n_i^T(x+R{r}_i{\rho }_i)+d_i=0$$其中，$x$为机器人的位置向量，$R$为机器人的姿态，${\rho }_i$是激光的测量距离。
把每一束激光的作为一个约束，然后来衡量约束的强度。若机器人的位姿轻微移动，但激光的测距信息${\rho }_i$变化不大，则该约束强度很弱，反之约束强度强。所以他就求取${\rho }_i$关于$x$和$R$的偏导。

把一帧中的每个约束构成矩阵
然后对其平方进行特征值分解，特征值小的所对应的特征向量就是退化方向。
但是作者又觉得单纯的特征值分解没有物理意义，他就把F和T矩阵重新映射到特征向量的空间中，然后与力与扭矩联系起来（这步感觉就是写论文用）。
之后是将UWB与激光雷达的测量值进行融合。（由于我没有考虑使用UWB方案，所以之后就没有仔细看…）

总结

退化问题还是挺常见的，特别是只依赖一种传感器的时候，比如我之前在项目中遇到过的在隧道环境中无法靠激光雷达根本无法建图定位，因为激光“看到”的环境都是一样的，在隧道方向是没有约束的；并且在隧道中GPS信号是没的，只能依靠高精度的姿态传感器和轮速机的融合来做，当然随着运行时间的增加，误差会慢慢增加。
所以在已知地图的定位问题中，可以对地图环境进行分析，比如到了隧道的环境中，可以对环境的点云空间进行分析，按照paperⅡ paper Ⅲ的思路判断出环境的退化性和退化方向。之后就靠融合来解决退化问题了。
在优化问题时，也会有退化问题，就像paperⅠ中一样，我最近也遇到了一个问题，使用ceres来优化，由于一个方向上缺少了约束，优化后的结果就很遭，其实按照paperⅠ的思路来解决就可以的，但是LOAM是自己写的优化器，但ceres的框架有点写死的感觉，没有找到很好的解决方法，头疼。。。

参考

SLAM中状态估计优化退化问题(degraded optimization problem)浅析和源码实现