路径规划 | 五次多项式曲线 Quintic Polynomial

baoshijie

局部路径规划是无人驾驶车辆运动规划的一个重要部分，其中五次多项式是局部路径规划中常用的一种算法。笔者将结合开源的课程和代码学习一下五次多项式的应用。

01

曲线插值法

我们常用三次多项式曲线或者五次多项式曲线规划无人车运动轨迹。多项式曲线一般而言都是奇数，这是由于边界条件引起的。我们可以这样理解：边界条件一般包含车辆的初始状态和终止状态，因此两倍的车辆状态有偶数个系数，也就造成了方程有奇数多项式。

三次多项式：求解位置和速度
五次多项式：求解位置、速度、加速度七次多项式：求解位置、速度、加速度、加加速度

02
五次多项式曲线方程

写在最前面：五次多项式曲线做路径规划时，y不是关于x的曲线，而是 y和x都是关于t的曲线，这一点初学者需要搞清楚

五次多项式曲线插值轨迹规划



把上述方程合并成矩阵形式可以写成：



在这个等式中，X矩阵和y矩阵的数值我们都是已知的。x0，x0’，x0’’ 分别表示初始位置的横向坐标，速度，加速度；y0，y0’，y0’’ 分别表示初始位置的纵向坐标，速度，加速度。其次，时间t0和t1也是已知的，分别表示初始位置和终点位置的时刻。这样一来X，Y，T矩阵都已知，我们便容易求出矩阵A。

之后，我们再设置时间间隔Δt代入T矩阵，就能利用A矩阵和T矩阵求出初始位置和终点位置之间的点的位置、速度和加速度了。

03
代码讲解

3.1 参数设置

# 从起点到终点的最短时间和最长时间MAX_T = 100.0# maximum time to the goal   MIN_T = 5.0# minimum time to the goal# 起点条件    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

3.2 构造五次多项式规划器

规划器中先采用单车模型求解出起点和终点的横向和纵向加速度、速度。

以最短运动时间和最长运动时间之间每隔Δ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)
defquintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):"""quinticpolynomial planner
inputs_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
returntime: time resultrx: x position result listry: y position result listryaw: yaw angle result listrv: velocity result listra: 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 = [], [], [], [], [], [], []
forT 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 = [], [], [], [], [], [], []
fort 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)iflen(rv) >= 2 and rv[-1] - rv[-2] < 0.0:a*= -1ra.append(a)
jx = xqp.calc_third_derivative(t)  # 三阶导数jy = yqp.calc_third_derivative(t)j = np.hypot(jx, jy)iflen(ra) >= 2 and ra[-1] - ra[-2] < 0.0:j*= -1rj.append(j)
ifmax([abs(i) for i in ra]) <= max_accel and max([abs(i) for i in rj]) <= max_jerk:print("findpath!!")break

3.3 五次多项式类

对照着上一节的T矩阵可以分别计算出位置，速度，加速度和加加速度，对应下面的 calc_point,calc_first_derivative,calc_second_derivative,calc_third_derivative。

classQuinticPolynomial:
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 = xsself.a1 = vxsself.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]
defcalc_point(self, t):        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5return xt
defcalc_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 ** 4return xt
defcalc_second_derivative(self, t):        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3return xt
defcalc_third_derivative(self, t):        xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2return xt

3.4 判断终止条件

如果该条路径上的所有点的加速度和加加速度都满足约束条件，则找到一条符合条件的路径。
ifmax([abs(i) for i in ra]) <= max_accel and max([abs(i) for i in rj]) <= max_jerk:print("find path!!")break