SLAM14讲学习笔记 —— 第六讲 —— 非线性优化

一. 状态估计问题

经典的SLAM/模型由2个方程构成：

如上式所示：上面的为运动方程；下面的为观测方程。
参数：
k:时刻
x_k:k时刻的相机位姿。（用变换矩阵或李代数表示）
u_k:输入（运动传感器的读数）
y_j: 相机位姿为x_k时，观测点的路标点
z_k,j:相机位姿为x_k时，对路标y_j进行了一次观测，对应到图像上的像素位置。
w_k:噪声项
v_k,j:噪声项
f() 、h(): 某函数

本讲主要讲观测方程：

根据第五讲的知识：假设在x_k处对路标进行了一次观测，对应到图像上的像素位置z_k,j，则观测方程可表示：
注1：K为相机内参；s为像素点的距离（现实空间的观测点在相机坐标系下的Z坐标），z_k,j和y_j皆为齐次坐标的形式。其中exp(&^)=T（变换矩阵）,&为六维向量。
&^即为将六维向量转换为四维矩阵。y_j为世界坐标系下的坐标的齐次表示。

注2：通常认为两个噪声项w_k,v_k,j都满足零均值的高斯分布：

我们的工作就是：希望通过带有噪声的数据z和u，推断出位姿x和地图y。这构成了一个状态估计问题。
对机器人的状态估计问题，就是求已知输入数据u和观测数据z的条件下，计算状态x的条件概率分布。即：

当没有测量运动的传感器，只有一张张图象时，即只考虑观测方程时，相当于估计P（x | z)的条件概率分布。根据贝叶斯：

注1：P（x | z)称为后验概率。P（x）称为先验。P（z | x)称为似然。
注2：分母部分与估计量x无关，所以可以略去。
注3：直接求后验很难，但求一个状态最优估计，使得在该状态下，后验概率最大化（此问题即为MAP）是可行的。即：

而求解最大后验概率，即为最大化似然和先验的乘积。当没有先验时，即为P（x)不知道，求解x的最大似然估计（MLE)：

注：最大似然估计可以理解为：在什么样的状态下，最可能产生现在观测到的数据。

二 . 求解问题

上面讲到，对观测方程的估计，就是求解x的最大似然估计。下面讲解如何求解。

1. 对于某一次的观测，观测方程为：
   
   因为认为噪声项v_k,j符合零均值的概率分布，即v_k,j ~ N(0, Q_k,j)。所以观测数据的条件概率符合：
   
   仍然是一个高斯分布，为了计算使其最大的x_k,y_j，使用最小化负对数的方法。

注：先来学习一下：对符合高斯分布的原分布求最大，即为对其负对数求最小 的过程。

观察上式，等号右侧的第一项与x无关，略去。于是，只要对后一项最小化就可得到该状态的最大似然。

将上面的过程带入到观测模型的满足高斯分布的条件概率P（z_k,j|x_k,y_j) = N ( h(y_j,x_k),Q_k,j)得到。求此概率的最大，即为最小化：

注：这里的Q_k,j为噪声v_k,j分布的协方差，它的逆称为信息矩阵。

由上式可以看出，求上式的最小即为最小化噪声项（z_k,j-h(x_k,y_j即为噪声项)的平方。

注：怎么看出来我也不懂，但是可能应该是（z_k,j-h(x_k,y_j)^TQ（z_k,j-h(x_k,y_j）即为（z_k,j-h(x_k,y_j）²。但这里的平方应该不是通常的理解上的平方。唉，管他的，听高博的没错。

所以对于所有的运动和任意的观测定义数据与估计值的误差：

并定义该误差的平方和：

即上式的最优解等价于状态的最大似然估计！！！
而上式的最优解是一个最小二乘问题。

注1：上式的目标函数J（x)由许多个误差的平方和组成。
注2：如果使用李代数，则为无约束的最小二乘，使用R、T则因为有约束而很复杂。所以使用李代数。
注3：误差是使用2范数（平方形式）来度量。

三 . 非线性最小二乘

对于简单的线性函数最小二乘很简单就是求导并令其为0，不介绍，但对于复杂的非线性的，使用迭代的方式。

这让求解导函数为零的问题，变成了一个不断寻找梯度并下降的过程。直到某个时刻 增量非常小，无法再使函数下降。此时算法收敛，目标达到了一个极小，我们完成了寻找 极小值的过程。在这个过程中，我们只要找到迭代点的梯度方向即可，而无需寻找全局导 函数为零的情况。

这里就有两个问题：

1）初始值怎么给？
2）增量怎么确定？

先来解决第二个问题，增量的问题：
求解增量的最直观的方法是将目标函数在x附近进行泰勒展开：

这里 J 是∥f(x)∥² 关于 x 的导数（雅可比矩阵），而 H 则是二阶导数（海塞（Hessian） 矩阵）。我们可以选择保留泰勒展开的一阶或二阶项，对应的求解方法则为一阶梯度或二 阶梯度法

注：雅可比矩阵是函数的一阶偏导以一定形式排列成的矩阵。

下面介绍2种常用的方法——高斯牛顿法和列文伯格—马夸尔特法

1. 高斯牛顿法：
   思想是将 f(x) 进行一阶的 泰勒展开（请注意不是目标函数 f(x)²）
   
   这里 J(x) 为 f(x) 关于 x 的导数，实际上是一个 m×n 的矩阵，也是一个雅可比矩 阵。根据前面的框架，当前的目标是为了寻找下降矢量 ∆x，使得 ∥f (x + ∆x)∥² 达到最 小。为了求 ∆x，我们需要解一个线性的最小二乘问题：

对上式进行展开：

注：这里的展开平方处理成转置乘原式，我没有看懂，数学上的问题缺失。

求上式关于 ∆x的倒数并令其等于0：


但是这种方法存在缺点：
1） 在求解增量方程时，要求所用的近似H矩阵是可逆的（且正定的），但实际中J^TJ只有半正定。也就是在求解过程中可能会出现J^TJ为奇异矩阵或者病态的情况，这种情况下，增量的稳定性较差，导致算法不收敛。
2）就算H满足需求，如果求出来的步长太大，也会导致采用的局部近似不准确，甚至无法保证其收敛，甚至让目标函数变得更大。

所以SLAM中通常采用的是以下的方法 —— 列文伯格—马夸尔特（也叫阻尼牛顿法）

2. 阻尼牛顿法：

因为高斯牛顿法采用的二阶近似泰特展开只能在展开点附近有较好的近似效果。所以给∆x添加一个信赖区间。非线性优化有一系列的这类方法，这类方法被称为信赖区域方法。

那么如何来确定这个信赖区域的范围，比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定。即如果差异小，则范围大；如差异大，则缩小范围。

怎么来判断泰勒近似是否足够好？使用下面的式子来判断：

p的分子是实际函数下降的值，分母是近似模型下降的值。
当p接近1，则近似是好的；
当p不接近1(太小），则认为近似太差，需要缩小近似范围；
当p较大时，则放大近似范围。

所以阻尼牛顿法的步骤如下：
在上图的式子6.24中，我们把增量限定于一个半径为 µ 的球中，认为只在这个球内才是有效的。带上 D 之后，这 个球可以看成一个椭球。Levenberg 提出的优化方法中，把 D 取成单位阵 I，相当于 直接把 ∆x 约束在一个球中。随后，Marqaurdt 提出将 D 取成非负数对角阵——实际中通常用 J^TJ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些。

不论如何，在 L-M 优化中，我们都需要解式（6.24）那样一个子问题来获得梯度。这 个子问题是 带 不等式约束 的优化问题，我们用 Lagrange 乘子将它转化为一个无约束优化 问题：

这里 λ 为 Lagrange 乘子。类似于 Gauss-Newton 中的做法，把它展开后，我们发现 该问题的核心仍是计算增量的线性方程：
注1：这里的展开类似于上文中高斯牛顿法的：

但我没算出来，哈哈，我好垃圾。

注2：H = J(x)^TJ(x);
g = -J (x)^Tf(x);
D为非负数对角阵。实际中通常使用J^T的对角元素平方根。简化形式下：认为D=I；

当认为D为单位矩阵I时，则上式则相当于求解：

我们看到，当参数 λ 比较小时，H 占主要地位，这说明二次近似模型在该范围内是比 较好的，L-M 方法更接近于 G-N 法。另一方面，当 λ 比较大时，λI 占据主要地位，L-M 更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。L-M 的求解方 式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定更准确 的增量 ∆x。

在实际中，还存在许多其它的方式来求解函数的增量，例如 Dog-Leg 等方法。我们在 这里所介绍的，只是最常见而且最基本的方式，也是视觉 SLAM 中用的最多的方式。总而 言之，非线性优化问题的框架，分为 Line Search 和 Trust Region 两类。Line Search 先固 定搜索方向，然后在该方向寻找步长，以最速下降法和 Gauss-Newton 法为代表。而 Trust Region 则先固定搜索区域，再考虑找该区域内的最优点。此类方法以 L-M 为代表。实际 问题中，我们通常选择 G-N 或 L-M 之一作为梯度下降策略。