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

---

前言

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

---

曲线插值法

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

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

五次多项式曲线方程

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

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

1. 位置：
   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)=a0 a1t a2t2 a3t3 a4t4 at5
   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)=b0 b1t b2t2 b3t3 b4t4 bt5

2. 速度：
   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)=a1 2a2t 3a3t2 4a4t3 5at4
   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)=b1 2b2t 3b3t2 4b4t3 5bt4

3. 加速度：
   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)=2a2 6a3t 12a4t2 20at3
   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)=2b2 6b3t 12b4t2 20bt3

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

在这个等式中，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

---

参考资料

https://www.bilibili.com/video/BV1dv411W77J

https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py

这篇好文章是转载于：学新通技术网