基于动态规划的自动驾驶路径与速度联合规划系统解析

基于动态规划的路径规划和速度规划 参考apollo 的dp路径规划和速度规划 更新:增加cpp代码实现在自动驾驶系统中轨迹规划模块承担着将感知与决策结果转化为可执行运动指令的关键任务。本文将深入剖析一套基于动态规划Dynamic Programming, DP的路径与速度联合规划系统该系统参考了 Apollo 自动驾驶平台中 EM Planner 的设计思想实现了高效、鲁棒且可配置的局部轨迹生成能力。一、系统架构与设计思想该系统采用经典的“路径-速度解耦”策略将复杂的六维轨迹规划问题分解为两个相互关联但可独立求解的子问题路径规划Path Planning在 Frenet 坐标系SL 图中规划一条从当前位置到目标区域的横向l偏移曲线。该曲线仅关注几何形状不涉及时间信息。速度规划Speed Planning在 ST 图纵向距离-时间图中规划一条纵向s随时间t变化的速度曲线。该曲线决定了车辆沿已规划路径前进的快慢。这种解耦设计极大地简化了问题复杂度。系统首先通过动态规划进行粗糙规划Coarse Planning快速生成一条满足基本约束的候选轨迹后续可在此基础上进行基于优化的精细规划Fine Planning以进一步提升轨迹的平滑性和舒适性。本文聚焦于其核心的动态规划实现部分。整个规划流程遵循“逆向构建正向回溯”的动态规划原则。系统从终点或最远规划点开始逐层或逐时间步向前计算每个状态点的最优代价并记录其最优前驱节点。完成所有状态的代价计算后从起点出发根据记录的前驱信息即可高效地回溯出整条全局最优轨迹。二、路径规划模块详解路径规划模块的目标是在给定的道路参考线上找到一条避开静态与动态障碍物、同时满足车辆运动学约束如曲率、加速度的平滑路径。1. 图构建Graph Construction系统首先在 Frenet 坐标系下构建一个分层图Layered Graph纵向s 方向将规划范围例如 90 米均匀划分为若干层如 5 层。每一层代表一个固定的纵向距离。横向l 方向在每一层上根据道路边界如lmin-2,lmax2米均匀采样多个离散点如 9 个点。这些点构成了图中的节点。这种网格化的节点布局为后续的动态规划搜索提供了离散化的状态空间。2. 代价函数与连接对于图中任意两个相邻层的节点系统使用五次多项式进行连接。五次多项式能够精确约束起点和终点的位置、速度和加速度在路径规划中通常设为0从而生成平滑的过渡曲线。基于动态规划的路径规划和速度规划 参考apollo 的dp路径规划和速度规划 更新:增加cpp代码实现连接的代价函数是规划效果的核心它综合了以下因素舒适性与平顺性通过计算路径的横向偏移l、横向速度dl和横向加速度ddl的平方和来衡量。值越小路径越平直、越舒适。安全性进行精确的碰撞检测。系统将自车和障碍物都建模为矩形并在路径的每个采样点检查二者是否发生重叠。一旦检测到碰撞该连接的代价被设为无穷大inf从而在规划中被彻底排除。3. 动态规划求解系统从第二层开始逐层向前更新每个节点的累计最小代价。对于当前层的每个节点遍历上一层的所有节点计算连接代价并更新当前节点的最优代价及其父节点索引。这一过程确保了每个节点都存储了从起点到该节点的最优路径信息。规划完成后从最后一层中选择累计代价最小的节点并根据记录的父节点索引逐层回溯即可得到完整的最优路径点序列。三、速度规划模块详解速度规划模块在已知路径的前提下决定车辆沿该路径行驶的速度剖面以安全、高效地处理与障碍物的时空交互。1. 障碍物时空映射这是速度规划的关键前置步骤。系统将动态障碍物如前车根据其预测轨迹速度 × 时间投影到已规划的路径上。对于 ST 图中的每一个时间采样点系统计算出障碍物在该时刻会占据的纵向距离s区间。这个区间在 ST 图上形成了一个禁止区域。根据决策模块给出的指令如“跟车”或“超车”系统会动态调整这些禁止区域跟车-1车辆必须始终位于障碍物后方因此禁止区域是[0, s_min]。超车1车辆必须完成超越因此禁止区域是[s_max, ∞]。2. ST 图构建与状态转移速度规划同样构建一个分层图但其维度是时间和纵向距离ST 图。时间t 方向将规划时域例如 7 秒划分为若干层。纵向距离s 方向在每一时间层上根据物理约束最大/最小速度、加速度和障碍物禁止区域动态计算出一个合理的 s 值采样范围并在其中生成节点。与路径规划不同速度规划在连接相邻时间层的节点时会严格考虑车辆的运动学边界如最大加速度 10 m/s²最小加速度 -5 m/s²从而大幅减少无效的连接提高搜索效率。3. 代价函数与求解速度规划的代价函数主要关注巡航效率惩罚与期望巡航速度如 10 m/s的偏差。乘坐舒适性惩罚过大的加速度jerk 的近似。求解过程与路径规划类似通过动态规划填充 ST 图中每个节点的最优代价并最终回溯得到最优的 (s, t) 序列即速度曲线。四、系统集成与扩展性该系统设计具有良好的模块化和可扩展性弯道适应通过引入referencePathFrenet等平滑参考线工具系统可以轻松处理弯曲道路。所有在 Frenet 坐标系下的规划结果最终都能通过坐标变换映射回全局笛卡尔坐标系用于控制执行。动态仿真系统提供了完整的动态仿真框架能够将规划出的路径和速度曲线结合起来实时模拟车辆的运动并动态更新障碍物位置直观地验证规划算法的有效性。C 实现除了 MATLAB/Python 原型系统还提供了完整的 C 实现包括基于 Clothoid回旋曲线的高精度参考线拟合、以及使用 IPOPT 求解器进行后续优化的接口为实际工程部署奠定了基础。综上所述这套基于动态规划的路径与速度规划系统通过清晰的解耦设计、严谨的代价函数和高效的图搜索算法为自动驾驶车辆提供了一种可靠且高效的局部轨迹生成方案是理解现代自动驾驶规划栈的重要范例。