EYES 乱1. 五次多项式换道轨迹概述
在自动驾驶系统的开发中，车辆轨迹规划是核心模块之一。其中换道轨迹的平滑性和可行性直接关系到乘坐舒适性和安全性。五次多项式因其独特的数学特性，成为解决这一问题的理想选择。
为什么选择五次多项式而非其他阶次？三次多项式虽然计算简单，但无法满足加速度连续的要求；七次及以上多项式虽然更灵活，但计算复杂度显著增加。五次多项式恰好能在计算复杂度和轨迹平滑性之间取得最佳平衡。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 五次多项式数学原理详解
2.1 基本表达式与导数关系
五次多项式的一般形式为：
s(t) = a₀ + a₁t + a₂t² + a₃t³ + a₄t⁴ + a₅t⁵
其物理意义非常明确：
- s(t)表示t时刻车辆在横向（或纵向）的位置
- 一阶导数v(t)表示瞬时速度
- 二阶导数a(t)表示瞬时加速度
- 三阶导数j(t)表示瞬时加加速度（jerk）
这种多阶可导的特性使得轨迹在起止点处的位置、速度、加速度都能精确控制，这正是自动驾驶轨迹规划最看重的特性。
2.2 边界条件设置要点
在实际应用中，边界条件的设置需要特别注意：
-
初始状态通常设为当前车辆状态：
- s₀ = 当前位置
- v₀ = 当前速度
- a₀ = 当前加速度
-
目标状态设置要考虑：
- 目标车道中心线位置sf
- 建议vf设为与当前车道相同的速度
- af一般设为0，使车辆完成换道后保持匀速
-
换道时间T的确定：
- 通常3-5秒为宜
- 时间过短会导致加速度过大
- 时间过长会影响交通效率
3. 系数求解的数值实现
3.1 矩阵方程的构建
边界条件转化为矩阵方程的过程如下：
对于t=0时刻：
s(0) = a₀ = s₀
v(0) = a₁ = v₀
a(0) = 2a₂ = a₀
对于t=T时刻：
s(T) = a₀ + a₁T + a₂T² + a₃T³ + a₄T⁴ + a₅T⁵ = sf
v(T) = a₁ + 2a₂T + 3a₃T² + 4a₄T³ + 5a₅T⁴ = vf
a(T) = 2a₂ + 6a₃T + 12a₄T² + 20a₅T³ = af
这6个方程构成了完整的线性方程组。
3.2 Python实现细节
代码实现中有几个关键点值得注意：
-
系数矩阵A的构建采用了硬编码方式，这是因为五次多项式的边界条件矩阵是固定的。这种实现方式效率最高。
-
numpy.linalg.solve()使用LAPACK例程进行求解，对于6×6这样的小矩阵计算效率极高。
-
在generate_trajectory函数中，coeffs[::-1]的操作是因为numpy.polyval要求系数从高次到低次排列，而我们的存储顺序相反。
4. 轨迹评估与优化
4.1 运动学约束检查
生成的轨迹需要检查是否满足车辆运动学约束：
-
最大横向加速度：
- 通常不超过2 m/s²
- 可接受范围取决于车辆类型和路面条件
-
最大横向加加速度：
- 建议不超过5 m/s³
- 过高会导致乘坐不适
-
曲率连续性：
- 检查轨迹曲率是否连续
- 曲率突变会导致方向盘抖动
4.2 参数调整策略
当轨迹不满足约束时，可考虑以下调整：
-
延长换道时间T：
- 最简单有效的方法
- 以0.5秒为步长逐步增加
-
调整目标状态：
- 适当降低目标速度vf
- 设置非零的目标加速度af
-
分段多项式：
- 将换道过程分为加速段和减速段
- 每段使用不同的五次多项式
5. 实际应用中的注意事项
5.1 动态环境适应
在实际道路环境中，还需要考虑：
-
周围车辆预测：
- 使用交互式多模型预测
- 考虑最坏情况下的安全距离
-
轨迹重规划：
- 设置重规划触发条件
- 保留计算余量应对突发情况
5.2 计算效率优化
为提高实时性，可采用以下优化：
-
预计算技术：
- 离线计算常见场景的系数
- 运行时进行插值
-
矩阵运算加速：
- 使用SIMD指令优化
- 考虑GPU加速
-
简化计算：
- 在安全前提下降低轨迹更新频率
- 使用近似解法
6. 扩展应用场景
五次多项式不仅适用于换道，还可用于：
-
弯道轨迹生成：
- 将道路中心线转化为参考系
- 在Frenet坐标系下规划
-
泊车轨迹规划：
- 分段使用五次多项式
- 考虑非零起止速度
-
紧急避障：
- 结合势场法
- 动态调整边界条件
在实际项目中，我发现五次多项式虽然理论优美，但直接应用时经常会遇到加速度超限的问题。经过多次调试，总结出一个实用技巧：可以先用三次多项式快速计算一个粗略解，评估其加速度需求，再据此调整五次多项式的时间参数T，这样能大幅减少迭代次数。