SLAM中常用的非线性优化算法学习笔记

Table of Contents

1.优化的由来和梯度下降法

2.最速下降法和牛顿法

3.（Guass-Newton）高斯-牛顿法

4.（Levenburg-Marquadt）列文伯格-马夸尔特方法

5.小结

---

本文为高博《视觉SLAM十四讲》第六讲的学习笔记。

1.优化的由来和梯度下降法

了解过机器学习相关算法的话，都知道优化这个词语。优化指的是通过改变某个函数中的自变量，来得到的最小值或者最大值。

我们把这里的优化函数称为目标函数，变量称为优化变量。一些求解中还带有某些约束条件，要求的函数最小值或者最大值必须满足这些约束条件，这里的约束条件称为优化约束。

函数的导数的几何意义是在处的斜率，如果在处，则表示函数在自变量处存在一个区间，在这个区间内是单调递增的。当在处的导数时，表示可能在处取得极大值、极小值或者鞍点，遍历比较这些值就可以获得最小值。当目标函数是凸函数时，沿着梯度下降方向更新，对求得的最小值是全局最小值，一般情况下其值不能保证是全局最小值。

通常我们将称为函数的梯度，想象人从山底下往山上边走，最快上山的办法就是沿着梯度往上走，那么求最小值也就相当于往下走（找到最小的函数值），就需要沿着梯度的反方向走。

实际问题中，由于通常表示的是我们对某个值进行逼近的一种误差，只要不断调整变量使得误差值达到最小，也就相当于求出了合适的值。比如在机器学习中将计算所得的值和实际的真实值（标签值）相减来获得的误差函数；在SLAM中是实际位姿和估计值之间的误差。问题是，往往有许多个变量，对所有变量直接求导比较难求，所以采取的方法是从变量的某个初始值开始迭代，每次给初始值增加一个小的增量（想象成下山的时候每次沿着下山最快的方向跨出一步，跨的步子太大也不行）来计算函数值。当或者（通常是个很小的常数）的时候，就是使得取得最小值的的取值。

实际求解中使用的是非线性最小二乘法：对该式关于变量选定一个初始值，开始沿着梯度下降方向每次给增加一个来遍历求解最小值。

其实笔者在刚开始接触最小二乘法的时候有好一段时间不能理解这么一个问题：为什么要给取平方然后前边加呢？后来慢慢意识到其实这里的表达的是计算值和目标值之间的误差，我们要求的是误差最小化（也就是尽量逼近0），那么误差可能存在正值也可能存在负值。为了便于求解，对其进行平方，前边加是为了便于求导时消掉导数中的2。

如何理解最小二乘法？这篇文章对最小二乘法的几何意义解释比较清楚，推荐阅读来增加理解。

2.最速下降法和牛顿法

对于一些比较复杂的函数，直接对其所有变量求导的话比较困难，通常采用的方法是希望用一些简单的函数来近似表达复杂函数。由于用多项式表示的函数，只要对自变量进行有限次加、减、乘三种算术运算，便能求出它的函数值来，因此我们经常用多项式来近似表达函数。

上边这段话是高数课本里边引入泰勒公式的时候的铺垫语。至少对笔者来说，读了这段明白了为什么在优化中要引入泰勒公式。

到这里，泰勒展开式就要闪亮登场了。高数课本中对泰勒中值定理（也就是泰勒展开式）是如下描述的：

泰勒中值定理：如果函数在含有的某个开区间内具有直到阶的导数，则对任一有

                                          

其中                                

这里是与之间的某个值。

有了泰勒展开式，这里先说两个矩阵的定义，Jacobian矩阵和Hessian矩阵。

有时候需要计算输入输出都为向量的函数的所有偏导数，包含所有这些偏导数的矩阵被称为Jacobian矩阵，也称为雅克比矩阵。

矩阵中所有变量的二阶导数构成的矩阵，称为Hessian矩阵，也称为海塞矩阵。

这里用泰勒展开式对目标函数在附近进行泰勒展开可得：

                                                      

这里的是关于的一阶导数，也就是上边说的雅克比矩阵，则是关于的二阶导数，也就是上边说的海塞矩阵。注意上式所用的是符号而不是等号。

至此，可以对上式选择保留泰勒展开式的一阶或二阶项来求解，对应的求解方法则为一阶梯度或二阶梯度法。

如果保留一阶梯度，则目标函数变为：

                                                                  

对该式关于变量进行求导，右侧第一项和无关，所以求导后为0，右侧第二项关于求导后为，则表示关于自变量的一阶偏导数为。

那么增量的方向为偏导数（梯度）取相反的方向，也就是。它的直观意义就是沿着反向梯度方向来取值就可以获得目标函数的最小值。这时候通常还要计算一个该方向上的步长，也就是说沿着这个方向每次移动的量。这种方法称为最速下降法。

---

如果对上边的泰勒展开式保留二阶梯度信息，那么目标函数为：

                                                         

上式对求导，右侧变为，根据极小值的必要条件（一阶偏导数等于0），令其值为0则得到增量方程并进行求解：

                                                       

该方法又称为牛顿法。据此，我们可以看到进行泰勒展开后进行最小化求解的方便性。

---

最速下降法缺点：过于贪心，容易走出锯齿路线，反而增加了迭代次数。

牛顿法缺点：需要计算目标函数的海塞矩阵H，在问题规模较大时非常困难。

3.（Guass-Newton）高斯-牛顿法

高斯-牛顿法的思想是将进行一阶泰勒展开（请注意不是目标函数）：

                                                           

这里是关于的导数，也是一个雅克比矩阵。

现在的目标是寻找下降的矢量，使得达到最小值。为了求解，构造了一个线性最小二乘问题：

                                                               

上式展开得：

                                                                       

对上式关于求导，并令其为0可得：

                                                                      

这个方程可以称为高斯-牛顿方程，该方程求解所得的方法，也就称为高斯-牛顿法。

将左侧的系数定义为，右侧定义为，上式变为：

                                                                                    

对比牛顿法，高斯牛顿法用作为牛顿法中的Hession矩阵的近似，从而省略了计算的过程。

高斯牛顿法算法步骤如下：

1.给定初始值； 2.对第k次迭代，求出当前的雅克比矩阵和误差; 3.求解增量方程：; 4.若足够小，则停止。否则，令，返回第二步继续求解。

高斯牛顿法缺点：

1）可能出现为奇异矩阵或者病态的情况，也就是说不能保证中的可逆，此时增量的稳定性较差，导致算法不收敛。

2）更严重的是，就算非奇异也非病态，如果求出的步长太大，也会导致我们对使用泰勒展开近似取值不够准确，这样一来无法保证迭代收敛性。

4.（Levenburg-Marquadt）列文伯格-马夸尔特方法

列文伯格-马夸而特方法在一定程度上修正了高斯牛顿法存在的问题，一般情况下都比高斯牛顿法健壮。

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以很自然的想到应该给增加一个信赖区域（Trust Region），认为是在信赖区域内是可靠的。

如何确定这个区域呢？考虑使用来判断泰勒近似是否够好。的分子是实际函数下降的值，分母是近似模型下降的值。

1）如果太小，则减小近似范围；（说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围）。

2）如果太大，则增大近似范围。（说明实际下降的比预计的更大，需要放大近似范围）。

Levenburg-Marquadt算法流程：

1.给定初始值，以及初始化优化半径； 2.对于第k次迭代，求解：                                                    （式1） 这里是信赖区域的半径。 的取值在列文伯格提出的优化方法中把=，即取一个球；马夸尔特优化方法取非负数对角阵，相当于取一个椭圆。 3.计算。 4.若，则. 5.若，则. 6.如果大于某阈值，则认为近似可行。令。 7.判断算法是否收敛。如果不收敛，则返回第2步，否则结束。

这里近似范围扩大的倍数和阈值，都是经验值。在（式1）中，我们把增量限定在一个半径为的球中，认为只在这个球内才是有效的。带上之后，这个球可以看做一个椭球。

利用拉格朗日乘数法，将（式1）转换为一个无约束优化问题：

                                                                      

这里的为拉格朗日乘子，把它展开后得：

                                          

类似与高斯牛顿法，给上式关于求导并整理可得方程：

                                                                           

对比高斯牛顿法，可以发现增量方程比高斯牛顿法多了一项，如果考虑，则相当于求解：

                                                                            

1）当较小时，占主要地位，LM算法更接近于高斯牛顿法。

2）当较大时，占主要地位，LM算法更接近于一阶梯度下降法。

5.小结

非线性优化框架分为线性搜索（Line Search）和置信域搜索（Trust Region）两类。

线性搜索先固定搜索方向（例如梯度下降方向），然后在该方向上寻找步长进行求解，以最速下降法和高斯牛顿法为代表；

置信域搜索则是先固定搜索区域，再考虑找该区域内的最优点。以列文伯格-马夸尔特方法为代表。