【五】非线性优化之最小二乘法及其解法

1.状态估计问题

根据经典的SLAM模型，它有一个运动方程一个观测方程。
$$\{\begin{array}{l}{x}_k=f\left(x_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k\\ {\mathbf{z}}_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)+{\mathbf{v}}_{k,j}\end{array}$$
运动方程可以理解为：在$x_k$点处，通过传感器得到的数据$u_k$和上一时刻的位置$x_{k-1}$进而计算出当前时刻的位置$x_k$。
观测方程可以理解为：在$x_k$处观测$y_i$点时的路标时得到的数据$z_{k,j}$。具体这样的形式是从针孔模型得到的。
假设在$x_k$处对路标${\mathbf{y}}_j$进行了一次观测，对应到图像上的像素位置$z_{k,j}$，那么，观测方程可以表示成
$$s{z}_{k,j}=\mathbf{K}\left({\mathbf{R}}_k{\mathbf{y}}_j+{\mathbf{t}}_k\right)$$
其中$\mathbf{K}$为相机参数，$s$为像素点的距离，也是$\left({\mathbf{R}}_k{\mathbf{y}}_j+{\mathbf{t}}_k\right)$的第三个分量。

在运动和观测方程中，我们通常假设两个噪声项${\mathbf{w}}_k,{\mathbf{v}}_{k,j}$满足零均值的高斯分布。
$$w_k\sim \mathcal{N}\left(0,{\mathbf{R}}_k\right),{\mathbf{v}}_k\sim \mathcal{N}\left(0,{\mathbf{Q}}_{k,j}\right)$$
在这里，$\mathcal{N}$表示高斯分布，0表示零均值，${\mathbf{R}}_k,{\mathbf{Q}}_{k,j}$表示协方差矩阵。在这样带有噪声的情况下，希望数据$\mathbf{z}$和$\mathbf{u}$推断出位姿$\mathbf{x}$和地图$\mathbf{y}$，这就构成了状态估计的问题。

1.1状态估计问题的解决方法

状态估计的解决方法分为两种：

1. 增量/渐进（滤波器）：因为这是一个随时间逐渐到来的数据，因此，应该持有一个当前时刻的估计状态，然后用新的数据来更新它。
2. 批量：把数据“攒起来”一并处理的一种方法。 也就是用从开始到结束的数据来估计整个这个过程的轨迹与地图。

缺点：

1. 增量/渐进：只考虑当前时刻的状态估计，对之前的状态不多加考虑，达不到最大范围的最优化。
2. 批量：需要采集到数据最后一并处理，这就形成了一定情况的不实时性，不符合SLAM的运用场景。

折衷方法：仅对当前时刻附近的一些轨迹进行优化。

1.2具体的求解

从概率学的观点来看，就是已知输入数据$\mathbf{u}$和观测数据$\mathbf{z}$的条件下，求状态$\mathbf{x},\mathbf{y}$的条件概率分布：
$$P(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u})$$

当我们不知道控制输入时，只有一张张地图像时，即只考虑观测方程带来的数据时，相当于估计$P(\mathbf{x},\mathbf{y}\mid \mathbf{z})$的条件概率分布，该问题就是SfM,即如何从许多图像中重建三维空间结构。

在对上式运用贝叶斯法则：
$$P(x,y\mid z,u)=\frac{P(z,u\mid x,y)P(x,y)}{P(z,u)}\propto \underset{\text{似然}}{P(z,u\mid x,y)}\underset{\text{先验}}{P(x,y)}.$$
左侧部分为后验概率，似然和先验已经在图中标注出来了。
直接求后验分布式困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化是方便的
$$(x,y{)}^{\ast }\text{ Map }=\arg \max P(x,y\mid z,u)=\arg \max P(z,u\mid x,y)P(x,y)$$

分母部分由于与带估计的状态$x,y$无关，因此可以忽略。因此求最大后验概率就是求最大似然和先验的乘积。如果没有先验，那么就可以求最大似然估计（MLE）

$$(\mathbf{x},\mathbf{y}{)}^{\ast }\mathrm{M}\mathrm{L}\mathrm{E}=\arg \max P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})$$

思想：从传感器或者图片观测数据中获取位置和路标信息并不容易时，就反过来思考，什么样的位置和路标信息能够得到这样的观测数据

2.SLAM中的最小二乘

要求最大似然估计，这时，最大似然估计的简单表达式在干扰为正态分布（又名“高斯分布”）时最简单。
对观测模型，某一次观测：

$$z_{k,j}=h\left(y_j,x_k\right)+v_{k,j}$$

噪声项${\mathbf{v}}_k\sim \mathcal{N}\left(0,{\mathbf{Q}}_{k,j}\right)$，所以观测数据的条件概率为：
$$P\left(z_{j,k}\mid x_k,y_j\right)=\mathcal{N}\left(h\left(y_j,x_k\right),{\mathbf{Q}}_{k,j}\right)$$

仍旧是正态分布，考虑单次观测的最大似然估计，可以使用最小化负对数来求一个正态分布的最大似然。
对于任意高维正态分布$\mathbf{x}\sim \mathcal{N}(\mu ,\mathbf{\Sigma })$，其概率密度函数展开形式为：
$$P(x)=\frac{1}{\sqrt{(2\pi {)}^N\det (\Sigma )}}\exp \left(-\frac{1}{2}(x-\mu {)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(x-\mu )\right)$$

取负对数：
$$-\ln (P(x))=\frac{1}{2}\ln \left((2\pi {)}^N\det (\Sigma )\right)+\frac{1}{2}(x-\mu {)}^{\mathrm{T}}{\Sigma }^{-1}(x-\mu )$$

因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。在最小化上式的$x$时，第一项与$x$无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。对于SLAM相当于在求：
$$\begin{array}{rl}{\left(x_k,y_j\right)}^{\ast }& =\arg \max \mathcal{N}\left(h\left(y_j,x_k\right),Q_{k,j}\right)\\ & =\arg \min \left({\left(z_{k,j}-h\left(x_k,y_j\right)\right)}^{\top }{Q}_{k,j}^{-1}\left(z_{k,j}-h\left(x_k,y_j\right)\right)\right)\end{array}$$
该式等价于最小化噪声项(即误差)的一个二次型。这个二次型称为马哈拉诺比斯距离，又叫马氏距离它也可以看成由$Q_{k,j}^{-1}$加权之后的欧氏距离（二范数），这里$Q_{k,j}^{-1}$也叫作信息矩阵，即高斯分布协方差矩阵之逆。

假设各个时刻的输入和观测是相互独立的，这意味着各个输入之间是独立的，各个观测之间是独立的，并且输入和观测是相互独立的。因此：
$$P(z,u\mid x,y)=\prod _{k}P\left(u_k\mid x_{k-1},x_k\right)\prod _{k,j}P\left(z_{k,j}\mid x_{k,}{y}_j\right)$$
这说明我们可以独立地处理各时刻地运动和观测。定义各次输入和观测数据与模型之间地误差：
$$\begin{array}{c}{e}_{u,k}=x_k-f\left(x_{k-1},u_k\right)\\ e_{z,j,k}=z_{k,j}-h\left(x_k,y_j\right)\end{array}$$
那么，最小化所有时刻估计值与真实读数之间地马氏距离，等价于求最大似然估计。负对数允许把乘积变成求和：

$$\min J(x,y)=\sum _k{e}_{u,k}^T{R}_k^{-1}{e}_{u,k}+\sum _k\sum _j{e}_{z,k,j}^T{Q}_{k,j}^{-1}{e}_{z,k,j}$$
这样就得到了一个最小二乘问题，它的解等价于状态的最大似然估计。

SLAM中的最小二乘问题具有的一些特定的结构：
1.整个问题的目标函数由许多个误差（加权的）二次型组成。虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。
2.如果使用李代数表示增量，则该问题是无约束的最小二乘问题。但如果用旋转矩阵/变换矩阵描述位姿，则会引入旋转矩阵自身的约束。额外的约束会使优化变得更困难。这体现了李代数的优势。
3.我们使用了二次型度量误差。误差的分布将影响此项在整个问题中的权重。如果某次观测的非常准确，那么协方差矩阵就会“小”，而信息矩阵就会“大”，所以这个误差项会在整个问题中占有较高的权重。

我们对状态的估计值进行微调，使得整体的误差下降一些，一般会下降达到一个极小值。这就是一个典型的非线性优化的过程。
接下来使用最小二乘的解法来解决上述问题。

可以看这个链接的最小二乘最小二乘问题总结也可以看接下来的SLAM书上的解决方法

非线性最小二乘

一个简单的最小二乘问题：
$$\underset{x}{\min }F(x)=\frac{1}{2}\Vert f(\mathbf{x}){\Vert }_2^2$$

其中，自变量$\mathbf{x}\in {\mathbb{R}}^n$，$f$是任意的标量非线性函数$f(x)$。这里的$1/2$是无关紧要的，不影响结论。求最优值就求导，并令导数为0
$$\frac{\mathrm{d}F}{\mathrm{d}\mathbf{x}}=0$$

即得到的值，可能是极大、极小或鞍点处的值，只要比较大小即可。如果$f(x)$是简单的线性函数，那么就是线性最小二乘问题。

有些函数形式复杂，不容易求导或者导数后不容易求解，而且又可能需要知道目标函数的全局性质，因此求导并求解这一步比较难，因此可以使用迭代的方式：从一个初始值出发不断地更新当前地优化变量，使目标函数下降。具体步骤为：

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

这让求解导数为0的问题变成了一个不断**寻找下降增量$\Delta x_k$**的问题。在找$\Delta x_k$的过程中，只需了解$f$在迭代值处的局部性质而非全局性质。当函数下降直到增量非常小的时候，就认为算法收敛，目标函数达到了一个极小值。

一阶和二阶梯度法

考虑第$k$次迭代，假设在$x_k$处，想要寻找到增量$\Delta x_k$，最直观的方式是讲目标函数在$x_k$附近进行泰勒展开：

$$F\left(x_k+\Delta x_k\right)\approx F\left(x_k\right)+J{\left(x_k\right)}^{\mathrm{T}}\Delta x_k+\frac{1}{2}\Delta x_k^{\mathrm{T}}H\left(x_k\right)\Delta x_k$$

其中$J\left(x_k\right)$是$F(x)$关于$x$的一阶导数（也叫梯度、雅可比（Jacobian）矩阵）,$H$则是二阶导数（海塞（Hessian）矩阵），他们都在$x_k$处取值。
保留泰勒展开的一阶或者二阶项，对应的求解方法称为一阶梯度或二阶梯度法。如果保留一阶梯度，那么取增量为反向的梯度，即可保证函数下降：
$$\Delta {\mathbf{x}}^{\ast }=-\mathbf{J}\left({\mathbf{x}}_k\right)$$

这种方法被称为最速下降法。直观意义就是只要我们沿着反向梯度方向前进，在一阶（线性）的近似下，目标函数必定会下降。

保留二阶梯度信息，增量方程为：
$$\Delta x^{\ast }=\arg \min \left(F(x)+J(x{)}^{\mathrm{T}}\Delta x+\frac{1}{2}\Delta x^{\mathrm{T}}H\Delta x\right)$$

右侧只含了$\Delta x$的零次、一次和二次项。求右侧等式关于$\Delta x$的导数并令它为零，得到：
$$J+H\Delta x=0\Rightarrow H\Delta x=-J$$
求解这个线性方程就得到了增量，这个方法为牛顿法
一阶和二阶梯度法从字面意思可以理解，就是对误差函数进行泰勒展开保留一阶或者二阶，然后求$\Delta x$使得整个函数值向小的方向进展，保留一阶就是最速下降法、保留二阶就是牛顿法，方法就是把函数在迭代点附近及逆行泰勒展开，并针对更新量做最小化即可
缺点：最速下降法过于贪心。容易走出锯齿路线，反而增加了迭代次数，牛顿法则需要计算木匾函数的$H$矩阵，规模较大时不方便计算
针对上面的缺点，提出的拟牛顿法：高斯牛顿法和列文伯格–马夸尔特方法

高斯牛顿法

高斯牛顿法的思想：对$f(x)$进行一阶的泰勒展开，与牛顿法的最大区别就是：牛顿法是对$F(x)$进行的一阶泰勒展开，它俩的关系是${\min }_{x}F(x)=\frac{1}{2}\Vert f(\mathbf{x}){\Vert }_2^2$

(这里可以把$f(x)$理解为误差$e(x)$，而$F(x)$理解为包含误差的函数)
$$f(\mathbf{x}+\Delta \mathbf{x})\approx f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}$$

寻求$\Delta x$使得$\Vert f(x+\Delta x){\Vert }^2$达到最小(为什么是求它达到最小：1.因为$f(x)$与$F(x)$的关系。2.是因为后边的展开后的式子包含了$\Delta x$的平方项，这就跟牛顿相比减少了求$H$的效果，进而说明了为什么前面展开一阶就可以了)，进而转换成求解一个线性的最小二乘问题：
$$\Delta {\mathbf{x}}^{\ast }=\arg \underset{\Delta \mathbf{x}}{\min }\frac{1}{2}\Vert f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}{\Vert }^2$$
展开后的式子为：
$$\begin{array}{rl}\frac{1}{2}\Vert f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}{\Vert }^2& =\frac{1}{2}(f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x}{)}^{\mathrm{T}}(f(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta \mathbf{x})\\ & =\frac{1}{2}\left(\Vert f(\mathbf{x}){\Vert }_2^2+2f(\mathbf{x}{)}^{\mathrm{T}}\mathbf{J}(\mathbf{x})\Delta \mathbf{x}+\Delta {\mathbf{x}}^{\mathrm{T}}\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}\mathbf{J}(\mathbf{x})\Delta \mathbf{x}\right).\end{array}$$
对$\Delta x$求导，并令其为0：
$$2\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}f(\mathbf{x})+2\mathbf{J}(\mathbf{x}{)}^{\mathrm{T}}\mathbf{J}(\mathbf{x})\Delta \mathbf{x}=0$$
得到：
$$\underset{\mathbf{H}(\mathbf{x})}{\mathbf{J}(\mathbf{x}){\mathbf{J}}^{\mathrm{T}}}(\mathbf{x})\Delta \mathbf{x}=\underset{\mathbf{g}(\mathbf{x})}{-\mathbf{J}(\mathbf{x})f(x)}$$

这个方程为增量方程，是$\Delta x$的线性方程，也叫高斯牛顿方程或者正规方程。那么上式就为：
$$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}.$$
拿这个与牛顿法进行对比：$H\Delta x=-J$可以看出：高斯牛顿法是用$J{J}^T$作为牛顿法中的$H$，进而省略了$H$的计算。
整个步骤可以总结为：

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

增量方程的求解占据着主要地位，只要能顺利求出增量，就能保证目标函数正确下降。
**缺点：**它要求$H$是可逆的（而且是正定的），但实际数据中计算得到的$J{J}^T$却只有半正定性。也就是说，在使用高斯牛顿法时，可能出现$J{J}^T$为奇异矩阵或者病态的情况，此时增量的稳定性较差，导致算法不收敛。更严重的是，就算我们假设非奇异也非病态，如果我们求出来的步长$\Delta x$太大，也会导致我们采用的局部近似不够准确，这样一来我们甚至无法保证它的迭代收敛，哪怕是让目标函数变得更大都是有可能的。

列文伯格–马夸尔特方法（阻尼牛顿法）

这个方法的思想是：一阶梯度下降法也就是最速下降法与高斯牛顿法相结合，定义了一个信赖区域来决定谁来起主导作用。
信赖区域：给$\Delta x$添加一个范围，定义了在什么情况下二阶近似（是对$F(x)$说的二阶，对$f(x)$说的一阶）是有效的。出了区域近似可能会出问题。
定义了一个指标$\rho$来计算二阶及其更高阶对近似的作用大小：
$$\rho =\frac{f(\mathbf{x}+\Delta \mathbf{x})-f(\mathbf{x})}{\mathbf{J}(\mathbf{x})\Delta \mathbf{x}}$$
分子：实际函数下降的值，分母：近似模型下降的值
如果$\rho$接近于1，则近似是好的。如果$\rho$太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果$\rho$比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。

改良版的非线性优化框架，比高斯牛顿法会有更好的效果：

1. 给定初始值$x_0$，以及初始优化半径$\mu$
2. 对于第k次迭代，在高斯牛顿法的基础上机上信赖区域，求解：
   $$\underset{\Delta {\mathbf{x}}_k}{\min }\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}\left({\mathbf{x}}_k\right)\Delta {\mathbf{x}}_k\Vert }^2,\text{ s.t. }{\Vert \mathbf{D}\Delta {\mathbf{x}}_k\Vert }^2⩽\mu$$
   其中，$\mu$是信赖区域的半径，$D$为系数矩阵，如果$D$为$I$，则表示把$\Delta x$约束在一个球中。
3. 按上式计算$\rho$
4. 若$\rho >\frac{3}{4}$，则设置$\mu =2\mu$
5. 若$\rho <\frac{1}{4}$，则设置$\mu =0.5\mu$
6. 如果$\rho$大于某阈值，则认为近似可行。令$x_{k+1}=x_k+\Delta x$
7. 判断算法是否收敛。如果不收敛返回第2步，否则结束。

用拉格朗日乘子把约束项放到目标函数中，构成拉个朗日函数：
$$\underset{\Delta {\mathbf{x}}_k}{\min }\frac{1}{2}{\Vert f\left({\mathbf{x}}_k\right)+\mathbf{J}{\left({\mathbf{x}}_k\right)}^T\Delta {\mathbf{x}}_k\Vert }^2+\frac{\lambda }{2}\Vert \mathbf{D}\Delta \mathbf{x}-\mu {\Vert }^2.$$
这里$\lambda$为拉格朗日算子。求导，计算增量：
$$\left(\mathbf{H}+\lambda {\mathbf{D}}^{\mathrm{T}}\mathbf{D}\right)\Delta \mathbf{x}=\mathbf{g}$$

$$\left(\mathbf{H}+\lambda \mathbf{I}\right)\Delta \mathbf{x}=\mathbf{g}$$

我们看到，当参数$\lambda$比较小时，$H$占主要地位，这说明二次近似模型在该范围内是比较好的，列文伯格—马夸尔特方法更接近于高斯牛顿法。另一方面，当$\lambda$比较大时，$\lambda I$占据主要地位，列文伯格—马夸尔特方法更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。列文伯格—马夸尔特方法的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量$\Delta x$

参考

1.视觉SLAM十四讲