[SLAM基础学习简记]非线性优化

非线性优化

状态估计问题

问题描述

SLAM模型：
$$\begin{gathered} {\mathbf{x}}_k=f({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{w}}_k \\ {\mathbf{z}}_{k,j}=h({\mathbf{y}}_j,{\mathbf{x}}_k)+{\mathbf{v}}_{k,j} \end{gathered}$$
运动方程与输入的具体形式有关，观测方程由针孔模型给定。

数据受噪声影响，有：
$${\mathbf{w}}_k\sim \mathcal{N}(0,{\mathbf{R}}_k),{\mathbf{v}}_k\sim \mathcal{N}(0,{\mathbf{Q}}_{k,j})$$
通过带噪声的数据$\mathbf{z}$和$\mathbf{u}$推断位姿$\mathbf{x}$和$\mathbf{y}$（以及它们的概率分布），这构成了一个状态估计问题。

批量状态估计

处理状态估计问题的方法有两种：

• 持有一个当前时刻的估计状态，然后用新的数据来更新。称为**增量/渐进（incremental）**的方法，或者叫做滤波器，如卡尔曼滤波器
• 把所有的数据收集到一起再处理，称为批量（batch）方法，即SfM（Structure from Motion），缺点是不实时

仅对当前时刻附近的一些轨迹进行优化，称为滑动窗口估计法

最大后验估计

定义所有时刻的机器人位姿和路标点坐标：
$$\begin{gathered} \mathbf{x}={\mathbf{x}}_1,\cdots ,{\mathbf{x}}_N \\ \mathbf{y}={\mathbf{y}}_1,\cdots ,{\mathbf{y}}_M \end{gathered}$$
已知输入数据和观测数据，求$\mathbf{x},\mathbf{y}$的条件概率分布：
$$P(\mathbf{x},\mathbf{y}|\mathbf{z},\mathbf{u})$$
利用贝叶斯法则：
$$\underset{\text{后验}}{\underbrace{P(\mathbf{x},\mathbf{y}|\mathbf{z},\mathbf{u})}}=\frac{P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y})}{P(\mathbf{z},\mathbf{u})}\propto \underset{似然}{\underbrace{P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})}}\underset{先验}{\underbrace{P(\mathbf{x},\mathbf{y})}}$$
直接求后验分布是困难的，但是可以求一个状态最优估计，使得在该状态下后验概率最大化：
$${(\mathbf{x},\mathbf{y}{)}^{\ast }}_{\text{MAP}}=\arg \max P(\mathbf{x},\mathbf{y}|\mathbf{z},\mathbf{u})=\arg \max P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y})$$
求解最大后验概率等价于最大化似然和先验的乘积。先验未知时，可以求解最大似然估计（Maximize Likelihood Estimation, MLE）：
$${(\mathbf{x},\mathbf{y}{)}^{\ast }}_{\text{MLE}}=\arg \max P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})$$
可以理解为：在什么状态下，最可能产生现在观测到的数据

非线性最小二乘

引出

考虑任意高维高斯分布$\mathbf{x}\sim \mathcal{N}(\mathbf{\mu },\mathbf{\Sigma })$，有：
$$P(\mathbf{x})=\frac{1}{\sqrt{(2\pi {)}^N\det (\mathbf{\Sigma })}}\exp (-\frac{1}{2}{(\mathbf{x}-\mathbf{\mu })}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu }))$$
取其负对数，有：
$$-\ln (P(x))=\frac{1}{2}\ln ((2\pi {)}^N\det (\mathbf{\Sigma }))+\frac{1}{2}{(\mathbf{x}-\mathbf{\mu })}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })$$
对原函数求最大化相当于对负对数求最小化（负对数第一项与自变量无关）。
$$\max P(x)=\min -\ln (P(x))=\min \frac{1}{2}{(\mathbf{x}-\mathbf{\mu })}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })$$
假设噪声项服从高斯分布：
$${\mathbf{w}}_k\sim \mathcal{N}(0,{\mathbf{R}}_k),{\mathbf{v}}_k\sim \mathcal{N}(0,{\mathbf{Q}}_{k,j})$$
观测和输入（运动传感器读数）的条件概率均服从高斯分布：
$$\begin{gathered} P({\mathbf{u}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_k)=\mathcal{N}(f({\mathbf{x}}_{k-1},{\mathbf{u}}_k),{\mathbf{R}}_k) \\ P({\mathbf{z}}_{k,j}|{\mathbf{x}}_k{\mathbf{y}}_j)=\mathcal{N}(h({\mathbf{y}}_j,{\mathbf{x}}_k),{\mathbf{Q}}_{k,j}) \end{gathered}$$
对联合分布（似然）进行因式分解：
$$P(\mathbf{z},\mathbf{u}|\mathbf{x},\mathbf{y})=\prod _{k}P({\mathbf{u}}_k|{\mathbf{x}}_{k-1},{\mathbf{x}}_k)\prod _{k,j}P({\mathbf{z}}_{k,j}|{\mathbf{x}}_k{\mathbf{y}}_j)$$
定义输入和观测与模型之间的误差：
$$\begin{gathered} e_{\mathbf{u},k}={\mathbf{x}}_k-f({\mathbf{x}}_{k-1},{\mathbf{u}}_k) \\ {e}_{\mathbf{z},j,k}={\mathbf{z}}_{k,j}-h({\mathbf{y}}_j,{\mathbf{x}}_k) \end{gathered}$$
最大似然估计等价于最小化噪声项（误差）的二次型：
$$\min J(\mathbf{x},\mathbf{y})=\sum _k{e}_{\mathbf{u},k}^T{\mathbf{R}}_k^{-1}{e}_{\mathbf{u},k}+\sum _k\sum _j{e}_{\mathbf{z},j,k}^T{\mathbf{Q}}_{k,j}^{-1}{e}_{\mathbf{z},j,k}$$
误差项的二次型称为马哈拉诺比斯距离（Mahalanobis distance），又称为马氏距离。也可以看作是${\mathbf{R}}_k^{-1},{\mathbf{Q}}_{k,j}^{-1}$加权后的欧氏距离，${\mathbf{R}}_k^{-1},{\mathbf{Q}}_{k,j}^{-1}$称为信息矩阵（高斯分布协方差矩阵的逆）

这样就得到了一个最小二乘问题，它的解等价于状态的最大似然估计。

求解

考虑简单的最小二乘问题：
KaTeX parse error: Got function '\boldsymbol' with no arguments as subscript at position 7: \min_\̲b̲o̲l̲d̲s̲y̲m̲b̲o̲l̲ ̲xF(\boldsymbol …
$f$形式简单时，另目标函数导数为0有解析解：
$$\frac{dF}{d\mathbf{x}}=0$$
形式复杂时用迭代方法，从一个初始值出发，不断更新当前的优化变量，使得目标函数下降：

---

1. 给定初始值${\mathbf{x}}_0$
2. 对于第$k$次迭代，寻找一个增量$\Delta {\mathbf{x}}_k$，使得$\Vert f({\mathbf{x}}_k+\Delta {\mathbf{x}}_k){\Vert }_2^2$达到极小值
3. 若$\Delta {\mathbf{x}}_k$足够小则停止
4. 否则，令${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$，返回第2步

---

一阶和二阶梯度法

将目标函数在${\mathbf{x}}_k$附近进行泰勒展开：
$$F({\mathbf{x}}_k+\Delta {\mathbf{x}}_k)\approx F({\mathbf{x}}_k)+\mathbf{J}({\mathbf{x}}_k{)}^T\Delta {\mathbf{x}}_k+\frac{1}{2}\Delta {\mathbf{x}}_k^T\mathbf{H}({\mathbf{x}}_k)\Delta {\mathbf{x}}_k$$
其中，$\mathbf{J}({\mathbf{x}}_k)$是$F(\mathbf{x})$关于$\mathbf{x}$的一阶导数雅可比（Jacobian）矩阵，$\mathbf{H}({\mathbf{x}}_k)$是二阶导数**海塞（Hessian）**矩阵。

保留一阶梯度，称为一阶梯度法：
$$\Delta {\mathbf{x}}^{\ast }=-\mathbf{J}({\mathbf{x}}_k)$$
保留二阶梯度信息，即二阶梯度法，增量方程为：
$$\Delta {\mathbf{x}}^{\ast }=\arg \min (F(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x})$$
另右侧关于$\Delta \mathbf{x}$的导数为零，有：
$$\mathbf{H}\Delta \mathbf{x}=-\mathbf{J}$$
求此方程，得到增量。又称为牛顿法

高斯牛顿法

高斯牛顿法的思想是将$f(\mathbf{x})$（不是$F(\mathbf{x})$）进行一阶泰勒展开：
$$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}$$
这里$\mathbf{J}(\mathbf{x}{)}^T$是$f(\mathbf{x})关于\mathbf{x}$的导数。目标是寻找增量$\Delta \mathbf{x}$使得$\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }^2$最小，即如下最小二乘问题：
$$\Delta {\mathbf{x}}^{\ast }=\arg \underset{\Delta \mathbf{x}}{\min }\frac{1}{2}\Vert f(\mathbf{x})+\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}{\Vert }^2$$
求导并使其为零，最终得到如下方程组：
$$\underset{\mathbf{H}(\mathbf{x})}{\underbrace{\mathbf{J}(\mathbf{x}){\mathbf{J}}^T(\mathbf{x})}}\Delta \mathbf{x}=\underset{\mathbf{g}(\mathbf{x})}{\underbrace{-\mathbf{J}(\mathbf{x})f(\mathbf{x})}}$$
这个方程是关于变量$\Delta \mathbf{x}$的线性方程组，称为增量方程，也可以称为高斯牛顿方程（Gauss-Newton equation）或正规方程（Normal equation）。上式可变为：
$$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}$$

高斯牛顿法用$\mathbf{J}{\mathbf{J}}^T$作为牛顿法中二阶Hessian矩阵的近似，从而省略了计算$\mathbf{H}$的过程。求解增量方程是整个优化问题的核心所在。

高斯牛顿法的算法步骤：

---

1. 给定初始值${\mathbf{x}}_0$
2. 对于第$k$次迭代，求出当前的雅可比矩阵$\mathbf{J}({\mathbf{x}}_k)$和误差$f({\mathbf{x}}_k)$
3. 求解增量方程：$\mathbf{H}\Delta {\mathbf{x}}_k=\mathbf{g}$
4. 若$\Delta {\mathbf{x}}_k$足够小，则停止；否则，令${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$，返回第2步

---

列文伯格-马夸尔特方法

在高斯牛顿法中，可能出现$\mathbf{J}{\mathbf{J}}^T$为奇异矩阵或者病态的情况，此时增量的稳定性较差，导致算法不收敛，原函数在这个点的局部近似不像一个二次函数。

高斯牛顿法中的近似二阶泰勒展开只能在展开点附近有较好的近似效果。因此，想到给$\Delta \mathbf{x}$添加一个范围，称为信赖区域（Trust Region），这个范围定义了在什么情况下二阶近似是有效的，这类方法也称为信赖区域方法。

根据近似模型跟实际模型之间的差异来确定信赖区域的范围：如果差异小，近似效果好，扩大近似的范围；反之缩小近似范围。

定义指标来刻画近似的好坏程度：
$$\rho =\frac{f(\mathbf{x}+\Delta \mathbf{x})-f(\mathbf{x})}{\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}}$$
分子是实际函数下降值，分子是模型下降值。如果接近于1，则近似效果好。

改良版非线性优化框架：

---

1. 给定初始值${\mathbf{x}}_0$，以及初始优化半径$\mu$

2. 对于第$k$次迭代，在高斯牛顿法的基础上加上信赖区域，求解：
   $$\underset{\Delta {\mathbf{x}}_k}{\min }\frac{1}{2}\Vert f({\mathbf{x}}_k)+\mathbf{J}({\mathbf{x}}_k{)}^T\Delta {\mathbf{x}}_k{\Vert }^2,\text{s.t.}\Vert \mathbf{D}\Delta {\mathbf{x}}_k{\Vert }^2\le \mu$$

   其中，$\mu$是信赖区域的半径，$\mathbf{D}$是系数矩阵。列文伯格把$\mathbf{D}$取成单位阵$\mathbf{I}$，相当于直接把$\Delta {\mathbf{x}}_k$约束在一个球中；马夸尔特将$\mathbf{D}$取成非负数对角阵，使得梯度小的维度上约束范围更大一些。

3. 按下式计算$\rho$：
   $$\rho =\frac{f(\mathbf{x}+\Delta \mathbf{x})-f(\mathbf{x})}{\mathbf{J}(\mathbf{x}{)}^T\Delta \mathbf{x}}$$

4. 若$\rho >\frac{3}{4}$，则设置$\mu =2\mu$

5. 若$\rho <\frac{1}{4}$，则设置$\mu =0.5\mu$

6. 若$\rho$大于某阈值，则认为近似可行，令${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$

7. 若$\Delta {\mathbf{x}}_k$足够小，则停止；否则，令${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$，返回第2步

---

在列文伯格-马夸尔特优化中，需要求解如下带不等式约束的优化子问题：
$$\underset{\Delta {\mathbf{x}}_k}{\min }\frac{1}{2}\Vert f({\mathbf{x}}_k)+\mathbf{J}({\mathbf{x}}_k{)}^T\Delta {\mathbf{x}}_k{\Vert }^2,\text{s.t.}\Vert \mathbf{D}\Delta {\mathbf{x}}_k{\Vert }^2\le \mu$$
用拉格朗日法求解，构成拉格朗日函数：
$$\mathcal{L}(\Delta {\mathbf{x}}_k,\lambda )=\frac{1}{2}\Vert f({\mathbf{x}}_k)+\mathbf{J}({\mathbf{x}}_k{)}^T\Delta {\mathbf{x}}_k{\Vert }^2+\frac{\lambda }{2}(\Vert \mathbf{D}\Delta {\mathbf{x}}_k{\Vert }^2-\mu )$$
这里$\lambda$为拉格朗日乘子。令拉格朗日函数关于$\Delta \mathbf{x}$的导数为零，其核心仍然是计算增量的线性方程：
$$(\mathbf{H}+\lambda {\mathbf{D}}^T\mathbf{D})\Delta {\mathbf{x}}_k=\mathbf{g}$$
考虑其简化形式（$\mathbf{D}=\mathbf{I}$），则相当于求解：
$$(\mathbf{H}+\lambda \mathbf{I})\Delta {\mathbf{x}}_k=\mathbf{g}$$
当$\lambda$较小时，列文伯格-马夸尔特方法更接近于高斯牛顿法；$\lambda$较大时，更接近于一阶梯度下降法（最速下降）