与之前EM规划和Lattice规划不同,Apollo 5.0版本使用的路径规划,更加的灵活方便,原因主要是采用了数值优化的思想,通过边界约束等,保证了密集障碍物场景的灵活性。 而之前的lattice等算法由于采样的局限,导致在复杂环境下可能存在无解的情况。
我们的规划任务实际上分为路径规划和速度规划。您提到的更改是在路径规划方面,以使用piecewise_jerk_path_optimizer(即QP)处理静态障碍。在速度规划上,我们仍然使用 DP + QP(piecewise_jerk_speed_optimizer) 框架。
在piecewise_jerk_path_optimizer中,不考虑与上一帧路径的距离,我们只考虑当前状态,或者说路径应该始终与当前状态一致,这足以让路径至少与当前状态平滑连接,我们不真的不需要与未来的道路保持一致。DP非常耗时。其次,有时我们可能会因为分辨率和采样点的位置而无法找到路径。我们目前的解决方案是:
- 根据地图、交通规则和障碍物找到路径边界。里面的区域是“可驾驶的”。
- 然后使用分段jerk算法来寻找路径。它解决了DP解决方案中的上述两个主要问题。
Frenet坐标系、参考线是基础,叠加交规决策、路径边界决策、速度边界决策;
主要关注lane follow场景,配置文件为modules\planning\conf\scenario\lane_follow_config.pb.txt
scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGE
stage_config: {
stage_type: LANE_FOLLOW_DEFAULT_STAGE
enabled: true
task_type: LANE_CHANGE_DECIDER
task_type: PATH_REUSE_DECIDER
task_type: PATH_LANE_BORROW_DECIDER
task_type: PATH_BOUNDS_DECIDER
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: ST_BOUNDS_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: SPEED_HEURISTIC_OPTIMIZER
task_type: SPEED_DECIDER
task_type: SPEED_BOUNDS_FINAL_DECIDER
# task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
task_type: RSS_DECIDER
......
一种包含15/16个任务,本文主要关注第五个PIECEWISE_JERK_PATH_OPTIMIZER
一、算法思想:
分段常加加速度路径优化 Piecewise jerk Path optimization
二、算法理论
离散s,优化变量为l,l',l'',建模为QP问题
优化变量:
目标函数:
最后一项:凸空间中央
约束条件:
(1)边界约束
(2)连续性约束
(3)曲率约束
三、代码解析:
planning\tasks\optimizers\piecewise_jerk_path\piecewise_jerk_path_optimizer.cc
planning\math\piecewise_jerk\piecewise_jerk_problem.cc
planning\math\piecewise_jerk\piecewise_jerk_path_problem.cc
目标函数各项和约束条件各项需要结合代码深入理解
【规划】Apollo QSQP接口详解_lemon_zy的博客-CSDN博客
csc_matrix稀疏矩阵理解_WangN2的博客-CSDN博客_csc稀疏矩阵
Apoollo中piecewise_jerk_problem中CSC矩阵及连续性约束相关问题 - 知乎
自动驾驶决策规划算法第二章第四节(中) 路径二次规划算法_哔哩哔哩_bilibili
Planning-Apollo路径决策规划及问题_Schulz King的博客-CSDN博客_apollo路径决策
Planning-小鹏汽车NGP系统Planning模块分析_Schulz King的博客-CSDN博客
【开发者讲堂】Parking与Public_road示例_哔哩哔哩_bilibili how to debug planning
Apollo Planning决策规划代码详细解析 (10):PiecewiseJerkPathOptimizer_自动驾驶 Player的博客-CSDN博客
Apollo源码分析:路径规划 (v5.5)_yuyuelongfly的博客-CSDN博客_apollo 路径规划
Apollo 算法阅读之Public Road轨迹规划算法--路径规划(含源代码)_同学醒醒放学了的博客-CSDN博客_apollo public road