SLAM中非线性优化问题求解方法

前言

这篇文章是为了解释下面几个问题:

1. Levenberg-Marquardt算法、Dog-Leg算法详细实现流程？
2. Gauss-Newton算法为什么优于最速下降算法？
3. Levenberg-Marquardt算法为什么优于Gauss-Newton算法？

简单对比

算法类型	优点	缺点
最速下降法	一定能收敛	线性收敛
牛顿法	二阶收敛	需要计算复杂的Hessian矩阵、Hessian矩阵半正定会带来增量求解的不稳定或不收敛
高斯牛顿法	正常线性收敛，当目标函数值为0时二阶收敛	近似的Hessian矩阵半正定，会带来增量求解的不稳定和不收敛
列文伯格-马夸尔特法	解决高斯牛顿法Hessian矩阵半正定问题，能以较稳定较快速的方法收敛到局部最优值	一般为线性收敛
狗腿法	能较稳较快的收敛到局部最优值，在参考文献中有通过例子说明该方法在求解非线性方程组时在速度上优于LM法	一般为线性收敛

上面表格中出现了线性收敛、二阶收敛等收敛性名词，其定义如下:
$$\begin{array}{ccc}线性收敛:& \Vert {\mathbf{e}}_{k+1}\Vert \le \Vert a{\mathbf{e}}_k\Vert & 当\Vert {\mathbf{e}}_k\Vert 比较小，并且0<a<1\\ 二阶收敛:& \Vert {\mathbf{e}}_{k+1}\Vert \le O({\Vert {\mathbf{e}}_k\Vert }^2)& 当\Vert {\mathbf{e}}_k\Vert 比较小\\ 超线性收敛:& \Vert {\mathbf{e}}_{k+1}\Vert /\Vert {\mathbf{e}}_k\Vert \to 0& 当k\to \infty \end{array}$$

Levenberg-Marquardt算法流程

上面伪码中需要说明的有:

1. 最小二乘问题形式如下:
   $$\mathbf{F}(\mathbf{x})=\frac{1}{2}\sum _{i=1}^m({\mathbf{f}}_i(\mathbf{x}){)}^2=\frac{1}{2}{\Vert \mathbf{f}(\mathbf{x})\Vert }^2=\frac{1}{2}{\mathbf{f}(\mathbf{x})}^T\mathbf{f}(\mathbf{x})\text{\hspace{1em}(0)}$$
2. $\mathbf{L}(\mathbf{h})$的形式如下:
   $$\mathbf{F}(\mathbf{x}+\mathbf{h})\simeq \mathbf{L}(\mathbf{h})=\mathbf{F}(\mathbf{x})+{\mathbf{h}}^T{\mathbf{J}}^T\mathbf{f}+\frac{1}{2}{\mathbf{h}}^T{\mathbf{J}}^T\mathbf{J}\mathbf{h}\text{\hspace{1em}(1)}$$
   其中 $\mathbf{J}(\mathbf{x})$ 是 $\mathbf{f}(\mathbf{x})$ 相对于 $\mathbf{x}$ 的雅克比矩阵，可以看出 $\mathbf{L}(\mathbf{h})$ 其实就是 $\mathbf{F}(\mathbf{x})$ 在 $\mathbf{x}$ 处的近似二阶泰勒展开，用 ${\mathbf{J}}^T\mathbf{J}$ 近似代替 Hessian矩阵 ${\mathbf{F}}^{\prime \prime }(\mathbf{x})$。
3. 增益系数理解:
   在"计算增益系数"公式中可以看出分子是 $\mathbf{F}(\mathbf{x})$ 实际变化，而分母是其近似 $\mathbf{L}(\mathbf{h})$ 的变化，因此该分数值主要是衡量了近似的好坏，分数值越大说明近似得越好，反之则近似得越差。
4. 阻尼系数理解:
   从更新阻尼系数 $\mu$ 的过程来看，其值随着增益系数 $\rho$ 的增大而不断减小，结合增益系数理解部分内容，可以看出二阶泰勒近似得越好，则减小 $\mu$ ，反之则增大 $\mu$。
   再从求解线性方程过程中的线性方程角度来看，当 $\mu$ 逐渐变大到极大时，此时增量 ${\mathbf{h}}_{lm}=-\mathbf{g}/\mu$，此为最速下降法计算增量过程，也就是说此时LM法变成了最速下降法。而当 $\mu$ 逐渐变小到极小时，线性方程则近似为 ${\mathbf{A}\mathbf{h}}_{lm}=-\mathbf{g}$，此为高斯牛顿法增量计算过程，也就是说此时LM法变成了高斯牛顿法。
   综合以上来看，LM算法是高斯牛顿法和最速下降法结合的一个混合算法，当二阶近似结果越接近真实 $\mathbf{F}(\mathbf{x})$ 时，此混合算法越偏向于利用高斯牛顿法来计算增量，反之则更偏向于利用最速下降法来计算增量。
5. 迭代停止条件
   LM算法迭代停止条件有三个，分别是函数值 $\mathbf{g}$ 的范数小于给定的阈值 ${\epsilon }_1$， 迭代次数超过给定阈值 $k_{max}$，迭代获得的增量范数小于给定阈值 ${\epsilon }_2(\Vert \mathbf{x}\Vert +{\epsilon }_2)$。此三个阈值都是结合实际处理问题来人为设定的。

Dog-Leg算法流程

上面伪码中需要说明的有:

1. 计算最速下降法方向
   先计算相对增益:
   $$\frac{\mathbf{F}(\mathbf{x})-\mathbf{F}(\mathbf{x}+\alpha {\mathbf{h}}_{\mathbf{s}\mathbf{d}})}{\alpha ||{\mathbf{h}}_{sd}||}=-\frac{1}{||{\mathbf{h}}_{sd}||}{\mathbf{h}}_{sd}^T{\mathbf{F}}^{\prime }(\mathbf{x})=-||{\mathbf{F}}^{\prime }(\mathbf{x})||cos(\theta )$$
   式中 $\theta$ 角是向量 ${\mathbf{h}}_{sd}$和一阶导数 ${\mathbf{F}}^{\prime }(\mathbf{x})$ 之间的夹角，可以看出当两者方向相反时，即 $\theta$ 最大时，相对增益最大，也就是下降速度最快的方向，所以:
   $${\mathbf{h}}_{sd}=-{\mathbf{F}}^{\prime }(\mathbf{x})$$
2. 计算最速下降法步长
   一阶泰勒展开:
   $$\begin{array}{rl}\mathbf{F}(\mathbf{x}+\alpha {\mathbf{h}}_{sd})& \simeq \frac{1}{2}{\Vert \mathbf{f}(\mathbf{x})+\alpha \mathbf{J}(\mathbf{x}){\mathbf{h}}_{sd}\Vert }^2\\ & =\mathbf{F}(\mathbf{x})+\alpha {\mathbf{h}}_{sd}^T{\mathbf{J}(\mathbf{x})}^T\mathbf{f}(\mathbf{x})+\frac{1}{2}{\alpha }^2{\Vert {\mathbf{J}(\mathbf{x})\mathbf{h}}_{sd}\Vert }^2\end{array}$$
   再令 $\mathbf{F}(\mathbf{x}+\alpha {\mathbf{h}}_{sd})$ 相对于 $\alpha$ 的导数为0，可得:
   $$\alpha =-\frac{{\mathbf{h}}_{sd}^T\ast {\mathbf{J}(\mathbf{x})}^T\mathbf{f}(\mathbf{x})}{{\Vert {\mathbf{J}(\mathbf{x})\mathbf{h}}_{sd}\Vert }^2}=\frac{{\Vert \mathbf{g}\Vert }^2}{{\Vert \mathbf{J}(\mathbf{x})\mathbf{g}\Vert }^2}$$
3. 计算狗腿法增量
   
4. 增益系数、迭代终止条件等与LM算法一样

回答问题

1. 上面的LM算法和DL算法的伪码流程已经非常清晰了，并对里面的计算部分进行了详细的推导计算说明
2. 从上面表格对比来看，主要是高斯牛顿法收敛速度更快
3. 高斯牛顿法的近似Hessian矩阵是半正定的，必然会存在计算不稳定不收敛的情况，但LM算法混合了最速下降法在里面，最速下降法有个非常显著的特性就是一定会往收敛方向收敛，因此LM算法能有效的改善高斯牛顿法的不稳定问题。该问题也可通过数学证明LM算法的系数矩阵是正定的，从而避免不稳定问题，可参考 关于Levenberg-Marquadt的一些求解和证明。

参考

METHOD FOR NON-LINEAR LEAST SQUARES PROBLEMS
梯度下降法和高斯牛顿法的区别在哪里，各自的优缺点呢？
关于Levenberg-Marquadt的一些求解和证明
视觉slam十四讲