【最小二乘及非线性优化】

前言

\quad 最小二乘及非线性优化是SLAM技术的基础，在此梳理该问题相关基础知识，以加深理解。

最小二乘问题以及SLAM中的最小二乘

\quad 最简单的最小二乘问题可以表示为：
$$\underset{x}{\min }F(x)=\frac{1}{2}||f(x)|{|}_2^2\text{\hspace{1em}(1)}$$

\quad 而在SLAM问题中，仅考虑其观测方程：
$$z_{k,j}=f(y_j,x_k)+v_{k,j}\text{\hspace{1em}(2)}$$
\quad 其中， $x_k$表示系统状态，$y_j$表示路标点，$v_{k,j}$表示观测噪声，$z_{k,j}$表示系统观测。在已知系统观测的条件下，估计系统状态，即为求解条件概率 $P(x,y|z)$， 根据贝叶斯公式：
$$P(x,y|z)=\frac{P(z|x,y)P(x,y)}{P(z)}\text{\hspace{1em}(3)}$$
\quad 上式中的 $P(x,y|z)$ 称之为后验概率，可以形象理解为在已知系统的结果（后）的条件下估计系统状态（先），$P(x,y)$ 称为状态先验，$P(z|x,y)$称为似然，可以形象理解为估计系统处于何种状态下，最有可能得到如今的系统状态。
\quad 上式中，单纯的系统状态$z$的概率与带估计变量$(x,y)$无关，因此可以得出重要的结论：最大后验概率等价于最大化先验与似然的乘积。 而更进一步地，当系统缺乏先验时，则变成了 最大后验概率等价于最大似然估计 ，即对应上式中的$P(z|x,y)$。
\quad 普遍假设公式（2）中的噪声服从高斯分布，即$v_{k,j}$ ~ $N(0,Q_{k,j})$，则$z_{k,j}$~$N(f(y_j,x_k),Q_{k,j})$。
\quad 对于一个高斯分布$x$ ~ $N(\mu ,\Sigma )$，其概率分布为：
$$P(x)=\frac{1}{\sqrt{(2\pi {)}^N|\Sigma |}}exp(-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu ))\text{\hspace{1em}(4)}$$
\quad 为求解$P(x)$的最大值，对其取负对数：
$$\begin{gathered} -ln(P(x))=\frac{1}{2}ln((2\pi {)}^N|\Sigma |)+ \\ \frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )\text{\hspace{1em}(5)} \end{gathered}$$
\quad ln(x)单调递增，由此P(x)最大值等价于其负对数最小值，且式（5）中第一项与x无关，由此：
$$\begin{gathered} (x_k,y_j{)}^{\ast }=argmax(P(z_{k,j})) \\ =argmin((z_{k,j}-f(y_j,x_k){)}^T{Q}_{k,j}^{-1}(z_{k,j}-f(y_j,x_k))\text{\hspace{1em}(6)} \end{gathered}$$
\quad 通常假设系统各个历史时刻的观测相互独立，因此多个系统状态的联合分布可以写作乘积形式，则该分布的负对数可以写作加和形式，令$e_{z,k,j}=z_{k,j}-h(y_j,x_k)$（即为估计误差），则各时刻历史状态的估计问题就转化为如下的最小二乘问题（即为最小化误差的加权二范数）：
$$(\mathbf{x},\mathbf{y}{)}^{\ast }=argmin\sum _{j,k}{e}_{z,k,j}^T{Q}_{k,j}^{-1}{e}_{z,k,j}\text{\hspace{1em}(7)}$$

最小二乘求解

\quad 最小二乘问题的求解，复杂程度取决于$h(y_j,x_k)$的复杂度。当$h$是线性函数时，则简单地通过求导即可得到系统状态的有效估计；而当$h$是一个非线性函数时，则需要利用非线性迭代求解，典型的方法包括一阶梯度法（最速下降法）、二阶梯度法（牛顿法）、Gauss-Newton、Levenberg-Marquardt 算法。

线性最小二乘求解

\quad 仍利用式（2）为例，当$f$函数为线性函数时，可以将不同时刻的系统观测转化为矩阵形式：$\mathbf{z}=\mathbf{A}\mathbf{x}$，此处$\mathbf{x}$表示不同时刻批量系统状态向量， $\mathbf{z}$表示批量系统观测向量。此时最小二乘问题转化为：
$${\mathbf{x}}^{\ast }=argmin((\mathbf{z}-\mathbf{A}\mathbf{x}{)}^T{\Sigma }^{-1}(\mathbf{z}-\mathbf{A}\mathbf{x}))\text{\hspace{1em}(8)}$$
\quad 此时，根据矩阵求导思路，易得：
$$\begin{gathered} 2{\mathbf{A}}^{\mathbf{T}}{\Sigma }^{-1}(\mathbf{A}\mathbf{x}-\mathbf{z})=0 \\ {\mathbf{x}}^{\ast }=({\mathbf{A}}^{\mathbf{T}}{\mathbf{\Sigma }}^{-1}\mathbf{H}){\mathbf{A}}^{\mathbf{T}}{\mathbf{\Sigma }}^{-1}\mathbf{z}\text{\hspace{1em}(9)} \end{gathered}$$
\quad 关于矩阵求导相关讲解，可以参考：矩阵求导与矩阵微分

非线性最小二乘求解

\quad 当上文中的$f$函数为非线性函数时，定义$\mathbf{r}(\mathbf{x})=\mathbf{z}-\mathbf{A}\mathbf{x}$，则最小二乘问题转化为: $argmi{n}_x\mathbf{F}(\mathbf{r})=argmi{n}_x({\mathbf{r}}^{\mathbf{T}}{\mathbf{\Sigma }}^{-1}\mathbf{r})$，对于此类的最小二乘问题，通用方案都是通过Taylor将目标函数近似为线性函数，而后迭代计算求解。迭代过程如下图所示（取自高博SLAM十四讲）：

一阶梯度法

\quad 一阶梯度法直接将$\mathbf{F}(\mathbf{r})$展开为如下形式:
$$\mathbf{F}(\mathbf{r})=\mathbf{F}(\mathbf{r})+{{\mathbf{J}}_{\mathbf{F}}}^{\mathbf{T}}\xi \text{\hspace{1em}(10)}$$
\quad 此时，每次的迭代增量 $\xi =-{{\mathbf{J}}_{\mathbf{F}}}^{\mathbf{T}}$，即向目标函数的逆梯度方向进行迭代，该方法也称为最速下降法。

二阶梯度法

\quad 如式（9），若保留$\mathbf{F}(\mathbf{e})$的二阶展开项，形式如下：
$$\mathbf{F}(\mathbf{r})=\mathbf{F}(\mathbf{r})+{{\mathbf{J}}_{\mathbf{F}}}^{\mathbf{T}}\xi +\frac{1}{2}{\xi }^{\mathbf{T}}\mathbf{H}\xi \text{\hspace{1em}(11)}$$
\quad 为求解$\xi$使上式极小，因此对$\xi$求导，结果如下：
$$\xi ={{\mathbf{J}}_{\mathbf{F}}}^{\mathbf{T}}+\mathbf{H}\xi \to \mathbf{H}\xi =-{{\mathbf{J}}_{\mathbf{F}}}^{\mathbf{T}}\xi \text{\hspace{1em}(12)}$$
\quad 二阶梯度法又称为牛顿法，但考虑到该方法每次求解过程中均需更新二阶导数H矩阵，计算复杂度较高

Gauss-Newton

\quad 不同于上述两种方法，Gauss-Newton对$\mathbf{r}(\mathbf{x})$进行一阶Taylor展开：$\mathbf{r}(\mathbf{x})=\mathbf{r}(\mathbf{x})+{{\mathbf{J}}_{\mathbf{r}}}^{\mathbf{T}}\xi$，此时目标函数为：
$$\begin{gathered} \mathbf{F}(\mathbf{r})=(\mathbf{r}+{{\mathbf{J}}_{\mathbf{r}}}^{\mathbf{T}}\xi {)}^{\mathbf{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{r}+{{\mathbf{J}}_{\mathbf{r}}}^{\mathbf{T}}\xi ) \\ ={\mathbf{r}}^{\mathbf{T}}{\mathbf{\Sigma }}^{-1}\mathbf{r}+2{\xi }^{\mathbf{T}}{\mathbf{J}}_{\mathbf{r}}{\mathbf{\Sigma }}^{-1}\mathbf{r}+\xi {\mathbf{J}}_{\mathbf{r}}{\mathbf{\Sigma }}^{-1}{{\mathbf{J}}_{\mathbf{r}}}^{\mathbf{T}}\xi \text{\hspace{1em}(13)} \end{gathered}$$
\quad 对$\xi$进行求导计算迭代增量：
$${\mathbf{J}}_{\mathbf{r}}{\mathbf{\Sigma }}^{-1}\mathbf{r}+{\mathbf{J}}_{\mathbf{r}}{\mathbf{\Sigma }}^{-1}{{\mathbf{J}}_{\mathbf{r}}}^{\mathbf{T}}\xi =0\to {\xi }^{\ast }=-({\mathbf{J}}_{\mathbf{r}}{\mathbf{\Sigma }}^{-1}{{\mathbf{J}}_{\mathbf{r}}}^{\mathbf{T}}{)}^{-1}{\mathbf{J}}_{\mathbf{r}}{\mathbf{\Sigma }}^{-1}\mathbf{r}$$

Levenberg-Marquardt

\quad 高斯牛顿法中采用的Taylor展开仅能在展开点附近产生较好的近似。LM算法在此基础上，为增量$\xi$添加了一个信赖区域$\mu$，并定义了一个指标，用于衡量近似效果：$\rho =\frac{r(x+\xi )-r(x)}{{J_r}^T(\xi )}=\frac{实际变化量}{近似变化量}$，当$\rho$过小时，则近似变化量远超实际变化量，需缩小信赖区域；反之，$\rho$过大时，则近似变化量不及实际变化量，需扩大信赖区域。
\quad 由此，LM方法中的最小二乘问题转化为：
$$\begin{gathered} \mathbf{m}\mathbf{i}{\mathbf{n}}_{\mathbf{x}}(\mathbf{r}+{{\mathbf{J}}_{\mathbf{r}}}^{\mathbf{T}}\xi {)}^{\mathbf{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{r}+{{\mathbf{J}}_{\mathbf{r}}}^{\mathbf{T}}\xi )) \\ s.t.{||\mathbf{D}\xi ||}^2<\mu \text{\hspace{1em}(14)} \end{gathered}$$
\quad 上式属于带有不等式约束的最优化问题，可以通过构造拉格朗日函数，统一为无约束的最优化问题：
$$\begin{gathered} \mathbf{m}\mathbf{i}{\mathbf{n}}_{\mathbf{x}}(\mathbf{r}+{{\mathbf{J}}_{\mathbf{r}}}^{\mathbf{T}}\xi {)}^{\mathbf{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{r}+{{\mathbf{J}}_{\mathbf{r}}}^{\mathbf{T}}\xi )) \\ +\frac{\lambda }{2}({||\mathbf{D}\xi ||}^2-\mu )\text{\hspace{1em}(15)} \end{gathered}$$
\quad 其中$\lambda$表示拉格朗日乘子。此后求导过程与Gauss-Newton方法相同，得到迭代增量结果：
$$\begin{gathered} {\mathbf{J}}_{\mathbf{r}}{\mathbf{\Sigma }}^{-1}\mathbf{r}+{\mathbf{J}}_{\mathbf{r}}{\mathbf{\Sigma }}^{-1}{{\mathbf{J}}_{\mathbf{r}}}^{\mathbf{T}}\xi +\lambda {\mathbf{D}}^{\mathbf{T}}\mathbf{D}\xi =0 \\ \to {\xi }^{\ast }=-({\mathbf{J}}_{\mathbf{r}}{\mathbf{\Sigma }}^{-1}{{\mathbf{J}}_{\mathbf{r}}}^{\mathbf{T}}+\lambda {\mathbf{D}}^{\mathbf{T}}\mathbf{D}{)}^{-1}{\mathbf{J}}_{\mathbf{r}}{\mathbf{\Sigma }}^{-1}\mathbf{r} \end{gathered}$$

方案优缺点总结

1. 一阶梯度法（最速下降法）是最直观的迭代算法，计算量最小，但在迭代后期，易在极小值附近左右横跳，导致迭代次数增加；
2. 二阶梯度法（牛顿法）同样较为直观，但由于每次迭代都要计算Hessian矩阵，计算量较大；
3. Gauss-Newton法可以视为牛顿法的改进，利用$J{J}^T$近似Hessian矩阵，计算量较小，但问题在于$J{J}^T$仅能保证半正定性质，可能存在奇异或病态问题；且当增量计算结果较大时，对目标函数的近似准确度下降，可能导致迭代无法收敛
4. LM算法是高斯牛顿法的一种改进形式，通过增加信赖区域的方式，降低了病态问题出现的可能，整体迭代过程更为健壮，但迭代收敛速度一般比高斯牛顿法更慢。

---

\quad 在实际问题中，一般选择GN或者LM算法作为迭代方案，当问题接近病态时选择LM方案，其余时刻则选择GN方案。