SLAM入门(4)：非线性函数优化方法
一、前言
激光SLAM建图主要分为两个部分，前端的激光里程计和后端的非线性优化，激光里程计参考当前帧和地图参考帧的帧间匹配算法来完成位姿估计[1]，因此帧间匹配是SLAM建图非常重要的一环，常用的帧间匹配算法主要由两大类，第一类是基于ICP的迭代最近点法，基于传统的ICP方法演化出很多改进的方法，使得帧间匹配的精度大大提高，另一种是基于势场的方法，通过构造目标函数进行迭代优化。很多方法都需要对非线性函数进行优化，在SLAM中，往往需要求一个已知观测数据和控制输入条件下机器人位姿的最大后验估计，利用贝叶斯法则将问题转换为求一个与观测模型最为接近的预测模型对应的机器人位姿的最大似然估计。在求解这个最大似然估计的过程中，往往需要用到非线性优化(最小二乘)的内容，在这里进行一下总结。
二、非线性最小二乘
最简单的最小二乘问题就是求解下面的方程： m i n x F ( x ) = 1 2 ∥ f ( x ) ∥ 2 2 min_xF(x) = \frac{1}{2}\parallel f(x) \parallel_2^2 minxF(x)=21∥f(x)∥22如果 f ( x ) f(x) f(x)是简单的线性函数，那么该方程可以通过求解 d F d x = 0 \frac{dF}{dx} = 0 dxdF=0找到所有的极值点，然后进行比较，得到最小值对应的 x x x就是我们的所求。但是往往 F ( x ) F(x) F(x)比较复杂，很难进行求导，因此需要采用迭代的方式进行求解。
具体步骤[6] ：
- 给定某个初值 x 0 x_0 x0
- 对于第 k k k次迭代，寻找增量 Δ x k \Delta x_k Δxk，使得 ∥ f ( x k + Δ x k ) ∥ 2 2 \parallel f(x_k+\Delta x_k) \parallel_2^2 ∥f(xk+Δxk)∥22达到极小值
- 若 Δ x k \Delta x_k Δxk足够小，迭代停止
- 否则，令 x k + 1 = x k + Δ x k x_{k+1} = x_k+\Delta x_k xk+1=xk+Δxk，返回第二步继续迭代
因变量与自变量之间的关系不是线性的关系，比如平方关系、对数关系、指数关系、三角函数关系的函数我们成为非线性函数。对于此类方程，求解n元实函数f在整个n维向量空间Rn上的最优值点往往很难得到精确解或者说解析解，经常需要求近似数值解。主要的求解方法有：
（1）梯度下降法，也称最速下降法，一阶收敛，速度慢
（2）牛顿法，二阶收敛，速度比梯度下降法快，但是不稳定，计算Hessian矩阵比较复杂，后来有高斯牛顿法，但目标作用不同
（3）共轭梯度法：收敛较快，效果好
（4）变尺度法：效率较高，常用DFP法(Davidon Fletcher Powell)[3]
1.梯度下降法
非线性最小二乘的思想非常简单，关键是求得合适的增量 Δ x \Delta x Δx，使函数以最快的速度逼近真实的极值点。我们知道，在梯度方向上是函数增加最快的方向，很自然想到对函数在 x k x_k xk处进行泰勒展开： F ( x k + Δ x k ) ≈ F ( x k ) + J ( x k ) T Δ x k + 1 2 Δ x k T H ( x k ) Δ x k F(x_k+\Delta x_k)\approx F(x_k)+J(x_k)^T\Delta x_k+\frac{1}{2}{\Delta x_k}^TH(x_k)\Delta x_k F(xk+Δxk)≈F(xk)+J(xk)TΔxk+21ΔxkTH(xk)Δxk梯度下降法只保留到一阶项，牛顿法保留到二阶项。
梯度下降法： Δ x ∗ = − J ( x k ) \Delta x^* = -J(x_k) Δx∗=−J(xk)只保留一阶梯度，增量反向乘上步长即可，步长的选取也比较重要，这里不再说明。
2.牛顿法
一元函数牛顿法：
假设 f ( x , β ) : R ⇒ R f(x,\beta): \mathbb{R}\Rightarrow\mathbb{R} f(x,β):R⇒R是我们需要优化的函数，也就是当 β \beta β取何值时， f f f取得极值，高斯—牛顿迭代法的基本思想是使用泰勒级数展开式去近似地代替非线性回归模型，然后通过多次迭代，多次修正回归系数，使回归系数不断逼近非线性回归模型的最佳回归系数，最后使原模型的残差平方和达到最小[5]。
将一元函数在 x x x附近进行二阶泰勒展开：
f ( x ) ≈ f ( x ) + f ′ ( x ) Δ x + 1 2 f ′ ′ ( x ) Δ x 2 f(x)\approx f(x)+f'(x)\Delta x+\frac{1}{2}f''(x)\Delta x^2 f(x)≈f(x)+f′(x)Δx+21f′′(x)Δx2如下图所示：
令 f ′ ( x ) = 0 f'(x)=0 f′(x)=0得： Δ x = − f ′ ( x ) f ′ ′ ( x ) \Delta x=-\frac{f'(x)}{f''(x)} Δx=−f′′(x)f′(x)因此，迭代公式为： x n + 1 = x n − f ′ ( x ) f ′ ′ ( x ) x_{n+1}=x_n-\frac{f'(x)}{f''(x)} xn+1=xn−f′′(x)f′(x)多元函数牛顿法：
a.梯度Gradient
梯度是多元函数的各个偏导数组成的向量 ∇ f ( x 1 , x 2 , ⋅ ⋅ ⋅ , x 3 ) = [ ∂ f ∂ x 1 , ∂ f ∂ x 2 , ⋅ ⋅ ⋅ , ∂ f ∂ x n ] \nabla f(x_1,x_2,···,x_3)=\left[ \frac{\partial f}{\partial x_1}, \frac{\partial f}{\partial x_2},···,\frac{\partial f}{\partial x_n}\right ] ∇f(x1,x2,⋅⋅⋅,x3)=[∂x1∂f,∂x2∂f,⋅⋅⋅,∂xn∂f]
b.雅可比矩阵Jacobian Matrix
雅可比矩阵是一阶偏导数以一定方式排列成的矩阵 J = [ ∂ f ∂ x 1 ∂ f ∂ x 2 ⋅ ⋅ ⋅ ∂ f ∂ x n ] J=\begin{bmatrix} \frac{\partial f}{\partial x_1} \\ \frac{\partial f}{\partial x_2} \\ ···\\ \frac{\partial f}{\partial x_n} \\ \end{bmatrix} J=⎣⎢⎢⎢⎡∂x1∂f∂x2∂f⋅⋅⋅∂xn∂f⎦⎥⎥⎥⎤c.海森矩阵Hessian Matrix
海森矩阵是一个自变量为向量的实值函数的二阶偏导数组成的方块矩阵, 此函数如下： H [ f ( x 1 , x 2 ) ] = [ ∂ f ∂ x 1 2 ∂ f ∂ x 1 ∂ x 2 ∂ f ∂ x 1 ∂ x 2 ∂ f ∂ x 2 2 ] H[f(x_1,x_2)]= \begin{bmatrix} \frac{\partial f}{\partial x_1^2} & \frac{\partial f}{\partial x_1\partial x_2} \\ \frac{\partial f}{\partial x_1\partial x_2} &\frac{\partial f}{\partial x_2^2}\end{bmatrix} H[f(x1,x2)]=[∂x12∂f∂x1∂x2∂f∂x1∂x2∂f∂x22∂f]对于多元函数 f ( x 1 , x 2 , ⋅ ⋅ ⋅ , x n , β ) : R n ⇒ R f(x_1,x_2,···,x_n,\beta): \mathbb{R}^n\Rightarrow\mathbb{R} f(x1,x2,⋅⋅⋅,xn,β):Rn⇒R，同样参照上面的一元函数的例子在 x k − 1 x_{k-1} xk−1处进行二阶泰勒展开(此处 x x x为向量)： f ( x ) ≈ f ( x k − 1 ) + ∇ f ( x k − 1 ) T ( x − x k − 1 ) + 1 2 ( x − x k − 1 ) T H ( x k − 1 ) ( x − x k − 1 ) f(x)\approx f(x_{k-1})+\nabla f(x_{k-1})^T(x-x_{k-1})+\frac{1}{2}(x-x_{k-1})^TH(x_{k-1})(x-x_{k-1}) f(x)≈f(xk−1)+∇f(xk−1)T(x−xk−1)+21(x−xk−1)TH(xk−1)(x−xk−1)如下图所示：
由 ∇ f ( x ) = 0 \nabla f(x) = 0 ∇f(x)=0得： ∇ f ( x k − 1 ) + H ( x k − 1 ) ( x − x k − 1 ) = 0 \nabla f(x_{k-1})+H(x_{k-1})(x-x_{k-1})=0 ∇f(xk−1)+H(xk−1)(x−xk−1)=0迭代公式为： x k = x k − 1 − H ( x k − 1 ) − 1 ∇ f ( x k − 1 ) x_k=x_{k-1}-H(x_{k-1})^{-1}\nabla f(x_{k-1}) xk=xk−1−H(xk−1)−1∇f(xk−1)
牛顿法计算量大，不能保证收敛
2.高斯牛顿法
高斯牛顿法是牛顿法的一个特例，对于： m i n x F ( x ) = 1 2 ∥ f ( x ) ∥ 2 2 min_xF(x) = \frac{1}{2}\parallel f(x) \parallel_2^2 minxF(x)=21∥f(x)∥22我们不是直接求 F ( x ) F(x) F(x)的导数，而是直接求 f ( x ) f(x) f(x)的导数，即对 f ( x ) f(x) f(x)进行一阶泰勒展开： f ( x + Δ x ) ≈ f ( x ) + J ( x ) T Δ x f(x+\Delta x) \approx f(x)+{J(x)}^T \Delta x f(x+Δx)≈f(x)+J(x)TΔx将该式带入到上式中： m i n x F ( x ) = 1 2 ∥ f ( x ) + J ( x ) T Δ x ∥ 2 2 min_xF(x) = \frac{1}{2}\parallel f(x)+{J(x)}^T \Delta x \parallel_2^2 minxF(x)=21∥f(x)+J(x)TΔx∥22根据我们的目标，寻找增量 Δ x \Delta x Δx使得 ∥ f ( x k + Δ x k ) ∥ 2 2 \parallel f(x_k+\Delta x_k) \parallel_2^2 ∥f(xk+Δxk)∥22最小，对上式进行展开并求导，令导数等于0： J ( x ) f ( x ) + J ( x ) J T ( x ) Δ x = 0 J(x)f(x)+J(x){J^T(x)}\Delta x = 0 J(x)f(x)+J(x)JT(x)Δx=0令 H ( x ) = J ( x ) J T ( x ) , g ( x ) = − J ( x ) f ( x ) H(x) =J(x)J^T(x),g(x)=-J(x)f(x) H(x)=J(x)JT(x),g(x)=−J(x)f(x): H ( x ) Δ x = g ( x ) H(x)\Delta x=g(x) H(x)Δx=g(x)我们可以看到，与普通牛顿法不同的是高斯牛顿法用 J J T JJ^T JJT来近似Hessian矩阵，减少计算量。
这样的一个问题还可以描述为：
- 已知 n n n个数据点 ( x 1 , y 1 ) , ( x 2 , y 2 ) , ⋅ ⋅ ⋅ , ( x n , y n ) ) (x_1,y_1),(x_2,y_2),···,(x_n,y_n)) (x1,y1),(x2,y2),⋅⋅⋅,(xn,yn))
- 函数原型 y = f ( x , β ) y=f(x,\beta) y=f(x,β)未知，其中 β = [ β 1 , β 1 , ⋅ ⋅ ⋅ , β m ] \beta=[\beta_1,\beta_1,···,\beta_m] β=[β1,β1,⋅⋅⋅,βm]为我们要优化的参数，这里要注意，以后我们把 β \beta β看做自变量了。
- 找到最优解 β \beta β，使得残差平方和 S = ∑ i = 1 m r i 2 S=\sum_{i=1}^{m}r_i^{2} S=∑i=1mri2最小，其中 r i = y i − f ( x i , β ) r_i=y_i-f(x_i,\beta) ri=yi−f(xi,β)
同样的，对S函数求偏导数，让其梯度为0求解出 β \beta β是我们的中心思想： ∇ S ( β ) = [ ∂ S ∂ β 1 , ∂ S ∂ β 2 , ⋅ ⋅ ⋅ , ∂ S ∂ β n ] T \nabla S(\beta)=\begin{bmatrix}\frac{\partial S}{\partial \beta_1},\frac{\partial S}{\partial \beta_2},···,\frac{\partial S}{\partial \beta_n}\end{bmatrix}^T ∇S(β)=[∂β1∂S,∂β2∂S,⋅⋅⋅,∂βn∂S]T ∂ S ∂ β j = 2 ∑ i = 1 m r i ∂ r i ∂ β j \frac{\partial S}{\partial \beta_j}=2\sum_{i=1}^mr_i\frac{\partial r_i}{\partial \beta_j} ∂βj∂S=2i=1∑mri∂βj∂ri其中， r = [ r 1 , r 2 , ⋅ ⋅ ⋅ , r m ] r=[r_1,r_2,···,r_m] r=[r1,r2,⋅⋅⋅,rm] J ( r ( β ) ) = [ ∂ r 1 ∂ β 1 ⋯ ∂ r 1 ∂ β n ⋮ ⋱ ⋮ ∂ r n ∂ β 1 ⋯ ∂ r n ∂ β n ] J(r(\beta))=\begin{bmatrix}\frac{\partial r_1}{\partial \beta_1} & \cdots &\frac{\partial r_1}{\partial \beta_n}\\ \vdots & \ddots & \vdots \\\frac{\partial r_n}{\partial \beta_1} & \cdots & \frac{\partial r_n}{\partial \beta_n} \end{bmatrix} J(r(β))=⎣⎢⎡∂β1∂r1⋮∂β1∂rn⋯⋱⋯∂βn∂r1⋮∂βn∂rn⎦⎥⎤可以得到： ∇ S = 2 J T r \nabla S=2J^Tr ∇S=2JTr接下来求海森矩阵，即对梯度求二阶导数，以第k行第j列为例： ∂ 2 S ∂ β k ∂ β j = 2 ∂ ∂ β k ( ∑ i = 1 m r i ∂ r i ∂ β j ) = 2 ∑ i − 1 m ( ∂ r i ∂ β k ∂ r i ∂ β j + r i ∂ 2 r i ∂ β k ∂ β j ) \frac{\partial^2S}{\partial \beta_k\partial \beta_j}=2\frac{\partial}{\partial \beta_k}(\sum_{i=1}^mr_i\frac{\partial r_i}{\partial \beta_j})=2\sum_{i-1}^m(\frac{\partial r_i}{\partial \beta_k}\frac{\partial r_i}{\partial \beta_j}+r_i\frac{\partial^2r_i}{\partial \beta_k\partial \beta_j}) ∂βk∂βj∂2S=2∂βk∂(i=1∑mri∂βj∂ri)=2i−1∑m(∂βk∂ri∂βj∂ri+ri∂βk∂βj∂2ri) H = 2 ( J T J + O ) H=2(J^TJ+O) H=2(JTJ+O)得到了梯度和海森矩阵之后，根据牛顿法： x k = x k − 1 − H ( x k − 1 ) − 1 ∇ f ( x k − 1 ) x_k=x_{k-1}-H(x_{k-1})^{-1}\nabla f(x_{k-1}) xk=xk−1−H(xk−1)−1∇f(xk−1) β k + 1 = β k − H − 1 ∇ S = β k − [ 2 J T J + O ] − 1 2 J T r \beta_{k+1}=\beta_{k}-H^{-1}\nabla S=\beta_k-[2J^TJ+O]^{-1}2J^Tr βk+1=βk−H−1∇S=βk−[2JTJ+O]−12JTr β k + 1 = β k − ( J T J ) − 1 J T r \beta_{k+1}=\beta_k-(J^TJ)^{-1}J^Tr βk+1=βk−(JTJ)−1JTr
基于高斯-牛顿迭代法的激光里程计
参考文献：
1.https://blog.csdn.net/orange_littlegirl/article/details/89262501
2.https://blog.csdn.net/piaoxuezhong/article/details/60135153
3.https://blog.csdn.net/qq_39521554/article/details/79919041
4.https://zhuanlan.zhihu.com/p/103724149
5.https://blog.csdn.net/tclxspy/article/details/51281811
6.《视觉SLAM十四讲-从理论到实践》