【路径规划】局部路径规划算法——曲线插值法
文章目录
- 参考资料
- 1. 算法简介
- 2. 算法精讲
-
- 2.1 多项式曲线
- 2.2 示例——五次多项式曲线
- 2.3 双圆弧段曲线
- 3. python代码实现
参考资料
- 路径规划与轨迹跟踪系列算法
1. 算法简介
-
曲线插值的方法是按照车辆在某些特定条件（安全、快速、高效）下， 进行路径的曲线拟合，常见的有多项式曲线、双圆弧段曲线、正弦函数曲线、贝塞尔曲线、 B样条曲线等。
-
曲线插值法的核心思想就是基于预先构造的曲线类型，根据车辆期望达到的状态（比如要求车辆到达某点的速度和加速度为期望值），将此期望值作为边界条件代入曲线类型进行方程求解，获得曲线的相关系数（简单地说就是待定系数法！）。
-
曲线所有的相关系数一旦确定，轨迹规划随之完成。
2. 算法精讲
2.1 多项式曲线
下面以多项式曲线为例讲解曲线插值法轨迹规划。
多项式曲线分为三次多项式曲线、 五次多项式曲线、 七次多项式曲线
- 针对三次多项式曲线，最多能确定每一个期望点的两个维度的期望状态，一般来说就是位置和速度。
{ x ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 y ( t ) = b 0 + b 1 t + b 2 t 2 + b 3 t 3 (1) \tag{1} \left\{\begin{array}{l} x(t)=a_{0}+a_{1} t+a_{2} t^{2}+a_{3} t^{3} \\ y(t)=b_{0}+b_{1} t+b_{2} t^{2}+b_{3} t^{3} \end{array}\right. {x(t)=a0+a1t+a2t2+a3t3y(t)=b0+b1t+b2t2+b3t3(1)
- 针对五次多项式曲线，最多能确定每一个期望点的三个维度的期望状态，一般来说就是位置、速度、加速度。
{ x ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 y ( t ) = b 0 + b 1 t + b 2 t 2 + b 3 t 3 + b 4 t 4 + b 5 t 5 (2) \tag{2} \left\{\begin{array}{l} x(t)=a_{0}+a_{1} t+a_{2} t^{2}+a_{3} t^{3}+a_{4} t^{4}+a_{5} t^{5} \\ y(t)=b_{0}+b_{1} t+b_{2} t^{2}+b_{3} t^{3}+b_{4} t^{4}+b_{5} t^{5} \end{array}\right. {x(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5y(t)=b0+b1t+b2t2+b3t3+b4t4+b5t5(2)
- 针对七次多项式曲线，最多能确定每一个期望点的四个维度的期望状态，一般来说就是位置、速度、 加速度、加加速度（加加速度称为
jerk，加加加速度称为snap，无人机轨迹规划中有用到snap）
{ x ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 + a 6 t 6 + a 7 t 7 y ( t ) = b 0 + b 1 t + b 2 t 2 + b 3 t 3 + b 4 t 4 + b 5 t 5 + b 6 t 6 + b 7 t 7 (3) \tag{3} \left\{\begin{array}{l} x(t)=a_{0}+a_{1} t+a_{2} t^{2}+a_{3} t^{3}+a_{4} t^{4}+a_{5} t^{5}+a_{6} t^{6}+a_{7} t^{7} \\ y(t)=b_{0}+b_{1} t+b_{2} t^{2}+b_{3} t^{3}+b_{4} t^{4}+b_{5} t^{5}+b_{6} t^{6}+b_{7} t^{7} \end{array}\right. {x(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5+a6t6+a7t7y(t)=b0+b1t+b2t2+b3t3+b4t4+b5t5+b6t6+b7t7(3)
2.2 示例——五次多项式曲线
以五次多项式曲线为例讲解曲线揷值法轨迹规划。
设 t 0 t_0 t0为初始时间，位置、速度、加速度均已知，显然 x 和 y x和y x和y方向分别有以下三个方程：
- 位置
{ x ( t 0 ) = a 0 + a 1 t 0 + a 2 t 0 2 + a 3 t 0 3 + a 4 t 0 4 + a 5 t 0 5 y ( t 0 ) = b 0 + b 1 t 0 + b 2 t 0 2 + b 3 t 0 3 + b 4 t 0 4 + b 5 t 0 5 (4) \tag{4} \left\{\begin{array}{l}x\left(t_{0}\right)=a_{0}+a_{1} t_{0}+a_{2} t_{0}^{2}+a_{3} t_{0}^{3}+a_{4} t_{0}^{4}+a_{5} t_{0}^{5} \\ y\left(t_{0}\right)=b_{0}+b_{1} t_{0}+b_{2} t_{0}^{2}+b_{3} t_{0}^{3}+b_{4} t_{0}^{4}+b_{5} t_{0}^{5}\end{array} \quad\right. {x(t0)=a0+a1t0+a2t02+a3t03+a4t04+a5t05y(t0)=b0+b1t0+b2t02+b3t03+b4t04+b5t05(4) - 速度
{ x ′ ( t 0 ) = a 1 + 2 t 0 a 2 + 3 t 0 2 a 3 + 4 t 0 3 a 4 + 5 t 0 4 a 5 y ′ ( t 0 ) = b 1 + 2 t 0 b 2 + 3 t 0 2 b 3 + 4 t 0 3 b 4 + 5 t 0 4 b 5 (5) \tag{5} \left\{\begin{array}{l}x^{\prime}\left(t_{0}\right)=a_{1}+2 t_{0} a_{2}+3 t_{0}{ }^{2} a_{3}+4 t_{0}{ }^{3} a_{4}+5 t_{0}{ }^{4} a_{5} \\ y^{\prime}\left(t_{0}\right)=b_{1}+2 t_{0} b_{2}+3 t_{0}{ }^{2} b_{3}+4 t_{0}{ }^{3} b_{4}+5 t_{0}{ }^{4} b_{5}\end{array} \quad\right. {x′(t0)=a1+2t0a2+3t02a3+4t03a4+5t04a5y′(t0)=b1+2t0b2+3t02b3+4t03b4+5t04b5(5) - 加速度
{ x ′ ′ ( t 0 ) = 2 a 2 + 6 t 0 a 3 + 12 t 0 2 a 4 + 20 t 0 3 a 5 y ′ ′ ( t 0 ) = 2 b 2 + 6 t 0 b 3 + 12 t 0 2 b 4 + 20 t 0 3 b 5 (6) \tag{6} \left\{\begin{array}{l}x^{\prime \prime}\left(t_{0}\right)=2 a_{2}+6 t_{0} a_{3}+12 t_{0}{ }^{2} a_{4}+20 t_{0}{ }^{3} a_{5} \\ y^{\prime \prime}\left(t_{0}\right)=2 b_{2}+6 t_{0} b_{3}+12 t_{0}{ }^{2} b_{4}+20 t_{0}{ }^{3} b_{5}\end{array} \quad\right. {x′′(t0)=2a2+6t0a3+12t02a4+20t03a5y′′(t0)=2b2+6t0b3+12t02b4+20t03b5(6)
定义换道终点时间为 t 1 t_1 t1 ，横纵向(即 x , y x,y x,y方向)均有期望的位置、速度、 加速度，又分别可以得到以下三个方程：
- 位置
{ x ( t 1 ) = a 0 + a 1 t 1 + a 2 t 1 2 + a 3 t 1 3 + a 4 t 1 4 + a 5 t 1 5 y ( t 1 ) = b 0 + b 1 t 1 + b 2 t 1 2 + b 3 t 1 3 + b 4 t 1 4 + b 5 t 1 5 (7) \tag{7} \left\{\begin{array}{l}x\left(t_{1}\right)=a_{0}+a_{1} t_{1}+a_{2} t_{1}^{2}+a_{3} t_{1}^{3}+a_{4} t_{1}^{4}+a_{5} t_{1}^{5} \\ y\left(t_{1}\right)=b_{0}+b_{1} t_{1}+b_{2} t_{1}^{2}+b_{3} t_{1}^{3}+b_{4} t_{1}^{4}+b_{5} t_{1}^{5}\end{array} \quad\right. {x(t1)=a0+a1t1+a2t12+a3t13+a4t14+a5t15y(t1)=b0+b1t1+b2t12+b3t13+b4t14+b5t15(7) - 速度
{ x ′ ( t 1 ) = a 1 + 2 t 1 a 2 + 3 t 1 2 a 3 + 4 t 1 3 a 4 + 5 t 1 4 a 5 y ′ ( t 1 ) = b 1 + 2 t 1 b 2 + 3 t 1 2 b 3 + 4 t 1 3 b 4 + 5 t 1 4 b 5 (8) \tag{8} \left\{\begin{array}{l}x^{\prime}\left(t_{1}\right)=a_{1}+2 t_{1} a_{2}+3 t_{1}{ }^{2} a_{3}+4 t_{1}{ }^{3} a_{4}+5 t_{1}{ }^{4} a_{5} \\ y^{\prime}\left(t_{1}\right)=b_{1}+2 t_{1} b_{2}+3 t_{1}{ }^{2} b_{3}+4 t_{1}{ }^{3} b_{4}+5 t_{1}{ }^{4} b_{5}\end{array} \quad\right. {x′(t1)=a1+2t1a2+3t12a3+4t13a4+5t14a5y′(t1)=b1+2t1b2+3t12b3+4t13b4+5t14b5(8) - 加速度
{ x ′ ′ ( t 1 ) = 2 a 2 + 6 t 1 a 3 + 12 t 1 2 a 4 + 20 t 1 3 a 5 y ′ ′ ( t 1 ) = 2 b 2 + 6 t 1 b 3 + 12 t 1 2 b 4 + 20 t 1 3 b 5 (9) \tag{9} \left\{\begin{array}{l}x^{\prime \prime}\left(t_{1}\right)=2 a_{2}+6 t_{1} a_{3}+12 t_{1}{ }^{2} a_{4}+20 t_{1}{ }^{3} a_{5} \\ y^{\prime \prime}\left(t_{1}\right)=2 b_{2}+6 t_{1} b_{3}+12 t_{1}{ }^{2} b_{4}+20 t_{1}{ }^{3} b_{5}\end{array} \quad\right. {x′′(t1)=2a2+6t1a3+12t12a4+20t13a5y′′(t1)=2b2+6t1b3+12t12b4+20t13b5(9)
把起末两点的横纵向方程统一用矩阵表达为：
X = [ x 0 x 0 ′ x 0 ′ ′ x 1 x 1 ′ x 1 ′ ′ ] = [ t 0 5 t 0 4 t 0 3 t 0 2 t 0 1 5 t 0 4 4 t 0 3 3 t 0 2 2 t 0 1 0 20 t 0 3 12 t 0 2 6 t 0 2 0 0 t 1 5 t 1 4 t 1 3 t 1 2 t 1 1 5 t 1 4 4 t 1 3 3 t 1 2 2 t 1 1 0 20 t 1 3 12 t 1 2 6 t 1 2 0 0 ] [ a 5 a 4 a 3 a 2 a 1 a 0 ] = T × A Y = [ y 0 y 0 ′ y 0 ′ ′ y 1 y 1 ′ y 1 ′ ′ ] = [ t 0 5 t 0 4 t 0 3 t 0 2 t 0 1 5 t 0 4 4 t 0 3 3 t 0 2 2 t 0 1 0 20 t 0 3 12 t 0 2 6 t 0 2 0 0 t 1 5 t 1 4 t 1 3 t 1 2 t 1 1 5 t 1 4 4 t 1 3 3 t 1 2 2 t 1 1 0 20 t 1 3 12 t 1 2 6 t 1 2 0 0 ] [ b 5 b 4 b 3 b 2 b 1 b 0 ] = T × B (10) \tag{10} \begin{aligned} &X=\left[\begin{array}{l} x_{0} \\ x_{0}^{\prime} \\ x_{0}^{\prime \prime} \\ x_{1} \\ x_{1}^{\prime} \\ x_{1}^{\prime \prime} \end{array}\right]=\left[\begin{array}{llllll} t_{0}{ }^{5} & t_{0}{ }^{4} & t_{0}{ }^{3} & t_{0}{ }^{2} & t_{0} & 1 \\ 5 t_{0}{ }^{4} & 4 t_{0}{ }^{3} & 3 t_{0}{ }^{2} & 2 t_{0} & 1 & 0 \\ 20 t_{0}{ }^{3} & 12 t_{0}{ }^{2} & 6 t_{0} & 2 & 0 & 0 \\ t_{1}^{5} & t_{1}{ }^{4} & t_{1}{ }^{3} & t_{1}{ }^{2} & t_{1} & 1 \\ 5 t_{1}^{4} & 4 t_{1}{ }^{3} & 3 t_{1}{ }^{2} & 2 t_{1} & 1 & 0 \\ 20 t_{1}{ }^{3} & 12 t_{1}{ }^{2} & 6 t_{1} & 2 & 0 & 0 \end{array}\right]\left[\begin{array}{l} a_{5} \\ a_{4} \\ a_{3} \\ a_{2} \\ a_{1} \\ a_{0} \end{array}\right]=T \times A\\ \\ &Y=\left[\begin{array}{l} y_{0} \\ y_{0}^{\prime} \\ y_{0}^{\prime \prime} \\ y_{1} \\ y_{1}^{\prime} \\ y_{1}^{\prime \prime} \end{array}\right]=\left[\begin{array}{llllll} t_{0}{ }^{5} & t_{0}{ }^{4} & t_{0}{ }^{3} & t_{0}{ }^{2} & t_{0} & 1 \\ 5 t_{0}{ }^{4} & 4 t_{0}{ }^{3} & 3 t_{0}{ }^{2} & 2 t_{0} & 1 & 0 \\ 20 t_{0}{ }^{3} & 12 t_{0}{ }^{2} & 6 t_{0} & 2 & 0 & 0 \\ t_{1}^{5} & t_{1}{ }^{4} & t_{1}{ }^{3} & t_{1}{ }^{2} & t_{1} & 1 \\ 5 t_{1}^{4} & 4 t_{1}{ }^{3} & 3 t_{1}{ }^{2} & 2 t_{1} & 1 & 0 \\ 20 t_{1}{ }^{3} & 12 t_{1}{ }^{2} & 6 t_{1} & 2 & 0 & 0 \end{array}\right]\left[\begin{array}{l} b_{5} \\ b_{4} \\ b_{3} \\ b_{2} \\ b_{1} \\ b_{0} \end{array}\right]=T \times B \end{aligned} X=⎣⎢⎢⎢⎢⎢⎢⎡x0x0′x0′′x1x1′x1′′⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡t055t0420t03t155t1420t13t044t0312t02t144t1312t12t033t026t0t133t126t1t022t02t122t12t010t110100100⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡a5a4a3a2a1a0⎦⎥⎥⎥⎥⎥⎥⎤=T×AY=⎣⎢⎢⎢⎢⎢⎢⎡y0y0′y0′′y1y1′y1′′⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡t055t0420t03t155t1420t13t044t0312t02t144t1312t12t033t026t0t133t126t1t022t02t122t12t010t110100100⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡b5b4b3b2b1b0⎦⎥⎥⎥⎥⎥⎥⎤=T×B(10)
多项式曲线的自变量为时间 t t t，故一旦求解出系数矩阵 A , B A,B A,B，即可确定曲线方程（说白了，求系数的方法就是我们初中学过的待定系数法）。曲线唯一确定后， 则曲线上每一点的位置、速度等便确定了，轨迹即可求出。曲线上每一点的导数就代表了车辆经过该点时的速度，表明多项式曲线换道轨迹规划是路径+速度的耦合结果。
注意，五次多项式换道轨迹曲线特指横向位置/纵向位置是关于时间 t t t的五次多项式，而不是指纵向位置 y y y关于横向位置 x x x的五次多项式。
2.3 双圆弧段曲线
如图，对于双圆弧段换道轨迹，它由弧AC+线段CD+弧DF构成。
显然，在C点，轨迹曲率由弧AC段的定值突变为0，故为了让车辆能完全跟随轨迹， 考虑到方向盘转角是一个连续缓变过程，车辆行驶到在C点后必须速度为0， 让方向盘回正后才能继续行驶，因此无法应用于行车路径规划，而应用于泊车路径规划。
3. python代码实现
下面实现五次多项式曲线插值法轨迹规划
import numpy as np
import matplotlib.pyplot as plt
if __name__=="__main__":
# 场景定义
# 换道场景路段与车辆相关参数的定义
d = 3.5 # 道路标准宽度
len_line = 30 # 直线段长度
W = 1.75 # 车宽
L = 4.7 # 车长
# 车辆换道初始状态与终点期望状态
t0 = 0
t1 = 3
state_t0 = np.array([0, -d/2, 5, 0, 0, 0]) # 分别表示小车的x,y; vx,vy; ax,ay
state_t1 = np.array([20, d/2, 5, 0, 0, 0])
"""计算A和B两个系数矩阵"""
## 把起末两点的横纵向方程统一用矩阵表达
X = np.concatenate((np.array([state_t0[i] for i in range(6) if i%2==0]),np.array([state_t1[i] for i in range(6) if i%2==0])))
Y = np.concatenate((np.array([state_t0[i] for i in range(6) if i%2!=0]),np.array([state_t1[i] for i in range(6) if i%2!=0])))
#矩阵T表示
T = np.matrix([
[t0 ** 5,t0 ** 4,t0 ** 3,t0 ** 2,t0,1],
[5 * t0 ** 4,4 * t0 ** 3,3 * t0 ** 2,2 * t0,1,0],
[20 * t0 ** 3,12 * t0 ** 2,6 * t0,1,0,0],
[t1 ** 5,t1 ** 4,t1 ** 3,t1 ** 2,t1,1],
[5 * t1 ** 4,4 * t1 ** 3,3 * t1 ** 2,2 * t1,1,0],
[20 * t1 ** 3,12 * t1 ** 2,6 * t1,1,0,0]
])
# # 解法1
# A=np.linalg.pinv(T)@X
# B=np.linalg.pinv(T)@Y.T
# A=A.T
# B=B.T
# # 解法2
A = np.linalg.solve(T,X)
B = np.linalg.solve(T,Y)
# 将时间从t0到t1离散化，获得离散时刻的轨迹坐标
t = np.transpose((np.arange(t0,t1+0.05,0.05)))
path = np.zeros((len(t),4)) # 1-4列分别存放x,y,vx,vy
for i in range(len(t)):
# 纵向位置坐标
path[i,0] = np.array([t[i] ** 5,t[i] ** 4,t[i] ** 3,t[i] ** 2,t[i],1]) @ A # @符号是矩阵相乘的意思
# 横向位置坐标
path[i,1] = np.array([t[i] ** 5,t[i] ** 4,t[i] ** 3,t[i] ** 2,t[i],1]) @ B
# 纵向速度
path[i,2] = np.array([5 * t[i] ** 4,4 * t[i] ** 3,3 * t[i] ** 2,2 * t[i],1,0]) @ A
# 横向速度
path[i,3] = np.array([5 * t[i] ** 4,4 * t[i] ** 3,3 * t[i] ** 2,2 * t[i],1,0]) @ B
## 画场景示意图
plt.figure(1)
# 画灰色路面图
GreyZone = np.array([[- 5,- d - 0.5],[- 5,d + 0.5],[len_line,d + 0.5],[len_line,- d - 0.5]])
plt.fill(GreyZone[:,0],GreyZone[:,1],'gray')
# 画小车
plt.fill(np.array([state_t1[0],state_t1[0],state_t1[0] + L,state_t1[0] + L]),np.array([- d / 2 - W / 2,- d / 2 + W / 2,- d / 2 + W / 2,- d / 2 - W / 2]),'b')
plt.fill(np.array([state_t0[0],state_t0[0],state_t0[0] - L,state_t0[0] - L]),np.array([- d / 2 - W / 2,- d / 2 + W / 2,- d / 2 + W / 2,- d / 2 - W / 2]),'y')
# 画分界线
plt.plot(np.array([- 5,len_line]),np.array([0,0]),'w--')
plt.plot(np.array([- 5,len_line]),np.array([d,d]),'w')
plt.plot(np.array([- 5,len_line]),np.array([- d,- d]),'w')
# 设置坐标轴显示范围
plt.axis('equal')
plt.savefig("场景示意图.png")
# 画换道轨迹
plt.plot(path[:,0],path[:,1],'r--')
## 分析速度
# 横向速度
plt.figure(2)
plt.plot(t,path[:,3],'k')
plt.xlabel('time/s ')
plt.ylabel('m/s ')
plt.savefig("横向速度.png")
# 纵向速度
plt.figure(3)
plt.plot(t,path[:,2],'k')
plt.xlabel('time/s ')
plt.ylabel('m/s ')
plt.savefig("纵向速度.png")
plt.show()
结果如下：
-
换道轨迹
-
纵向速度
-
横向速度
代码仓库见github