史上最详细的轨迹优化教程-机器人避障及轨迹平滑实现（干货满满）

有一些朋友问我到底如何用优化方法实现轨迹优化（避障+轨迹平滑等），今天就出一个干货满满的教程，绝对是面向很多工业化场景的讲解，为了便于理解，我选用二维平面并给出详细代码实现，三维空间原理相似。

本教程禁止转载，主要是有问题可以联系我探讨，我的邮箱 fanzexuan135@163.com

下面提供的代码实际运行结果如下图（下面会有数学和代码的详解）

轨迹优化教程

本教程将详细介绍如何在二维平面上优化一条轨迹,使其避免碰撞并保持平滑。我们将从数学原理出发,逐步推导并给出代码实现。同时,我也会介绍一些常用的优化器库。

问题定义

假设我们在一个二维平面上有一组障碍物,我们的目标是在起点和终点之间规划一条轨迹,使其满足以下条件:

1. 轨迹不与任何障碍物相交(避免碰撞)
2. 轨迹尽可能平滑,没有急转弯(保持平滑)
3. 轨迹尽可能短,减少不必要的绕路(最小化长度)

我们可以将这个问题形式化为一个优化问题:

$$\underset{\mathbf{x}}{\min }L(\mathbf{x})+{\lambda }_{s}S(\mathbf{x})+{\lambda }_{c}C(\mathbf{x})$$

其中:

• $\mathbf{x}$是轨迹的参数化表示,例如一系列的路径点坐标
• $L(\mathbf{x})$表示轨迹的长度
• $S(\mathbf{x})$表示轨迹的平滑度,可以用轨迹的曲率或加速度等量度
• $C(\mathbf{x})$表示轨迹与障碍物之间的碰撞代价
• ${\lambda }_s$和${\lambda }_c$是平滑项和碰撞项的权重系数

轨迹参数化

为了优化轨迹,我们首先需要选择一种方式来参数化轨迹。常见的方法有:

1. 路径点参数化:用一系列的路径点$(x_i,y_i)$来表示轨迹,路径点之间可以用直线或曲线连接
2. 样条曲线参数化:用贝塞尔曲线、B样条曲线等来表示轨迹,控制点的坐标作为优化变量
3. 函数参数化:用一个显式或隐式的函数$f(x,y)=0$来表示轨迹,函数的参数作为优化变量

这里我们选择路径点参数化,因为它简单直观,易于实现。假设我们用$n$个路径点来表示轨迹,优化变量为:

$$\mathbf{x}=[x_1,y_1,x_2,y_2,\dots ,x_n,y_n{]}^T$$

其中$(x_1,y_1)$和$(x_n,y_n)$分别为起点和终点的坐标,是固定的。

目标函数构建

接下来,我们要为每一项设计合适的目标函数。

长度项

轨迹的长度可以用路径点之间的欧氏距离之和来近似:

$$L(\mathbf{x})=\sum _{i=1}^{n-1}\sqrt{(x_{i+1}-x_i{)}^2+(y_{i+1}-y_i{)}^2}$$

这个函数是非线性的,但我们可以用泰勒展开来线性化:

$$\begin{array}{rl}L(\mathbf{x}+\Delta \mathbf{x})& \approx L(\mathbf{x})+\nabla L(\mathbf{x}{)}^T\Delta \mathbf{x}\\ & =L(\mathbf{x})+\sum _{i=1}^{n-1}\frac{x_{i+1}-x_i}{d_i}\Delta x_i+\frac{y_{i+1}-y_i}{d_i}\Delta y_i\end{array}$$

其中$d_i=\sqrt{(x_{i+1}-x_i{)}^2+(y_{i+1}-y_i{)}^2}$。这样长度项就变成了关于$\Delta \mathbf{x}$的线性函数。

平滑项

轨迹的平滑度可以用相邻路径点之间的角度变化来度量。假设我们希望轨迹尽可能接近一条直线,即相邻线段的夹角接近180°,我们可以定义平滑项为:

$$S(\mathbf{x})=\sum _{i=2}^{n-1}(1-\cos {\theta }_i)$$

其中${\theta }_i$是第$i-1$条线段和第$i$条线段的夹角:

$$\cos {\theta }_i=\frac{(x_i-x_{i-1})(x_{i+1}-x_i)+(y_i-y_{i-1})(y_{i+1}-y_i)}{d_{i-1}{d}_i}$$

这个函数也是非线性的,我们可以用泰勒展开来线性化:

$$\begin{array}{rl}S(\mathbf{x}+\Delta \mathbf{x})& \approx S(\mathbf{x})+\nabla S(\mathbf{x}{)}^T\Delta \mathbf{x}\\ & =S(\mathbf{x})+\sum _{i=2}^{n-1}\sin {\theta }_i(\frac{\partial {\theta }_i}{\partial x_{i-1}}\Delta x_{i-1}+\frac{\partial {\theta }_i}{\partial y_{i-1}}\Delta y_{i-1}+\frac{\partial {\theta }_i}{\partial x_i}\Delta x_i+\frac{\partial {\theta }_i}{\partial y_i}\Delta y_i+\frac{\partial {\theta }_i}{\partial x_{i+1}}\Delta x_{i+1}+\frac{\partial {\theta }_i}{\partial y_{i+1}}\Delta y_{i+1})\end{array}$$

其中$\sin {\theta }_i$和$\cos {\theta }_i$可以用上面的公式计算,而$\frac{\partial {\theta }_i}{\partial x_i}$等偏导数项可以通过求导得到,这里不再赘述。

碰撞项

碰撞项的设计是最复杂的,因为它需要考虑轨迹与障碍物之间的位置关系。一种简单的方法是把轨迹离散成一系列点,检查每个点是否在障碍物内部,如果是则给予惩罚。设障碍物$O_j$的签名距离函数为$d_j(x,y)$(在障碍物外部为正,内部为负),我们可以定义碰撞项为:

$$C(\mathbf{x})=\sum _{i=1}^n\sum _{j=1}^m\max (0,-d_j(x_i,y_i){)}^2$$

这个函数是光滑的,梯度为:

$$\nabla C(\mathbf{x})=\sum _{i=1}^n\sum _{j=1}^{m}2\max (0,-d_j(x_i,y_i))\nabla d_j(x_i,y_i)$$

其中$\nabla d_j(x,y)=[\frac{\partial d_j}{\partial x},\frac{\partial d_j}{\partial y}{]}^T$是$d_j$在$(x,y)$处的梯度。

优化求解

将三项合并,我们得到完整的目标函数:

$$\underset{\mathbf{\Delta x}}{\min }\nabla L(\mathbf{x}{)}^T\Delta \mathbf{x}+{\lambda }_s\nabla S(\mathbf{x}{)}^T\Delta \mathbf{x}+{\lambda }_c\nabla C(\mathbf{x}{)}^T\Delta \mathbf{x}$$

这是一个线性最小二乘问题,可以用最小二乘法求解:

$$\Delta \mathbf{x}=-(\nabla L(\mathbf{x})+{\lambda }_s\nabla S(\mathbf{x})+{\lambda }_c\nabla C(\mathbf{x}))$$

然后我们用梯度下降法来更新优化变量:

$$\mathbf{x}\leftarrow \mathbf{x}+\alpha \Delta \mathbf{x}$$

其中$\alpha$是步长,可以用线搜索或信赖域方法来自适应调节。

我们重复这个过程,直到$\Delta \mathbf{x}$的norm小于一个阈值,或者达到最大迭代次数。

工程实践及代码实现

我们可以使用scipy.optimize库来实现这个优化问题。scipy.optimize提供了多种优化算法,这里我们使用BFGS算法,它是一种拟牛顿法,利用梯度信息来近似Hessian矩阵,具有超线性收敛速度。

以下是使用scipy.optimize库实现的代码:

import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import minimize

# 障碍物签名距离函数
def signed_distance(x, y, obstacles):
    d = np.inf
    for obs in obstacles:
        d = min(d, np.sqrt((x-obs[0])**2 + (y-obs[1])**2) - obs[2])
    return d

# 计算路径长度
def path_length(xy):
    x, y = xy.reshape(2, -1)
    dx = x[1:] - x[:-1]
    dy = y[1:] - y[:-1]
    d = np.sqrt(dx**2 + dy**2)
    L = np.sum(d)
    return L

# 计算路径平滑度
def path_smoothness(xy):
    x, y = xy.reshape(2, -1)
    dx1 = x[1:-1] - x[:-2]
    dy1 = y[1:-1] - y[:-2] 
    dx2 = x[2:] - x[1:-1]
    dy2 = y[2:] - y[1:-1]
    
    cos_theta = (dx1*dx2 + dy1*dy2) / np.sqrt((dx1**2 + dy1**2) * (dx2**2 + dy2**2))
    cos_theta = np.clip(cos_theta, -1, 1)
    S = np.sum(1 - cos_theta)
    return S

# 计算路径与障碍物的碰撞代价
def path_collision(xy, obstacles):
    x, y = xy.reshape(2, -1)
    C = 0
    for i in range(len(x)):
        d = signed_distance(x[i], y[i], obstacles)
        if d < 0:
            C += d**2
    return C

# 总代价函数
def total_cost(xy, obstacles, lambda_smooth, lambda_collision):
    L = path_length(xy)
    S = path_smoothness(xy)
    C = path_collision(xy, obstacles)
    J = L + lambda_smooth * S + lambda_collision * C
    return J

# 优化主函数
def optimize_path(x0, y0, obstacles, lambda_smooth, lambda_collision):
    xy0 = np.concatenate([x0, y0])
    
    res = minimize(total_cost, xy0, args=(obstacles, lambda_smooth, lambda_collision), 
                   method='BFGS', options={'gtol': 1e-6, 'disp': True})
    
    xy = res.x
    x = xy[:len(x0)]
    y = xy[len(x0):]
    
    return x, y

# 测试
if __name__ == '__main__':
    # 障碍物列表,每个障碍物用(x,y,r)表示圆心和半径
    obstacles = [(-2, 0, 1), (0, 2, 1.5), (2, -1, 1)]
    
    # 起点和终点
    start = (-4, -2)
    end = (4, 2)
    
    # 初始路径(直线)
    x0 = np.linspace(start[0], end[0], 20)
    y0 = np.linspace(start[1], end[1], 20)
    
    # 优化参数
    lambda_smooth = 1.0
    lambda_collision = 10.0
    
    # 优化
    x, y = optimize_path(x0, y0, obstacles, lambda_smooth, lambda_collision)
    
    # 绘图
    fig, ax = plt.subplots(figsize=(8,6))
    
    for obs in obstacles:
        circle = plt.Circle((obs[0], obs[1]), obs[2], color='r', alpha=0.5)
        ax.add_patch(circle)
    
    ax.plot(x0, y0, 'g--', label='Initial path')    
    ax.plot(x, y, 'b-', label='Optimized path', zorder=10)
    ax.plot(start[0], start[1], 'go', label='Start')
    ax.plot(end[0], end[1], 'ro', label='End')
    
    ax.set_xlim(-5, 5)
    ax.set_ylim(-4, 4)
    ax.set_aspect('equal')
    ax.legend()
    
    plt.show()

运行这段代码,你应该可以看到优化后的轨迹(蓝色实线)成功避开了所有障碍物(红色圆圈),同时也比初始轨迹(绿色虚线)更加平滑。

使用scipy.optimize库可以大大简化优化问题的实现,同时也提供了多种优化算法供选择。这使得我们可以更加专注于问题的建模和求解,而不必过多关注优化算法的细节。

当然,对于更加复杂的优化问题,我们可能需要使用其他的优化库或算法,如IPOPT、NLopt、OSQP等。选择合适的优化工具需要根据问题的特点和需求来权衡。

问题描述

给定一个二维平面,平面上分布着一些圆形障碍物。我们的目标是规划一条从起点到终点的轨迹,使其满足以下要求:

1. 轨迹不与任何障碍物相交(避免碰撞)
2. 轨迹尽可能平滑,没有急转弯(保持平滑)
3. 轨迹尽可能短,减少不必要的绕路(最小化长度)

数学建模

我们可以将这个问题形式化为一个优化问题:

$$\underset{\mathbf{x},\mathbf{y}}{\min }L(\mathbf{x},\mathbf{y})+{\lambda }_{s}S(\mathbf{x},\mathbf{y})+{\lambda }_{c}C(\mathbf{x},\mathbf{y})$$

其中:

• $\mathbf{x},\mathbf{y}$分别是轨迹上点的x坐标和y坐标向量
• $L(\mathbf{x},\mathbf{y})$表示轨迹的长度
• $S(\mathbf{x},\mathbf{y})$表示轨迹的平滑度,可以用轨迹的曲率或加速度等量度
• $C(\mathbf{x},\mathbf{y})$表示轨迹与障碍物之间的碰撞代价
• ${\lambda }_s$和${\lambda }_c$是平滑项和碰撞项的权重系数

接下来,我们将分别定义这三个代价函数。

长度代价

轨迹的长度可以用相邻路径点之间的欧氏距离之和来表示:

$$L(\mathbf{x},\mathbf{y})=\sum _{i=1}^{n-1}\sqrt{(x_{i+1}-x_i{)}^2+(y_{i+1}-y_i{)}^2}$$

其中$n$是路径点的数量。

平滑代价

轨迹的平滑度可以用相邻线段之间的夹角来度量。假设我们希望轨迹尽可能接近一条直线,即相邻线段的夹角接近180°,我们可以定义平滑代价为:

$$S(\mathbf{x},\mathbf{y})=\sum _{i=2}^{n-1}(1-\cos {\theta }_i)$$

其中${\theta }_i$是第$i-1$条线段和第$i$条线段的夹角,可以用向量的点乘计算:

$$\cos {\theta }_i=\frac{(x_i-x_{i-1},y_i-y_{i-1})\cdot (x_{i+1}-x_i,y_{i+1}-y_i)}{\sqrt{(x_i-x_{i-1}{)}^2+(y_i-y_{i-1}{)}^2}\sqrt{(x_{i+1}-x_i{)}^2+(y_{i+1}-y_i{)}^2}}$$

碰撞代价

为了计算轨迹与障碍物之间的碰撞代价,我们首先定义障碍物的签名距离函数(signed distance function, SDF)。对于第$j$个障碍物,其SDF为:

$$d_j(x,y)=\sqrt{(x-x_j{)}^2+(y-y_j{)}^2}-r_j$$

其中$(x_j,y_j)$和$r_j$分别是障碍物的圆心坐标和半径。SDF的值在障碍物外部为正,内部为负,边界上为零。

有了SDF,我们可以将碰撞代价定义为:

$$C(\mathbf{x},\mathbf{y})=\sum _{i=1}^n\sum _{j=1}^m\max (0,-d_j(x_i,y_i){)}^2$$

其中$m$是障碍物的数量。这个函数的含义是,对于每个路径点,我们计算其到所有障碍物的SDF值,如果SDF为负(即路径点在障碍物内部),就累加一个惩罚项,惩罚项的大小与SDF的平方成正比。

基于梯度的数值优化

将长度、平滑和碰撞三项代价相加,我们得到了完整的优化目标函数。接下来,我们需要选择一个优化算法来求解这个问题。这里我们使用scipy.optimize库中的BFGS算法,它是一种拟牛顿法,利用梯度信息来近似Hessian矩阵,具有超线性收敛速度。

为了使用BFGS算法,我们需要将优化变量从二维数组$(\mathbf{x},\mathbf{y})$转化为一维向量$\mathbf{z}$:

$$\mathbf{z}=[{\mathbf{x}}^T,{\mathbf{y}}^T{]}^T$$

相应地,我们也需要将代价函数改写为关于$\mathbf{z}$的函数。这一点在代码实现中也体现了出来。

让我来分析一下这段代码:

1. 我们首先定义了几个辅助函数,用于计算路径的长度、平滑度和碰撞代价。这些函数对应了前面提到的三个代价项。注意,为了适应scipy.optimize的接口,我们将路径表示为一个一维向量xy,其中前半部分是x坐标,后半部分是y坐标。

2. 在total_cost函数中,我们将三个代价项加权求和,得到总的代价函数。这个函数将作为优化的目标函数。

3. optimize_path函数是优化的主函数。它先将起点和终点拼接成一个初始路径,然后调用scipy.optimize.minimize函数进行优化。我们选择了BFGS算法,并设置了一些优化选项,如终止条件的梯度阈值gtol和显示选项disp。

4. 优化得到的结果存储在res.x中,我们将其拆分为x坐标和y坐标,作为函数的返回值。

5. 在测试部分,我们设置了障碍物、起点和终点,以及一条初始路径(直线)。我们还设置了平滑代价和碰撞代价的权重系数。然后,我们调用optimize_path函数进行优化,并将结果绘制出来。

运行这段代码,你应该可以看到优化后的轨迹(蓝色实线)成功避开了所有障碍物(红色圆圈),同时也比初始轨迹(绿色虚线)更加平滑。

关于轨迹优化,以下是一些值得参考的文献:

• Zucker M, Ratliff N, Dragan A D, et al. CHOMP: Covariant Hamiltonian optimization for motion planning[J]. The International Journal of Robotics Research, 2013, 32(9-10): 1164-1193.[6]
• Schulman J, Duan Y, Ho J, et al. Motion planning with sequential convex optimization and convex collision checking[J]. The International Journal of Robotics Research, 2014, 33(9): 1251-1270.[7]
• Oleynikova H, Taylor Z, Fehr M, et al. Continuous-time trajectory optimization for online UAV replanning[C]//2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2016: 5332-5339.[8]
• Mukadam M, Yan X, Boots B. Gaussian process motion planning[C]//2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016: 9-15.[9]

这些文献提供了更多关于轨迹优化的理论和应用细节,感兴趣的读者可以进一步阅读。

常用优化器库

在实际应用中,我们通常不需要从头实现优化算法,而是可以使用现成的优化器库。以下是一些常用的优化器库:

• scipy.optimize: SciPy的优化子模块,提供了多种优化算法,如BFGS,L-BFGS-B,SLSQP,trust-constr等。
• IPOPT: 大规模非线性优化器,支持稀疏矩阵,适用于大型问题。
• NLopt: 非线性优化库,支持多种局部和全局优化算法。
• Ceres Solver: 来自Google的非线性最小二乘优化库,支持自动求导,常用于机器人,计算机视觉等领域。
• GTSAM: 来自佐治亚理工的图优化库,常用于SLAM,状态估计等问题。
• g2o: 来自图斯勒的通用图优化框架,常用于SLAM,BA等。
• OSQP: 二次规划优化器,适用于凸二次规划问题。

这些优化器库通常采用了比我们上面示例更加高效和鲁棒的算法,并提供了方便的接口。感兴趣的读者可以进一步了解和学习。

小结

优化是机器人领域中一个重要而广泛的话题。从运动规划到控制,从定位到建图,从感知到决策,优化无处不在。掌握优化的基本原理和常用方法,对于从事机器人相关研究和开发的人员来说是必不可少的。

实际的无人机或是机器人轨迹要复杂的多，我们可能需要在更高维度的空间中规划轨迹,可能需要考虑运动学和动力学约束,可能需要处理更复杂的环境和任务需求。这就需要我们在问题建模,约束处理,求解算法等方面有更深入的理解和更巧妙的设计。优化也不是万能的,有时候我们还需要与其他方法(如采样,搜索,学习等)结合,才能更好地解决问题。

如有任何问题和建议,欢迎交流和指正。