数值优化 | 图解梯度下降法与共轭梯度下降法(附案例分析与Python实现)

0 专栏介绍

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

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

1 梯度下降法与轨迹优化

在复杂环境中，机器人需要寻找从起始点到目标点的最优路径。通过将路径表示为一个代价函数，梯度下降法可以用于最小化该代价，从而找到最符合期望的路径。例如在轨迹优化 | 基于ESDF的共轭梯度优化算法(附ROS C++/Python仿真)中，我们针对路径序列 X = { x i = ( x i , y i ) ∣ i ∈ [ 1 , N ] } \mathcal{X} =\left\{ \boldsymbol{x}_i=\left( x_i,y_i \right) |i\in \left[ 1,N \right] \right\} X={xi​=(xi​,yi​)∣i∈[1,N]}设计了优化目标函数

$$f\left(\mathcal{X}\right)=w_o{P}_{\mathrm{o}\mathrm{b}\mathrm{s}}\left(\mathcal{X}\right)+w_{\kappa }{P}_{\mathrm{c}\mathrm{u}\mathrm{r}}\left(\mathcal{X}\right)+w_s{P}_{\mathrm{s}\mathrm{m}\mathrm{o}}\left(\mathcal{X}\right)$$

最终实现了轨迹优化。此外，还有GRIP、Constrianed Smoother等算法都基于梯度下降算法。

然而，在实际操作过程中，很多同学并不知道梯度法的数值原理，导致调参没有头绪，本节就从数值优化的层面讲解梯度下降算法的基本方法

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)+O\left(\mathbf{x}\right)$$

期望下一次迭代满足

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

则

$${\nabla }^{T}F\left({\mathbf{x}}_k\right)\left({\mathbf{x}}_{k+1}-{\mathbf{x}}_k\right)<0$$

取${\mathbf{x}}_{k+1}-{\mathbf{x}}_k=-\alpha \nabla F\left({\mathbf{x}}_k\right)$，则

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

保证了函数值下降，这种迭代方式

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

称为梯度下降算法(Gradient Descent, GD)。其中$\alpha$是优化步长或学习率，可取为常数或通过步长搜索计算。必须指出，泰勒公式成立的条件是$\mathbf{x}\to {\mathbf{x}}_k$，故$\nabla F\left({\mathbf{x}}_k\right)$不能太大，否则${\mathbf{x}}_{k+1}$与${\mathbf{x}}_k$距离太远产生余项误差。由于只利用了目标函数一阶梯度信息，所以梯度下降算法属于一阶无约束优化方法。由于梯度下降法每次都沿负梯度方向下降，所以可能锯齿状收敛且受初值影响大，如图所示；同时也无法避免局部最优陷落。

从收敛性角度，当目标函数满足Lipschitz连续且优化步长满足Wolfe准则时，梯度下降法可以保证收敛，收敛速度为线性。关于Wolfe准则可以参考数值优化 | 图解线搜索之Armijo、Goldstein、Wolfe准则(附案例分析与Python实现)

3 共轭梯度下降算法原理

共轭梯度法(Conjugate Gradient)是针对于$n$维二次优化问题的梯度下降算法，给定

$${\mathbf{x}}^{\ast }=\mathrm{a}\mathrm{r}\mathrm{g}\underset{\mathbf{x}}{\min }\frac{1}{2}{\mathbf{x}}^T\mathbf{Q}\mathbf{x}+{\mathbf{q}}^T\mathbf{x}$$

其中$\mathbf{Q}$是$n$维对称正定阵，$\mathbf{q}\in {\mathbb{R}}^n$，共轭梯度法既克服了梯度下降法收敛慢的缺点，又避免存储和计算牛顿类算法所需的二阶梯度信息。

共轭梯度法的核心原理是：求解矩阵$\mathbf{Q}$的共轭向量组${\mathbf{d}}_0,{\mathbf{d}}_1,\cdots ,{\mathbf{d}}_n$作为$n$个优化方向，由于优化方向间彼此正交，故每次迭代只需沿着一个方向${\mathbf{d}}_i$寻优而互不影响。所以理论上最多$n$次迭代就能找到最优解，收敛速度快，但实际应用中需要视具体情况确定阈值。这里给出一个具体的应用案例：轨迹优化 | 基于ESDF的共轭梯度优化算法(附ROS C++/Python仿真)

共轭梯度法迭代式为

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k+{\alpha }_k{\mathbf{d}}_k$$

下面主要计算每次迭代的步长和优化方向。

1. 优化方向${\mathbf{d}}_k$

   设${\mathbf{d}}_k=-\nabla F\left({\mathbf{x}}_k\right)+{\gamma }_{k-1}{\mathbf{d}}_{k-1}$，等式两边同乘${\mathbf{d}}_{k-1}^T\mathbf{Q}$，则根据共轭关系有

   $$\begin{array}{rl}-{\mathbf{d}}_{k-1}^T\mathbf{Q}\nabla F\left({\mathbf{x}}_k\right)+{\gamma }_{k-1}{\mathbf{d}}_{k-1}^T{\mathbf{Q}\mathbf{d}}_{k-1}& ={\mathbf{d}}_{k-1}^T{\mathbf{Q}\mathbf{d}}_k\\ \Rightarrow {\gamma }_{k-1}{\mathbf{d}}_{k-1}^T{\mathbf{Q}\mathbf{d}}_{k-1}& ={\nabla }^{T}F\left({\mathbf{x}}_k\right){\mathbf{Q}\mathbf{d}}_{k-1}\\ \Rightarrow {\gamma }_{k-1}& =\frac{{\nabla }^{T}F\left({\mathbf{x}}_k\right){\mathbf{Q}\mathbf{d}}_{k-1}}{{\mathbf{d}}_{k-1}^T{\mathbf{Q}\mathbf{d}}_{k-1}}\end{array}$$

   其中$\nabla F\left({\mathbf{x}}_k\right)={\mathbf{Q}\mathbf{x}}_k+\mathbf{q}$。考虑到$\nabla F\left({\mathbf{x}}_{k+1}\right)-\nabla F\left({\mathbf{x}}_k\right)=\mathbf{Q}\left({\mathbf{x}}_{k+1}-{\mathbf{x}}_k\right)={\alpha }_k{\mathbf{Q}\mathbf{d}}_k$，则

   $$\begin{array}{rl}{\gamma }_{k-1}& =\frac{{\nabla }^{T}F\left({\mathbf{x}}_k\right)\left(\nabla F\left({\mathbf{x}}_k\right)-\nabla F\left({\mathbf{x}}_{k-1}\right)\right)}{{\mathbf{d}}_{k-1}^T\left(\nabla F\left({\mathbf{x}}_k\right)-\nabla F\left({\mathbf{x}}_{k-1}\right)\right)}\end{array}$$

   根据

   $$\begin{array}{rl}{\mathbf{d}}_k& =-\nabla F\left({\mathbf{x}}_k\right)+{\gamma }_{k-1}{\mathbf{d}}_{k-1}\\ \Rightarrow {\mathbf{d}}_k^T\nabla F\left({\mathbf{x}}_{k+1}\right)& =-{\nabla }^{T}F\left({\mathbf{x}}_k\right)\nabla F\left({\mathbf{x}}_{k+1}\right)+{\gamma }_{k-1}{\mathbf{d}}_{k-1}^T\nabla F\left({\mathbf{x}}_{k+1}\right)\end{array}$$

   由子空间扩展定理知${\mathbf{d}}_k^T\nabla F\left({\mathbf{x}}_{k+1}\right)={\mathbf{d}}_{k-1}^T\nabla F\left({\mathbf{x}}_{k+1}\right)=0$，从而${\nabla }^{T}F\left({\mathbf{x}}_k\right)\nabla F\left({\mathbf{x}}_{k-1}\right)=0$，简化

   $${\gamma }_{k-1}=\frac{-{\nabla }^{T}F\left({\mathbf{x}}_k\right)\nabla F\left({\mathbf{x}}_k\right)}{{\mathbf{d}}_{k-1}^T\nabla F\left({\mathbf{x}}_{k-1}\right)}$$

   因为

   $${\mathbf{d}}_{k-1}^T\nabla F\left({\mathbf{x}}_{k-1}\right)={\left(-\nabla F\left({\mathbf{x}}_{k-1}\right)+{\gamma }_{k-2}{\mathbf{d}}_{k-2}\right)}^T\nabla F\left({\mathbf{x}}_{k-1}\right)=-{\nabla }^{T}F\left({\mathbf{x}}_{k-1}\right)\nabla F\left({\mathbf{x}}_{k-1}\right)$$

   故

   $${\gamma }_{k-1}=\frac{{\nabla }^{T}F\left({\mathbf{x}}_k\right)\nabla F\left({\mathbf{x}}_k\right)}{{\nabla }^{T}F\left({\mathbf{x}}_{k-1}\right)\nabla F\left({\mathbf{x}}_{k-1}\right)}$$

2. 步长$\alpha$

   因为

   $$\begin{gathered} F\left({\mathbf{x}}_{k+1}\right)=\frac{1}{2}{\mathbf{x}}_{k+1}^T{\mathbf{Q}\mathbf{x}}_{k+1}+{\mathbf{q}}^T{\mathbf{x}}_{k+1} \\ =\frac{1}{2}{\left({\mathbf{x}}_k+{\lambda }_k{\mathbf{d}}_k\right)}^T\mathbf{Q}\left({\mathbf{x}}_k+{\lambda }_k{\mathbf{d}}_k\right)+{\mathbf{q}}^T\left({\mathbf{x}}_k+{\lambda }_k{\mathbf{d}}_k\right) \end{gathered}$$

   所以令$\partial F\left({\mathbf{x}}_{k+1}\right)/\partial {\alpha }_k=0$，立即得到

   $${\alpha }_k=\frac{-{\mathbf{d}}_k^T\nabla F\left({\mathbf{x}}_k\right)}{{\mathbf{d}}_k^T{\mathbf{Q}\mathbf{d}}_k}$$

4 案例分析与Python实现

二次型是常用的数值优化形式，给定

$$f\left(\mathbf{x}\right)=\frac{1}{2}{\mathbf{x}}^T\mathbf{Q}\mathbf{x}+{\mathbf{q}}^T\mathbf{x}$$

其中

$$\mathbf{Q}=\left[\begin{array}{cc}10& -9\\ -9& 10\end{array}\right],\mathbf{q}=\left[\begin{array}{c}4\\ -15\end{array}\right]$$

下面是梯度下降算法的实现

def run(self, func, g_func, x0: np.ndarray) -> np.ndarray:
	x, x_next = x0, None
	traj = []
	iteration = 0
	start_time = time.time()
	while iteration < self.max_iters:
	    iteration += 1
	    traj.append(x)
	    gx = g_func(x)
	    if np.sqrt(np.sum(gx ** 2)) < self.eps:
	        break
	
	    alpha = self.alpha if self.alpha else self.line_searcher_.search(func, g_func, x, -gx)
	    x_next = x - alpha * gx
	
	    if math.fabs(func(x) - func(x_next)) < self.eps:
	        break
	    
	    x = x_next
	end_time = time.time()
		
	return x, traj

下面是共轭梯度算法的实现

 def run(self, Q: np.ndarray, q: np.ndarray, x0: np.ndarray) -> np.ndarray:
	 """
	 0.5 * x^T Q x + q^T * x
	 """
	 def func(x: np.ndarray) -> np.ndarray:
	     return 0.5 * x.T @ Q @ x + q.T @ x
	
	 def g_func(x: np.ndarray) -> np.ndarray:
	     return Q @ x + q
	
	 x, x_next = x0, None
	 d, d_next = g_func(x0), None
	 traj = []
	 iteration = 0
	 start_time = time.time()
	 while iteration < self.max_iters:
	     iteration += 1
	     traj.append(x)
	
	     gx = g_func(x)
	     if np.sqrt(np.sum(gx ** 2)) < self.eps:
	         break
	
	     alpha = -d @ gx / (d.T @ Q @ d)
	     x_next = x + alpha * d
	     gx_next = g_func(x_next)
	     d_next = -gx_next + gx_next.T @ gx_next / (gx.T @ gx) * d
	     
	     d = d_next
	     x = x_next
	 end_time = time.time()
	
	 return x, traj

结果如图所示，从共轭梯度法与梯度下降法的迭代轨迹，可以看出共轭梯度法的轨迹更平滑，收敛速度更快(这个案例上大约2000倍的增速)

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

🔥 更多精彩专栏：

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