自动驾驶速度规划 QP 算法实战：S-T图避障与5次多项式速度平滑

自动驾驶速度规划实战从S-T图避障到5次多项式平滑的完整实现在自动驾驶系统中速度规划模块如同车辆的节奏大师需要精确协调安全避障、乘坐舒适和通行效率这三重目标。传统方法如恒定加速度或三次样条插值往往难以兼顾动态环境中的复杂约束而基于QP二次规划的S-T图方法通过将时空关系可视化为这一难题提供了系统性解决方案。本文将深入解析QP算法的工程实现细节包括完整的Python代码示例和参数调优经验。1. S-T图原理与动态障碍物投影S-T图Speed-Time Graph是速度规划的核心数据结构它将时间T作为横轴沿参考线的纵向位移S作为纵轴形成一个二维决策空间。任何动态障碍物在这个空间中都会投射出一个禁区多边形规划目标就是找到一条从起点(0,0)到目标点(T_end, S_goal)的连续曲线完美避开所有禁区。动态障碍物投影的关键步骤运动预测假设某障碍物初始状态为位置(x₀,y₀)、速度(vx,vy)、加速度(ax,ay)其t时刻位置可通过运动学公式预测def predict_obstacle_pos(t, state): x0, y0, vx, vy, ax, ay state x x0 vx*t 0.5*ax*t**2 y y0 vy*t 0.5*ay*t**2 return (x, y)路径投影将预测的(x,y)位置投影到参考线上得到纵向位移s。对于弯曲的参考线需要计算最近点def project_to_refline(x, y, refline): # refline: [(x0,y0), (x1,y1),...] min_dist float(inf) s 0 for i in range(len(refline)-1): p1, p2 refline[i], refline[i1] # 计算点到线段的投影 proj, dist point_to_segment_projection((x,y), p1, p2) if dist min_dist: min_dist dist s np.linalg.norm(np.array(p1)-np.array(proj)) return s占据区域计算考虑障碍物物理尺寸计算其在参考线上的占据区间[s_min, s_max]并在S-T图中形成矩形禁区当障碍物移动时禁区变为不规则多边形。提示实际工程中会采用更高效的KD-Tree进行最近邻搜索上述代码仅为原理演示。对于弯曲道路Frenet坐标系能更准确地描述纵向位移。2. S-T图栅格化与A*搜索实现将连续的S-T图离散化为网格是搜索算法应用的前提。栅格化需要平衡计算精度和效率参数典型值影响分析时间分辨率Δt0.1-0.3s值越小越精确但计算量指数增长位移分辨率Δs0.5-1.0m影响速度控制精度规划时域T5-8s过短可能漏判远距离风险A*搜索算法的Python实现class STGrid: def __init__(self, t_max, s_max, dt, ds): self.grid np.zeros((int(t_max/dt)1, int(s_max/ds)1)) self.dt, self.ds dt, ds def mark_obstacle(self, t_start, t_end, s_start, s_end): ti_start int(t_start / self.dt) ti_end int(t_end / self.dt) 1 si_start int(s_start / self.ds) si_end int(s_end / self.ds) 1 self.grid[ti_start:ti_end, si_start:si_end] 1 def a_star_search(st_grid): # 定义移动方向右(时间增加)、上(位移增加)、右上 moves [(1,0), (0,1), (1,1)] start (0, 0) goal_t, goal_s st_grid.grid.shape[0]-1, st_grid.grid.shape[1]-1 open_set {start} came_from {} g_score {start: 0} f_score {start: heuristic(start, (goal_t, goal_s))} while open_set: current min(open_set, keylambda x: f_score.get(x, float(inf))) if current (goal_t, goal_s): return reconstruct_path(came_from, current) open_set.remove(current) for move in moves: neighbor (current[0]move[0], current[1]move[1]) if (neighbor[0] st_grid.grid.shape[0] or neighbor[1] st_grid.grid.shape[1] or st_grid.grid[neighbor[0], neighbor[1]] 1): continue tentative_g g_score[current] move_cost(current, neighbor) if tentative_g g_score.get(neighbor, float(inf)): came_from[neighbor] current g_score[neighbor] tentative_g f_score[neighbor] tentative_g heuristic(neighbor, (goal_t, goal_s)) if neighbor not in open_set: open_set.add(neighbor) return None # 未找到路径代价函数设计技巧基础代价优先选择接近参考速度v_ref的路径代价函数设为(v-v_ref)²舒适性惩罚对加速度大的路径增加二次惩罚项决策偏好根据行为层指令调整障碍物周边代价如变道时降低目标车道前车后方区域的代价3. 5次多项式速度平滑的梯度下降实现A*搜索得到的路径往往存在锯齿现象需要通过5次多项式拟合获得平滑的速度曲线。5次多项式形式为s(t) k₀ k₁t k₂t² k₃t³ k₄t⁴ k₅t⁵边界条件约束初始状态s(0)s₀, s(0)v₀, s(0)a₀终止状态s(T)s_T, s(T)v_T, s(T)a_T梯度下降优化实现def quintic_poly_smoothing(path_points, t_total, init_state, target_s): # path_points: [(t1,s1), (t2,s2),...] # init_state: (s0, v0, a0) s0, v0, a0 init_state k0 s0 k1 v0 k2 a0 / 2 # 初始化k3,k4,k5 k3, k4, k5 0, 0, 0 learning_rate 1e-8 epochs 1000 for _ in range(epochs): grad_k4, grad_k5 0, 0 total_loss 0 for ti, si in path_points: pred_s k0 k1*ti k2*ti**2 k3*ti**3 k4*ti**4 k5*ti**5 error pred_s - si # 根据推导的梯度公式 grad_k4 error * (ti**4 - t_total * ti**3) grad_k5 error * (ti**5 - t_total**2 * ti**3) total_loss error**2 # 更新系数 k4 - learning_rate * grad_k4 / len(path_points) k5 - learning_rate * grad_k5 / len(path_points) # 根据终止条件s(T)s_T重新计算k3 k3 (target_s - k0 - k1*t_total - k2*t_total**2 - k4*t_total**4 - k5*t_total**5) / t_total**3 if total_loss 1e-6: break return [k0, k1, k2, k3, k4, k5]工程优化技巧热启动将上一帧的优化结果作为当前帧的初始值可减少50%以上迭代次数自适应学习率采用Adam优化器替代固定学习率收敛更快约束处理对超出加速度/加加速度限制的曲线进行二次修正4. 动态仿真与参数调优实战为验证算法效果我们搭建了一个简单的动态仿真环境包含以下要素参考线生成器直线/曲线动态障碍物模拟器实时可视化界面关键参数调优经验参数调优方法典型值范围加速度权重w_acc从较小值开始逐步增加直到急加速现象消失0.3-1.0加加速度权重w_jerk根据乘客舒适度反馈调整0.1-0.5障碍物膨胀距离按传感器误差安全余量设置0.5-1.5m规划时域T根据车速调整高速需更长时域5s(城市)-8s(高速)可视化代码片段import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation def visualize_st_graph(st_grid, path, obstacles): fig, ax plt.subplots() ax.imshow(st_grid.grid.T, cmapBlues, originlower) # 绘制障碍物 for obs in obstacles: ax.add_patch(plt.Rectangle((obs[0]/dt, obs[2]/ds), (obs[1]-obs[0])/dt, (obs[3]-obs[2])/ds, colorred, alpha0.5)) # 绘制搜索路径 if path: t_coords [p[0]/dt for p in path] s_coords [p[1]/ds for p in path] ax.plot(t_coords, s_coords, g-, linewidth2) ax.set_xlabel(Time (steps)) ax.set_ylabel(Distance (steps)) plt.show()实际调试中发现当障碍物速度超过自车速度30%时单纯的速度调整可能无法避免碰撞此时需要触发重新路径规划如变道。这引出了速度规划与路径规划的协同问题——在复杂场景下需要采用SLTSpatial-Lateral-Temporal三维规划框架但这会显著增加计算复杂度。