前置学习内容：【自动驾驶】【零基础】基础自动驾驶控制算法笔记

注意：最好学习前置控制算法，因为决策规划仿真中需要用到

感谢：忠厚老实的老王

下面是他的主页：忠厚老实的老王的个人空间_哔哩哔哩_bilibili

序章：决策规划算法概述

第一章：数学基础

第二章：Apollo EM Planner理论篇

第三章：Apollo EM Planner代码篇

终章：决策规划算法总结

序章：决策规划算法概述

自动驾驶6个级别：L0-L5

L0	没有任何自动驾驶功能
L1	有横向和纵向的自动驾驶功能，但是横纵向无法联合使用
L2	横纵向可以联合使用，但驾驶员必须对一切状况负责
L3	功能与L2基本相同，最大的区别在于责任，对于部分场景，驾驶员不必负责
L4	大部分道路皆可自动驾驶，大部分场景都不需要驾驶员负责
L5	完全自动驾驶

两条关键因素区分：功能和责任

L0-L2主要是功能区分，L3-L5主要是责任区分，而不同公司L2与L2之间差距巨大，只要厂家宣称驾驶员需要负全责，即使功能做到了L2，本质上还是L2

L2本身比较宽泛，只有定速巡航+车道保持的汽车也是L2，什么都能做，但是驾驶员要负全责的汽车也是L2

决策规划算法就是普通L2到什么都能做的L2的一个重要模块

感知	人的眼睛，耳朵
控制	人的小脑，双脚
决策规划	人的大脑

功能越往上做，越丰富，越复杂，决策规划算法越重要，也越难在L4中决策规划模块可以说是最重要，也是最复杂，最难做的模块

难做到把整个模块一分为三，还要加上地图模块，每块单独地去做才能勉强完成“大脑”的工作，而且截止到2021年也不能说完全的解决

整个决策规划模块一分为三，下面分别介绍

1、导航规划算法，此算法将计算出大地图上A到B的最优路径，此算法与机器人导航，手机导航算法基本一致，长度在几公里到几百公里不等，该算法是整个规划模块中最为成熟的算法

特点：导航算法会给一个粗略的，大范围的路径，但是此路径不会考虑如何避障，也不会考虑车辆动力学约束，一般规划的路径时不规则的折线。导航算法一般只需要执行一次，只有遇到大范围的拥堵，施工，偏航等情况才会再次执行

2、行为规划算法，又叫决策算法，决定车辆行驶意图的算法，对于静态障碍物，到底往左绕还是往右绕，对于动态障碍物，到底该减速避让，还是加速超车？决策算法决定了车辆的意图，这也是整个规划算法中最难做的部分。

特点：决策算法会给一个车辆的行驶意图，会指导车辆该避让，该超车，该往左转该右转，但是决策不会给具体的运动建议，例如往左转多少度，车辆加速到多少。

实际环境瞬息万变，因此决策算法需要较高的执行频率，一般为10Hz

决策算法需要有一定的稳定性，不允许在周围环境稳定时出现“朝令夕改”现象

3、运动规划算法：根据决策给出的行为意图在相关的时空中搜索出（或优化出）一条具有详细路径速度信息，并且满足各个约束条件的轨迹，并将此轨迹发给控制模块去跟踪，此轨迹长度一般在几米到几十米不等

特点：运功规划生成的轨迹是决策规划模块的最终输出，具有详细的路径速度的信息，执行频率与决策相同，为10Hz，同样，运动规划也要求具有一定的稳定性。

本次教学将详细讲解决策规划算法与运动规划算法，不讲当行算法（因为相对成熟），以Apollo EM planner为例，EM planner是Apollo的经典决策规划算法，擅长处理复杂环境下的决策规划问题，也是Apollo默认的决策规划算法。

提醒：截止到2021年，Apollo已经迭代到6.0，EM planner在Apollo 1.5中加入，目前6.0的EM planner与1.5相比已经发生了一些变化，现在6.0的EM planner换了个名字叫做OnLane Planning，我现在讲解的是最初的1.5版本的EM planner，当然思想上殊途同归，不过建议学完后看一看6.0的OnLane Planning

第一章：数学基础

一、细说五次多项式

五次多项式时规划论文里的常客，本节将详细解释五次多项式的特殊性

tips：本节难度较大，但并不重要，听不懂记住结论即可

1、Jerk介绍

车辆运动规划中，一个非常重要的指标就是舒适性，在物理中，衡量舒适性的物理量是跃度，英语为Jerk

Jerk的定义为加速度的导数，即 $Jerk = \frac{da}{dt}$ （a为加速度），Jerk的绝对值越小，意味着a的变化越平缓，a的变化越平缓也就意味着越舒适

设有一个质点的轨迹s=f(t)，则 $Jerk = \frac{d^{3}f}{dt^{3}}$ ，若在[0,T]区间的时间中，Jerk的绝对值都比较小，那就意味着在[0,T]内规划的轨迹是比较舒适的

那么数学问题就变成了：若有一个函数s = f(t)，那么什么样的f(t)使得在[0,T]内Jerk的绝对值变化平缓？由于绝对值处理较繁，一般改成平方，也就是说，问题变为

$find \ x\ s.t.\ min\int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt$

显然，积分 $\int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt$ 是一个关于f(t)的泛函，积分的值取决于f(t)在[0,T]上的整体形状

那么 $\int_{0}^{T}(\frac{d^{3}f}{dt^{3}})^{2}dt$ 取极小值的f(t)是什么？显然，当f(t)为二次或者二次以下函数时，

$\int_{0}^{T}(\dddot{f})^{2}dt = 0$ 最小

所以若要让Jerk在[0,T]上的绝对值最小，f(t)应取二次或者二次以下函数

但是真实情况远比想象中复杂，因为真实的s = f(t)往往带有约束

6个边界条件

二次函数 $y = ax^{2} + bx + c$ 只有3个系数（a,b,c），无法满足6个边界条件

在真实情况下，往往要求带边界约束的泛函

那么满足带约束的泛函 $\int_{0}^{T}(\dddot{f})^{2}dt = 0$ 极值问题的解是什么？

答案：五次多项式

解：显然f(t)只可能是在[0,T]上是有界连续函数，因为无论是无界还是有界间断函数都会使Jerk出现无穷大

所以不妨设

带入边界条件

最终得到

观察这个式子，由于 $Jerk = \dddot{f}(t)$ ，所以 $s_0,v_0,a_0$ 的值不影响Jerk

其次边界条件

记

则有

最终，问题变为求下的极小值

泛函极值的必要条件为Euler-Lagrange方程

复习：使泛函取极小值的f，满足E-L方程

求解：泛函 $\int_{0}^{T}(\dddot{f})^{2}dt = 0$

约束：

Lagrange乘子

广义E-L方程：

分别求偏导得到

带入

结论：五次多项式是带约束的泛函 $\int_{0}^{T}(\dddot{f})^{2}dt = 0$ 取极小值的解函数

所以五次多项式在规划中才这么常见

二、凸优化与非凸优化

自动驾驶规划目标：算出一条满足约束的最优轨迹

然而，什么是“最优”？

指标：①平滑性；②舒适性；③尽可能短，耗时少。

约束：①轨迹连续性；②无碰撞；③遵守交规；④车辆动力学。

衡量轨迹质量用Cost function表示

假设 $s = f(t)=a_{0}+a_{1}t+...+a_{5}t^{5}$ （系数均为未知常数）

则Cost Function $J = w_{1}\dot{f}^{2} + w_{2}\ddot{f}^{2} + w_{3}\dddot{f}^{2}$

J越小，意味着越平滑舒适，当然也要满足各种约束

数学问题：求解Cost Function在约束条件下的最小值问题

如何计算y=f(x)的最小值（在约束下）

回忆：高中时如何解y = f(x)在x∈[a,b]上的最小值（f(x)是连续可导函数）

①算出 $f(a),f(b)$

②求出y' = f'(x)

③令y' = 0，解出f'(x)= 0的根，设为 $x_1, x_2, ...,x_n$

④计算 $f(x_1),f(x_2),...,f(x_n)$ 与 $f(a),f(b)$ 比较， $f(x_1),f(x_2),...,f(x_n),f(a),f(b)$ 中最小的那个就是y = f(x)在[a,b]上的最小值

但是此方法无法快速求解高维度复杂约束下的最小值问题，比如

有的时候就算求出极值点，但是极值点很多，难以快速计算，或者约束复杂

一般求复杂函数在复杂约束下的最小值问题都采用迭代法

常见迭代法有

1、牛顿法《数值分析》

2、梯度下降法

3、高斯牛顿法《视觉slam十四讲》

梯度下降法大概过程

按照梯度的反向来迭代，通过导数大小来判断收敛，要么收敛到极值点，要么收敛到边界

迭代法缺点：对初值敏感，有可能会收敛到局部极小值

也有一些性质较好的问题，比如：

或者

这种性质比较好的问题叫做凸优化

凸优化必有两个性质：

①Cost function只有一个极值点，且为极小值（凸函数）

②约束空间是一块“完整的”“不破碎的”空间（凸空间）

求凸函数在凸空间的最小值问题称为凸优化

凸空间的严谨定义，由如多边形引申

凸多边形定义：对于多边形内部任意的点x,y，都有(x+y)/2也在多边形内部，此多边形为凸多边形

凹多边形定义：对于多边形内部存在点x,y，使得(x+y)/2不在多边形内部，此多边形为凹多边形

凸空间

非凸空间

凸优化是最简单的唯一研究明白的非线性优化方法，是所有优化问题的基石

自动驾驶规划的Cost function是凸的，约束空间也得是凸空间才可以当作凸优化问题求解

问题：自动驾驶避障约束空间是不是凸空间答案：不是

如图，上面的线和下面都满足避障约束，但是加起来除以二就不满足了，因此不是凸空间，是一个“破碎的空间”

对于动态的障碍物也是一样的

假设场景为一个人和一辆车，车要动态避障行人

规划车速快或者慢，都满足避障约束

但是如果取二分之车速，就不满足约束，会发生交通事故

静态和动态避障约束空间都不是凸的，所以规划问题是比较难的

如何求解非凸问题的最小值？

答：目前为止，并无完美的非凸问题算法，求解非凸问题的主要思路是找非凸问题中的凸结构

启发式算法：先随机在约束空间采样一些离散的函数值，比大小，取最小的作为迭代初值

举例：在这些黄色采样点中找到最小点作为初值迭代

对于非凸函数或者非凸空间，也是一样，先在约束空间采样，找到采样点的最小值，本质上是连续空间离散化后，离散约束空间的最优解

采样，比大小，求出A最小，A本质上是离散约束空间的最优解（粗解）

非凸空间 --> 离散化 --> 粗解 --> 迭代 --> 最终解

对于一个非凸问题，如果采样越少，越容易收敛到局部最优，而不是全局最优

那么我采样点多一些，就会出现“维度灾难”

一维要采样100个点，二维要采样100*100 = 1w个点，三维可能要100w个点

所以非凸问题没有一个尽善尽美的解法，只能根据实际问题去调整

三、直角坐标与自然坐标转换

本节主要推导Frenet坐标系与Cartesian坐标转换

龙格现象：高次多项式拟合可能会出现震荡，慎用高次多项式

尽可能使用分段低次多项式去拟合，而不是高次多项式

自然坐标与直角坐标转换的难度巨大，需要对微积分非常熟练，熟悉曲线坐标系

幸运的是，不需要理解推导过程，只需要记住结论

给出两种方案：

①参考博客，实在看不懂记住结论

Frenet坐标系与Cartesian坐标系互转（一）：公式推导_windSeS的博客-CSDN博客_frenet坐标系②和老王一起推导，难度稍低

老王自创的向量法，可以降低推导难度

1、准备工作以及辅助公式推导

下面开始推导的准备工作

车记为host vehicle

已知车在Cartesian坐标系下的 $\overrightarrow{r_{h}} ,\overrightarrow{v_{h}} ,\overrightarrow{a_{h}} ,k_{h}$ ，求车在以道路为坐标轴的frenet坐标下的坐标 $s, \dot{s},\ddot{s},l, l',l'',\dot{l},\ddot{l}$ ，其中 $\dot{s} = \frac{ds}{dt},\dot{l} = \frac{dl}{dt},l' = \frac{dl}{ds}$

frenet坐标系与Cartesian坐标系定义如下

对于 EM planner，求 $s, \dot{s},\ddot{s},l, l',l''$

对于lattice planner，求 $s, \dot{s},\ddot{s},l,\dot{l},\ddot{l}$

$l',l'',\dot{l},\ddot{l}$ 可以互相转化

EM planner采用 $s, \dot{s},\ddot{s},l, l',l''$

在推导中，难度最大的就是曲线坐标系

曲线坐标系与直角坐标系的两点不同：

①曲线坐标系的基向量一般不是常向量

②点的曲线坐标变换与点的实际位移一般不一致

在直角坐标系下只有一个dx，在自然坐标系下有不同的ds

比如，车的轨迹在道路上的投影，两个速度一般不相等的

预备知识1：

拓展：

预备知识2： frenet公式

在曲线坐标系中， $\frac{d\tau }{ds}$ 一般不为 $\overrightarrow{0}$

frenet公式：

$\frac{d\overrightarrow{\tau} }{ds}=k\overrightarrow{n}$

$\frac{d\overrightarrow{n} }{ds}=-k\overrightarrow{\tau}$

其中：k为曲率

拓展：

$l, l',l''$ ， $l' = \frac{dl}{ds}$ ，ds指的是坐标轴的ds，上面四个公式的k都是直角坐标系下的k

拓展2：

求 $\overrightarrow{a}$

总结：

以下变量都以Cartesian坐标为准

$\overrightarrow{r_n}$	车的位矢	$\overrightarrow{r_r}$	投影位矢
$\overrightarrow{v}_{h}$	车的速度	$\dot{s}$	车在道路上的投影的速率
$\overrightarrow{a}_{h}$	车的加速度	$k_{r}$	投影位矢在道路几何上的曲率
$k_h$	车的位矢在车轨迹上的曲率	$\overrightarrow{\tau_r}$	投影位矢在道路几何上的切向方向单位向量
$\overrightarrow{\tau_h}$	车的位矢在车轨迹上的切线方向单位向量	$\overrightarrow{n_r}$	投影位矢在道路几何上的法向方向单位向量
$\overrightarrow{n_h}$	车的位矢在车轨迹上的法线方向单位向量

辅助公式

2、Frenet坐标转换为Cartesian坐标

问题定义如下：

已知frenet坐标系的起点 $(x_0,y_0)$

求 $s, \dot{s},\ddot{s},l, l',l''$ ，其中 $\dot{s} = \frac{ds}{dt},\dot{l} = \frac{dl}{dt},l' = \frac{dl}{ds}$ ，ds为frenet坐标轴的弧长

要解这个问题我们分三步：

第一步：7个辅助公式

第二步：找到车在frenet坐标下的投影点在Cartesian的坐标，记为 $x_r,y_r,\theta_r,k_r$

计算投影位矢以及其切向法向单位向量

第三步：利用向量三角形，以及微积分求出 $s, \dot{s},\ddot{s},l, l',l''$

核心公式： $\overrightarrow{r_r} + l\overrightarrow{n_r} = \overrightarrow{r_h}$

如何找到 $(x_h,y_h)$ 在frenet坐标下的投影 $(x_r,y_r,\theta_r,k_r)$ 暂时先不管，假设已经找到了 $x_r,y_r,\theta_r,k_r$

自然可得到 $\overrightarrow{r_r},\overrightarrow{\tau_r},\overrightarrow{n_r},k_r$

下面开始正式计算：

（1）计算 $l$

核心公式 $\overrightarrow{r_r} + l\overrightarrow{n_r} = \overrightarrow{r_h}$ ，把 $l \overrightarrow{n_r}$ 挪到等式右边得到 $l\overrightarrow{n_r} = \overrightarrow{r_h} - \overrightarrow{r_r}$ ，

然后两边点乘 $\overrightarrow{n_r}$ 得到： $l= (\overrightarrow{r_h} - \overrightarrow{r_r})\cdot \overrightarrow{n_r}$

注意：这里\overrightarrow{n_r} \cdot \overrightarrow{n_r} = 1,\overrightarrow{\n_r} \cdot \overrightarrow{\tau_r} = 0

（2）计算 $\dot{s}$

对核心公式两边求导得到： $\dot{\overrightarrow{r_r}} + l\dot{\overrightarrow{n_r}} + \dot{l}\overrightarrow{n_r} = \dot{\overrightarrow{r_h}}$

利用辅助公式①②⑤⑥带入

和博客有所不同，但其实一样

（3）计算 $\dot{l}$

由（2）得到，两边同时点乘 $\overrightarrow{n_r}$

立刻得到

（4）计算 $l'$

利用上面计算的 $\dot{l}$ 与 $\dot{s}$ 计算得到

(ds为frenet坐标轴的弧坐标的导数，所以ds/dt = $\dot{s}$ )

（5）计算 $\ddot{s}$

博客公式推导：

辅助公式⑦

（6）计算 $\ddot{l}$

（7）计算 $l''$

最终得到7个公式，可以把直角坐标系转换为自然坐标系

遗留的三个问题

1、如何找到 $x_r,y_r,\theta_r,k_r$

2、如何计算 s

3、如何从 $s, \dot{s},\ddot{s},l, l',l'',\dot{l},\ddot{l}$ 转回到直角坐标系去

3、匹配点

如何找到 $x_r,y_r,\theta_r,k_r$ 在《自动驾驶控制算法第七讲》有讲过

一般reference line都是离散点，只讲离散点的 $x_r,y_r,\theta_r,kr$

假设曲线上有一系列离散点，如果有蓝线的表达式，可以得到理想投影，

然而一般没有蓝线，只有离散点，以及他们的 $x_r,y_r,\theta_r,kr$ ，在离散曲线下的投影，必然是一个近似解，不可能得到一个理想投影。因为离散曲线缺失信息。只要近似程度满足自动驾驶要求即可。

找到匹配点可以进行近似

找到距离最短的，此点成为匹配点

原理

若为弧线，当曲率不太大，且离散的点较密时，精度也较高

求 $\theta_{r},k_r$

下面看如何计算s：以直线代替弧长

当路径点足够密，误差可以忽略

有其他更精确的算法，但是不稳定，以分段直线近似曲线虽然比较糙，但是对参数变化不敏感（robust）

精确算法例子：

虽然算法比较糙，但是计算的快，而且鲁棒，大多数场景适用

下面看如何从自然坐标转换到直角坐标，即

4、Frenet坐标转Cattisian坐标

关键还是找投影点的 $x_r,y_r,\theta_r,k_r$

1、计算 $x_i\rightarrow s_i$ 的对应表，即每个frenet坐标轴上的离散点的 $x_i$ 与 $s_i$ 的对应关系

得到

2、查找 $x_i \leftrightarrow s_i$ 表，找到 $s_n$ 使得 $s_n<s<s_{n+1}$ ， $s_n$ 与 $s_{n+1}$ 对应的 $x_i$ 记为 $x_n,x_{n+1}$

找到 $x_n,y_n,\theta_n,k_n$ 以及 $x_{n+1},y_{n+1},\theta_{n+1},k_{n+1}$

x_{n+1},y_{n+1},\theta_{n+1},k_{n+1}

3、其余变量使用公式算出

4、至此，我们就可以从自然坐标转换到直角坐标

下面进行推导：

已知 $s, \dot{s},\ddot{s},l, \dot{l},\ddot{l}, l',l'',\overrightarrow{r_r},\overrightarrow{\tau_r},\overrightarrow{n_r},k_r,k_r'$ ，求 $\overrightarrow{r_h},\overrightarrow{v_h},\overrightarrow{a_h},k_h$