前置学习内容: 【自动驾驶】【零基础】基础自动驾驶控制算法笔记
注意:最好学习前置控制算法,因为决策规划仿真中需要用到
感谢:忠厚老实的老王
下面是他的主页:忠厚老实的老王的个人空间_哔哩哔哩_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的定义为加速度的导数,即(a为加速度),Jerk的绝对值越小,意味着a的变化越平缓,a的变化越平缓也就意味着越舒适
设有一个质点的轨迹s=f(t),则 ,若在[0,T]区间的时间中,Jerk的绝对值都比较小,那就意味着在[0,T]内规划的轨迹是比较舒适的
那么数学问题就变成了:若有一个函数s = f(t),那么什么样的f(t)使得在[0,T]内Jerk的绝对值变化平缓?由于绝对值处理较繁,一般改成平方,也就是说,问题变为
显然,积分是一个关于f(t)的泛函,积分的值取决于f(t)在[0,T]上的整体形状
那么取极小值的f(t)是什么?显然,当f(t)为二次或者二次以下函数时,
所以若要让Jerk在[0,T]上的绝对值最小,f(t)应取二次或者二次以下函数
但是真实情况远比想象中复杂,因为真实的s = f(t)往往带有约束
6个边界条件
在真实情况下,往往要求带边界约束的泛函
答案:五次多项式
解:显然f(t)只可能是在[0,T]上是有界连续函数,因为无论是无界还是有界间断函数都会使Jerk出现无穷大
所以不妨设
带入边界条件
其次边界条件
则有
泛函极值的必要条件为Euler-Lagrange方程
Lagrange乘子
分别求偏导得到
所以五次多项式在规划中才这么常见
自动驾驶规划目标:算出一条满足约束的最优轨迹
然而,什么是“最优”?
指标:①平滑性;②舒适性;③尽可能短,耗时少。
约束:①轨迹连续性;②无碰撞;③遵守交规;④车辆动力学。
衡量轨迹质量用Cost function表示
假设(系数均为未知常数)
则Cost Function
J越小,意味着越平滑舒适,当然也要满足各种约束
数学问题:求解Cost Function在约束条件下的最小值问题
如何计算y=f(x)的最小值(在约束下)
回忆:高中时如何解y = f(x)在x∈[a,b]上的最小值(f(x)是连续可导函数)
但是此方法无法快速求解高维度复杂约束下的最小值问题,比如
一般求复杂函数在复杂约束下的最小值问题都采用迭代法
常见迭代法有
1、牛顿法《数值分析》
2、梯度下降法
3、高斯牛顿法《视觉slam十四讲》
梯度下降法大概过程
迭代法缺点:对初值敏感,有可能会收敛到局部极小值
凸优化必有两个性质:
①Cost function只有一个极值点,且为极小值(凸函数)
②约束空间是一块“完整的”“不破碎的”空间(凸空间)
求凸函数在凸空间的最小值问题称为凸优化
凸空间的严谨定义,由如多边形引申
凹多边形定义:对于多边形内部存在点x,y,使得(x+y)/2不在多边形内部,此多边形为凹多边形
凸空间
凸优化是最简单的唯一研究明白的非线性优化方法,是所有优化问题的基石
自动驾驶规划的Cost function是凸的,约束空间也得是凸空间才可以当作凸优化问题求解
问题:自动驾驶避障约束空间是不是凸空间 答案:不是
对于动态的障碍物也是一样的
假设场景为一个人和一辆车,车要动态避障行人
静态和动态避障约束空间都不是凸的,所以规划问题是比较难的
如何求解非凸问题的最小值?
答:目前为止,并无完美的非凸问题算法,求解非凸问题的主要思路是找非凸问题中的凸结构
启发式算法:先随机在约束空间采样一些离散的函数值,比大小,取最小的作为迭代初值
举例:在这些黄色采样点中找到最小点作为初值迭代
采样,比大小,求出A最小,A本质上是离散约束空间的最优解(粗解)
非凸空间 --> 离散化 --> 粗解 --> 迭代 --> 最终解
对于一个非凸问题,如果采样越少,越容易收敛到局部最优,而不是全局最优
一维要采样100个点,二维要采样100*100 = 1w个点,三维可能要100w个点
所以非凸问题没有一个尽善尽美的解法,只能根据实际问题去调整
本节主要推导Frenet坐标系与Cartesian坐标转换
龙格现象:高次多项式拟合可能会出现震荡,慎用高次多项式
自然坐标与直角坐标转换的难度巨大,需要对微积分非常熟练,熟悉曲线坐标系
幸运的是,不需要理解推导过程,只需要记住结论
给出两种方案:
①参考博客,实在看不懂记住结论
Frenet坐标系与Cartesian坐标系互转(一):公式推导_windSeS的博客-CSDN博客_frenet坐标系②和老王一起推导,难度稍低
老王自创的向量法,可以降低推导难度
下面开始推导的准备工作
车记为host vehicle
已知车在Cartesian坐标系下的,求车在以道路为坐标轴的frenet坐标下的坐标,其中
frenet坐标系与Cartesian坐标系定义如下
对于 EM planner,求
对于lattice planner,求
可以互相转化
在推导中,难度最大的就是曲线坐标系
曲线坐标系与直角坐标系的两点不同:
①曲线坐标系的基向量一般不是常向量
在直角坐标系下只有一个dx,在自然坐标系下有不同的ds
预备知识2: frenet公式
在曲线坐标系中,一般不为
其中:k为曲率
拓展2:
| 车的位矢 | | 投影位矢 |
| 车的速度 | | 车在道路上的投影的速率 |
| 车的加速度 | | 投影位矢在道路几何上的曲率 |
| 车的位矢在车轨迹上的曲率 | | 投影位矢在道路几何上的切向方向单位向量 |
| 车的位矢在车轨迹上的切线方向单位向量 | | 投影位矢在道路几何上的法向方向单位向量 |
| 车的位矢在车轨迹上的法线方向单位向量 |
辅助公式
要解这个问题我们分三步:
第一步:7个辅助公式
第二步:找到车在frenet坐标下的投影点在Cartesian的坐标,记为
计算投影位矢以及其切向法向单位向量
第三步:利用向量三角形,以及微积分求出
核心公式:
如何找到在frenet坐标下的投影暂时先不管,假设已经找到了
自然可得到
下面开始正式计算:
(1)计算
核心公式,把挪到等式右边得到,
然后两边点乘得到:
注意:这里\overrightarrow{n_r} \cdot \overrightarrow{n_r} = 1,\overrightarrow{\n_r} \cdot \overrightarrow{\tau_r} = 0
(2)计算
对核心公式两边求导得到:
和博客有所不同,但其实一样
(3)计算
(4)计算
利用上面计算的与计算得到
(ds为frenet坐标轴的弧坐标的导数,所以ds/dt = )
(5)计算
1、如何找到
2、如何计算 s
3、如何从转回到直角坐标系去
如何找到在《自动驾驶控制算法第七讲》有讲过
一般reference line都是离散点,只讲离散点的
假设曲线上有一系列离散点,如果有蓝线的表达式,可以得到理想投影,
找到匹配点可以进行近似
原理
求
有其他更精确的算法,但是不稳定,以分段直线近似曲线虽然比较糙,但是对参数变化不敏感(robust)
精确算法例子:
虽然算法比较糙,但是计算的快,而且鲁棒,大多数场景适用
下面看如何从自然坐标转换到直角坐标,即
关键还是找投影点的
1、计算 的对应表,即每个frenet坐标轴上的离散点的与的对应关系
2、查找 表,找到使得, 与 对应的 记为
找到以及
x_{n+1},y_{n+1},\theta_{n+1},k_{n+1}
3、其余变量使用公式算出
接下来计算
(1)算
由公式②③
由此计算出方向向量
第二章如下
自动驾驶决策规划算法第二章_免费教学录影带的博客-CSDN博客