0 专栏介绍

🔥课设、毕设、创新竞赛必备！🔥本专栏涉及更高阶的运动规划算法轨迹优化实战，包括：曲线生成、碰撞检测、安全走廊、优化建模(QP、SQP、NMPC、iLQR等)、轨迹优化(梯度法、曲线法等)，每个算法都包含代码实现加深理解

🚀详情：运动规划实战进阶：轨迹优化篇

---

1 引例

给定如图所示的某个函数，如何计算函数零点$x_0$？

在数学上我们如何处理这个问题？

最简单的办法是解方程$f(x)=0$，在代数学上还有著名的零点判定定理

如果函数$y=f(x)$在区间$[a,b]$上的图象是连续不断的一条曲线，并且有$f(a)\cdot f(b)<0$,那么函数$y=f(x)$在区间$(a,b)$内有零点，即至少存在一个$c\in (a,b)$,使得$f(c)=0$,这个$c$也就是方程$f(x)=0$的根。

然而，数学上的方法并不一定适合工程应用，当函数形式复杂，例如出现超越函数形式；非解析形式，例如递推关系时，精确的方程解析一般难以进行，因为代数上还没发展出任意形式的求根公式。而零点判定定理求解效率也较低，需要不停试错。因此，引入今天的主题——牛顿迭代法，服务于工程数值计算。

记第$k$轮迭代后，自变量更新为$x_k$，令目标函数$f(x)$在$x=x_k$泰勒展开：

$$f\left(x\right)=f\left(x_k\right)+f^{\prime }\left(x_k\right)\left(x-x_k\right)+o(x)$$

我们希望下一次迭代到根点，忽略泰勒余项，令$f(x_{k+1})=0$，则

$$x_{k+1}=x_k-\frac{f(x_k)}{f^{\prime }(x_k)}$$

不断重复运算即可逼近根点。

在几何上，上面过程实际上是在做$f(x)$在$x=x_k$处的切线，并求切线的零点，在工程上称为局部线性化。如图所示，若$x_k$在$x_0$的左侧，那么下一次迭代方向向右。

若$x_k$在$x_0$的右侧，那么下一次迭代方向向左。

2 牛顿迭代法

记第$k$轮迭代后，自变量更新为${\mathbf{x}}_k$，令目标函数$F\left(\mathbf{x}\right)$在$\mathbf{x}={\mathbf{x}}_k$二阶泰勒展开

$$F\left(\mathbf{x}\right)=F\left({\mathbf{x}}_k\right)+{\nabla }^{T}F\left({\mathbf{x}}_k\right)\left(\mathbf{x}-{\mathbf{x}}_k\right)+\frac{1}{2}{\left(\mathbf{x}-{\mathbf{x}}_k\right)}^T\mathbf{H}\left({\mathbf{x}}_k\right)\left(\mathbf{x}-{\mathbf{x}}_k\right)+O\left(\mathbf{x}\right)$$

其一阶梯度为

$$\nabla F\left(\mathbf{x}\right)=\nabla F\left({\mathbf{x}}_k\right)+\mathbf{H}\left({\mathbf{x}}_k\right)\left(\mathbf{x}-{\mathbf{x}}_k\right)$$

求$F\left(\mathbf{x}\right)$的极小值等价于求$F^{\prime }\left(\mathbf{x}\right)=0$的根，根据引例的方法，期望下一次迭代满足$F^{\prime }\left({\mathbf{x}}_{k+1}\right)=0$，则

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k-{\mathbf{H}}^{-1}\left({\mathbf{x}}_k\right)\nabla F\left({\mathbf{x}}_k\right)$$

此即为牛顿法的基本公式，可视为步长为${\mathbf{H}}^{-1}\left({\mathbf{x}}_k\right)$的自适应梯度下降算法(梯度下降法可以参考数值优化 | 图解梯度下降法与共轭梯度下降法(附案例分析与Python实现))

3 阻尼牛顿法

虽然牛顿法保证收敛，但并非每次迭代都能使函数值下降。设$G\left(\alpha \right)=F\left({\mathbf{x}}_k+\alpha {\mathbf{d}}_k\right)$，其中${\mathbf{d}}_k=-{\mathbf{H}}^{-1}\left({\mathbf{x}}_k\right)\nabla F\left({\mathbf{x}}_k\right)$，则

$$G^{\prime }\left(\alpha \right)=\nabla F{\left({\mathbf{x}}_k+\alpha {\mathbf{d}}_k\right)}^T{\mathbf{d}}_k$$

当$\mathbf{H}\left(\mathbf{x}\right)$正定时有

$$G^{\prime }\left(0\right)=\nabla F{\left({\mathbf{x}}_k\right)}^T{\mathbf{d}}_k=-\nabla F{\left({\mathbf{x}}_k\right)}^T{\mathbf{H}}^{-1}\left({\mathbf{x}}_k\right)\nabla F\left({\mathbf{x}}_k\right)<0$$

根据导数的连续性，必然存在$\alpha >0$使$G^{\prime }\left(\alpha \right)<0$，即

$$F\left({\mathbf{x}}_k+\alpha {\mathbf{d}}_k\right)<F\left({\mathbf{x}}_k\right)$$

所以可以在牛顿法的基础上手工进行步长的一维线搜索来保证目标函数下降，称为阻尼牛顿法(Damped Newton’s Method)，线搜索的相关理论可以参考数值优化 | 图解线搜索之Armijo、Goldstein、Wolfe准则(附案例分析与Python实现)

4 高斯牛顿法

高斯牛顿法通常用于解决最小二乘问题

$$\underset{\mathbf{x}}{\min }F\left(\mathbf{x}\right)=\frac{1}{2m}\underset{\mathbf{\theta }}{\min }\sum _{i=1}^m{\Vert f_i\left(\mathbf{x}\right)\Vert }_2^2=\frac{1}{2m}\underset{\mathbf{\theta }}{\min }{\mathbf{f}}^T\left(\mathbf{x}\right)\mathbf{f}\left(\mathbf{x}\right)$$

其中$\mathbf{f}\left(\mathbf{x}\right)={\left[\begin{array}{cccc}{f}_1\left(\mathbf{x}\right)& f_2\left(\mathbf{x}\right)& \cdots & f_m\left(\mathbf{x}\right)\end{array}\right]}^T$，$f_i\left(\cdot \right)$是关于第$i$个观测样本的目标函数，当$f_i\left(\cdot \right)$为线性函数时为线性最小二乘问题，否则是非线性最小二乘问题

考虑一阶泰勒展开$\mathbf{f}\left({\mathbf{x}}_{k+1}\right)=\mathbf{f}\left({\mathbf{x}}_k\right)+\mathbf{J}\left({\mathbf{x}}_k\right)\Delta {\mathbf{x}}_k$，令

$$\begin{array}{rl}\frac{\partial F\left({\mathbf{x}}_{k+1}\right)}{\partial {\mathbf{x}}_{k+1}}& =\frac{\partial \left(\frac{1}{2}{\mathbf{f}}^T\left({\mathbf{x}}_k\right)\mathbf{f}\left({\mathbf{x}}_k\right)+{\mathbf{f}}^T\left({\mathbf{x}}_k\right)\mathbf{J}\left({\mathbf{x}}_k\right)\Delta {\mathbf{x}}_k+\frac{1}{2}\Delta {\mathbf{x}}_k^T{\mathbf{J}}^T\left({\mathbf{x}}_k\right)\mathbf{J}\left({\mathbf{x}}_k\right)\Delta {\mathbf{x}}_k\right)}{\partial {\mathbf{x}}_{k+1}}\\ & ={\mathbf{J}}^T\left({\mathbf{x}}_k\right)\mathbf{f}\left({\mathbf{x}}_k\right)+{\mathbf{J}}^T\left({\mathbf{x}}_k\right)\mathbf{J}\left({\mathbf{x}}_k\right)\Delta {\mathbf{x}}_k\\ & =0\end{array}$$

从而得到高斯牛顿法应用于最小二乘问题的迭代公式

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k-{\left({\mathbf{J}}^T\left({\mathbf{x}}_k\right)\mathbf{J}\left({\mathbf{x}}_k\right)\right)}^{-1}{\mathbf{J}}^T\left({\mathbf{x}}_k\right)\mathbf{f}\left({\mathbf{x}}_k\right)$$

相当于用一阶梯度信息${\mathbf{J}}^T\left({\mathbf{x}}_k\right)\mathbf{J}\left({\mathbf{x}}_k\right)$来近似二阶的海森矩阵$\mathbf{H}\left(\mathbf{x}\right)$

5 案例分析与Python实现

5.1 牛顿法实现

核心代码如下所示

class NewtonMethodSolver:def __call__(self, func, g_func, h_func, x0: np.ndarray):x, x_next = x0, Nonetraj = []iteration = 0while iteration < self.max_iters:iteration += 1traj.append(x)gx = g_func(x)if np.sqrt(np.sum(gx ** 2)) < self.eps:breakB = h_func(x)H = np.linalg.inv(B)d = (H @ gx).squeeze()x_next = x - dif math.fabs(func(x) - func(x_next)) < self.eps:breakx = x_nextreturn x, traj

5.2 阻尼牛顿法实现

核心代码如下所示

class DampedNewtonMethodSolver:def __call__(self, func, g_func, h_func, x0: np.ndarray):x, x_next = x0, Nonetraj = []iteration = 0while iteration < self.max_iters:iteration += 1traj.append(x)gx = g_func(x)if np.sqrt(np.sum(gx ** 2)) < self.eps:breakB = h_func(x)H = np.linalg.inv(B)d = (H @ gx).squeeze()alpha = self.line_searcher_.search(func, g_func, x, -d)x_next = x - alpha * dif math.fabs(func(x) - func(x_next)) < self.eps:breakx = x_nextreturn x, traj

5.3 高斯牛顿法实现

核心代码如下所示

class GaussianNewtonSolver:def __call__(self, func, g_func, x0: np.ndarray):x, x_next = x0, Nonem = func(x0).shape[0]traj = []iteration = 0while iteration < self.max_iters:iteration += 1traj.append(x)fx = func(x)gx = g_func(x)d = (np.linalg.inv(gx.T @ gx) @ gx.T @ fx).squeeze()alpha = self.line_searcher_.search(lambda x : func(x).T @ func(x), lambda x : g_func(x).T @ func(x), x, -d)if np.sqrt(np.sum((gx.T @ fx) ** 2)) < self.eps:breakx_next = x - alpha * df_next = func(x_next)if math.fabs(fx.T @ fx - f_next.T @ f_next) / 2.0 / m < self.eps:breakx = x_nextreturn x, traj

5.4 案例分析

以曲线拟合为例展示三种牛顿类优化算法的效果

考虑如下方程：
$$\mathbf{y}=e^{a{\mathbf{x}}^2+b\mathbf{x}+c}+\mathbf{w}$$
其中$a$、$b$、$c$是待估计的曲线参数，$\mathbf{w}$是高斯噪声，假设有$n$个观测样本$(x_i,y_i)$，希望根据这组样本得到最佳的曲线参数，构造如下的优化问题
$$F(\mathbf{\theta })=\min \frac{1}{2n}\sum _{i=1}^n(y_i-e^{a{x}_i^2+b{x}_i+c}{)}^2$$

对于牛顿法和阻尼牛顿法来说，是对$F(\mathbf{\theta })$整体求导；对于高斯牛顿法来说，则是对

$$\mathbf{f}\left(\mathbf{\theta }\right)=\left[\begin{array}{c}{y}_1-e^{a{x}_1^2+b{x}_1+c}\\ y_2-e^{a{x}_2^2+b{x}_2+c}\\ ⋮\\ y_n-e^{a{x}_n^2+b{x}_n+c}\end{array}\right]\in {\mathbb{R}}^n$$

求导

下面来看结果

• 牛顿迭代法

  ===========================================================
  Method: Newton's method
  Iterations: 12, Using time: 0.032912254333496094
  Start Point: [ 2 -1  5]
  Start Value: 16427.297702943684
  End Point: [1.32294736 1.58499573 1.11544194]
  End Value: 0.4622053698036131
  ===========================================================
  

  

• 阻尼牛顿法

  ===========================================================
  Method: Damped Newton's method
  Iterations: 10, Using time: 0.2754700183868408
  Start Point: [ 2 -1  5]
  Start Value: 16427.297702943684
  End Point: [1.32324891 1.58452748 1.11561685]
  End Value: 0.46220541076102967
  ===========================================================
  

  

• 高斯牛顿法

  ===========================================================
  Method: Gaussian-Newton Method
  Iterations: 8, Using time: 0.7493696212768555
  Start Point: [ 2 -1  5]
  Start Value: [[16427.297]]
  End Point: [1.32328919 1.58446899 1.11562044]
  End Value: [[0.46220547]]
  ===========================================================
  

  

可以看到，在该案例中高斯牛顿法迭代次数最少，牛顿法计算耗时最少

完整工程代码请联系下方博主名片获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇