前言
本课程由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 + 1 − P k P_{k+1}-P_kP k+1之差,最小的情况是三个点在一条直线上,意味着这时的车是比较舒适的,没有左右摇摆。
第二项是Pk距离指引线(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}||两个向量之间的差值约等于||P_i-P_{i-1}||*sin(\theta_i/2)2∗又因为θ \thetaθ足够小,所以约等于PiPi1θ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 → i + 1 ′ ′ ′ = l i + 1 ′ ′ − l i ′ ′ △ s l^{'''}然后进行Multiple shooting formulation对△ s 进行采样,最后对状态进行泰勒展开,得到的结果就是我们的状态空间。

Objectives
接下来讨论下Cost Function优化函数是如何得到的:
在这里插入图片描述

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

其中:

在这里插入图片描述

表示车辆位置与boundary中心的距离,l i 表示车辆偏离s的距离,l m i n i l^i 和l m a x i l^i表示boundary的左边界和右边界。

在这里插入图片描述

这俩部分,是出于对舒适性的考虑。w l ′ ∗ ∑ i = 0 n − 1 l i ′ 2 w_{l'}*\displaystyle\sum^{n-1}_{i=0} l^{'2}表示速度,w l ′ ′ ∗ ∑ i = 0 n − 1 l i ′ ′ 2 w_{l''}*\displaystyle表示加速度,可以理解成横向不要走的太快,方向盘也不要打的太猛。w l ′ ′ ′ ∗ ∑ i = 0 n − 2 ( l i + 1 ′ ′ − l i ′ ′ △ s ) 2 可以理解成Jerk(三阶系统的体现),即方向盘不要打的太猛,也不要总是左右的变化。

Constraints
在这里插入图片描述

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

假设:

△ θ ≈ 0 ,车辆朝向与reference line平行。
l ′ ′ ≈ 0 图片,横向二阶导几乎为零。
这样,可以将以下公式(该公式取于某文献,目前尚未找到)

在这里插入图片描述


简化为:

在这里插入图片描述

这里需要对曲率K进行最大限制,否则转弯半径会过小,导致无法转弯。
再把K m a x 的约束K m a x = t a n ( α m a x ) L 代入,可以得到最终的约束:
在这里插入图片描述


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

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
在这里插入图片描述