3种自动驾驶速度规划方法对比：QP算法、T/S型曲线与样条插值

自动驾驶速度规划三大核心算法QP优化、T/S型曲线与样条插值的深度对比与实践指南速度规划是自动驾驶运动控制的关键环节其质量直接影响行驶安全性、乘坐舒适性和能源效率。面对城市复杂路况、高速巡航和紧急避障等多样化场景工程师需要根据实际需求在多种算法中做出技术选型。本文将深入解析QP二次规划算法、T/S型速度曲线和样条插值三大主流方法的核心原理通过量化对比和场景化分析为自动驾驶开发者提供切实可行的技术决策框架。1. 速度规划技术全景与核心挑战在自动驾驶系统的分层架构中速度规划模块位于行为决策与车辆控制之间承担着将高层指令转化为可执行运动轨迹的关键角色。不同于全局路径规划关注走哪条路的问题速度规划需要解决的是如何走——即在给定路径上确定每一时刻的理想速度、加速度及加加速度Jerk参数。典型速度规划流程包含四个核心环节环境建模将动态障碍物预测结果映射到STSpace-Time坐标系约束提取综合车辆动力学、交通规则和舒适性要求建立数学约束轨迹生成通过优化算法求解满足约束的速度剖面平滑处理消除离散化带来的抖动确保控制指令可执行当前行业面临的主要技术挑战集中在三个方面实时性城市道路场景需在100ms内完成规划迭代动态适应性对突然切入的车辆需在0.5秒内做出反应乘员舒适性保持加加速度Jerk低于15 m/s³的生理舒适阈值# 典型速度规划约束条件示例 constraints { velocity_limit: 13.89, # 50km/h转换为m/s max_accel: 2.0, # 舒适加速度上限 max_decel: -3.0, # 舒适减速度下限 max_jerk: 10.0, # 紧急制动时可放宽 safety_margin: 1.5, # 与障碍物最小距离缓冲 }2. QP算法基于优化的精准控制QPQuadratic Programming算法将速度规划建模为二次优化问题通过最小化目标函数同时满足线性约束条件来生成轨迹。其数学形式可表示为$$ \begin{aligned} \min_x \quad \frac{1}{2}x^TQx q^Tx \ \text{s.t.} \quad Ax \leq b \ Gx h \end{aligned} $$核心优势在于能够系统性地处理多目标优化平滑性目标最小化加加速度Jerk的平方积分效率目标追踪参考速度如道路限速安全目标通过不等式约束避免碰撞2.1 ST图构建与障碍物处理ST图是QP算法的核心数据结构将时间T作为横轴、路径进度S作为纵轴。动态障碍物在ST图中表现为禁区多边形规划目标是在避开所有禁区的前提下找到最优速度曲线。障碍物处理策略数学表达适用场景超车策略$s(t) s_{obs}(t) \Delta s$前车速度低于目标值跟车策略$s(t)-s_{obs}(t)紧急避让$\frac{ds}{dt} \leq \sqrt{2a_{max}\cdot (s_{obs}-s)}$TTC3秒时2.2 实际工程中的调参要点在Apollo开源框架的实践中QP算法效果高度依赖三个参数组的协调权重矩阵配置# 目标函数中各分项权重 weights { jerk: 1.0, # 平滑性 velocity: 0.5, # 效率 accel: 0.3 # 舒适性 }约束松弛因子硬约束可能导致无解需引入松弛变量$ε$ $$ a_{min} - ε \leq a(t) \leq a_{max} ε $$离散化粒度选择时间分辨率通常取0.1s对应10Hz控制频率路径分辨率建议为0.3-0.5m约车身长度的1/10提示QP求解器如OSQP的迭代次数应限制在500次以内以保证实时性超时则启用上一帧结果作为fallback3. T/S型速度曲线工业级可靠方案T型梯形和S型正弦速度曲线源于工业运动控制领域其特点是具有解析表达式计算效率极高特别适合计算资源有限的嵌入式系统。3.1 算法原理对比特性T型曲线S型曲线加速度剖面矩形脉冲不连续正弦变化连续Jerk表现理论无限大实际受执行器限制有限值可精确控制计算复杂度O(1)O(1)适用场景物料输送、泊车控制乘客运输、精密定位T型曲线三段式方程v(t) v₀ aₘₐₓ·t (加速段) vₘₐₓ (匀速段) vₘₐₓ - aₘₐₓ·(t-t₁) (减速段)S型曲线七段式方程jerk(t) Jₘₐₓ (加加速段) 0 (匀加速段) -Jₘₐₓ (减加速段) 0 (匀速段) -Jₘₐₓ (加减速段) 0 (匀减速段) Jₘₐₓ (减减速段)3.2 参数化设计方法为适应不同场景需求可通过六个关键参数调整曲线形态最大速度$v_{max}$受限于道路法规和车辆性能最大加速度$a_{max}$典型乘用车取值2-3 m/s²最大加加速度$J_{max}$舒适性要求通常20 m/s³起始速度$v_0$影响曲线过渡段时间终止速度$v_e$跟车场景可能不为零总位移$S_{total}$决定是否需要匀速段// 典型S曲线参数计算示例C实现 struct SCurveParams { double t[7]; // 各阶段持续时间 void calculate(double dist, double vmax, double amax, double jmax) { t[0] t[2] t[4] t[6] amax/jmax; t[1] t[5] (vmax - sqrt(2*amax*dist/jmax))/amax; t[3] (dist - 2*(pow(amax,3)/pow(jmax,2)))/vmax; } };4. 样条插值平滑性与灵活性兼备样条插值通过分段多项式函数拟合离散的速度指令点在保证平滑性的同时提供局部调整能力。三次样条Cubic Spline是最常用的实现形式其数学表达为$$ v(t) a_i b_i(t-t_i) c_i(t-t_i)^2 d_i(t-t_i)^3 \quad t_i \leq t t_{i1} $$4.1 关键技术实现边界条件处理是影响实际效果的关键因素自然样条二阶导数为零适用于起止静止场景 $$ v(t_0) v(t_n) 0 $$固定斜率指定起止速度适用于跟车汇流场景 $$ v(t_0) v_{start}, \quad v(t_n) v_{end} $$周期样条闭环轨迹适用于赛道测试场景 $$ v^{(k)}(t_0) v^{(k)}(t_n), \quad k0,1,2 $$工程实践中的性能优化技巧采用稀疏矩阵存储刚度矩阵90%以上元素为零使用Thomas算法追赶法求解三对角方程组复杂度O(n)对固定时间间隔的均匀样条可预计算基函数4.2 动态更新策略当遇到突发障碍物时样条轨迹需要快速调整局部调整法仅修改受影响区间的控制点def update_spline(spline, t_range, new_points): # 查找受影响的基础函数区间 i_start np.searchsorted(spline.t, t_range[0]) - 1 i_end np.searchsorted(spline.t, t_range[1]) # 局部更新系数 for i in range(i_start, i_end): spline.coef[i] solve_local_coeff(new_points)权重调整法通过代价函数重新平衡平滑性与避障需求 $$ J w_1\int \dddot{v}^2 dt w_2\sum (v(t_i)-v_{ref})^2 $$5. 三大算法量化对比与选型指南为直观展示各算法特性我们从六个维度进行量化评估评分范围1-5越高越优评估维度QP算法T型曲线S型曲线样条插值计算效率3542动态障碍适应性5234轨迹平滑性4135参数调节便利性2543实时性能保证3552紧急制动响应43315.1 典型场景适配建议城市跟车场景优先选择QP算法其跟车策略可精确控制安全距离参数配置侧重低Jerk10 m/s³和平稳加速示例代码实现安全距离策略def safe_following(v_ego, v_lead, a_lead): tau 1.5 # 时间裕度秒 min_gap 2.0 # 最小静态距离米 return min_gap max(0, tau*v_ego - (v_ego**2 - v_lead**2)/(2*a_lead))高速巡航场景推荐S型曲线平衡舒适性与燃油经济性速度波动应控制在±2 km/h以内长下坡路段可结合能量回收调整减速度曲线紧急制动场景采用QP算法紧急模式放宽Jerk约束触发条件TTCTime to Collision2秒多级制动策略普通预警0.3g → 强烈预警0.5g → 全力制动0.8g6. 前沿演进与融合趋势速度规划算法正呈现三个明显的发展方向学习增强型方法通过强化学习优化QP权重矩阵如DQN算法使用CNN-LSTM网络预测最优曲线类型云-端协同规划云端执行计算密集型的全局优化车端进行局部调整100ms延迟要求能效导向设计结合电池SOC状态调整加速度上限下坡路段预测性减速回收能量在实际项目中我们观察到混合架构往往能取得最佳效果。例如某L4级Robotaxi的方案QP全局优化 → 样条插值平滑处理 → S曲线执行保障