Apollo规划代码ros移植-EM planner

之前出了这一篇文章，Apollo6.0规划代码ros移植-路径规划可跑工程分享。

EM规划器

首先，规划的方法是先独立对所有参考线进行单独规划，每条参考线可以得到一个最优的行进路线；最后综合所有参考线的最优行进路线，选择"全局"最优的路线。EM规划器在最新的Apollo6.0里面在PublicRoadPlanner里面，也就是名字全改了，但是问题不大，我们就把核心代码提取出来用。现在分析单个车道的规划过程:

首先，要明白SL和ST：
SL投影：障碍物投影到Frenet坐标。

静态障碍物：直接通过Cartesian-Frenet 转换（笛卡尔坐标到F坐标）。
动态障碍物：在Apollo中通过障碍物移动轨迹描述，结合上一个cycle计算得到的轨迹，可以评估动态障碍物和ego car之间的在不同时间点的位置关系。其中两者重叠的部分会被标记在F坐标中。（由于动态障碍物往往会导致闪避nudge，所以SL投影里只会考虑低速和靠近中的障碍物）
ST投影：
根据生成的path，将所有障碍物（包含高速的）投影到station-time frame，这个ST图是基于参考线做的，但无论是障碍物的轨迹，还是自车的轨迹，都是基于每条path来存储及对比是否重叠的以及何时重叠的。如果障碍物轨迹与规划轨迹重叠，便标记出这个区域。

1.EM模块输入:

(1).感知模块Perception和预测模块Prediction得到的障碍物以及未来时间(e.g. 5s内)的运动轨迹
(2).当前路况，禁停区、减速带、停车标志、人行横道等等
(3).本车状态
(4).参考线 ReferenceLine
从参考线提供器ReferenceLineProvider可以看到，当无人车面临变道时，就会存在多条参考线(变道时间可随机扰动，变道早可能进入车道1，变道晚就可能进入车道2)。那么EM规划器就需要计算每条参考线上的最优行驶路径，最后进行综合，得到开销(也就是cost)最小的一条路径。这里，我们从最简单的单车道避障开始学起。

2.EM模块步骤：

(1)路径规划的DP：
EM planner中，路径的选择是基于S-L坐标系进行的，主要分为以下几个步骤：
a.道路撒点
撒点规则主要由以下几个方面确定：车辆的宽度，车辆的位置，车道宽度，车辆速度，撒点的最大步长（S和L方向），撒点的最小步长（S和L方向），撒点的最小长度，撒点的最大长度等。如下图，定义level数量和间隔，再定义每层level的采样数量。

b.利用DP生成Cost最小的路径。
总结就是：撒点，会得到图中一列列黑色的点。forward过程，相邻两列之间的采样点通过五次多项式进行连接，并计算cost、更新父节点。backword过程，顺藤摸瓜，找到cost最小的路径。
(2)路径规划的QP：
S-L域路径规划的QP过程主要进行曲线的平滑，这个曲线平滑的思路，很巧妙的将离散点转换为连续的多项式曲线表达式，速度的非线性优化NLP过程中，就用这个思路把路标点中离散的曲率kappa拟合成了一个关于路程S的连续形式的函数，以便计算向心加速度时使用，速度的上下界也是用这个思路从离散转连续的。通过DP过程可以得到采样点中最优的一串点，虽然DP过程是用五次多项式连接这些点，但是还不够平滑，这时候就需要对这串离散的点平滑一下。
这里遇到了太多坑，虽然说关于二次规划qp的原理有很多，甚至官网也有，自行百度。简单来说，就是建立优化目标函数，和建立边界约束等各种约束，我代码里有学习注释。
第一个坑就是，使用的求解器是qpoases，而不是Lattice二次规划的osqp，虽然都是二次优化库，但是写法不一样。第二个坑就是，在Apollo6.0版本里面一开始找不到路径规划DP输出的路径是怎么样输入给QP的，后面发现6.0的DP弃用了，直接使用了类似Lattice的二次规划进行求解。接着，在3.5里面看到使用了分段sqp优化。当然，这里有很多参数可以调节，来设置轨迹的样子，具体可以下载我的代码自己学习调参。

个人见解（非正确）：
Apollo6.0的路径QP是直接跟lattice的二次规划一样，这么说来，DP好像没什么必要。

(3)速度规划的DP：
在路径规划中，已经给出了一条从规划起始点到规划终点开销cost最小的路径，如下图的蓝色星星组成的路径，每个蓝色的路径点都有相对于参考线的累计距离s和侧方相对距离l。那么剩下最后一个问题：每个规划点的速度怎么设定？换句话说无人车应该在什么时间点到达轨迹点？这就需要考虑障碍物在每个时间间隔的位置。

与路径DP过程的区别：
a.多了剪枝操作。
b.不用五次多项式连接两点计算EdgeCost。
c.回溯终点的选择，遍历每一列找到best_end_point，因为可能在最大采样时刻前就已经到达终点。
与3.5版的代码相比，新版代码在对s采样稍微复杂一些，不再是均匀采样，而是分为密集和稀疏两种采样。在应用过程中，为方便起见，可以采用老版代码中均匀的采样方法。
6.0版本的采样:
采样的时候横坐标是t，纵坐标是s，可以想象一个图，在每一t时刻（从0开始），采样很多个离散的s值。采样函数如图，其中，密集采样点的个数dense_dimension_s_、密集采样点的单位距离dense_unit_s_、稀疏采样点的单位距离sparse_unit_s_是事先设置好的。total_length_s_是DP Path中规划出的路径的长度，因此，稀疏采样的长度sparse_length_s就是总的路径长度total_length_s_减去密集采样点的距离。

采样示意图：

3.5版本的均匀采样:

一看代码，我选择老的版本哈哈哈。工程代码里面两个版本的采样都有，3.5/5.0在dp_st_graph.cpp，6.0在gridded_path_time_graph.cpp。

(4)速度规划的QP：
动态规划版本的速度规划器对无人车的速度v、加速度a以及方向theta等信息处理能力较弱，只能通过临近几个st节点来估测，存在偏差。而基于二次规划QP的速度规划器，实用多项式拟合st点，得到多项式函数，这时候就可以根据导数求速度，二阶导求加速度等信息。
移植的时候碰到大问题，速度的DP规划感觉没问题，但是呢，使用3.5版本的sqp优化之后的速度，居然是减速的，搞不明白为什么，还在研究。
然后我先使用了6.0的qp优化。

总的框架

1.路径规划DP，Apollo6.0的采样，参考文章：
Apollo 6.0的EM Planner (1)：路径规划的动态规划DP过程.

2.路径规划QP，Apollo3.5的SQP优化，参考文章：
Baidu Apollo代码解析之EM Planner中的QP Path Optimizer 2

自动驾驶决策规划算法第二章第四节(中) 路径二次规划算法

3.速度规划DP，Apollo6.0的采样，参考文章：
Apollo 6.0的EM Planner (3)：速度规划的动态规划DP过程

4.速度规划QP，Apollo6.0QP优化，参考文章：
Apollo 6.0的EM Planner (4)：速度规划的二次规划QP过程

感谢

我还参考了这些大佬的文章，感谢这些博客的博主对我移植的帮助，非常感激！大家原理上面可以看这些大佬的博客代码解析。
链接:
【最新版】Baidu Apollo代码详细解析——EM Planner中的DP Speed Optimizer.

Baidu Apollo代码解析之EM Planner中的QP Speed Optimizer 2.

Apollo EMPlanner.

Lattice与EM_planner的区别

摘自：Baidu Apollo代码解析之轨迹规划中的轨迹评估代价函数.
从公式来看，Lattice是在已知轨迹的情况下评估，考虑的因素更为具体和全面。而EM是在轨迹未知的情况下求轨迹，考虑的因素更单纯，公式形式更简单。总体来说，二者的代价函数均是用来解决同一问题的，因此考虑的因素大体一致，轨迹的平滑、避免碰撞、与参考方程（参考线、前步输出、目标速度等guidance）吻合等因素均纳入了考虑。但是，二者的代价函数没有必然联系。评估轨迹cost 时，可以结合局部的cost 和全局route 层面的cost，这样规划和决策会有一定程度的结合

效果可看课程系列：
https://www.bilibili.com/video/BV1j94y1B7Ai/?buvid=XY4BB7D48AA71766FB7E422428AB2162768E4&is_story_h5=false&mid=vLcmcVJZwLVM5M3cErNrKw%3D%3D&plat_id=240&share_from=ugc&share_medium=android&share_plat=android&share_source=QQ&share_tag=s_i&timestamp=1689339850&unique_k=CP6TAse&up_id=300556577

需要代码可私信。