基于Baidu Apollo EM Planner的决策规划算法总结

基于Baidu Apollo EM Planner的决策规划算法总结

EM Planner Framework

EM Planner是Apollo决策规划模块的核心架构，其核心思想为Expectation Maxmium，所谓E-step，可理解为获取周围环境信息，即获得SL和ST图；M-step可理解为使用动态规划和二次进行轨迹优化，DP的目的是获得一条粗略的path（speed profile），可理解为对环境有一个大概的感知，从而使QP的configuration space为凸空间，QP的目的是通过二次规划的方法，在一定约束下，获得一条使cost function最小的轨迹。

Frenet坐标系

在学习决策规划算法相关知识前，应先了解什么是Frenet坐标系，因为在自动驾驶决策规划模块，基本都是基于Frenet坐标系进行的。

如上图所示，Frenet坐标系是基于参考线的坐标系，在自动驾驶中通常选取道路中心线为参考线，在Cartesian坐标系下，寻找某点与参考线最近距离，此距离即为Frenet坐标系下横向坐标L，最近点与参考线起始点之间参考线的长度即为Frenet坐标系下纵坐标S，故Frenet坐标系又叫做SL坐标系，如下图所示。

DP & QP

在Baidu Apollo决策规划模块中，采用先DP后QP的方法来生成一条可供控制模块使用的符合车辆动力学要求的轨迹。这里还有非常重要的一点是将路径和速度分开优化，从而将一个三维问题拆分为二维问题，即path-speed optimization。

DP(dynamc programming)

DP的作用是根据当前环境信息，通过路径规划算法搜索出一条粗略的轨迹，从而使QP的configuration space为凸空间，尽量避免QP的最优解收敛到局部最优解的情况。

如上图所示，DP模块首先根据预测模块传入的数据建图，然后根据所建的图进行路径搜索，最后根据所得路径对障碍物打tag，从而使QP的configuration space为凸空间。

常用的路径规划算法

算法类型	算法名称	算法特点
基于采样	RRT	非最优
基于采样	RRT*	渐进最优
基于搜索	lattice sample dp	最优
基于搜索	Dijlstra	最优
基于搜索	A*	最优

上表为常用的经典搜索算法，目前路径规划中常用的经典搜索算法均为上表算法或其变种。

cost function构造

在DP路径规划（速度规划）中，cost function的构造是非常重要的一步
在路径规划中，cost function通常由平滑度（headinfg、curvature）、安全性（碰撞检测）、车道线代价（与参考线距离）三部分组成，如下图。

在速度规划中，cost function通常由舒适度（acc、jerk）、安全性（碰撞检测）、车速代价（在满足交通规则的情况下尽可能高速行驶）三部分组成，如下图。

QP(quartic programming)

QP部分是在DP粗略搜索出来的凸空间下进行二次规划，从而优化出一条符合车辆动力学要求的轨迹，将其发送给控制模块。

cost function构造

QP问题本质上与DP问题是相同的，都属于优化问题，因此cost function的构造十分重要。
对于路径优化，主要考虑平滑度（heading、curvature、dot curvature）、参考线代价两部分，如下图所示。

对于速度优化，主要考虑舒适性（velocity、acc、jerk），如下图。

constraints

QP约束主要包括等式约束和不等式约束两部分，等式约束主要考虑各段多项式之间至少满足二阶导数连续，不等式约束主要考虑满足DP给定的凸空间边界约束。
QP问题最终交由求解器求解。

总结

Baidu Apollo EM Planner是国内最常用的路径规划架构，其核心思想为先DP后QP，同时讲path和speed分开优化。