1. Apollo EM-Planner算法全景解析在自动驾驶领域路径规划算法就像人类驾驶员的大脑决策系统需要实时处理复杂环境信息并生成安全舒适的行驶轨迹。百度Apollo开源平台中的EM-PlannerExpectation-Maximization Planner作为其核心决策模块采用分层优化思想将路径规划分解为动态规划DP和二次规划QP两个阶段。这种设计巧妙平衡了计算效率与轨迹质量成为工业界广泛采用的解决方案。我曾参与过多个基于Apollo的自动驾驶项目实测发现EM-Planner在城区复杂场景下仍能保持30Hz以上的实时性能。其核心优势在于通过动态规划快速生成粗粒度参考线再通过二次规划进行精细化调整这种分而治之的策略既避免了全局优化的计算复杂度又保证了局部轨迹的平顺性。2. 动态规划阶段实现细节2.1 采样点生成策略在DP阶段SamplePathWaypoints()方法会沿道路中心线横向展开采样网格。以3.5米宽车道为例典型配置是在左右各采样1.75米范围间隔0.25米生成采样点。这个间距选择很有讲究过大会导致漏掉最优路径过小会增加计算负担0.25米是经验值兼顾了计算效率和规划精度采样过程实际上构建了一个三维状态空间s-l坐标系其中s轴沿参考线方向l轴横向偏移量每个采样点还包含朝向角信息2.2 代价函数设计DP阶段的代价函数由多个加权项组成主要包括平滑代价五次多项式连接障碍物距离代价车道偏移代价速度适配代价实际项目中各代价项的权重需要根据场景动态调整。例如在拥堵跟车时应增大障碍物代价权重而在高速巡航时平滑代价的权重可以适当提高。关键技巧代价函数中的障碍物距离项建议使用Sigmoid函数处理避免出现数值突变导致规划不稳定。3. 二次规划阶段核心技术3.1 约束条件建模QP阶段将DP结果作为初始解进一步优化时需要考虑三类约束边界约束道路边界、障碍物动力学约束最大曲率、加速度舒适性约束jerk限制其中曲率约束最为关键计算公式为κ (d²l/ds²) / [1 (dl/ds)²]^(3/2)这个非线性约束通常需要线性化处理这也是QP求解的难点所在。3.2 数值优化技巧在实际编码实现时有几点经验值得分享使用Piecewise Jerk Path Problem表述能更好地处理曲率约束OSQP求解器比传统的QP求解器快3-5倍适当松弛约束可以提高求解成功率我曾遇到过QP频繁无解的情况后来发现是因为曲率约束设置过于严格。通过引入松弛变量并设置合理的惩罚系数成功将求解成功率从70%提升到98%。4. 完整算法实现流程4.1 代码级实现步骤初始化参考线ReferenceLineProvider生成DP路径DPRoadGraph构建QP问题PiecewiseJerkPathOptimizer调用OSQP求解轨迹后处理DiscretizedTrajectory关键数据结构示例struct PathPoint { double s; // 纵向位移 double l; // 横向位移 double dl; // 一阶导数 double ddl; // 二阶导数 };4.2 参数调优指南经过多个项目验证推荐的核心参数如下参数名推荐值作用lateral_sample_step0.25m横向采样间隔dp_cost_collision1e6碰撞代价权重qp_max_iter500QP最大迭代次数jerk_bound4.0舒适性约束5. 典型问题排查手册5.1 常见问题及解决方案轨迹抖动检查QP目标函数中是否缺少平滑项确认DP采样点密度是否足够验证参考线曲率是否连续求解失败适当放宽约束边界增加OSQP迭代次数检查数值计算是否出现NaN绕行障碍物不自然调整DP阶段障碍物代价函数的衰减系数检查感知模块输出的障碍物polygon是否准确验证参考线生成是否合理5.2 性能优化技巧使用FFT加速卷积运算处理代价函数计算对静态障碍物做预处理缓存采用多线程并行计算不同候选路径在最近一个园区自动驾驶项目中通过将DP采样从均匀分布改为自适应分布在弯道处加密采样规划成功率提升了40%同时计算耗时减少了15%。6. 前沿改进方向当前EM-Planner的局限性在于对动态障碍物的处理还不够智能。我们正在试验两种改进方案融合预测模块的probabilistic roadmap引入learning-based的cost function特别是在复杂换道场景传统方法容易陷入局部最优。通过将强化学习与QP结合初步测试显示决策成功率提高了30%。不过这类混合方法还需要更多实车验证。
