轨迹优化教程

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

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

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

轨迹优化教程

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

问题定义

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

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

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

min⁡xL(x)+λsS(x)+λcC(x)

\min_{\mathbf{x}} \quad L(\mathbf{x}) + \lambda_s S(\mathbf{x}) + \lambda_c C(\mathbf{x})

xmin​L(x)+λs​S(x)+λc​C(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​是平滑项和碰撞项的权重系数

轨迹参数化

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

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

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

x=[x1,y1,x2,y2,…,xn,yn]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

其中(x1,y1)(x_1, y_1)(x1​,y1​)和(xn,yn)(x_n, y_n)(xn​,yn​)分别为起点和终点的坐标,是固定的。

目标函数构建

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

长度项

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

L(x)=∑i=1n−1(xi+1−xi)2+(yi+1−yi)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=1n−1xi+1−xidiΔxi+yi+1−yidiΔyi

\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−1​di​xi+1​−xi​​Δxi​+di​yi+1​−yi​​Δyi​​

其中di=(xi+1−xi)2+(yi+1−yi)2d_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=2n−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−1i-1i−1条线段和第iii条线段的夹角:

cos⁡θi=(xi−xi−1)(xi+1−xi)+(yi−yi−1)(yi+1−yi)di−1di

\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−1​di​(xi​−xi−1​)(xi+1​−xi​)+(yi​−yi−1​)(yi+1​−yi​)​

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

S(x+Δx)≈S(x)+∇S(x)TΔx=S(x)+∑i=2n−1sin⁡θi(∂θi∂xi−1Δxi−1+∂θi∂yi−1Δyi−1+∂θi∂xiΔxi+∂θi∂yiΔyi+∂θi∂xi+1Δxi+1+∂θi∂yi+1Δyi+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−1​sinθ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_isinθi​和cos⁡θi\cos\theta_icosθi​可以用上面的公式计算,而∂θi∂xi\frac{\partial \theta_i}{\partial x_i}∂xi​∂θi​​等偏导数项可以通过求导得到,这里不再赘述。

碰撞项

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

C(x)=∑i=1n∑j=1mmax⁡(0,−dj(xi,yi))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∑n​j=1∑m​max(0,−dj​(xi​,yi​))2

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

∇C(x)=∑i=1n∑j=1m2max⁡(0,−dj(xi,yi))∇dj(xi,yi)

\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∑n​j=1∑m​2max(0,−dj​(xi​,yi​))∇dj​(xi​,yi​)

其中∇dj(x,y)=[∂dj∂x,∂dj∂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是djd_jdj​在(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,yL(x,y)+λsS(x,y)+λcC(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,ymin​L(x,y)+λs​S(x,y)+λc​C(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=1n−1(xi+1−xi)2+(yi+1−yi)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​

其中nnn是路径点的数量。

平滑代价

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

S(x,y)=∑i=2n−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−1i-1i−1条线段和第iii条线段的夹角,可以用向量的点乘计算:

cos⁡θi=(xi−xi−1,yi−yi−1)⋅(xi+1−xi,yi+1−yi)(xi−xi−1)2+(yi−yi−1)2(xi+1−xi)2+(yi+1−yi)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)。对于第jjj个障碍物,其SDF为:

dj(x,y)=(x−xj)2+(y−yj)2−rj

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​

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

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

C(x,y)=∑i=1n∑j=1mmax⁡(0,−dj(xi,yi))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∑n​j=1∑m​max(0,−dj​(xi​,yi​))2

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

基于梯度的数值优化

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

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

z=[xT,yT]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: 二次规划优化器,适用于凸二次规划问题。

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

小结

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

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

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