有一些朋友问我到底如何用优化方法实现轨迹优化（避障+轨迹平滑等），今天就出一个干货满满的教程，绝对是面向很多工业化场景的讲解，为了便于理解，我选用二维平面并给出详细代码实现，三维空间原理相似。
本教程禁止转载，主要是有问题可以联系我探讨，我的邮箱 fanzexuan135@163.com
下面提供的代码实际运行结果如下图（下面会有数学和代码的详解）
轨迹优化教程
本教程将详细介绍如何在二维平面上优化一条轨迹,使其避免碰撞并保持平滑。我们将从数学原理出发,逐步推导并给出代码实现。同时,我也会介绍一些常用的优化器库。
问题定义
假设我们在一个二维平面上有一组障碍物,我们的目标是在起点和终点之间规划一条轨迹,使其满足以下条件:
- 轨迹不与任何障碍物相交(避免碰撞)
- 轨迹尽可能平滑,没有急转弯(保持平滑)
- 轨迹尽可能短,减少不必要的绕路(最小化长度)
我们可以将这个问题形式化为一个优化问题:
min x L ( x ) + λ s S ( x ) + λ c C ( x ) \min_{\mathbf{x}} \quad L(\mathbf{x}) + \lambda_s S(\mathbf{x}) + \lambda_c C(\mathbf{x}) xminL(x)+λsS(x)+λcC(x)
其中:
- x \mathbf{x} x是轨迹的参数化表示,例如一系列的路径点坐标
- L ( x ) L(\mathbf{x}) L(x)表示轨迹的长度
- S ( x ) S(\mathbf{x}) S(x)表示轨迹的平滑度,可以用轨迹的曲率或加速度等量度
- C ( x ) C(\mathbf{x}) C(x)表示轨迹与障碍物之间的碰撞代价
- λ s \lambda_s λs和 λ c \lambda_c λc是平滑项和碰撞项的权重系数
轨迹参数化
为了优化轨迹,我们首先需要选择一种方式来参数化轨迹。常见的方法有:
- 路径点参数化:用一系列的路径点 ( x i , y i ) (x_i, y_i) (xi,yi)来表示轨迹,路径点之间可以用直线或曲线连接
- 样条曲线参数化:用贝塞尔曲线、B样条曲线等来表示轨迹,控制点的坐标作为优化变量
- 函数参数化:用一个显式或隐式的函数 f ( x , y ) = 0 f(x,y)=0 f(x,y)=0来表示轨迹,函数的参数作为优化变量
这里我们选择路径点参数化,因为它简单直观,易于实现。假设我们用 n n n个路径点来表示轨迹,优化变量为:
x = [ x 1 , y 1 , x 2 , y 2 , … , x n , y n ] T \mathbf{x} = [x_1, y_1, x_2, y_2, \dots, x_n, y_n]^T x=[x1,y1,x2,y2,…,xn,yn]T
其中 ( x 1 , y 1 ) (x_1, y_1) (x1,y1)和 ( x n , y n ) (x_n, y_n) (xn,yn)分别为起点和终点的坐标,是固定的。
目标函数构建
接下来,我们要为每一项设计合适的目标函数。
长度项
轨迹的长度可以用路径点之间的欧氏距离之和来近似:
L ( x ) = ∑ i = 1 n − 1 ( x i + 1 − x i ) 2 + ( y i + 1 − y i ) 2 L(\mathbf{x}) = \sum_{i=1}^{n-1} \sqrt{(x_{i+1}-x_i)^2 + (y_{i+1}-y_i)^2} L(x)=i=1∑n−1(xi+1−xi)2+(yi+1−yi)2
这个函数是非线性的,但我们可以用泰勒展开来线性化:
L ( x + Δ x ) ≈ L ( x ) + ∇ L ( x ) T Δ x = L ( x ) + ∑ i = 1 n − 1 x i + 1 − x i d i Δ x i + y i + 1 − y i d i Δ y i \begin{aligned} 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{aligned} L(x+Δx)≈L(x)+∇L(x)TΔx=L(x)+i=1∑n−1dixi+1−xiΔxi+diyi+1−yiΔyi
其中 d i = ( x i + 1 − x i ) 2 + ( y i + 1 − y i ) 2 d_i = \sqrt{(x_{i+1}-x_i)^2 + (y_{i+1}-y_i)^2} di=(xi+1−xi)2+(yi+1−yi)2。这样长度项就变成了关于 Δ x \Delta \mathbf{x} Δx的线性函数。
平滑项
轨迹的平滑度可以用相邻路径点之间的角度变化来度量。假设我们希望轨迹尽可能接近一条直线,即相邻线段的夹角接近180°,我们可以定义平滑项为:
S ( x ) = ∑ i = 2 n − 1 ( 1 − cos θ i ) S(\mathbf{x}) = \sum_{i=2}^{n-1} (1-\cos\theta_i) S(x)=i=2∑n−1(1−cosθi)
其中 θ i \theta_i θi是第 i − 1 i-1 i−1条线段和第 i i i条线段的夹角:
cos θ i = ( 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 \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} cosθi=di−1di(xi−xi−1)(xi+1−xi)+(yi−yi−1)(yi+1−yi)
这个函数也是非线性的,我们可以用泰勒展开来线性化:
S ( x + Δ x ) ≈ S ( x ) + ∇ S ( x ) T Δ x = S ( x ) + ∑ i = 2 n − 1 sin θ i ( ∂ θ i ∂ x i − 1 Δ x i − 1 + ∂ θ i ∂ y i − 1 Δ y i − 1 + ∂ θ i ∂ x i Δ x i + ∂ θ i ∂ y i Δ y i + ∂ θ i ∂ x i + 1 Δ x i + 1 + ∂ θ i ∂ y i + 1 Δ y i + 1 ) \begin{aligned} 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{aligned} S(x+Δx)≈S(x)+∇S(x)TΔx=S(x)+i=2∑n−1sinθi(∂xi−1∂θiΔxi−1+∂yi−1∂θiΔyi−1+∂xi∂θiΔxi+∂yi∂θiΔyi+∂xi+1∂θiΔxi+1+∂yi+1∂θiΔyi+1)
其中 sin θ i \sin\theta_i sinθi和 cos θ i \cos\theta_i cosθi可以用上面的公式计算,而 ∂ θ i ∂ x i \frac{\partial \theta_i}{\partial x_i} ∂xi∂θi等偏导数项可以通过求导得到,这里不再赘述。
碰撞项
碰撞项的设计是最复杂的,因为它需要考虑轨迹与障碍物之间的位置关系。一种简单的方法是把轨迹离散成一系列点,检查每个点是否在障碍物内部,如果是则给予惩罚。设障碍物 O j O_j Oj的签名距离函数为 d j ( x , y ) d_j(x,y) dj(x,y)(在障碍物外部为正,内部为负),我们可以定义碰撞项为:
C ( x ) = ∑ i = 1 n ∑ j = 1 m max ( 0 , − d j ( x i , y i ) ) 2 C(\mathbf{x}) = \sum_{i=1}^n \sum_{j=1}^m \max(0, -d_j(x_i,y_i))^2 C(x)=i=1∑nj=1∑mmax(0,−dj(xi,yi))2
这个函数是光滑的,梯度为:
∇ C ( x ) = ∑ i = 1 n ∑ j = 1 m 2 max ( 0 , − d j ( x i , y i ) ) ∇ d j ( x i , y i ) \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) ∇C(x)=i=1∑nj=1∑m2max(0,−dj(xi,yi))∇dj(xi,yi)
其中 ∇ d j ( x , y ) = [ ∂ d j ∂ x , ∂ d j ∂ y ] T \nabla d_j(x,y) = [\frac{\partial d_j}{\partial x}, \frac{\partial d_j}{\partial y}]^T ∇dj(x,y)=[∂x∂dj,∂y∂dj]T是 d j d_j dj在 ( x , y ) (x,y) (x,y)处的梯度。
优化求解
将三项合并,我们得到完整的目标函数:
min Δ x ∇ L ( x ) T Δ x + λ s ∇ S ( x ) T Δ x + λ c ∇ C ( x ) T Δ x \min_{\mathbf{\Delta x}} \quad \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} Δxmin∇L(x)TΔx+λs∇S(x)TΔx+λc∇C(x)TΔx
这是一个线性最小二乘问题,可以用最小二乘法求解:
Δ x = − ( ∇ L ( x ) + λ s ∇ S ( x ) + λ c ∇ C ( x ) ) \Delta \mathbf{x} = -(\nabla L(\mathbf{x}) + \lambda_s \nabla S(\mathbf{x}) + \lambda_c \nabla C(\mathbf{x})) Δx=−(∇L(x)+λs∇S(x)+λc∇C(x))
然后我们用梯度下降法来更新优化变量:
x ← x + α Δ x \mathbf{x} \leftarrow \mathbf{x} + \alpha \Delta \mathbf{x} x←x+αΔx
其中 α \alpha α是步长,可以用线搜索或信赖域方法来自适应调节。
我们重复这个过程,直到 Δ x \Delta \mathbf{x} Δ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等。选择合适的优化工具需要根据问题的特点和需求来权衡。
问题描述
给定一个二维平面,平面上分布着一些圆形障碍物。我们的目标是规划一条从起点到终点的轨迹,使其满足以下要求:
- 轨迹不与任何障碍物相交(避免碰撞)
- 轨迹尽可能平滑,没有急转弯(保持平滑)
- 轨迹尽可能短,减少不必要的绕路(最小化长度)
数学建模
我们可以将这个问题形式化为一个优化问题:
min x , y L ( x , y ) + λ s S ( x , y ) + λ c C ( x , y ) \min_{\mathbf{x},\mathbf{y}} \quad L(\mathbf{x},\mathbf{y}) + \lambda_s S(\mathbf{x},\mathbf{y}) + \lambda_c C(\mathbf{x},\mathbf{y}) x,yminL(x,y)+λsS(x,y)+λcC(x,y)
其中:
- x , y \mathbf{x},\mathbf{y} x,y分别是轨迹上点的x坐标和y坐标向量
- L ( x , y ) L(\mathbf{x},\mathbf{y}) L(x,y)表示轨迹的长度
- S ( x , y ) S(\mathbf{x},\mathbf{y}) S(x,y)表示轨迹的平滑度,可以用轨迹的曲率或加速度等量度
- C ( x , y ) C(\mathbf{x},\mathbf{y}) C(x,y)表示轨迹与障碍物之间的碰撞代价
- λ s \lambda_s λs和 λ c \lambda_c λc是平滑项和碰撞项的权重系数
接下来,我们将分别定义这三个代价函数。
长度代价
轨迹的长度可以用相邻路径点之间的欧氏距离之和来表示:
L ( x , y ) = ∑ i = 1 n − 1 ( x i + 1 − x i ) 2 + ( y i + 1 − y i ) 2 L(\mathbf{x},\mathbf{y}) = \sum_{i=1}^{n-1} \sqrt{(x_{i+1}-x_i)^2 + (y_{i+1}-y_i)^2} L(x,y)=i=1∑n−1(xi+1−xi)2+(yi+1−yi)2
其中 n n n是路径点的数量。
平滑代价
轨迹的平滑度可以用相邻线段之间的夹角来度量。假设我们希望轨迹尽可能接近一条直线,即相邻线段的夹角接近180°,我们可以定义平滑代价为:
S ( x , y ) = ∑ i = 2 n − 1 ( 1 − cos θ i ) S(\mathbf{x},\mathbf{y}) = \sum_{i=2}^{n-1} (1-\cos\theta_i) S(x,y)=i=2∑n−1(1−cosθi)
其中 θ i \theta_i θi是第 i − 1 i-1 i−1条线段和第 i i i条线段的夹角,可以用向量的点乘计算:
cos θ i = ( x i − x i − 1 , y i − y i − 1 ) ⋅ ( x i + 1 − x i , y i + 1 − y i ) ( x i − x i − 1 ) 2 + ( y i − y i − 1 ) 2 ( x i + 1 − x i ) 2 + ( y i + 1 − y i ) 2 \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}} cosθi=(xi−xi−1)2+(yi−yi−1)2(xi+1−xi)2+(yi+1−yi)2(xi−xi−1,yi−yi−1)⋅(xi+1−xi,yi+1−yi)
碰撞代价
为了计算轨迹与障碍物之间的碰撞代价,我们首先定义障碍物的签名距离函数(signed distance function, SDF)。对于第 j j j个障碍物,其SDF为:
d j ( x , y ) = ( x − x j ) 2 + ( y − y j ) 2 − r j d_j(x,y) = \sqrt{(x-x_j)^2+(y-y_j)^2} - r_j dj(x,y)=(x−xj)2+(y−yj)2−rj
其中 ( x j , y j ) (x_j,y_j) (xj,yj)和 r j r_j rj分别是障碍物的圆心坐标和半径。SDF的值在障碍物外部为正,内部为负,边界上为零。
有了SDF,我们可以将碰撞代价定义为:
C ( x , y ) = ∑ i = 1 n ∑ j = 1 m max ( 0 , − d j ( x i , y i ) ) 2 C(\mathbf{x},\mathbf{y}) = \sum_{i=1}^n \sum_{j=1}^m \max(0, -d_j(x_i,y_i))^2 C(x,y)=i=1∑nj=1∑mmax(0,−dj(xi,yi))2
其中 m m m是障碍物的数量。这个函数的含义是,对于每个路径点,我们计算其到所有障碍物的SDF值,如果SDF为负(即路径点在障碍物内部),就累加一个惩罚项,惩罚项的大小与SDF的平方成正比。
基于梯度的数值优化
将长度、平滑和碰撞三项代价相加,我们得到了完整的优化目标函数。接下来,我们需要选择一个优化算法来求解这个问题。这里我们使用scipy.optimize库中的BFGS算法,它是一种拟牛顿法,利用梯度信息来近似Hessian矩阵,具有超线性收敛速度。
为了使用BFGS算法,我们需要将优化变量从二维数组 ( x , y ) (\mathbf{x},\mathbf{y}) (x,y)转化为一维向量 z \mathbf{z} z:
z = [ x T , y T ] T \mathbf{z} = [\mathbf{x}^T, \mathbf{y}^T]^T z=[xT,yT]T
相应地,我们也需要将代价函数改写为关于 z \mathbf{z} z的函数。这一点在代码实现中也体现了出来。
让我来分析一下这段代码:
-
我们首先定义了几个辅助函数,用于计算路径的长度、平滑度和碰撞代价。这些函数对应了前面提到的三个代价项。注意,为了适应scipy.optimize的接口,我们将路径表示为一个一维向量xy,其中前半部分是x坐标,后半部分是y坐标。
-
在total_cost函数中,我们将三个代价项加权求和,得到总的代价函数。这个函数将作为优化的目标函数。
-
optimize_path函数是优化的主函数。它先将起点和终点拼接成一个初始路径,然后调用scipy.optimize.minimize函数进行优化。我们选择了BFGS算法,并设置了一些优化选项,如终止条件的梯度阈值gtol和显示选项disp。
-
优化得到的结果存储在res.x中,我们将其拆分为x坐标和y坐标,作为函数的返回值。
-
在测试部分,我们设置了障碍物、起点和终点,以及一条初始路径(直线)。我们还设置了平滑代价和碰撞代价的权重系数。然后,我们调用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: 二次规划优化器,适用于凸二次规划问题。
这些优化器库通常采用了比我们上面示例更加高效和鲁棒的算法,并提供了方便的接口。感兴趣的读者可以进一步了解和学习。
小结
优化是机器人领域中一个重要而广泛的话题。从运动规划到控制,从定位到建图,从感知到决策,优化无处不在。掌握优化的基本原理和常用方法,对于从事机器人相关研究和开发的人员来说是必不可少的。
实际的无人机或是机器人轨迹要复杂的多，我们可能需要在更高维度的空间中规划轨迹,可能需要考虑运动学和动力学约束,可能需要处理更复杂的环境和任务需求。这就需要我们在问题建模,约束处理,求解算法等方面有更深入的理解和更巧妙的设计。优化也不是万能的,有时候我们还需要与其他方法(如采样,搜索,学习等)结合,才能更好地解决问题。
如有任何问题和建议,欢迎交流和指正。
