基于动态规划的自动驾驶路径与速度规划系统代码功能说明

基于动态规划的路径规划和速度规划 参考apollo 的dp路径规划和速度规划 更新:增加cpp代码实现一、系统概述本系统是一套参考Apollo EM Planner架构设计的自动驾驶规划模块采用动态规划Dynamic Programming, DP算法分别实现路径规划与速度规划通过解耦合的分层设计在保证规划结果安全性与舒适性的同时兼顾计算效率。系统核心包含路径规划模块基于SL图、速度规划模块基于ST图、参考线生成模块基于Clothoid曲线拟合三大核心组件支持直道、弯道等多场景下的自主规划并提供Qt可视化界面与MATLAB仿真验证工具链。基于动态规划的路径规划和速度规划 参考apollo 的dp路径规划和速度规划 更新:增加cpp代码实现系统整体工作流程遵循“参考线生成→路径规划→速度规划→轨迹跟踪”的经典自动驾驶规划逻辑其中路径规划与速度规划均采用“粗糙采样DP精细优化”的双层策略粗糙层通过动态规划快速筛选可行解空间精细层通过五次/四次多项式拟合与Ipopt优化器生成平滑轨迹满足车辆运动学约束与舒适性要求。二、核心模块功能解析2.1 参考线生成模块2.1.1 功能定位参考线是路径规划的基准负责将真实世界的笛卡尔坐标x/y转换为便于规划的Frenet坐标s/l同时通过平滑处理保证后续规划轨迹的连续性。本模块支持基于离散路点生成G2连续位置、角度、曲率连续的参考线为路径规划提供高精度基准。2.1.2 关键技术特性Clothoid曲线拟合采用Clothoid回旋线作为基础曲线元素相比传统的多项式曲线Clothoid曲线可通过曲率线性变化实现更平滑的转向过渡更符合车辆运动学特性。G2连续性优化通过Ipopt求解器对Clothoid样条的参数theta进行优化确保参考线在连接点处的位置、角度、曲率均连续避免车辆在参考线跟踪过程中出现急刹或急转弯。坐标转换能力提供全局笛卡尔坐标与Frenet坐标的双向转换接口支持将车辆实时状态位置、速度、加速度与障碍物信息投影到Frenet坐标系简化规划逻辑。2.1.3 核心功能接口接口名称功能描述输入参数输出参数FrenetReferencePath构造函数基于离散路点生成平滑参考线路点数量、x坐标数组、y坐标数组初始化后的参考线对象interpolate根据Frenet坐标s插值得到参考线状态纵向距离s笛卡尔坐标x/y、航向角th、曲率k、曲率变化率dkglobal2frenet全局笛卡尔坐标转换为Frenet坐标车辆全局状态x/y/th/v/a/kFrenet状态s/ds/dds/l/dl/ddlfrenet2globalFrenet坐标转换为全局笛卡尔坐标Frenet状态s/ds/dds/l/dl/ddl车辆全局状态x/y/th/v/a/k2.2 路径规划模块DPPathGraph2.2.1 功能定位路径规划模块基于SL图纵向距离s-横向偏移l开展规划核心目标是在满足车辆运动学约束、避障约束的前提下生成从起点到终点的最优横向轨迹。模块通过动态规划算法遍历可行解空间筛选出代价最小的路径并通过多项式拟合生成平滑轨迹。2.2.2 核心工作流程SL图节点初始化沿纵向s方向划分多层节点每层在横向l方向按固定间隔采样生成节点形成网格状的规划空间。第一层仅包含起点节点初始s/l后续层节点根据横向边界lmin/lmax均匀分布。节点连接与代价计算采用五次多项式连接相邻两层的节点多项式满足车辆横向运动学约束位置、速度、加速度连续。对每条连接路径计算代价代价包含两部分-避障代价通过矩形碰撞检测判断路径是否与障碍物重叠若重叠则代价设为无穷大不可行路径-平滑代价累计横向偏移l、横向速度dl、横向加速度ddl的平方和惩罚剧烈的横向运动保证舒适性。动态规划寻优从第二层开始遍历当前层每个节点与上一层所有节点的连接更新当前节点的最小累计代价与父节点索引最后一层找到最小代价节点后通过父节点索引回溯生成最优路径。轨迹平滑处理对回溯得到的关键节点再次通过五次多项式拟合生成连续轨迹确保最终输出的路径满足车辆运动学约束。2.2.3 关键约束与参数运动学约束通过五次多项式强制横向位置、速度、加速度连续避免车辆在路径切换时出现冲击避障约束采用矩形碰撞检测车辆与障碍物均视为矩形判断路径上所有采样点是否与障碍物重叠确保路径安全性可配置参数纵向层数maxlevelnum、横向采样点数maxlnum、纵向采样间隔sample_ds、横向边界lmin/lmax等参数可根据场景需求调整平衡规划精度与计算效率。2.3 速度规划模块DPSpeedGraph2.3.1 功能定位速度规划模块基于ST图时间t-纵向距离s开展规划在路径规划结果的基础上生成满足车辆动力学约束速度、加速度与避障约束的纵向速度轨迹。核心目标是在保证不与障碍物碰撞的前提下使车辆以接近巡航速度行驶同时兼顾舒适性。2.3.2 核心工作流程ST图障碍物投影根据障碍物的运动状态初始位置、速度预测其在不同时间t的位置再将障碍物投影到路径规划生成的轨迹上确定每个时间t内障碍物在ST图上占据的纵向距离范围smin/smax。同时结合决策策略超车1/跟车-1确定可行的s范围如超车时需避开障碍物上方区域跟车时需避开障碍物下方区域。ST图节点初始化沿时间轴t方向划分多层节点每层在纵向距离s方向根据车辆动力学约束最大/最小加速度、速度筛选可行节点。第一层仅包含起点节点初始t0s0后续层节点需满足-动力学约束当前层s的最大值/最小值由上一层的最大/最小速度与加速度计算得出-避障约束当前层s需避开障碍物在该时间t占据的范围。节点连接与代价计算采用线性模型计算相邻两层节点的速度vΔs/Δt与加速度aΔv/Δt代价包含两部分-速度偏差代价惩罚与巡航速度的偏差鼓励车辆以经济速度行驶-加速度代价惩罚过大的加速度保证乘坐舒适性。动态规划寻优与路径规划逻辑类似通过遍历节点连接更新最小累计代价最后从最后一层回溯生成最优ST轨迹再通过插值得到连续的速度曲线。2.3.3 关键约束与参数动力学约束限制最大加速度maxacc、最小加速度minacc负数值表示制动、最大速度maxv、最小速度minv确保规划的速度轨迹可被车辆执行决策约束根据障碍物的决策策略超车/跟车动态调整可行s范围如超车时需快速通过障碍物区域跟车时需保持安全距离可配置参数ST图时间总长totaltime、纵向距离总长totalpath_length、时间采样间隔dt、纵向采样间隔ds等参数可根据场景调整适配不同的行驶速度与障碍物密度。2.4 可视化与仿真模块2.4.1 功能定位可视化模块负责将规划过程与结果以图形化方式展示便于开发调试与效果验证仿真模块则通过动态模拟车辆行驶过程验证规划结果的可行性与安全性。2.4.2 关键功能特性Qt可视化界面基于QGraphicsView实现交互式视图支持轨迹参考线、规划路径、障碍物、车辆实时位置的绘制同时支持鼠标滚轮缩放与拖拽便于查看细节MATLAB仿真工具提供多种仿真脚本支持直道、弯道等场景的静态路径验证以及动态行驶过程的仿真如障碍物移动、车辆轨迹跟踪并可生成仿真视频用于结果分析多场景支持通过修改障碍物的决策策略超车/跟车、初始位置、速度等参数可快速验证不同场景下的规划效果如“前车跟车后车超车”“前后车均超车”等场景。三、系统交互逻辑3.1 模块间数据流向参考线生成→路径规划参考线模块将离散路点转换为Frenet坐标下的平滑参考线路径规划模块以该参考线为基准在SL图中开展规划路径规划→速度规划路径规划模块输出的最优路径SL坐标传递给速度规划模块速度规划模块将障碍物信息投影到该路径对应的ST图中生成速度轨迹速度规划→轨迹跟踪速度规划模块输出的ST轨迹s-t关系与路径规划的SL轨迹结合通过Frenet→笛卡尔坐标转换生成车辆可执行的全局轨迹x/y/t/v传递给控制模块执行。3.2 典型场景执行流程以直道避障为例场景输入初始车辆状态s0, l0, v15m/s、障碍物信息2个障碍物分别位于s20m/l2m、s40m/l-0.5m决策策略分别为“超车”“跟车”参考线生成基于道路中心线的离散路点生成G2连续的参考线路径规划在SL图中生成5层节点每层9个横向采样点通过DP算法筛选出避开障碍物的最优横向路径如对第一个障碍物选择左侧超车对第二个障碍物选择右侧跟车速度规划在ST图中预测障碍物运动轨迹生成7秒内的速度轨迹确保车辆在超车时加速至20m/s跟车时减速至5m/s同时避免与障碍物碰撞可视化与仿真通过Qt界面实时显示车辆行驶轨迹、障碍物位置通过MATLAB仿真验证轨迹的平滑性与安全性。四、技术优势与适用场景4.1 技术优势解耦合设计路径规划与速度规划分离便于单独优化与维护同时可根据场景需求灵活调整某一模块的策略动态规划的鲁棒性DP算法可遍历所有可行解空间确保找到全局最优解或近似最优解相比贪心算法更能应对复杂障碍物场景平滑性与舒适性通过Clothoid参考线、五次多项式轨迹拟合、加速度代价惩罚等设计确保规划轨迹满足车辆运动学约束与乘坐舒适性要求可扩展性强支持通过配置参数适配不同车型调整车辆尺寸、动力学约束与场景调整采样密度、决策策略无需大规模修改代码。4.2 适用场景高速场景适用于高速公路等结构化道路可处理“跟车”“超车”等典型驾驶行为结构化城市道路支持直道、弯道等道路形态可应对静态障碍物如施工区域与动态障碍物如其他车辆仿真与验证提供完整的可视化与仿真工具链可用于自动驾驶算法的离线验证与开发调试。五、使用建议与注意事项5.1 参数配置建议采样密度调整在障碍物密集场景如城市道路建议增加纵向层数maxlevelnum与横向采样点数maxlnum提高规划精度在空旷场景如高速公路可减少采样密度降低计算耗时动力学参数适配根据实际车型调整最大加速度maxacc、最小加速度minacc、车辆尺寸vehh/vehw确保规划轨迹符合车辆实际执行能力代价权重优化若需优先保证舒适性可增大加速度代价的权重若需优先保证效率可增大速度偏差代价的权重。5.2 注意事项Ipopt优化器依赖参考线生成模块依赖Ipopt求解器需确保编译环境中正确配置Ipopt库否则无法生成G2连续的参考线坐标转换精度Frenet坐标与笛卡尔坐标的转换精度依赖参考线的平滑性若离散路点过于稀疏可能导致转换误差增大建议路点间隔不超过5m动态障碍物预测速度规划模块对障碍物的预测基于恒定速度模型若障碍物存在加减速行为需扩展预测模型如恒定加速度模型否则可能出现碰撞风险。六、总结本系统通过动态规划算法实现了自动驾驶场景下的路径与速度规划核心优势在于“解耦合设计”“鲁棒性寻优”“平滑性保障”可满足结构化道路下的多场景规划需求。系统提供的参考线生成、坐标转换、可视化仿真等辅助功能形成了完整的规划工具链便于开发人员快速验证算法效果与迭代优化。后续可进一步扩展的方向包括支持非结构化道路场景如越野环境、引入更精准的障碍物预测模型如基于深度学习的轨迹预测、优化动态规划的计算效率如通过剪枝减少无效节点遍历以适应更复杂的自动驾驶场景需求。