0. 简介

对于激光和视觉里程计而言，我们在面试和日常工作中会经常听到，在长时间在平坦道路上直行会导致维度的退化。定位的退化主要是因为约束的减少，比如NDT需要三个正交方向的约束才能很好的匹配，但若在狭长的走廊上或者隧道环境，条件单一，即使人肉眼观看激光雷达数据，也很难判断机器人所处的位置。而这篇博客来回顾下LOAM的作者Ji Zhang发表在2016年IROS上的一篇关于优化问题的退化处理《On Degeneracy of Optimization-based State Estimation Problems》，该思想在LOAM源码中也有体现。这个博客中的思想还是值得我们去学习和思考的。

[video(video-8QnFqSpq-1664508845998)(type-bilibili)(url-https://player.bilibili.com/player.html?aid=713181648)(image-https://img-blog.csdnimg.cn/img_convert/477b8b7f3d534e8fd3b2c542d865d131.jpeg)(title-【学习充电站--SLAM】SLAM中退化处理)]

1. 文章背景

因为这是激光SLAM中防止非线性退化最早的一批论文。重要性不言而喻，我们就直接来解析文中的主要思路以及方法了。对于SLAM而言，传感器需要纹理信息决定特征点，在特征点缺乏时，状态估计方法会进行退化。

如下图所示，正常情况下约束应该是分布在空间中的多个方向，从各个角度约束解，绿色点表示问题的解，被非退化约束限制在了一个小区域。这样的解就比较准确，约束发生小变化时，解的变化也很小，被限制在一个局部区域，这样的解就是一个比较理想的解。如果解的约束大多近似平行，那么他们就是退化的方向（蓝色箭头表示的方向），这个时候解在退化方向收到的约束就很差。如果其中一个约束发生了小的偏移，那么解所在的局部区域会发生较大的变化,这样的解是比较糟糕的解。 在这里插入图片描述 在这篇文章中，提出了一种在线的方法，通过分析问题的约束的几何结构来缓解优化问题的退化。该方法确定了退化发生的方向，并且只在非退化的方向求解优化问题。

2. 文章方法

线性求解的最优可以转化为下面的公式，即可以直接使用奇异值分解SVD、QR分解求解

在这里插入图片描述

但是如果面对的是非线性方程，那基于优化的状态估计问题都可以描述为一个最小二乘问题，对于非线性问题也可以线性化，所以优化问题可以描述为

在这里插入图片描述

该式中，每一行 都可以视为解空间的一个超平面。那么所求得的解x的几何意义是，在解空间中找到一个点，应当满足到每个超平面的距离的欧式距离最小。通过方向 和位置 共同约束得到 的解，并研究平面的几何分布来确定退化方向。

接下来就是然后定义了的退化因子(degeneracy factor)，如下图所示，黑色线条表示平面约束，蓝色的圆点表示该问题的解。为了评估该系统的退化程度，给这个系统添加一个额外超平面约束，其中 是一个 向量表示法向量约束(图中的黑色箭头)。桔色线条表示该超平面的法向量。这个超平面经过解x0，所以添加这个约束并不改变原问

题的解。

在这里插入图片描述

我们的原始解为 ,一开始加入经过 的任意方向扰动 ，解并不发生变化。如果扰动 相对于 偏离 ，那么原始解 也会偏离 ，因为这个时候的约束区域已经发生了改变。由于扰动 的方向任意，因此存在一个原始解的最大偏移， 。

在这里插入图片描述

所以系统的退化因子的数学表达式为 。即用超平面移动距离除以解的最大位移量。该值越大，表示系统越稳定。具体公式推导为：

2.1 退化因子只与矩阵A有关，而与b无关

对于线性化系统，退化因子 是 的函数，与 无关。这说明退化方向只与原始的约束方向 有关，与原始约束的位置 无关。这和前面的推测一致，当原始约束方向平行的时候，就会引入退化。

在这里插入图片描述

该问题解仍然为 。现在给新增的约束在其法线方向上进行平移

在这里插入图片描述

设 等于x0的偏移的干扰。引入(7)的解为 。应用(6)和(7)的左伪逆

在这里插入图片描述 在这里插入图片描述

然后将(8)左乘到(6)(7)中，并用(7)减去(6)就能得到

在这里插入图片描述

考虑到c的模长为1，则解在c方向上的位移量有

在这里插入图片描述

因此，退化因子只与A有关，而与b无关。

这里除了公式外，我们发现对于原问题 ，任意修改 的值，实质上是修改了超平面在空间中的位置，但没有改变方向，从而其退化因子也是不变的。

2.2 退化因子 ，其中 为矩阵 的最小特征值

对于线性化系统，退化因子 的求解为 ，其中 是 的最小特征值。这说明 的特征值和特征向量分别决定了是否存在退化以及退化的方向，对于 ，它的退化方向就是 对应于 的特征向量 。

这里根据伪逆将 和 转换

在这里插入图片描述

为了让 最大， 需要满足

在这里插入图片描述

因为c的模长为1，所以上式也可以写成

在这里插入图片描述

上式是瑞利商（Rayleigh quotient）的形式。该式的最小值出现在c为A^TA的最小特征值时。将此时的c带入，有

在这里插入图片描述

3. 参考链接

https://blog.csdn.net/i_robots/article/details/108724606

https://www.youtube.com/watch?v=sQ5qeolTWQ0

https://blog.csdn.net/Curryfun/article/details/105838695

https://mp.weixin.qq.com/s/jq0pLmQG91bbh7eDMn8FCA