智能驾驶路径规划：QP优化与ST图应用

1. 基于QP的路径规划原理与实现

在智能驾驶系统中，路径规划的核心任务是在复杂环境中找到一条从起点到终点的最优路径。二次规划（Quadratic Programming, QP）因其高效的求解能力和良好的数学特性，成为路径规划领域的重要工具。

1.1 QP问题的数学表述

二次规划问题的标准形式可以表示为：
minimize (1/2)xᵀPx + qᵀx
subject to Gx ≤ h
Ax = b

在路径规划应用中：

• x代表待优化的路径参数（通常是路径点的坐标）
• P矩阵体现路径平滑度的权重
• q向量包含路径长度等线性目标项
• Gx ≤ h表示障碍物避让等不等式约束
• Ax = b表示起点终点位置等等式约束

1.2 实际应用中的关键考量

在实际工程实现中，有几个关键点需要特别注意：

1. 目标函数设计：通常需要平衡多个优化目标：

   • 路径长度最短
   • 曲率最小（保证行驶舒适性）
   • 与障碍物保持安全距离
   • 符合车辆动力学约束

2. 约束条件处理：

   • 静态障碍物可以用多边形或圆形区域表示
   • 动态障碍物需要引入时间维度（ST图）
   • 车辆本身的转弯半径限制等物理约束

3. 数值稳定性：

   • 合理设置优化变量的尺度
   • 添加正则化项避免矩阵奇异
   • 使用可靠的QP求解器（如OSQP、qpOASES）

2. ST图速度规划详解

2.1 ST图的基本概念

ST图（Space-Time Graph）是一种将空间和时间维度结合起来的表示方法，特别适合处理动态环境下的运动规划问题。在ST图中：

• 横轴（S轴）代表空间位置（可以是路径长度或车道纵向位置）
• 纵轴（T轴）代表时间
• 障碍物在ST图中表现为禁区（不可行区域）

2.2 速度规划的实现步骤

1. 环境建模：

   • 将预测的其他交通参与者轨迹映射到ST图中
   • 确定安全缓冲区大小（考虑感知误差和控制延迟）

2. 可行走廊构建：

   • 在ST图中找出未被障碍物占据的区域
   • 连接这些区域形成可行通道

3. QP问题构建：

   • 目标函数：舒适性（加速度最小）、效率（时间最短）
   • 约束条件：速度/加速度限制、避障约束、跟车距离等

4. 轨迹优化：

   • 使用QP求解得到最优速度剖面
   • 后处理确保轨迹的平滑性和可行性

3. 多场景避障策略

3.1 静态障碍物场景

对于静态障碍物，典型的处理流程包括：

1. 环境感知与地图构建
2. 可行路径搜索（如A*、RRT*等）
3. 使用QP进行路径优化
4. 速度规划与轨迹生成

关键技巧：

• 对障碍物进行适当膨胀（考虑车辆轮廓）
• 在狭窄区域使用多阶段规划
• 引入回旋曲线（Clothoid）保证曲率连续

3.2 动态障碍物场景

动态场景需要结合ST图进行处理：

1. 预测其他交通参与者的运动轨迹
2. 在ST图中标记障碍物区域
3. 规划本车的ST轨迹
4. 实时更新和重规划

注意事项：

• 预测不确定性处理（多假设预测）
• 规划时考虑制动能力
• 保留应急方案（如紧急停车区域）

3.3 复杂交互场景

在交叉口、合流区等复杂场景中：

• 考虑交通规则和优先权
• 使用博弈论方法处理交互
• 设计适当的让步策略
• 保持轨迹的可解释性（便于其他交通参与者预测）

4. 工程实现与优化

4.1 计算效率优化

在实际系统中，计算效率至关重要：

• 使用稀疏矩阵表示QP问题
• 采用热启动（warm start）技术
• 实现增量式更新
• 合理设置规划频率

4.2 鲁棒性增强

提高系统鲁棒性的方法：

• 多传感器融合提高感知可靠性
• 预测-规划闭环验证
• 安全监控层设计
• 降级模式处理

4.3 实际部署经验

从实际项目经验中总结的要点：

• 参数调试需要大量场景验证
• 重视corner case的收集和处理
• 规划模块与控制模块的接口设计
• 系统延迟的测量和补偿

5. 常见问题与解决方案

5.1 QP求解失败

可能原因及对策：

• 问题不可行：检查约束条件是否过严
• 数值不稳定：调整正则化参数
• 求解器配置不当：检查终止条件和迭代次数

5.2 轨迹抖动

解决方法：

• 增加路径平滑项权重
• 使用低通滤波器后处理
• 检查感知数据的噪声水平

5.3 实时性不足

优化方向：

• 降低规划问题维度
• 使用更高效的QP求解器
• 并行化计算流程

在实际应用中，我发现将QP规划与基于采样的方法结合使用往往能取得更好的效果。例如先用RRT*生成粗略路径，再用QP进行精细化优化，这样既能保证全局最优性，又能获得高质量的局部轨迹。