机器人中的数值优化（十一）——高斯牛顿法、LMF方法、Dogleg方法

   本系列文章主要是我在学习《数值优化》过程中的一些笔记和相关思考，主要的学习资料是深蓝学院的课程《机器人中的数值优化》和高立编著的《数值最优化方法》等，本系列文章篇数较多，不定期更新，上半部分介绍无约束优化，下半部分介绍带约束的优化，中间会穿插一些路径规划方面的应用实例

---

---

---

   十五、高斯牛顿法

   1、最小二乘问题

   最小二乘(Least Squares，LS)问题的定义如下：

   $$\min f(x)=\frac{1}{2}\sum _{i=1}^m{r}_i^2(x)=\frac{1}{2}r(x{)}^{\mathrm{T}}r(x),\dot{x}\in {\mathbb{R}}^n,m⩾n,$$

   这里$r(x)=(r_1(x),r_2(x)，\cdot ..,r_m(x){)}^T$称为剩余函数。点α处剩余函数的值称为剩余量。若$r_i(x)(i=l,...,m)$均为线性函数,则称为线性最小二乘问题;若至少有一个$r_i(x)$为非线性函数,则称为非线性最小二乘问题。

   最小二乘问题大量产生于数据拟合问题:给定一组试验数据(ti,yi) (i = 1,… , m)和一函数模型f(x; t)，我们要确定x,使得f(x; t)在剩余量平方和意义下尽可能好地拟合给定的数据,其中剩余量$r_i(x)$为

   $$r_i(x)=y_i-\tilde{f}(x;t_i),i=1,\cdots ,m,$$
   由此得到最小二乘问题，此外,最小二乘问题亦可用于解非线性方程组

   $$r_i(x)=0,i=1,\cdots ,m,$$

   当m=n时,方程组称为适定方程组;当m >n时，方程组称为超定方程组。

   最小二乘问题固然可以用前面讲过的一般无约束最优化方法去求解,然而由于该问题的目标函数有特殊结构,我们可以利用问题的结构对某些已讲过的方法进行改造,使之对最小二乘问题更简单或更有效.
此外,最小二乘问题亦可用于解非线性方程组

---

   2、最小二乘问题分类

   下面我们来看最小二乘问题的目标函数f(x)的一、二阶导数的形式.设J(x)是r(x)的雅可比矩阵:

   J ( x ) = [ ∇ r 1 T ⋮ ∇ r m T ] ∈ R m × n , J(x)=\left[

\right]\in\mathbb{R}^{m\times n}, J(x)= ​∇r1T​⋮∇rmT​​ ​∈Rm×n,

   则，f（x）的梯度和Hessian矩阵分别为：

   $$g(x)=\sum _{i=1}^m{r}_i(x)\nabla r_i(x)=J(x{)}^{\mathrm{T}}r(x),$$
   G ( x ) = ∑ i = 1 m ∇ r i ( x ) ∇ r i ( x ) T + ∑ i = 1 m r i ( x ) ∇ 2 r i ( x ) = J ( x ) T J ( x ) + S ( x ) ,

G(x)​=i=1∑m​∇ri​(x)∇ri​(x)T+i=1∑m​ri​(x)∇2ri​(x)=J(x)TJ(x)+S(x),​

   其中：

   $$S(x)=\sum _{i=1}^m{r}_i(x){\nabla }^2{r}_i(x).$$

   为方便描述，我们对上述符号进行如下的简记：

   J ⋆ = J ( x ⋆ ) , J k = J ( x k ) , S ⋆ = S ( x ⋆ ) , S k = S ( x k ) .

J⋆=J(x⋆),Jk​=J(xk​),S⋆=S(x⋆),Sk​=S(xk​).​

---

   在点x * 处，||S * || 的大小取决于剩余量与问题的非线性性.对零剩余或线性最小二乘问题||S * || = 0 . 随着剩余量的增大或$r_i(x)(i=l,...,m)$的非线性的增强,||S * ||的值变大.根据问题的这种特点,我们的算法将分为小剩余算法与大剩余算法.小剩余算法处理||S * ||为零或不太大的问题,大剩余算法处理||S * ||较大的问题.

---

   3、牛顿方法解最小二乘问题

   解最小二乘问题的Newton方程为

   $$(J_k^{\mathrm{T}}{J}_k+S_k)d_k=-J_k^{\mathrm{T}}{r}_k$$

   对最小二乘问题,Newton方法的缺点是每次迭代都要求$S_k$,即计算m个nxn对称矩阵．显然,对一个算法而言，$S_k$的计算是一个沉重的负担.解决这个问题的方法是或者在Newton方程中忽略 $S_k$,或者用一阶导数信息近似$S_k$.而要忽略$S_k$，则应在 $r_i(x)$接近于0或接近于线性时进行，即下面我们要讲的小剩余算法。

---

   4、高斯牛顿法

   在方程$(J_k^{\mathrm{T}}{J}_k+S_k)d_k=-J_k^{\mathrm{T}}{r}_k$中忽略$S_k$就可以得到Gauss-Newton (GN)方法，该方法以也可以理解为在点$x_k$处，线性化剩余函数$r_i(x_k+d),$，线性化后$S_k$的值即为0。

   忽略$S_k$后得到的Gauss-Newton (GN)方程如下：

   $$J_k^{\mathrm{T}}{J}_{k}d=-J_k^{\mathrm{T}}{r}_k.$$

   上述方程的解等价于求下述关于d的线性最小二乘问题的极小值问题。

   $$\underset{d\in {\mathbb{R}}^n}{\min }{q}_k(d)=\frac{1}{2}\Vert J_{k}d+r_k{\Vert }_2^2,$$

   其中：
   q k ( d ) = 1 2 ( J k d + r k ) T ( J k d + r k ) = 1 2 d T J k T J k d + d T ( J k T r k ) + 1 2 r k T r k .

qk​(d)​=21​(Jk​d+rk​)T(Jk​d+rk​)=21​dTJkT​Jk​d+dT(JkT​rk​)+21​rkT​rk​.​

   这里$q_k(d)$是对$f(x_k+d)$的一种二次近似,它与$f(x_k+d)$的二次Taylor近似的差别在于二次项中少了$S_k$

---

   用Gauss-Newton方法求解最小二乘问题的算法如下:

–
   基本Gauss-Newton方法是指${\alpha }_k$ = 1的 Gauss-Newton方法，带线搜索的 Gauss-Newton方法称为阻尼 Gauss-Newton方法.

   Gauss-Newton方法的优点在于它无须计算r(z)的二阶导数. 另外，当$J_k$满秩，$g_k$不为零的时候，可以保证$d_k$是下降方向。

---

   基本Gauss-Newton方法有如下两种情形的收敛速度:

·    （1）二阶收敛速度：若||S(x*)||= 0，即在零剩余问题或是线性最小二乘问题的情形,则方法在x * 附近具有Newton方法的收敛速度。

   （2）线性收敛速度：若||S(x*)||≠0,则方法的收敛速度是线性的。收敛速度随||S(x*)||的增大而变慢.

   由此可见,基本Gauss-Newton方法的收敛速度是与x*处剩余量的大小及剩余函数的线性程度有关的,即剩余量越小或剩余函数越接近线性,它的收敛速度就越快;反之就越慢,甚至对剩余量很大或剩余函数的非线性程度很强的问题不收敛。

   此外，高斯牛顿法要求矩阵J(x)列满秩.如若不然,则矩阵$J(x{)}^{\mathrm{T}}J(x)$奇异,我们不能从Gauss-Newton方程求得$d_k$。

---

   十六、LMF方法

   Gauss-Newton方法在迭代中会出现$J_k^{\mathrm{T}}{J}_k$为奇异的情形.为了克服这个困难,Levenberg在 1944年提出由下面的方程求解$d_k$，其中$v_k\ge 0$。这个方法由于1964年时Marquardt的努力而得到广泛应用,故称为LM (Levenberg-Marquardt)方法,下式称为LM方程.

   $$(J_k^{\mathrm{T}}{J}_k+{\nu }_{k}I)d=-J_k^{\mathrm{T}}{r}_k$$

   在上述方程中,对任意$v_k\ge 0$，$J_k^{\mathrm{T}}{J}_k+{\nu }_{k}I$正定。从计算的角度出发,为保证该矩阵充分正定,$v_k$可能需要取得适当的大,$J_k^{\mathrm{T}}{J}_k+{\nu }_{k}I$的正定性保证了由上述方程得到的方向是下降方向。

   LM 方法是一种信赖域型方法,$v_k$的值可以用信赖域方法的思想在迭代中修正得到，前文中我们介绍过信赖域方法中信赖域半径是如何修正的，现在只要找出 LM 方程与信赖域问题的关系，就可以根据修正信赖域半径的方法修正$v_k$的值。

   LM方程与信赖域问题的关系：

   $$\underset{d}{\min }\frac{1}{2}\Vert J_{k}d+r_k{\Vert }^2,$$
   $$\text{s.t.}\Vert d{\Vert }^2⩽{\Delta }_k^2,{\Delta }_k>0$$

   上式为信赖域子问题，$d_k$其全局极小解的充分必要条件是，对满足上式的的$d_k$,存在$v_k\ge 0$,使得

   ( J k T J k + ν k I ) d k = − J k T r k ˉ , ν k ( Δ k 2 − ∥ d k ∥ 2 ) = 0.

(JkT​Jk​+νk​I)dk​=−JkT​rkˉ​,νk​(Δk2​−∥dk​∥2)=0.​

---

   LM 方程与信赖域问题的关系是 Fletcher在1981年提出的.故由此建立起来的方法称为LMF (Levenberg-Marquardt-Fletcher)方法.

   下面我们来考虑$v_k$的修正方法,它与信赖域半径△k的修正是相关的.在信赖域方法中，从$x_k$到$x_k+d_k$ ， f(x)的实际减少量为

   $$\Delta f_k=f(x_k)-f(x_k+d_k),$$

   上文给出的近似函数
   q k ( d ) = 1 2 ( J k d + r k ) T ( J k d + r k ) = 1 2 d T J k T J k d + d T ( J k T r k ) + 1 2 r k T r k .

qk​(d)​=21​(Jk​d+rk​)T(Jk​d+rk​)=21​dTJkT​Jk​d+dT(JkT​rk​)+21​rkT​rk​.​

   的减少量为：

   $$\Delta q_k=q_k(0)-q_k(d_k),$$

   其中$q_k(0)=f_k$，另外，由LM方程与$d_k^{\mathrm{T}}{g}_k<0$知

   Δ g k = g k ( 0 ) − g k ( d k ) = − 1 2 d k T J k T J k d k − d k T ( J k T r k ) = 1 2 d k T ( − J k T J k d k − ν k d k + ν k d k − 2 J k T r k ) = 1 2 d k T ( − ( J k T J k + ν k ) d k + ν k d k − 2 J k T r k ) = 1 2 d k T ( v k d k − g k ) > 0 ,

Δgk​​=gk​(0)−gk​(dk​)=−21​dkT​JkT​Jk​dk​−dkT​(JkT​rk​)=21​dkT​(−JkT​Jk​dk​−νk​dk​+νk​dk​−2JkT​rk​)=21​dkT​(−(JkT​Jk​+νk​)dk​+νk​dk​−2JkT​rk​)=21​dkT​(vk​dk​−gk​)>0,​

   其中$g_k=J_k^{\mathrm{T}}{r}_k.$

   进行如下定义：

   $${\gamma }_k=\frac{\Delta f_k}{\Delta q_k}.$$

   在第k步迭代, ${\gamma }_k$的值可以反映出$q_k(d_k)$近似f(.zck + dk)的好坏.关于yxe的值如何反映 q(d)近似$f(x_k+d_k)$的好坏,以及如何由此修正△k的问题。

   由LM 方程知,$v_k$可以控制$||d_k||$的大小,从而可以控制信赖域的大小.若$v_k$变大的话,$||d_k||$会变小,反之亦然,所以对,$v_k$大小的修正，应该与信赖域方法中对△k大小的修正相反。

   下面给出 LMF方法的步骤:

   上述算法中$v_0$ >0可以任取. Fletcher指出该算法对0.25、0.75等常数并不敏感。LMF方法可以用于求解一般无约束最优化问题。在修正Newton方法中,我们曾经提到过这个方法。修正Newton方程与信赖域问题的关系如下:

  
  

---

   十七、Dogleg方法

   Dogleg方法是一种非线性最小化的数值优化方法，用于寻找函数的最小值。它的基本思想是将当前迭代点处的函数模型分为两部分：一部分是线性模型，另一部分是二次模型。在每次迭代中，该方法会将搜索方向限制在两个较小的半径内，以保证在可接受误差范围内找到局部极小值。

   具体来说，Dogleg方法的实现分为以下几步：

   构建当前迭代点处的函数模型，并计算其梯度和Hessian矩阵；

   计算当前迭代点处的两个半径：一是当函数模型为线性时的半径，另一个是当函数模型为二次时的半径；

   计算在两个半径内的最优搜索方向。当搜索方向在线性半径内时，直接沿着负梯度方向进行搜索；当搜索方向在二次半径内时，计算二次模型的极小值点，并将搜索方向设置为连接当前迭代点和极小值点的线段；

   如果最优搜索方向在线性半径内，直接更新迭代点；如果最优搜索方向在二次半径内，则需要计算更新点，以确保搜索方向在两个半径之间。

   与其他优化方法相比，Dogleg方法具有收敛速度快和收敛精度高的优点。但它也存在一些缺点，例如无法处理约束条件等。在实际应用中，需要根据具体问题选择合适的优化方法。

---

   算法流程如下所示：

   其中， Gauss-Newton方向$d_k^{GN}$由Gauss-Newton方程给出，$d_k^{SD}=-J_k^{\mathrm{T}}{r}_k$,最速下降方法的步长为:

   $${\alpha }_k=\arg \min q_k(\alpha d_k^{\mathrm{SD}})=\frac{\Vert d_k^{\mathrm{SD}}{\Vert }^2}{\Vert J_k{d}_k^{\mathrm{SD}}{\Vert }^2},$$

   其中：
   $$q_k(\alpha d_k^{\mathrm{SD}})\triangleq \frac{1}{2}\Vert \alpha J_k{d}_k^{\mathrm{SD}}+r_k{\Vert }^2=\frac{1}{2}\Vert J_k{d}_k^{\mathrm{SD}}{\Vert }^2{\alpha }^2-\Vert d_k^{\mathrm{SD}}{\Vert }^2\alpha +f_k.$$

---

---

   参考资料：

   1、数值最优化方法（高立 编著）

   2、机器人中的数值优化

---

---