自动驾驶决策规划算法第一章

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

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

感谢:忠厚老实的老王

下面是他的主页:忠厚老实的老王的个人空间_哔哩哔哩_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个边界条件

自动驾驶决策规划算法第一章_第1张图片

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

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

自动驾驶决策规划算法第一章_第2张图片

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

答案:五次多项式

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

所以不妨设

​带入边界条件

自动驾驶决策规划算法第一章_第3张图片

​最终得到

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

​其次边界条件

自动驾驶决策规划算法第一章_第4张图片

​记

则有 

自动驾驶决策规划算法第一章_第5张图片

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

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

复习:使泛函自动驾驶决策规划算法第一章_第6张图片​取极小值的f,满足E-L方程

自动驾驶决策规划算法第一章_第7张图片

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

 约束:自动驾驶决策规划算法第一章_第8张图片

 Lagrange乘子

自动驾驶决策规划算法第一章_第9张图片

​广义E-L方程:

 分别求偏导得到

​带入自动驾驶决策规划算法第一章_第10张图片

自动驾驶决策规划算法第一章_第11张图片

​结论:五次多项式是带约束的泛函\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)是连续可导函数)

自动驾驶决策规划算法第一章_第12张图片

①算出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]上的最小值

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

自动驾驶决策规划算法第一章_第13张图片

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

自动驾驶决策规划算法第一章_第14张图片

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

常见迭代法有

1、牛顿法《数值分析》

2、梯度下降法

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

梯度下降法大概过程

自动驾驶决策规划算法第一章_第15张图片

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

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

自动驾驶决策规划算法第一章_第16张图片

自动驾驶决策规划算法第一章_第17张图片

​也有一些性质较好的问题,比如:

自动驾驶决策规划算法第一章_第18张图片

​或者

自动驾驶决策规划算法第一章_第19张图片

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

凸优化必有两个性质:

①Cost function只有一个极值点,且为极小值(凸函数)

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

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

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

自动驾驶决策规划算法第一章_第20张图片

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

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

凸空间

非凸空间自动驾驶决策规划算法第一章_第21张图片

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

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

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

自动驾驶决策规划算法第一章_第22张图片

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

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

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

自动驾驶决策规划算法第一章_第23张图片

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

自动驾驶决策规划算法第一章_第24张图片

自动驾驶决策规划算法第一章_第25张图片

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

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

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

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

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

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

自动驾驶决策规划算法第一章_第26张图片

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

​采样,比大小,求出A最小,A本质上是离散约束空间的最优解(粗解)

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

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

自动驾驶决策规划算法第一章_第27张图片

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

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

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

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

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

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

自动驾驶决策规划算法第一章_第28张图片

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

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

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

给出两种方案:

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

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坐标系定义如下

自动驾驶决策规划算法第一章_第29张图片

对于 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''

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

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

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

自动驾驶决策规划算法第一章_第30张图片

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

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

自动驾驶决策规划算法第一章_第31张图片

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

自动驾驶决策规划算法第一章_第32张图片

​预备知识1:

自动驾驶决策规划算法第一章_第33张图片

​拓展:

​预备知识2: frenet公式

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

自动驾驶决策规划算法第一章_第34张图片

​frenet公式:

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

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

其中:k为曲率

自动驾驶决策规划算法第一章_第35张图片

​拓展:

自动驾驶决策规划算法第一章_第36张图片

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

拓展2:

自动驾驶决策规划算法第一章_第37张图片

​求\overrightarrow{a}

自动驾驶决策规划算法第一章_第38张图片

​总结:

自动驾驶决策规划算法第一章_第39张图片

​以下变量都以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} 车的位矢在车轨迹上的法线方向单位向量

辅助公式

自动驾驶决策规划算法第一章_第40张图片

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}计算得到

自动驾驶决策规划算法第一章_第41张图片

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

 (5)计算\ddot{s}

自动驾驶决策规划算法第一章_第42张图片

​博客公式推导:

辅助公式⑦ 

自动驾驶决策规划算法第一章_第43张图片

​(6)计算\ddot{l}

自动驾驶决策规划算法第一章_第44张图片

​ (7)计算l''

自动驾驶决策规划算法第一章_第45张图片

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

自动驾驶决策规划算法第一章_第46张图片

​遗留的三个问题

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

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

自动驾驶决策规划算法第一章_第47张图片

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

找到匹配点可以进行近似

自动驾驶决策规划算法第一章_第48张图片

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

​原理

自动驾驶决策规划算法第一章_第49张图片

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

​求\theta_{r},k_r

自动驾驶决策规划算法第一章_第50张图片

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

自动驾驶决策规划算法第一章_第51张图片

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

自动驾驶决策规划算法第一章_第52张图片

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

精确算法例子:

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

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

自动驾驶决策规划算法第一章_第53张图片

4、Frenet坐标转Cattisian坐标

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

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

自动驾驶决策规划算法第一章_第54张图片

​得到

 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}

自动驾驶决策规划算法第一章_第55张图片

找到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、其余变量使用公式算出

自动驾驶决策规划算法第一章_第56张图片

自动驾驶决策规划算法第一章_第57张图片

​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

首先这里给出上面推导过的直角坐标转换自然坐标公式:

自动驾驶决策规划算法第一章_第58张图片

​​接下来计算

 (1)算 \overrightarrow{r_h}

自动驾驶决策规划算法第一章_第59张图片

根据几何关系可以得到\overrightarrow{r_h}

​(2)算\overrightarrow{v_h}

由公式②③

​设\overrightarrow{v_h}​与x轴夹角为\theta_h

自动驾驶决策规划算法第一章_第60张图片

​带入得到v的模

​接下来算方向,吧上面两个式子相除得到,(注意这里结合公式⑤计算l')

​得到自动驾驶决策规划算法第一章_第61张图片

由此计算出方向向量

自动驾驶决策规划算法第一章_第62张图片

​(3)算\overrightarrow{a_h}

自动驾驶决策规划算法第一章_第63张图片

​和上面差不多

自动驾驶决策规划算法第一章_第64张图片

​注意,博客中的a_x​,实际上是| \overrightarrow{\dot{v}}|​。博客中的v_x​,实际上是| \overrightarrow{v}|

自动驾驶决策规划算法第一章_第65张图片

​(4)算k_h

自动驾驶决策规划算法第一章_第66张图片

​至此,已经介绍完第一章

第二章如下

自动驾驶决策规划算法第二章_免费教学录影带的博客-CSDN博客

你可能感兴趣的:(Plan,算法,自动驾驶,人工智能)