Apollo_路径优化
与之前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）边界约束 ![l\left ( s \right ) \in l_{B}\left ( s \right ), \forall s\in \left [ 0,s_{max} \right ]](http://img.e-com-net.com/image/info8/d2278226dc924a21976876815a66f168.gif){kind=small}
（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博客
Apollo轨迹规划技术分享 - 程十三 - 博客园
百度 Apollo 轨迹规划技术分享笔记 - 知乎
基于二次规划的路径规划算法 - 知乎
Apollo 6.0 QP（二次规划）算法解析 - 知乎
【开发者讲堂】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
Apollo 6.0的EM Planner (2)：路径规划的二次规划QP过程 - 知乎
Apollo EM Planner（DP决策+QP优化）_牛仔很忙吧的博客-CSDN博客