【路径规划】一文学懂五次多项式曲线 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

<think>好的,用户想了解车辆路径规划中使用五次多项式的实现方法或算法。首先,我需要回忆五次多项式路径规划中的应用场景和基本原理。根据引用[2]和[4],五次多项式常用于处理带有位置、速度和加速度约束的轨迹生成问题,这在换道等场景中非常关键。 接下来,用户可能需要具体的步骤或数学推导。根据引用[2]中的内容,五次多项式的推导涉及到泛函极值问题,需要设定边界条件。比如,在横向或纵向规划中,每个方向上的轨迹是关于时间t的五次多项式,这样能同时满足位置、速度和加速度的约束。 然后,用户可能想知道如何将横向和纵向的解耦规划结合起来。根据引用[3],横纵向分别使用五次多项式,然后合并得到最终轨迹。这里需要注意的是,虽然每个方向都是五次多项式,但合并后的轨迹并不是关于x的五次曲线,而是两者的组合。这点容易混淆,需要特别指出。 另外,用户可能关心具体的实现步骤,比如如何建立方程组、求解系数等。需要详细列出五次多项式般形式,然后根据起始和终止条件构建方程组,用矩阵求解系数。这部分需要数学推导的支持,引用[2]和[4]提供了相关的理论基础。 最后,用户可能会遇到实际应用中的问题,比如参数选择或优化方法。根据引用[1],五次多项式能够综合考虑车辆状态和环境因素,但实际应用中可能需要结合其他方法进行优化,比如考虑曲率连续性或动态调整参数。需要提到可能的优化方向,帮助用户进步研究。 总结下来,回答需要结构清晰,先介绍五次多项式的基本原理,再分步骤讲解实现方法,最后讨论优化和应用中的注意事项。同时,引用相关文献来支持每个步骤,确保信息的准确性。需要避免过于技术化的术语,用中文简明扼要地解释,并生成相关问题引导用户深入思考。</think>### 车辆路径规划五次多项式实现方法 #### 五次多项式的基本原理 在车辆路径规划中,五次多项式常用于描述横纵向运动轨迹,其核心优势在于能够同时满足**位置、速度、加速度的边界约束**。对于横向(Y轴)或纵向(X轴)运动,轨迹可表示为时间$t$的五次多项式函数: $$ s(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 $$ 其中$s(t)$代表横向位移$y(t)$或纵向位移$x(t)$[^2][^4]。 #### 二、实现步骤 1. **设定边界条件** 根据车辆初始和目标状态定义6个约束条件: - 初始时刻$t_0$:位置$s(t_0)$、速度$s'(t_0)$、加速度$s''(t_0)$ - 终止时刻$t_f$:位置$s(t_f)$、速度$s'(t_f)$、加速度$s''(t_f)$ 2. **构建线性方程组** 将多项式阶导(速度)和二阶导(加速度)代入边界条件: $$ \begin{cases} s(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 \\ s'(t_0) = a_1 + 2a_2 t_0 + 3a_3 t_0^2 + 4a_4 t_0^3 + 5a_5 t_0^4 \\ s''(t_0) = 2a_2 + 6a_3 t_0 + 12a_4 t_0^2 + 20a_5 t_0^3 \\ s(t_f) = a_0 + a_1 t_f + a_2 t_f^2 + \dots + a_5 t_f^5 \\ s'(t_f) = a_1 + 2a_2 t_f + \dots + 5a_5 t_f^4 \\ s''(t_f) = 2a_2 + 6a_3 t_f + 12a_4 t_f^2 + 20a_5 t_f^3 \end{cases} $$ 3. **矩阵求解系数** 将方程组转换为矩阵形式$A \cdot \mathbf{a} = \mathbf{b}$,其中$\mathbf{a}=[a_0,a_1,a_2,a_3,a_4,a_5]^T$。通过矩阵求逆或高斯消元法解出系数[^2]。 4. **横纵向解耦规划** - **横向规划**:生成$y(t)$关于$t$的五次多项式 - **纵向规划**:生成$x(t)$关于$t$的五次多项式 - **轨迹合成**:根据时间$t$合并$(x(t), y(t))$,形成最终路径[^3]。 #### 三、关键算法优化 1. **动态时间调整** 根据车辆动力学限制(如最大加速度、曲率约束),调整轨迹时间$t_f$,避免出现不合理的运动状态。 2. **曲率连续性保证** 通过引入更高阶导数约束(如加加速度$s'''(t)$),提升轨迹平滑性,公式扩展为七次多项式。 3. **多目标融合** 结合环境感知数据(如障碍物位置),在代价函数中增加碰撞风险项,实现安全轨迹优化[^1]。 #### 四、应用案例(换道轨迹生成) ```python import numpy as np def quintic_polynomial(t, a): """计算五次多项式及其导数""" pos = a[0] + a[1]*t + a[2]*t**2 + a[3]*t**3 + a[4]*t**4 + a[5]*t**5 vel = a[1] + 2*a[2]*t + 3*a[3]*t**2 + 4*a[4]*t**3 + 5*a[5]*t**4 acc = 2*a[2] + 6*a[3]*t + 12*a[4]*t**2 + 20*a[5]*t**3 return pos, vel, acc # 设定换道参数:横向位移Δy=3m,时间T=5s t0, tf = 0, 5 y0, yf = 0, 3 dy0, dyf = 0, 0 # 初始和终止横向速度 ddy0, ddyf = 0, 0 # 初始和终止横向加速度 # 构建系数矩阵 A = np.array([ [1, t0, t0**2, t0**3, t0**4, t0**5], [0, 1, 2*t0, 3*t0**2, 4*t0**3, 5*t0**4], [0, 0, 2, 6*t0, 12*t0**2, 20*t0**3], [1, tf, tf**2, tf**3, tf**4, tf**5], [0, 1, 2*tf, 3*tf**2, 4*tf**3, 5*tf**4], [0, 0, 2, 6*tf, 12*tf**2, 20*tf**3] ]) b = np.array([y0, dy0, ddy0, yf, dyf, ddyf]) a_coeffs = np.linalg.solve(A, b) ``` #### 五、注意事项 1. **时间参数敏感性**:较小的$t_f$可能导致加速度突变,需结合车辆动力学验证可行性。 2. **笛卡尔坐标系解耦**:横纵向独立规划时,最终轨迹的曲率需额外验证[^3]。 3. **实时性优化**:可预先计算不同场景的系数矩阵逆矩阵,减少在线计算量[^1]。
评论 5
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值