基于动态规划的Apollo路径规划和速度规划实现（附Cpp代码）
基于动态规划的路径规划和速度规划 参考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,lmax=2米）均匀采样多个离散点（如 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 求解器进行后续优化的接口，为实际工程部署奠定了基础。
综上所述，这套基于动态规划的路径与速度规划系统，通过清晰的解耦设计、严谨的代价函数和高效的图搜索算法，为自动驾驶车辆提供了一种可靠且高效的局部轨迹生成方案，是理解现代自动驾驶规划栈的重要范例。