Apollo 轨迹规划算法解析

这篇文章主要整理一下前段时间学习的轨迹规划算法，关于轨迹规划问题可以查到的相关资料很多，但能把轨迹规划问题详细说清楚的并不太多，这导致我在学习过程中出现很多疑惑。这里我把Apollo的各个版本轨迹规划算法进行简单总结和汇总。

主要参考资料：
1.b站大佬详细视频教程https://space.bilibili.com/287989852?spm_id_from=333.337.0.0
相关记录https://blog.csdn.net/zheshui69163?type=blog
2.https://blog.csdn.net/qq_23981335/article/details/121507369
3.https://blog.csdn.net/IHTY_NUI/article/details/113704591
4.https://zhuanlan.zhihu.com/p/444159631
5.https://www.cnblogs.com/liuzubing/p/11051390.html
6.https://blog.csdn.net/ChenGuiGan/article/details/122253193
7.https://github.com/Strange-AI/frenet_path_planning
8.https://github.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver
9.https://www.cnblogs.com/kin-zhang/p/15532655.html
10.《Baidu Apollo EM Motion Planner》

1.背景

1.1轨迹规划方法综述

参见https://blog.csdn.net/IHTY_NUI/article/details/113704591
本文所指轨迹规划是局部规划，对应其中的2.2和2.3节。

1.2轨迹规划主要策略

主要可以分为两种:
1.迭代优化方法，速度-路径解耦规划（即横纵向解耦规划，横向规划决定轨迹形状，纵向规划决定速度状态，两者均凸才是凸优化）
迭代优化算法过程包括交规决策、路径决策、路径优化、 速度决策、速度优化。

决策：为了限定非凸问题的解空间，通过决策的方法将问题转换为凸优化问题。包括路径决策和速度决策。

2.规划
2.SLT三维优化方法（时空联合规划算法）
如果采用时空联合规划算法，需要将时空联合决策结果提炼为凸可行域，求解复杂。目前业界广泛采用时空分离规划的自动驾驶方案，道路应变能力有限，容易造成急刹等不佳体验，轻舟依托时空联合规划，成为中国首家采用时空联合规划的自动驾驶公司。
总体来说，速度-路径解耦规划是目前主流的轨迹规划方法。本文主要针对解耦规划的方法

1.3一些重要的基础知识

1.3.1参考线

Frenet坐标系和大地坐标系的转换：很基础，没什么好说的
https://blog.csdn.net/zheshui69163/article/details/125963160

1.3.2五次多项式在规划问题中的重要性

离散的点一般用五次多项式来连接，这是因为：
首先，加速度变化率最小，即jerk最小化时，舒适性最高
Takahashi等人已经证明：任何Jerk最优化问题的解都可以用一个5次多项式来表示。

https://zhuanlan.zhihu.com/p/444159631
https://blog.csdn.net/zheshui69163/article/details/125841525

1.3.3轻决策和重决策

b站老王的视频里讲得非常清楚

https://blog.csdn.net/zheshui69163/article/details/126141171

Apollo各个版本轨迹规划

1.Apollo 1.5----Apollo3.0：EM planner，DP(动态规划)+QP(二次规划)，轻决策
这里需要注意的是：DP(动态规划)不是规划，是用于决策的
2.Apollo2.5：增加LatticePlanner
3.Apollo 3.5：重决策，DP+QP的最后一个版本，DP最后的尝试
4.Apollo 5.0----Apollo7.0：取消路径DP，保留速度DP，采用重决策，改为Public Road Planner
未来研究方向：规则兜底，深度学习决策
路径DP在最新版本中被取消，这是因为过多的场景下，每个场景需要使用不同的规划参数，失去了轻决策的最大优势，但是，了解DP+QP的算法对于我们学习轨迹规划仍然十分重要。

2.算法具体内容

1. EM Planner

具体步骤：

难点

如何将cost函数转换为QP公式
https://blog.csdn.net/qq_23981335/article/details/121507369
https://www.cnblogs.com/kin-zhang/p/15532655.html

2. Lattice Planner

Lattice规划过程相对简单，只需要采样轨迹末状态就可以生成一系列候选轨迹簇，然后根据cost function来选取最优。
Step1：初始状态已知，采样轨迹末状态。
Step2：横纵向采样，分别用五次多项式进行拟合。
Step3：遍历横纵向SL、ST曲线，组合成候选轨迹簇。
Step4：计算每条轨迹的cost，选取最优轨迹。

基于采样方法的路径规划代码，亲测有效https://github.com/ChenBohan/Robotics-Path-Planning-04-Quintic-Polynomial-Solver