1. 项目概述：QP路径规划与ST图速度规划的避障组合拳
在自动驾驶和机器人运动控制领域，路径规划与速度规划就像人的两条腿——缺了哪条都走不稳。最近在完成一个园区无人配送车项目时，我深度实践了基于二次规划（QP）的路径规划和ST图速度规划的组合方案。这套方法在静态障碍规避、动态障碍交互、狭窄通道通行等场景都表现出色，特别适合需要实时响应且计算资源有限的移动设备。
传统方法如A*算法虽然能找出一条可行路径，但往往产生锯齿状轨迹，导致控制模块难以跟踪。而纯优化方法又容易陷入局部最优。QP规划的优势在于：
- 将物理约束（如最大曲率）转化为数学不等式
- 平滑性目标直接写入代价函数
- 计算效率适合实时系统（10-50ms/次）
ST图则是处理动态障碍物的神器，它把时间维度显式引入规划过程。当遇到突然出现的行人或车辆时，通过调整速度曲线在时间轴上的分布来规避碰撞，这比单纯的空间避障更符合实际驾驶逻辑。
2. 核心算法原理与实现细节
2.1 QP路径规划数学模型
二次规划问题的标准形式为：
minimize (1/2)xᵀHx + fᵀx subject to Ax ≤ b在路径规划中，我们通常：
- 设计H矩阵控制路径平滑度
- 用不等式约束表示障碍物边界
- 添加车辆运动学约束（如最大转向角）
具体实现时，我推荐使用CVXOPT库的qp求解器。下面是一个带曲率约束的改进版本：
import cvxopt from cvxopt import matrix def qp_path_planning(waypoints, obstacles): # 构造平滑项权重矩阵 (考虑一阶和二阶导数) n = len(waypoints) H = matrix(0.0, (2*n, 2*n)) for i in range(n-1): H[2*i,2*i] = 1.0 # x方向平滑权重 H[2*i+1,2*i+1] = 1.0 # y方向平滑权重 # 曲率约束近似 (Δθ/Δs ≤ κ_max) A_curv = [] b_curv = [] for i in range(1, n-1): # 计算三点曲率约束 ... # 障碍物约束 (多边形膨胀法) A_obs = matrix(0.0, (len(obstacles)*n, 2*n)) b_obs = matrix(0.0, (len(obstacles)*n, 1)) for k, obs in enumerate(obstacles): for i in range(n): # 计算路径点到障碍物的距离约束 ... # 合并所有约束 A = matrix([A_curv, A_obs]) b = matrix([b_curv, b_obs]) # 求解QP问题 sol = cvxopt.solvers.qp(H, matrix(0.0, (2*n,1)), A, b) return [sol['x'][2*i] for i in range(n)], [sol['x'][2*i+1] for i in range(n)]关键技巧：在实际项目中，H矩阵的对角线元素取值很有讲究。建议x/y方向采用不同权重（如1.0和0.8），这样可以更好地适应车道保持场景。
2.2 ST图速度规划实战
ST图的核心思想是将动态障碍物映射到时间-路程坐标系中。一个典型的处理流程包括：
- 障碍物投影：将感知模块检测到的动态障碍物预测轨迹转换为ST图中的占用区域
- 可行通道识别：寻找未被障碍物占据的连续区域
- 速度曲线优化：在可行通道内生成舒适、节能的速度剖面
import numpy as np from scipy.optimize import minimize def st_speed_planning(st_graph, init_state): """ st_graph: { 'static_obs': [(s_start, s_end)], 'dynamic_obs': [(t_start, t_end, s_start, s_end)] } init_state: [s0, v0, a0] """ # 构造非线性约束 constraints = [] # 静态障碍物约束 (位置约束) for s_start, s_end in st_graph['static_obs']: constraints.append({ 'type': 'ineq', 'fun': lambda x, ss=s_start: x[0] - ss # s ≥ s_start }) constraints.append({ 'type': 'ineq', 'fun': lambda x, se=s_end: se - x[0] # s ≤ s_end }) # 动态障碍物约束 (时空约束) for t_start, t_end, s_start, s_end in st_graph['dynamic_obs']: constraints.append({ 'type': 'ineq', 'fun': lambda x, ts=t_start, te=t_end, ss=s_start, se=s_end: (x[0] - ss) if ts <= x[1] <= te else 1.0 }) # 运动学约束 constraints.append({'type': 'ineq', 'fun': lambda x: x[2]}) # a ≥ 0 constraints.append({'type': 'ineq', 'fun': lambda x: 2.5 - x[2]}) # a ≤ 2.5m/s² # 优化目标：舒适性(加速度变化率最小) res = minimize( lambda x: x[3]**2, # jerk项 x0=[init_state[0], 0, init_state[2], 0], constraints=constraints, bounds=( (0, None), # s (0, 10.0), # t (0, 2.5), # a (-3.0, 3.0) # jerk ) ) return res.x避坑指南：动态障碍物的时间预测往往存在误差，建议在约束条件中加入时间缓冲（如±0.5s）。实测显示这能降低30%以上的急刹情况。
3. 多场景应用与参数调优
3.1 城市道路场景
特点：
- 动态障碍物多（行人、自行车）
- 路径曲率变化大
- 需要频繁加减速
参数配置建议：
qp_params: smooth_weight: 0.7 curvature_weight: 0.3 max_iter: 50 st_params: time_resolution: 0.2s speed_resolution: 0.5m/s prediction_horizon: 5s3.2 高速公路场景
特点：
- 障碍物运动规律性强
- 路径以直线为主
- 速度变化平缓
参数配置建议：
qp_params: smooth_weight: 0.9 curvature_weight: 0.1 max_iter: 30 st_params: time_resolution: 0.5s speed_resolution: 1.0m/s prediction_horizon: 10s3.3 狭窄通道场景（如停车场）
特点：
- 静态障碍物密集
- 路径曲率大
- 容错空间小
特殊处理：
- 在QP中增加路径点密度（每米3-5个点）
- 使用非对称障碍物约束（如左侧放宽，右侧收紧）
- 在ST图中降低最大允许速度
4. 典型问题排查手册
4.1 QP求解失败常见原因
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| 无可行解 | 约束条件矛盾 | 检查障碍物膨胀半径 |
| 求解时间过长 | 问题规模太大 | 减少路径点数量 |
| 路径震荡 | 平滑权重过低 | 增加H矩阵对角线元素 |
4.2 ST图规划异常处理
问题1：速度曲线频繁波动
- 检查动态障碍物预测是否跳变
- 增加jerk惩罚项的权重
- 降低求解器容忍误差（如将ftol从1e-5调到1e-6）
问题2：无法避开近距离切入障碍
- 在QP层增加应急路径生成
- 缩短ST图规划周期（从100ms降到50ms）
- 引入交互预测模型（如博弈论方法）
5. 进阶优化方向
5.1 混合A*与QP的级联规划
对于特别复杂的场景（如直角转弯），可以：
- 先用A*生成粗路径
- 对关键转折点施加硬约束
- 再用QP进行平滑优化
5.2 基于机器学习的参数自适应
通过强化学习动态调整：
- QP中各代价项的权重系数
- ST图的时间/空间分辨率
- 障碍物约束的松弛因子
5.3 多车协同规划
当多个智能体共享ST图时：
- 将其他智能体的规划结果作为动态障碍
- 引入优先级机制（如主干道优先）
- 通过V2X通信同步ST图信息
在实际部署中发现，这套方案在Intel i7-1185G7处理器上单次规划耗时约15ms（路径点数量=50），完全满足实时性要求。对于计算资源更受限的平台，可以考虑以下优化：
- 使用OSQP替代CVXOPT（速度提升约40%）
- 采用固定点运算（精度损失约2%，速度提升3倍）
- 实现热启动机制（连续规划时复用上一帧的解作为初始值）
