前言
局部路径规划是无人驾驶车辆运动规划的一个重要部分，其中五次多项式是局部路径规划中常用的一种算法。笔者将结合开源的课程和代码学习一下五次多项式的应用。
曲线插值法
我们常用三次多项式曲线或者五次多项式曲线规划无人车运动轨迹。多项式曲线一般而言都是奇数，这是由于边界条件引起的。我们可以这样理解： 边界条件一般包含车辆的初始状态和终止状态，因此
两倍
的车辆状态有
偶数个系数
，也就造成了方程有奇数多项式。
- 三次多项式：求解位置和速度
- 五次多项式：求解位置、速度、加速度
- 七次多项式：求解位置、速度、加速度、加加速度
五次多项式曲线方程
写在最前面：
五次多项式曲线做路径规划时，y不是关于x的曲线，而是 y和x都是关于t的曲线，这一点初学者需要搞清楚
五次多项式曲线插值轨迹规划
-
位置：
x(
t
)
=
a
0
+
a
1
t
+
a
2
t
2
+
a
3
t
3
+
a
4
t
4
+
a
t
5
x(t) = a_0 + a_1t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_ t^5
x
(
t
)
=
a
0
+
a
1
t
+
a
2
t
2
+
a
3
t
3
+
a
4
t
4
+
a
t
5
y(
t
)
=
b
0
+
b
1
t
+
b
2
t
2
+
b
3
t
3
+
b
4
t
4
+
b
t
5
y(t) = b_0 + b_1t + b_2 t^2 + b_3 t^3 + b_4 t^4 + b_ t^5
y
(
t
)
=
b
0
+
b
1
t
+
b
2
t
2
+
b
3
t
3
+
b
4
t
4
+
b
t
5
-
速度：
x′
(
t
)
=
a
1
+
2
a
2
t
+
3
a
3
t
2
+
4
a
4
t
3
+
5
a
t
4
x'(t) = a_1 + 2a_2 t + 3a_3 t^2 + 4a_4 t^3 + 5a_ t^4
x
′
(
t
)
=
a
1
+
2
a
2
t
+
3
a
3
t
2
+
4
a
4
t
3
+
5
a
t
4
y′
(
t
)
=
b
1
+
2
b
2
t
+
3
b
3
t
2
+
4
b
4
t
3
+
5
b
t
4
y'(t) = b_1 + 2b_2 t + 3b_3 t^2 + 4b_4 t^3 + 5b_ t^4
y
′
(
t
)
=
b
1
+
2
b
2
t
+
3
b
3
t
2
+
4
b
4
t
3
+
5
b
t
4
-
加速度：
x′
′
(
t
)
=
2
a
2
+
6
a
3
t
+
12
a
4
t
2
+
20
a
t
3
x”(t) = 2a_2 + 6a_3 t + 12a_4 t^2 + 20a_ t^3
x
′
′
(
t
)
=
2
a
2
+
6
a
3
t
+
1
2
a
4
t
2
+
2
0
a
t
3
y′
′
(
t
)
=
2
b
2
+
6
b
3
t
+
12
b
4
t
2
+
20
b
t
3
y”(t) = 2b_2 + 6b_3 t + 12b_4 t^2 + 20b_ t^3
y
′
′
(
t
)
=
2
b
2
+
6
b
3
t
+
1
2
b
4
t
2
+
2
0
b
t
3
把上述方程合并成矩阵形式可以写成：
在这个等式中，X矩阵和y矩阵的数值我们都是已知的。x0，x0’，x0’’ 分别表示初始位置的横向坐标，速度，加速度；y0，y0’，y0’’ 分别表示初始位置的纵向坐标，速度，加速度。其次，
时间t0和t1也是已知的
，分别表示初始位置和终点位置的时刻。这样一来
X，Y，T矩阵都已知
，我们便容易求出矩阵A。
之后，我们再设置时间间隔Δt代入T矩阵，就能利用A矩阵和T矩阵求出初始位置和终点位置之间的点的位置、速度和加速度了。
更多详细讲解可以看文末的链接~
代码讲解
笔者通过github上的
开源代码
来学习五次多项式曲线，下面进行代码的讲解。
参数设置
# 从起点到终点的最短时间和最长时间
MAX_T = 100.0 # maximum time to the goal [s]
MIN_T = 5.0 # minimum time to the goal[s]
# 起点条件
sx = 10.0 # start x position [m]
sy = 10.0 # start y position [m]
syaw = np.deg2rad(10.0) # start yaw angle [rad]
sv = 1.0 # start speed [m/s]
sa = 0.1 # start accel [m/ss]
# 终点条件
gx = 30.0 # goal x position [m]
gy = -10.0 # goal y position [m]
gyaw = np.deg2rad(20.0) # goal yaw angle [rad]
gv = 1.0 # goal speed [m/s]
ga = 0.1 # goal accel [m/ss]
# 最大加速度与加加速度
max_accel = 1.0 # max accel [m/ss]
max_jerk = 0.5 # max jerk [m/sss]
# 时间间隔0.1
dt = 0.1 # time tick [s]
构造五次多项式规划器
规划器中先采用单车模型求解出起点和终点的横向和纵向加速度、速度。
以最短运动时间和最长运动时间之间每隔Δt个时间间隔的时间为单位，求解最优路径。
求解的方式是，对于每个运动时间构造一个五次多项式曲线QuinticPolynomial，QuinticPolynomial()的目的是
求解矩阵A
，以及计算位置，速度和加速度。
# 计算出时间、空间、速度、加速度和加加速度的信息
time, x, y, yaw, v, a, j = quintic_polynomials_planner(
sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt)
def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):
"""
quintic polynomial planner
input
s_x: start x position [m]
s_y: start y position [m]
s_yaw: start yaw angle [rad]
s_v: start speed [m/s]
sa: start accel [m/ss]
gx: goal x position [m]
gy: goal y position [m]
gyaw: goal yaw angle [rad]
ga: goal accel [m/ss]
max_accel: maximum accel [m/ss]
max_jerk: maximum jerk [m/sss]
dt: time tick [s]
return
time: time result
rx: x position result list
ry: y position result list
ryaw: yaw angle result list
rv: velocity result list
ra: accel result list
"""
# 起点与终点的横向与纵向速度
vxs = sv * math.cos(syaw)
vys = sv * math.sin(syaw)
vxg = gv * math.cos(gyaw)
vyg = gv * math.sin(gyaw)
# 起点与终点的横向与纵向加速度
axs = sa * math.cos(syaw)
ays = sa * math.sin(syaw)
axg = ga * math.cos(gyaw)
ayg = ga * math.sin(gyaw)
time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []
for T in np.arange(MIN_T, MAX_T, MIN_T): # 从最短的时间到最长的时间
xqp = QuinticPolynomial(sx, vxs, axs, gx, vxg, axg, T) # 横向
yqp = QuinticPolynomial(sy, vys, ays, gy, vyg, ayg, T) # 纵向
time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []
for t in np.arange(0.0, T + dt, dt):
time.append(t)
rx.append(xqp.calc_point(t))
ry.append(yqp.calc_point(t))
vx = xqp.calc_first_derivative(t)
vy = yqp.calc_first_derivative(t)
v = np.hypot(vx, vy)
yaw = math.atan2(vy, vx)
rv.append(v)
ryaw.append(yaw)
ax = xqp.calc_second_derivative(t)
ay = yqp.calc_second_derivative(t)
a = np.hypot(ax, ay)
if len(rv) >= 2 and rv[-1] - rv[-2] < 0.0:
a *= -1
ra.append(a)
jx = xqp.calc_third_derivative(t) # 三阶导数
jy = yqp.calc_third_derivative(t)
j = np.hypot(jx, jy)
if len(ra) >= 2 and ra[-1] - ra[-2] < 0.0:
j *= -1
rj.append(j)
if max([abs(i) for i in ra]) <= max_accel and max([abs(i) for i in rj]) <= max_jerk:
print("find path!!")
break
五次多项式类
对照着上一节的T矩阵可以分别计算出位置，速度，加速度和加加速度，对应下面的calc_point，calc_first_derivative， calc_second_derivative， calc_third_derivative。
class QuinticPolynomial:
def __init__(self, xs, vxs, axs, xe, vxe, axe, time):
# calc coefficient of quintic polynomial
# See jupyter notebook document for derivation of this equation.
self.a0 = xs
self.a1 = vxs
self.a2 = axs / 2.0
A = np.array([[time ** 3, time ** 4, time ** 5],
[3 * time ** 2, 4 * time ** 3, 5 * time ** 4],
[6 * time, 12 * time ** 2, 20 * time ** 3]])
b = np.array([xe - self.a0 - self.a1 * time - self.a2 * time ** 2,
vxe - self.a1 - 2 * self.a2 * time,
axe - 2 * self.a2])
x = np.linalg.solve(A, b)
self.a3 = x[0]
self.a4 = x[1]
self.a5 = x[2]
def calc_point(self, t):
xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5
return xt
def calc_first_derivative(self, t):
xt = self.a1 + 2 * self.a2 * t + \
3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4
return xt
def calc_second_derivative(self, t):
xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3
return xt
def calc_third_derivative(self, t):
xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2
return xt
判断终止条件
如果该条路径上的所有点的加速度和加加速度都满足约束条件，则找到一条符合条件的路径。
if max([abs(i) for i in ra]) <= max_accel and max([abs(i) for i in rj]) <= max_jerk:
print("find path!!")
break
参考资料