前言
在LOAM学习-代码解析（四）特征点运动估计 laserOdometry的3.3中，使用LM方法进行点云配准。
本部分对LM方法进行详细解析，需要一定的数学推导能力。
LM原论文网址：http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf
一、基本原理
对于一个非线性最优化问题，若目标函数可以写成以下形式，那么求其极小化的问题就可成为非线性最小二乘问题。
F ( x ) = ∑ i = 1 m f i 2 ( x ) F(x) = \sum_{i=1}^{m} f_{i}^{2}(x) F(x)=i=1∑mfi2(x)
其中， x = ( x 1 , x 2 , . . . , x n ) T x=(x_1,x_2,...,x_n)^T x=(x1,x2,...,xn)T， f i ( x ) f_{i}^{}(x) fi(x)为非线性函数。
把 f i ( x ) f_{i}^{}(x) fi(x)在 x k x_k xk处泰勒展开，用 ϕ i ( x ) \phi _i(x) ϕi(x)近似，如下
ϕ i ( x ) = f i ( x k ) + ▽ f i ( x k ) T ( X − X k ) \phi _i(x) = f_{i}^{}(x_k)+\triangledown f_{i}^{}(x_k)^T (X-X_k) ϕi(x)=fi(xk)+▽fi(xk)T(X−Xk)
= ▽ f i ( x k ) T x − [ ▽ f i ( x k ) T x k − f i ( x k ) ] =\triangledown f_{i}^{}(x_k)^Tx - \left [\triangledown f_{i}^{}(x_k)^Tx_k- f_{i}^{}(x_k) \right ] =▽fi(xk)Tx−[▽fi(xk)Txk−fi(xk)]
把上述公式表达为矩阵形式，令
J
=
[
▽
f
1
(
x
k
)
T
⋮
▽
f
m
(
x
k
)
T
]
J=\begin{bmatrix} \triangledown f_{1}^{}(x_k)^T\\ \vdots \\ \triangledown f_{m}^{}(x_k)^T \end{bmatrix}
J=⎣⎢⎡▽f1(xk)T⋮▽fm(xk)T⎦⎥⎤
b = [ ▽ f 1 ( x k ) T x k − f 1 ( x k ) ⋮ ▽ f m ( x k ) T x k − f m ( x k ) ] = J k x k − f k b=\begin{bmatrix} \triangledown f_{1}^{}(x_k)^T x_k - f_{1}^{}(x_k)\\ \vdots \\ \triangledown f_{m}^{}(x_k)^T x_k - f_{m}^{}(x_k) \end{bmatrix} =J_k x_k - f_k b=⎣⎢⎡▽f1(xk)Txk−f1(xk)⋮▽fm(xk)Txk−fm(xk)⎦⎥⎤=Jkxk−fk
f k = [ f 1 ( x k ) ⋮ f 1 ( x k ) ] f_k=\begin{bmatrix} f_1(x_k)\\ \vdots \\ f_1(x_k)\end{bmatrix} fk=⎣⎢⎡f1(xk)⋮f1(xk)⎦⎥⎤
可以得到以下公式
∑ i = 1 m ϕ i 2 ( x ) = ( J k x − b ) T ( J k x − b ) \sum_{i=1}^{m} \phi _{i}^{2}(x)= (J_kx-b) ^T(J_kx-b) i=1∑mϕi2(x)=(Jkx−b)T(Jkx−b)
对上式进行求导，并令导数为0可得
J k T J k x = J k T b = J k T ( J k x k − f k ) J_{k}^{T} J_{k}^{} x= J_{k}^{T}b=J_{k}^{T} \left (J_k x_k - f_k \right ) JkTJkx=JkTb=JkT(Jkxk−fk)
假设
J
k
T
J
k
J_{k}^{T} J_{k}^{}
JkTJk可逆，
x
x
x的求解公式为
x
−
x
k
=
−
(
J
k
T
J
k
)
−
1
J
k
T
f
k
x-x_k = - \left ( J_{k}^{T} J_{k}^{} \right )^{-1} J_{k}^{T} f_k
x−xk=−(JkTJk)−1JkTfk
− ( J k T J k ) − 1 J k T f k - \left ( J_{k}^{T} J_{k}^{} \right )^{-1} J_{k}^{T} f_k −(JkTJk)−1JkTfk被称为迭代步长，每次迭代求极小值，本质就是求 f i ( x k ) f_{i}^{}(x_k) fi(xk)在 x k x_k xk处函数值和其对 x k x_k xk的一阶偏导数值的过程，即高斯-牛顿法的思路。
在使用高斯-牛顿法的时候，可能会出现 J k T J k J_{k}^{T} J_{k}^{} JkTJk为奇异矩阵或者病态的情况，这时候算法就有可能不收敛。即使 J k T J k J_{k}^{T} J_{k}^{} JkTJk非奇异也非病态，算出来的迭代步长也可能太大，导致公式泰勒展开的近似不够准确，从而导致无法受凉。因此，添加一个信赖域使迭代步长不会太大，也就是在每次迭代的过程都寻找一个合适的阻尼因子 λ \lambda λ，用 J k T J k + λ I J_{k}^{T} J_{k}^{}+\lambda I JkTJk+λI代替 J k T J k J_{k}^{T} J_{k}^{} JkTJk， I I I为单位矩阵，求解的方程就会变为如下形式
x − x k = − ( J k T J k + λ I ) − 1 J k T f k x-x_k = - \left ( J_{k}^{T} J_{k}^{}+\lambda I \right )^{-1} J_{k}^{T} f_k x−xk=−(JkTJk+λI)−1JkTfk
这就是LM方法的主要思想。
二、计算步骤
根据基本原理，可以得到LM方法的计算步骤如下
- 选取初始点 x 0 x_0 x0和迭代终止条件 ε \varepsilon ε，设置初始化参数 λ \lambda λ，计算 F ( x 0 ) = ∑ i = 1 m f i 2 ( x 0 ) F(x_0) = \sum_{i=1}^{m} f_{i}^{2}(x_0) F(x0)=∑i=1mfi2(x0)， k = 0 k=0 k=0；
- 计算 f i ( x k ) f_{i}^{}(x_k) fi(xk)在 x k x_k xk处的函数值 f k f_k fk， f i ( x k ) f_{i}^{}(x_k) fi(xk)在 x k x_k xk处的Jacobi矩阵 J k J_k Jk和迭代步长 N ˉ k = J k T J k + λ \bar{N}_k=J_{k}^{T} J_{k}^{}+\lambda Nˉk=JkTJk+λ，构造增量正规方程 N ˉ k δ k = J k T f k \bar{N}_k^{} \delta _k= J_{k}^{T} f_{k}^{} Nˉkδk=JkTfk并求解得到 δ k = N ˉ k − 1 J k T f k \delta _k =\bar{N}_k^{-1} J_{k}^{T} f_{k}^{} δk=Nˉk−1JkTfk；
- 如果 F ( x 0 + δ k ) < F ( x 0 ) F(x_0+\delta _k)<F(x_0) F(x0+δk)<F(x0)，则令 x k + 1 = x k + δ k x_{k+1}=x_k+\delta _k xk+1=xk+δk，此时若 ∥ δ k ∥ < ε \left \|\delta _k \right \|<\varepsilon ∥δk∥<ε，就停止迭代，输出结果；否则，令 x k = x k + 1 = x k + δ k x_k=x_{k+1}=x_k+\delta _k xk=xk+1=xk+δk，缩小 λ \lambda λ，跳回第2步继续迭代；如果 F ( x 0 + δ k ) > F ( x 0 ) F(x_0+\delta _k)>F(x_0) F(x0+δk)>F(x0)，则增大 λ \lambda λ，重新求解增量正规方程。
求解增量正规方程的方法由很多，常见的有Cholesky、QR等分解方法。
结语
LM方法的求解方式在一定程度上避免了 J k T J k J_{k}^{T} J_{k}^{} JkTJk奇异和病态的问题，阻尼因子 λ \lambda λ是一个正实数，体现了LM方法的特别之处。当 λ = 0 \lambda=0 λ=0时，LM方法就变成了高斯-牛顿法；当 λ \lambda λ很大时，LM方法就变成了最速下降法。LM方法不仅具有高斯-牛顿法的局部快速收敛性，还克服了不能有效处理奇异和非正定矩阵的缺点，对初始点的选取也没有那么严苛，同时在远离解的时候，继承了梯度下降法的全局搜索特性，使精度得到提高。
扫一扫
3678
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
