视觉SLAM十四讲第六讲

第六章非线性优化

主要目标

理解最小二乘法的含义和处理方式。
理解高斯牛顿法（Gauss-Newton）、列文伯格—马夸尔特方法（LevenburgMarquadt）等下降策略。
学习 Ceres 库和 g2o 库的基本使用方法。
一、状态估计问题
1）批量状态估计与最大后验估计
经典 SLAM 模型由一个运动方程和一个观测方程构成：

在运动和观测方程中，我们通常假设两个噪声项 wk, vk,j 满足零均值的高斯分布，像这样：

其中 N 表示高斯分布，0 表示零均值，Rk, Qk,j 为协方差矩阵。在这些噪声的影响下，我们希望通过带噪声的数据 z 和 u 推断位姿 x 和地图 y（以及它们的概率分布），这构成了一个状态估计问题。
处理这个状态估计问题的方法大致分成两种：
1）增量式：有一个当前时刻的估计状态，然后用新的数据来更新它。
2）批量式：把数据“攒”起来一并处理它。
接下来讨论一下批量方法：
考虑从 1 到 N 的所有时刻，并假设有 M 个路标点。定义所有时刻的机器人位姿和路标点坐标为：

用不带下标的 u 表示所有时刻的输入，z 表示所有时刻的观测数据。那么我们说，对机器人状态的估计，从概率学的观点来看，就是已知输入数据 u 和观测数据 z 的条件下，求状态 x, y 的条件概率分布：

为了估计状态变量的条件分布，利用贝叶斯法则，有：

贝叶斯法则左侧称为后验概率，右侧的 P(z|x) 称为似然（Likehood），另一部分 P(x) 称为先验（Prior）。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化（Maximize a Posterior，MAP），则是可行的：

进一步，我们当然也可以说，对不起，我不知道机器人位姿或路标大概在什么地方，此时就没有了先验。那么，可以求解最大似然估计（MaximizeLikelihood Estimation，MLE）：

直观讲，似然是指“在现在的位姿下，可能产生怎样的观测数据”。由于我们知道观测数据，所以最大似然估计可以理解成：“在什么样的状态下，最可能产生现在观测到的数据”。这就是最大似然估计的直观意义。
2）最小二乘的引入
最小二乘法可以由最大似然估计进行导出
对于某一次观测：

假设噪声项 vk ∼ N (0, Qk,j )，所以观测数据的条件概率为：

考虑任意高维高斯分布 x ∼ N (µ, Σ)，它的概率密度函数展开形式为：

对其取负对数，则变为：

因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。在最小化上式的 x时，第一项与 x 无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。代入 SLAM 的观测模型，相当于在求：

该式等价于最小化噪声项（即误差）的平方（ Σ 范数意义下）。
二、非线性最小二乘
1）最小二乘问题：

1、给定某个初始值 x0。
2、对于第 k 次迭代，寻找一个增量 ∆xk，使得 ∥f (xk + ∆xk)∥22 达到极小值。
3、若 ∆xk 足够小，则停止。
4、否则，令 xk+1 = xk + ∆xk，返回第 2 步。
注：非线性优化问题关键是求解增量 ∆xk
2）一阶和二阶梯度法
将最小二乘函数在x附近进行二阶泰勒展开：
注：J 是 ∥f(x)∥2 关于 x 的导数（雅可比矩阵），而 H 则是二阶导数（海塞（ Hessian）矩阵）

如果保留一阶梯度，这为最速下降法：
如果保留二阶梯度，这为牛顿法：

求右侧等式关于 ∆x 的导数并令它为零，就得到了增量的解：

注：最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的 H 矩阵，这在问题规模较大时非常困难，我们通常倾向于避免 H 的计算。

3）Gauss-Newton
将 f(x) 进行一阶的泰勒展开（请注意不是目标函数 f(x)2）

注：对比牛顿法可见， Gauss-Newton 用 J T J 作为牛顿法中二阶 Hessian 矩阵的近似，从而省略了计算 H 的过程。

原则上，它要求我们所用的近似 H 矩阵是可逆的（而且是正定的），但实际数据中计算得到的 J T J 却只有半正定性。
也就是说，在使用 Gauss Newton 方法时，可能出现 J T J 为奇异矩阵或者病态 (illcondition) 的情况，此时增量的稳定性较差，导致算法不收敛。

4）Levenberg-Marquadt
Levenberg-Marquadt 方法在一定程度上修正了这些问题，一般认为它比 Gauss Newton 更为鲁棒。尽管它的收敛速度可能会比 Gauss Newton 更慢，被称之为阻尼牛顿法(Damped Newton Method)

给 ∆x 添加一个信赖区域（ Trust Region），不能让它太大而使得近似不准确。一个比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定这个范围：如果差异小，我们就让范围尽可能大；如果差异大，我们就缩小这个近似范围。

注：ρ 的分子是实际函数下降的值，分母是近似模型下降的值。如果 ρ 接近于 1，则近似是好的。如果 ρ 太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果 ρ 比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。

算法步骤

注：这里 λ 为 Lagrange 乘子。

多了一项 λDT D。如果考虑它的简化形式，即 D = I，那么相当于求解：

当参数 λ 比较小时， H 占主要地位，这说明二次近似模型在该范围内是比较好的， L-M 方法更接近于 G-N 法。

另一方面，当 λ 比较大时， λI 占据主要地位， L-M更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。

L-M 的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定更准确的增量 ∆x。