当前位置：首页 > news >正文

探索五次多项式换道轨迹：代码与原理交织之旅

news 2026/5/12 18:19:14

五次多项式换道轨迹

在自动驾驶领域，车辆换道轨迹规划是个关键课题。五次多项式在其中扮演着相当重要的角色，它能帮助我们生成平滑且符合车辆动力学的换道轨迹。今天咱们就一起深入了解五次多项式换道轨迹背后的奥秘，并通过代码来实际感受它的魅力。

五次多项式基础理论

五次多项式的一般表达式为：

\( s(t) = a0 + a1t + a2t^2 + a3t^3 + a4t^4 + a5t^5 \)

这里的 \( s(t) \) 代表在时间 \( t \) 时车辆的位置（在我们换道场景里，可能是横向位置），\( a0, a1, \cdots, a_5 \) 是多项式的系数。

为了确定这些系数，我们需要一些边界条件。比如，在换道开始时刻 \( t0 \)，车辆的初始位置 \( s0 \)、初始速度 \( v0 \) 和初始加速度 \( a0 \) ；在换道结束时刻 \( tf \)，车辆的目标位置 \( sf \)、目标速度 \( vf \) 和目标加速度 \( af \) 。通过将这些边界条件代入到五次多项式及其一阶导数（速度）、二阶导数（加速度）表达式中，就可以求解出系数。

五次多项式换道轨迹

速度 \( v(t) \) 是位置 \( s(t) \) 对时间 \( t \) 的一阶导数：

\( v(t) = a1 + 2a2t + 3a3t^2 + 4a4t^3 + 5a_5t^4 \)

加速度 \( a(t) \) 是位置 \( s(t) \) 对时间 \( t \) 的二阶导数：

\( a(t) = 2a2 + 6a3t + 12a4t^2 + 20a5t^3 \)

Python 代码实现

import numpy as np import matplotlib.pyplot as plt def quintic_polynomial_coeffs(s0, v0, a0, sf, vf, af, T): A = np.array([ [1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 2, 0, 0, 0], [1, T, T ** 2, T ** 3, T ** 4, T ** 5], [0, 1, 2 * T, 3 * T ** 2, 4 * T ** 3, 5 * T ** 4], [0, 0, 2, 6 * T, 12 * T ** 2, 20 * T ** 3] ]) B = np.array([s0, v0, a0, sf, vf, af]) return np.linalg.solve(A, B) def generate_trajectory(coeffs, T, num_points=100): t = np.linspace(0, T, num_points) s = np.polyval(coeffs[::-1], t) return t, s # 设定边界条件 s0 = 0 # 初始位置 v0 = 0 # 初始速度 a0 = 0 # 初始加速度 sf = 3 # 目标位置（假设横向移动3米完成换道） vf = 0 # 目标速度 af = 0 # 目标加速度 T = 5 # 换道总时间，单位：秒 coeffs = quintic_polynomial_coeffs(s0, v0, a0, sf, vf, af, T) t, s = generate_trajectory(coeffs, T) plt.plot(t, s) plt.xlabel('Time (s)') plt.ylabel('Lateral Position (m)') plt.title('Quintic Polynomial Lane - Change Trajectory') plt.grid(True) plt.show()

代码分析

quinticpolynomialcoeffs函数：
- 这个函数的目的是求解五次多项式的系数。首先构建了系数矩阵 \( A \)，矩阵 \( A \) 的每一行对应一个边界条件。例如，第一行[1, 0, 0, 0, 0, 0]对应 \( t = 0 \) 时 \( s(0) = a_0 \) 这个条件。
- 然后构建了结果向量 \( B \)，包含初始和目标位置、速度及加速度。
- 最后使用np.linalg.solve函数求解线性方程组 \( Ax = B \)，得到五次多项式的系数。

generate_trajectory函数：
- 使用np.linspace函数生成从 \( 0 \) 到换道总时间 \( T \) 的等间隔时间点t。
- 通过np.polyval函数计算在这些时间点上的位置s。np.polyval要求系数顺序是从低次到高次，所以使用coeffs[::-1]来调整系数顺序。

主程序部分：
- 设定了初始位置s0、初始速度v0、初始加速度a0、目标位置sf、目标速度vf、目标加速度af以及换道总时间T。
- 调用quinticpolynomialcoeffs函数计算系数，再调用generate_trajectory函数生成轨迹。
- 最后使用matplotlib库绘制时间 - 横向位置的轨迹图，直观展示换道过程。