赞
踩
有一些朋友问我到底如何用优化方法实现轨迹优化(避障+轨迹平滑等)
,今天就出一个干货满满的教程,绝对是面向很多工业化场景的讲解,为了便于理解,我选用二维平面并给出详细代码实现,三维空间原理相似。
本教程禁止转载,主要是有问题可以联系我探讨,我的邮箱 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)
其中:
为了优化轨迹,我们首先需要选择一种方式来参数化轨迹。常见的方法有:
这里我们选择路径点参数化,因为它简单直观,易于实现。假设我们用 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
其中 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
)
其中 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)
其中:
接下来,我们将分别定义这三个代价函数。
轨迹的长度可以用相邻路径点之间的欧氏距离之和来表示:
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函数进行优化,并将结果绘制出来。
运行这段代码,你应该可以看到优化后的轨迹(蓝色实线)成功避开了所有障碍物(红色圆圈),同时也比初始轨迹(绿色虚线)更加平滑。
关于轨迹优化,以下是一些值得参考的文献:
这些文献提供了更多关于轨迹优化的理论和应用细节,感兴趣的读者可以进一步阅读。
在实际应用中,我们通常不需要从头实现优化算法,而是可以使用现成的优化器库。以下是一些常用的优化器库:
这些优化器库通常采用了比我们上面示例更加高效和鲁棒的算法,并提供了方便的接口。感兴趣的读者可以进一步了解和学习。
优化是机器人领域中一个重要而广泛的话题。从运动规划到控制,从定位到建图,从感知到决策,优化无处不在。掌握优化的基本原理和常用方法,对于从事机器人相关研究和开发的人员来说是必不可少的。
实际的无人机或是机器人轨迹要复杂的多,我们可能需要在更高维度的空间中规划轨迹,可能需要考虑运动学和动力学约束,可能需要处理更复杂的环境和任务需求。这就需要我们在问题建模,约束处理,求解算法等方面有更深入的理解和更巧妙的设计。优化也不是万能的,有时候我们还需要与其他方法(如采样,搜索,学习等)结合,才能更好地解决问题。
如有任何问题和建议,欢迎交流和指正。
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。