LOAM SLAM原理之防止非线性优化解退化

Robots.

墨衍会员 · AI 创作全网分发码龄8年

关注

VIP免费原创最新推荐文章于 2026-07-14 10:57:07 发布 5.8k 阅读 AI 写同款· GEO 优化 ›

SLAM（状态估计）中的退化问题 SLAM（状态估计）中的退化问题，On Degeneracy of Optimization-based State Estimation Problems论文理解阅读详情

On Degeneracy of Optimization-based State Estimation Problems

1. 概述
2. 理解
3. 参考代码
4. 参考链接

1. 概述

LOAM中用于防止非线性优化解退化的方法，指出原始参考代码中出现错误的地方，希望能帮助大家，同时供自己以后参考。

2. 理解

对于SLAM而言，视觉传感器需要纹理信息决定特征点，距离传感器需要空间几何结构信息决定特征点，在特征点缺乏时，状态估计方法会进行退化，为了解决这种退化问题，本文提出了一种在线方法。
处理退化的常用方法有:

1）当出现退化时换一种方法;这要求在设计时需要一个备用方法可用
2）在状态估计过程中加入人工约束，如恒速模型的约束。即使问题本身是可解决的情况下也会带来不必要的误差。

我们的方法是在原始问题的部分子空间添加约束。原始问题的解可以分为非退化方向和退化方向，在处理问题时，首先确定退化方向，然后将求出原始问题非退化方向的解，对于退化方向的解使用猜测值。在最后的实践算法中，只使用非线性优化解在非退化方向的分量，不考虑退化方向的分量。

正常情况下，约束应该是分布在空间中的多个方向，从各个角度约束解，如下面所示，绿色点表示问题的解，被非退化约束限制在了一个小区域。这样的解就比较准确，约束发生小变化时，解的变化也很小，被限制在一个局部区域，这样的解就是一个比较理想的解。
在这里插入图片描述
如果解的约束大多近似平行，那么他们就是退化的方向（蓝色箭头表示的方向），这个时候解在退化方向收到的约束就很差。考虑绝对平行时，解的约束也是一个平行的方向，那么这种情况下的解是需要避免的，如果其中一个约束发生了小的偏移，那么解所在的局部区域会发生较大的变化，这样的解是比较糟糕的解。
在这里插入图片描述
我们提出的方法只是线性问题、非线性问题求解的一个小步骤，计算复杂度可以忽略。实验结果表明该方法能够改善环境退化下的位姿解估计结果。

这种方法有点类似可观测子空间、可控子空间的概念。
每次求解时

优化解=原始解在非退化方向投影+估计值在退化方向投影（实际算法中可以将退化方向解丢弃，只考虑非退化方向）

考虑方程
$\tag{1}\underset{x}{\min}{f^2(x)}$
那么 $(1)$ 为线性方程时可以直接使用奇异值分解SVD、QR分解求解，但是 $(1)$ 为非线性方程时需要进行局部微分转换为求解线性方程，因此线性、非线性问题都可以转换为下面的形式
$\tag{2}arg\underset{x}{\min}{||Ax-b||^2}$
我们的方法将(2)中的每一行看作 $x$ 空间中的一个(超)平面，这些平面约束着 $x$ （通过方向 $A$ 和位置 $b$ 共同约束得到 $x$ 的解），并研究平面的几何分布来确定退化方向。
做出两个假设：

假设在考虑传感器噪声的情况下对矩阵A进行了适当的加权。也就是 $(1)$ 问题的线性加权形式
原始问题不受约束，并且在问题非退化时，解由真正的传感器测量值决定。

第一个问题：给定线性化系统 $(2)$ ，确定状态空间中时候存在退化并确定相应的退化方向。在退化的情况下，防止在退化方向上出现错误解。
假设 $(3)$ 的原始解为 $x_0$ ，一开始加入经过 $x_0$ 的任意方向扰动 $c$ ，解并不发生变化
在这里插入图片描述
如果扰动 $c$ 相对于 $x_0$ 偏离 $\delta{d}$ ，那么原始解 $x_0$ 也会偏离 $\delta{x_c}$ ，因为这个时候的约束区域已经发生了改变。由于扰动 $c$ 的方向任意，因此存在一个原始解的最大偏移， $\delta{x_c}^*=\underset{c}{\max}{x_c}$
定义1：退化因子 $D$
$\tag{3}D=\frac{\delta{d}}{\delta{x_c}^*}$