本文深入探讨了非线性最小二乘问题在机器人技术中的应用，介绍了牛顿法、高斯牛顿法、梯度下降法、LM算法和狗腿算法等经典方法。重点讲解了这些方法如何处理误差函数，寻找下降方向和步长，以及如何判断局部最小值。LM算法和狗腿法作为混合方法，结合了牛顿法和梯度下降法的优点，适用于不同迭代阶段的需求。文章最后提到了算法的迭代停止条件和代码实现的计划。
《 METHODS FOR NON-LINEAR LEASTSQUARES PROBLEMS》论文学习
在传感器误差纠正、SLAM感知、轨迹生成与优化等机器人技术中，最优化理论是极其基本又重要的理论。根据朴素贝叶斯定理，机器人的状态估计问题一般可以转换为求最大似然概率的问题，而最大似然概率问题又可以转换到最小二乘问题上。本篇论文探讨了解决非线性最小二乘问题的一些经典优化方法，例如牛顿法、高斯牛顿法、梯度下降法、LM算法、狗腿算法等。这是一篇比较综合且理解起来不难的论文。我就直接通过这篇论文来入门最优化理论吧。当然后续要系统的看一些经典的书籍
一、基本概念
首先介绍什么叫最小二乘问题
这个问题在高中数学都有学过，最经典的例子就是曲线拟合。我们需要找出一条合适的曲线来拟合给定的坐标，衡量的标准是每个点到曲线的距离平方加起来最小。我们用数学语言描述它：
假 设 拟 合 曲 线 函 数 为 M ( b , x ) = b 1 e b 2 x + b 3 e b 4 x , 其 中 b = [ b 1 , b 2 , b 3 , b 4 ] 为 一 组 参 数 假设拟合曲线函数为M(b,x) = b_1e^{b_2x} + b_3e^{b_4x} ,其中 b=[b_1,b_2,b_3,b_4]为一组参数 假设拟合曲线函数为M(b,x)=b1eb2x+b3eb4x,其中b=[b1,b2,b3,b4]为一组参数
误 差 函 数 为 f i ( b ) = y i − M ( b , x ) i , 距 离 平 方 和 表 示 为 Σ f i ( b ) 2 误差函数为f_i(b) = y_i - M(b,x)_i, 距离平方和表示为 \Sigma f_i(b)^2 误差函数为fi(b)=yi−M(b,x)i,距离平方和表示为Σfi(b)2
我们把衡量标准用数学语言描述出来，只要距离平方和越小，拟合效果就越好。通过函数表达式，我们发现曲线拟合问题实质上就是找到参数b的取值使得衡量函数的值最小。通过曲线拟合的例子我们引出最小二乘问题的定义。
F ( b ) = 1 2 ∑ i = 1 m ( f i ( b ) ) 2 F(b) = \frac{1}{2} \sum_{i=1}^m(f_i(b))^2 F(b)=21i=1∑m(fi(b))2
b ∗ = a r g m i n F ( b ) b^* = argminF(b) b∗=argminF(b)
其 中 F ( b ) 称 为 目 标 函 数 ， f ( b ) 称 为 误 差 函 数 ， b ∗ 称 为 目 标 值 其中F(b)称为目标函数，f(b)称为误差函数，b^*称为目标值 其中F(b)称为目标函数，f(b)称为误差函数，b∗称为目标值
通过上式我们知道我们的任务就是要找到目标b值，它对应的目标函数最小。我们很自然的想到求一阶导数等于0，求解b。考虑到求导后的式子的美观，所以才在F(b)前加了一个系数项1/2。
非线性和线性最小二乘问题的区别？
这里的线性与非线性指的是误差函数f(b)与b的关系，在上面的例子中显然是非线性的。所以是非线性最小二乘问题。
如果我们用多项式去拟合
f ( b ) = y − ( b 1 x + b 2 x 2 + b 3 x 3 + . . . . ) f(b) = y - (b_1x+b_2x^2+b_3x^3+ ....) f(b)=y−(b1x+b2x2+b3x3+....)
显然这里的f(b)与b就是一个线性关系。这就变成了线性最小二乘问题。
本篇文章主要探讨非线性最小二乘问题
关于最小值与局部最小值
解决最小二乘问题的终极目标就是要找到一个自变量b，使得此处的F(b)最小，我们把这个b叫做全局最小点。然而这并不是能偶很容易办到的事情。
首先，对于复杂的F(b)进行求导不一定能成功。即使可以求导，得到的也只是极值，没有很好的办法证明他同时也是最值。如果直接让F(b)=0，也不一定有普适的求解公式。基于以上问题，人们提出了局部最小值和迭代求解的概念。
局部最小值的思路是要求目标函数值在某一个区域内为最小即可。用数学语言描述如下(从这里以后把b替代成习惯性的x)
在 ∣ ∣ x − x ∗ ∣ ∣ < σ 区 间 内 满 足 F ( x ∗ ) < F ( x ) 其 中 σ 是 一 个 小 的 正 数 在||x-x^*||<\sigma~~~区间内满足F(x^*)<F(x)~~~ 其中\sigma是一个小的正数 在∣∣x−x∗∣∣<σ 区间内满足F(x∗)<F(x) 其中σ是一个小的正数
有了定义后，我们该怎么判断是否是局部最小值呢？我们清楚在局部最小值在小领域内必定有导数等于0.然而导数为零的也可能是局部最大值或者是鞍点（既不是最大值也不是最小值但导数为零），导数等于零处称为驻点。所以我们还需要寻找一个充分条件来判断局部最小值。
假设目标函数F是一个光滑可微的函数，那么他就可以进行泰勒展开
F ( x + h ) = F ( x ) + h T J + 1 2 h T H h 其 中 J 为 雅 可 比 矩 阵 H 为 海 塞 矩 阵 F(x+h) = F(x) + h^TJ + \frac{1}{2}h^THh ~~~其中J为雅可比矩阵~~H为海塞矩阵 F(x+h)=F(x)+hTJ+21hTHh 其中J为雅可比矩阵 H为海塞矩阵
已知在目标点处J=0，那么上式可以化简为：
F ( x ∗ + h ) = F ( x ∗ ) + 1 2 h T H h F(x^*+h) = F(x^*)+ \frac{1}{2}h^THh F(x∗+h)=F(x∗)+21hTHh
通过局部最小值的定义我们知道
F ( x ∗ ) < F ( x ∗ + h ) F(x^*) <F(x^*+h) F(x∗)<F(x∗+h)
所以我们可以得出结论
1 2 h T H h > 0 \frac{1}{2}h^THh>0 21hTHh>0
也就是说海塞矩阵必须为正定二次型矩阵，才能判断驻点为局部最小值处。我们可以总结成以下规律：
{ 1 2 3 \left\{ \begin{matrix} 1 \\ 2 \\ 3 \\ \end{matrix} \right. ⎩⎨⎧123
驻 点 { 局 部 最 小 值 ， H 正 定 鞍 点 ， H 既 不 正 定 也 不 负 定 局 部 最 大 值 ， H 负 定 驻点 \left\{ \begin{matrix} 局部最小值 ，H正定 \\ 鞍点，H既不正定也不负定\\ 局部最大值，H负定 \end{matrix} \right. 驻点⎩⎨⎧局部最小值，H正定鞍点，H既不正定也不负定局部最大值，H负定
二、下降方法
下降方法就是在迭代求解的每个过程中保证函数值都是在不断减小的。这主要分为两个工作
- 寻找一个下降的方向
- 寻找一个步长沿着下降方向可以使函数值减小
基于以上原则，我们可以把下降方法概括为以下过程：
假设我们在x0这点做下降操作，我们对这一点处附近进行泰勒展开：
F ( x 0 + α h ) = F ( x 0 ) + α h T J ， 其 中 h 为 下 降 方 向 ， α 为 步 长 F(x_0+\alpha h) = F(x_0)+\alpha h^TJ ，其中h为下降方向，\alpha为步长 F(x0+αh)=F(x0)+αhTJ，其中h为下降方向，α为步长
根据下降的要求我们很容易得到寻找下降方向的一个标准
只 要 满 足 h T J < 0 , h 即 为 下 降 方 向 只要满足h^TJ<0,h即为下降方向 只要满足hTJ<0,h即为下降方向
接下来我们就该确定步长的取值，关心在当前这个下降方向上我们选择哪一个步长值，才能让函数值最小。用数学语言描述为
α ∗ = a r g m i n ( F ( x + α h ) ) \alpha^* = argmin(F(x+\alpha h)) α∗=argmin(F(x+αh))
求解上面问题的过程被称为 线搜索 。一般在高斯牛顿法和牛顿法中都默认步长为1.关于线搜索我会再写一篇文章讨论它，我们暂时把注意力放在 寻找下降方向 上，而寻找下降方法有两个非常经典的算法：牛顿法 和 梯度下降法 。
梯度下降法
我们可以用一个公式来衡量函数下降的程度
lim α → 0 F ( x ) − F ( x + α h ) α ∣ ∣ h ∣ ∣ = − 1 ∣ ∣ h ∣ ∣ h T F ′ ( x ) = − ∣ ∣ F ′ ( x ) ∣ ∣ c o s θ \lim\limits_{\alpha\rightarrow 0} \frac{F(x)-F(x+\alpha h)}{\alpha ||h||} = - \frac{1}{||h||} h^T F\prime(x) = -||F \prime (x)|| cos \theta α→0limα∣∣h∣∣F(x)−F(x+αh)=−∣∣h∣∣1hTF′(x)=−∣∣F′(x)∣∣cosθ
其 中 θ 是 下 降 方 向 与 梯 度 向 量 的 夹 角 其中\theta 是下降方向与梯度向量的夹角 其中θ是下降方向与梯度向量的夹角
从上式中，我们可以得出当夹角为0，也就是下降方向与梯度一致时，下降程度最大。所以我们可以得到下降方向：
h s d = − F ′ ( x ) h_{sd} = -F \prime(x) hsd=−F′(x)
基于上面这个方式确定下降方向的方法就被称为梯度下降法。下降方向的选择是“最佳”（局部）的，我们可以将其与精确的线搜索相结合。这样的方法是收敛的，但最终的收敛是线性的，而且通常非常缓慢。然而，对于许多问题，该方法在迭代过程的初始阶段具有相当好的性能。
牛顿法
寻找局部最小值点意味着我们需要先找到一阶导等于0的地方。我们把目标点附近进行泰勒展开
F ′ ( x + h ) = J + H h F \prime (x+h) = J + Hh F′(x+h)=J+Hh
令 上 式 等 于 0 ， 可 以 得 到 h n = − J H 令上式等于0，可以得到 h_n= -\frac{J}{H} 令上式等于0，可以得到hn=−HJ
然后进行迭代
x = x l a s t + h n x = x_{last} + h_n x=xlast+hn
假设在迭代过程中海塞矩阵都是正定的，那一定满足下式(正定矩阵的性质)
h
n
T
H
h
n
>
0
h_n^T H h_n > 0
hnTHhn>0
又因为
H
h
n
=
−
J
Hh_n = -J
Hhn=−J
代入就可以的得到
h
n
T
J
<
0
h_n^TJ <0
hnTJ<0
这满足了下降的条件，因此我们可以证明hn是一个下降方向。从以上的证明我们知道牛顿法在临近目标点处且附近的海森矩阵为正定时，收敛效果较好。然而当海森矩阵为负定时，牛顿法并不会让迭代朝着函数值下降的方向进行。
混合方法
基于牛顿法在海森矩阵不正定处无法再进行下降操作，人们自然而然的想到在这种情况时可以切换到继续下降的方法。所以提出了一种将牛顿法和梯度下降法结合起来的混合方法。如下图
当海森矩阵正定时，我们继续采用牛顿法进行下降操作。当海森矩阵负定时，我们便采用梯度下降法。这样的混合方法是非常有效的，但是面临一个问题： 复杂函数的海森矩阵的求解是计算量非常大的工作,所以我们在后面要继续讨论下面两个问题。
- 混合算法需要满足目标点附近的迭代又能保持持续下降
- 避免陷入对海森矩阵的大量计算
三、解决非线性最小二乘问题
回到我们在第一章的讨论，解决非线性最小二乘问题可以用数学语言描述为
F ( x ) = 1 2 ∑ i = 1 m ( f i ( x ) ) 2 = 1 2 f ( x ) T f ( x ) , f ( x ) = [ f 1 ( x ) f 2 ( x ) . . . f i ( x ) ] F(x) = \frac{1}{2} \sum_{i=1}^m(f_i(x))^2 = \frac{1}{2} f(x)^Tf(x),~~f(x) = [f_1(x) ~~ f_2(x) ~~...~~f_i(x)] F(x)=21i=1∑m(fi(x))2=21f(x)Tf(x), f(x)=[f1(x) f2(x) ... fi(x)]
x ∗ = a r g m i n ( F ( x ) ) x^* = argmin(F(x)) x∗=argmin(F(x))
牛顿法
我们先用牛顿法来解决这个问题，以更好的说明高斯牛顿法与牛顿法的不同。在目标点附近的一阶导进行泰勒展开
F ′ ( x + h ) = F ′ ( x ) + F ′ ′ ( x ) h F \prime (x+h) = F \prime(x) +F\prime \prime(x)h F′(x+h)=F′(x)+F′′(x)h
令上式等于0，可以得到牛顿法的下降方向为
h n = − F ′ ( x ) F ′ ′ ( x ) h_n = - \frac{F \prime(x)}{F\prime \prime(x)} hn=−F′′(x)F′(x)
F
′
(
x
)
=
J
f
T
f
F \prime(x) = J_f^T f
F′(x)=JfTf
F
′
′
(
x
)
=
H
f
T
f
+
J
f
T
J
f
F\prime \prime(x) = H_f^Tf +J_f^TJ_f
F′′(x)=HfTf+JfTJf
由以上三个式子便可以解出牛顿法的下降方向，但我们之前也提到过求f的海塞矩阵通常是不容易的。
高斯牛顿法
高斯牛顿法是一种基于对误差函数f进行线性化处理的优化方法。所以我们将误差函数进行一阶泰勒展开
f ( x + h ) = f ( x ) + J f h f(x+h) = f(x) + J_fh f(x+h)=f(x)+Jfh
注意这里的雅可比矩阵是对误差函数f的一阶导，和对目标函数F的一阶导是不同的
我们把上式代入到目标函数的泰勒展开式可以得到
F ( x + h ) = 1 2 f ( x + h ) T f ( x + h ) = F ( x ) + h T J f T f + 1 2 h T J f T J f h F(x+h) =\frac{1}{2} f(x+h)^Tf(x+h) = F(x) +h^TJ_f^Tf+\frac{1}{2}h^TJ_f^TJ_fh F(x+h)=21f(x+h)Tf(x+h)=F(x)+hTJfTf+21hTJfTJfh
找下降方向的本质就是：选择什么样的h才能让F(x+h)最小。在寻找方向这个步骤中h是自变量，x并不是自变量。 问题就变成了解决下面这个局部最小问题
h g n = a r g m i n h ( F ( x + h ) ) h_{gn} = argmin_h(F(x+h)) hgn=argminh(F(x+h))
我们求F(x+h)对h的导数并令它等于0可得
F ′ ( x + h ) = J f T f + J f T J f h = 0 F \prime(x+h) = J_f^Tf+J_f^T J_f h = 0 F′(x+h)=JfTf+JfTJfh=0
h g n = − J f T f J f T J f h_{gn} = - \frac{J_f^Tf}{J_f^T J_f} hgn=−JfTJfJfTf
我们之前求过
F ′ ( x ) = J f T f F \prime(x) = J_f^Tf F′(x)=JfTf
代入上式可以得到高斯牛顿法的下降方向也可以表示为
h g n = − F ′ ( x ) J f T J f h_{gn} = - \frac{F \prime (x)}{ J_f^T J_f } hgn=−JfTJfF′(x)
把它和牛顿法求出的下降方向hn比较一下我们可以发现实际上高斯牛顿法的分母省略了带有海塞矩阵的项。可以理解为高斯牛顿法中做了下面的处理
F ′ ′ ( x ) = H f T f + J f T J f F\prime \prime(x) = H_f^Tf +J_f^TJ_f F′′(x)=HfTf+JfTJf
F ′ ′ ( x ) ≈ J f T J f F\prime \prime(x) \approx J_f^TJ_f F′′(x)≈JfTJf
这样做的依据是在目标点附近误差函数接近为0，所以可以省略带海塞矩阵的项，而这样的处理正好避免了计算海塞矩阵的缺点。当然这也表示了这种方法适合在临近目标点附近处的迭代。当初始点距离目标点较远时，这样的方法会失效。
Levenberg-Marquardt算法
LM算法的核心思想是在高斯牛顿法中加入一个阻尼因子来决定迭代时下降的更激进还是更保守。
( J T J + μ I ) h l m = − J T f a n d μ ≥ 0 (J^TJ + \mu I )h_{lm} = - J^Tf~~~ and~~~ \mu \ge 0 (JTJ+μI)hlm=−JTf and μ≥0
这个阻尼因子会根据一个指标来变化它自己的大小，我们定义这个指标
ρ = F ( x ) − F ( x + h ) L ( 0 ) − L ( h ) \rho = \frac{F(x)-F(x+h)}{L(0)-L(h)} ρ=L(0)−L(h)F(x)−F(x+h)
p为增益比，L表示的是对原函数的一阶泰勒近似模型，分子表示真实原函数的下降量，分母表示近似模型的下降量。当p>0时，说明近似模型的下降方向和真实函数减小保持一致，可以接受，并将阻尼因子变小。而p<0时，说明这时的下降方向已经与真实情况不一致了，这时需要增大阻尼因子减小下降的力度。规则如下：
阻尼因子影响着下降方向的选择。它的取值会让LM算法有时倾向于高斯牛顿法，有时倾向于梯度下降法，实现了混合算法对迭代初始阶段和临近目标点时的要求。
当u趋于0时上式变为
J T J h l m = − J T f 高 斯 牛 顿 法 J^TJ h _{lm} = -J^Tf ~~高斯牛顿法 JTJhlm=−JTf 高斯牛顿法
当u较大时
μ h l m = J T f 梯 度 下 降 法 \mu h_{lm} = J^Tf ~~~ 梯度下降法 μhlm=JTf 梯度下降法
有了步长的求解方法，现在我们需要迭代停止的条件。
- 一阶导接近0
∣ ∣ F ′ ( x ) ∣ ∣ < ϵ 1 ϵ 1 为 足 够 小 的 正 数 ||F \prime(x)||<\epsilon_1 ~~~ \epsilon_1为足够小的正数 ∣∣F′(x)∣∣<ϵ1 ϵ1为足够小的正数
- 下降步长足够小
∣ ∣ x n e w − x ∣ ∣ ≤ ϵ 2 ( ∣ ∣ x ∣ ∣ + ϵ 2 ) ||x_{new}-x|| \leq \epsilon_2(||x||+\epsilon_2) ∣∣xnew−x∣∣≤ϵ2(∣∣x∣∣+ϵ2)
- 迭代次数的限制
k ≥ k m a x k \geq k_{max} k≥kmax
整个算法流程如下：
狗腿法
狗腿法也是一种高斯牛顿法和梯度下降法的混合方法。区别于LM算法中的引入的阻尼因子，狗腿法引入了置信区域的概念。在置信区域内，我们认为泰勒一阶展开模型足够精确。因此当置信区域范围较大时，下降的程度倾向于变大，以快速向目标点进发。当置信区域小的时候，就需要我们减小步长一步一步“小心翼翼”的向前进发。
上图是狗腿法选择步长的示意图。其实狗腿法的下降方向与步长选择原则就是尽量x_new 落在置信区域的边缘，尽可能让下降更快且还在置信区域内。
置信区域的大小也是在不停变化的，衡量指标和LM算法的一样。另外迭代停止条件也是一样的。算法流程如下:
四、总结
其实这篇论文在后面还讲到了LM算法与拟牛顿法的混合，以及割线版本的LM算法和狗腿法，但LM算法和狗腿法对我来说就已经够用了，所以后面也没再去详细深究了。
下一步主要是用代码实现LM算法，不得不说最近我的代码能力下降的挺多的。表现在看别人代码看不懂，自己写不知道怎么入手，尤其是这些数学算法。所以talk it easy，show me code。
扫一扫
1162
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
