自动驾驶之轨迹规划4——Apollo规划模块算法解析_20201219直播讲解

前言

本课程由Apollo平台fuyiqun主讲，视频链接如下：现场录屏，另外有官方解析文章：直播回顾丨Apollo自动驾驶论坛①规划模块算法解析（本篇另增问答整理），视频官方PPT在此（可免费下载）。下面是随堂记录。

Apollo规划模块算法解析

本文将为大家介绍Apollo规划模块（Planning）的算法解析，主要内容包括：

功能：基于预测的结果、路由模块（routing）的全局路线指引和地图信息，生成安全舒适的驾驶轨迹。

首先输入来自3个模块，包括Prediction、Chassis、Localization，目前Palnning模块是由这3个消息来触发的，触发之后我们会创建几个reader来读取Traffic Light和Routing信息，最后的输出是ADC Trajectory，也就是Planning的轨迹线。

---

1. 基于场景分类的规则和算法调用框架

在Apollo中，Planning的场景和任务管理是如何实现的？我们有一个双层的状态管理机，上层是一个场景管理器，叫做Scenario Manager，这里举了3个场景的例子，Pull Over、Parking、Junction，每个场景的配置文件/参数是不一样的。

有了场景之后，会基于不同的规划器（Planner）进行规划，Apollo中目前（20201219）有俩个Planner，一个是On Lane Planning Tasks（目前的主要规划方法），一个是Open Space Planning Tasks（泊车）。

On Lane Planning Tasks是默认的规划器，接下来重点介绍（Open Space Planning Tasks在GitHub有相关的论文和代码）。

有了规划器之后，会通过Path decision，把道路边界测出来。主要是通过不同脚本的task拆分完成的，比如Path decision，可以拆分成Lane Borrow和Lane Change。当decision做好之后，还会进行Path Planning。

有了轨迹后，可以再进行速度决策，速度决策也是通过小的task进行的。这里列了两个：

• Cross Lane Safety：主要是我们进行交叉变道时，对安全上的考虑。
• Lane Merge Waiting：主要是进行车辆汇流时，我们如何决策的。比如车辆需要等待，就需要生成一个Stop Fence。

有了Speed decision之后，用Speed的Optimizer ，来进行速度的规划。

---

2. 基于Frenet Frame的解耦合路径速度规划

2.1. 基于Frenet Frame的解耦合路径速度规划_Frent Frame

首先为什么要进行Frenet Frame？可以将笛卡尔坐标系下的运动状态进行了横纵向的分解。横向是指车辆l离我们的reference line的距离，纵向是沿着reference line走了多远，用s来表示。

通过这样的拆分，相当于把Planning降维了，可以大大的降低计算的复杂度。

2.2. 基于Frenet Frame的解耦合路径速度规划_reference line smoother

介绍下reference line smoother是如何做的：我们会直接优化离散点位置，用QCQP来解优化点间曲率，这样的运算时间大概是10ms。具体Cost Function如上图：

• 第一项表示俩个向量$P_k-P_{k-1}$、$P_{k+1}-P_k$之差，最小的情况是三个点在一条直线上，意味着这时的车是比较舒适的，没有左右摇摆。
• 第二项是$P_k$距离指引线（reference line）的距离，距离越小越好。
• 第三项是指俩个相邻点之间的距离不要差的太远。

上式的约束条件：

• $P_k$不能超过boundary的约束范围。
• 符合曲率的限制。

下面简单介绍下曲率限制的推导过程，另有一篇文章详细描述Apollo中的离散点平滑方法：

• $P_i$表示坐标。
• $\theta$表示向量$P_k-P_{k-1}$与$P_{k+1}-P_k$之间的夹角。
• $d$表示平均离散化后的长度。

假设：

• $||P_k-P_{k-1}||$与$||P_{k+1}-P_k||$俩点之间的距离是相等的。
• $\theta$足够小，方便计算时做近似。
• smoother前后的长度是相等的。

接下来我们可以进行计算：
$||2P_i-P_{i-1}-P_{i+1}||$两个向量之间的差值约等于$2\ast ||P_i-P_{i-1}||\ast sin({\theta }_i/2)$，又因为$\theta$足够小，所以约等于$||P_i-P_{i-1}||\ast {\theta }_i$，再对$\theta$进行曲率的转化，就可以变成$||P_i-P_{i-1}|{|}^2/R$，因为有最小的转弯半径，所以有了图中的不等式，也就是我们的约束条件。
有了约束条件我们就可以对原来的Loss Function进行求解。

---

3. 基于规则的路径和速度决策

接下来介绍下基于规则的路径和速度决策，即Path boundary和Speed boundary是如何生成的。

3.1. 基于规则的路径和速度决策_path bound decision

如图所示，存在这样一个场景：车辆在正常行驶，在它的前方停了一个障碍车。

这时，来看一下Path Bound Decision是如何做的。首先，车在起始状态时，会判断需不需要进行换道：

• 如果不需要进行换道，还需要判断是否需要借道（向左变道，再变回来），如果不需要借道，boundary就会限制在本车道内。如果我们需要借道，需要判断借道是否安全，如果不安全，boundary还会限制在本车道内。如果判断是安全的，就会进行借道路径的限制。
• 如果需要换道，首先还需要进行安全性判断。如果不安全，依然限制在本车道内，如果安全，就会进行换道的路径限制。

3.2. 基于规则的路径和速度决策_speed bound decision

4. 基于数值优化算法的路径和速度轨迹生成

4.1. 基于数值优化算法的路径和速度轨迹生成_piecewise jerk path optimization

此算法Piecewise Jerk Path Optimization主要思想是将车辆横向位置运动当作一个三阶系统，应用模型预测的方法求解二次规划问题。这个方法的单帧运算时间大概是10ms。

上图为Piecewise Jerk Path Optimization的全貌，左边为状态空间，右边第一部分为Cost Function，第二部分是Cost Function的展开，最下方是限制条件。

详细为大家介绍下：
States

首先将车辆横向位置运动建模成三阶系统$l_{i\to i+1}^{{}^{\prime \prime \prime }}=\frac{l_{i+1}^{{}^{\prime \prime }}-l_i^{{}^{\prime \prime }}}{△s}$，然后进行Multiple shooting formulation对$△s$进行采样，最后对状态进行泰勒展开，得到的结果就是我们的状态空间。

Objectives
接下来讨论下Cost Function优化函数是如何得到的：

左边是优化函数，做优化的目的是让车辆可以保持安全、舒适的行驶，生成一个相对安全和舒适的轨迹。

其中：

表示车辆位置与boundary中心的距离，$l_i$表示车辆偏离s的距离，$l_{min}^i$和$l_{max}^i$表示boundary的左边界和右边界。

这俩部分，是出于对舒适性的考虑。$w_{l^{\prime }}\ast \sum _{i=0}^{n-1}{l}_i^{{}^{\prime }2}$表示速度，$w_{l^{\prime \prime }}\ast \sum _{i=0}^{n-1}{l}_i^{{}^{\prime \prime }2}$表示加速度，可以理解成横向不要走的太快，方向盘也不要打的太猛。$w_{l^{\prime \prime \prime }}\ast \sum _{i=0}^{n-2}(\frac{l_{i+1}^{{}^{\prime \prime }}-l_i^{{}^{\prime \prime }}}{△s}{)}^2$可以理解成Jerk（三阶系统的体现），即方向盘不要打的太猛，也不要总是左右的变化。

Constraints

因为车辆存在最小的转弯半径，所以我们要对车辆运动学进行限制。由于转弯半径是基于笛卡尔坐标系的，需要基于Frenet坐标系进行转换。

假设：

• $△\theta \approx 0$，车辆朝向与reference line平行。
• $l^{{}^{\prime \prime }}\approx 0$图片，横向二阶导几乎为零。

这样，可以将以下公式（该公式取于某文献，目前尚未找到）

简化为：

这里需要对曲率K进行最大限制，否则转弯半径会过小，导致无法转弯。
再把$K_{max}$的约束$K_{max}=\frac{tan({\alpha }_{max})}{L}$代入，可以得到最终的约束：

其中$L$是指车身的长度。

---

4.2. 基于数值优化算法的路径和速度轨迹生成_piecewise jerk speed optimization

Piecewise Jerk Speed Optimization算法的主要思想是将车辆纵向位置运动当做一个三阶系统，应用模型预测的方法求解非线性优化问题，单帧的运算时间大概是40ms。

图中依然是我们的算法全貌，左边是状态空间，右上为目标函数，然后是限制条件。

state

刚刚做Path Optimization时，变量是s和l，现在做Speed Optimization时，则变成t和s。然后我们先建模成三阶系统：

然后进行Multiple shooting formulation 对△t进行采样，生成状态空间。再对状态空间进行泰勒展开得到：

Objectives

接下来时速度规划的Cost Function，如左图，解释下每项的意义：

是指时间的Objective达到最小

后面三项依然时基于舒适性考虑的，第一项是横向的加速度，第二项是纵向的加速度，第三项是Jerk（加速度的变化是舒适性的重要考虑因素）。

Constraints

限制条件主要考虑：

图片属于free space，然后有一个限速，加速度和刹车有最大值，然后jerk也有限制，图片表示向心加速度的限制，也就是转弯时不要转太猛。

以上基本及时Planning模块最常用的规划器算法，随着Apollo版本的迭代，也出现了不用应用场景下的Planner，如Open Space Planner和Learning based Planning。

5. 参考文献

reference文件如下：
DL-IAPS_and_PJSO_A_PathSpeed_Decoupled_Trajectory.pdf
TDR-OBCA A Reliable Planner for Autonomous Driving in Free-Space Environment.pdf