一、概述
决策规划算法概述自动驾驶6个级别：L0-L5
L0 没有任何自动驾驶功能
L1 有横向和纵向的自动驾驶功能，但是横纵向无法联合使用
L2 横纵向可以联合使用，但驾驶员必须对一切状况负责
L3 功能与L2基本相同，最大的区别在于责任，对于部分场景，驾驶员不必负责
L4 大部分道路皆可自动驾驶，大部分场景都不需要驾驶员负责
L5 完全自动驾驶
两条关键因素区分：功能和责任
L0-L2主要是功能区分，L3-L5主要是责任区分，而不同公司L2与L2之间差距巨大，只要厂家宣称驾驶员需要负全责，即使功能做到了L2，本质上还是L2
L2本身比较宽泛，只有定速巡航+车道保持的汽车也是L2，什么都能做，但是驾驶员要负全责的汽车也是L2
决策规划算法就是普通L2到什么都能做的L2的一个重要模块
/home/zqht/图片/2023-03-22 11-34-21屏幕截图.png
功能越往上做，越丰富，越复杂，决策规划算法越重要，也越难在L4中决策规划模块可以说是最重要，也是最复杂，最难做的模块
难做到把整个模块一分为三，还要加上地图模块，每块单独地去做才能勉强完成“大脑”的工作，而且截止到2021年也不能说完全的解决
整个决策规划模块一分为三，下面分别介绍：
1、导航规划：A*、djkstar，导航规划算法，此算法将计算出大地图上A到B的最优路径，此算法与机器人导航，手机导航算法基本一致，长度在几公里到几百公里不等，该算法是整个规划模块中最为成熟的算法
特点：导航算法会给一个粗略的，大范围的路径，但是此路径不会考虑如何避障，也不会考虑车辆动力学约束，一般规划的路径时不规则的折线。导航算法一般只需要执行一次，只有遇到大范围的拥堵，施工，偏航等情况才会再次执行
2、行为规划：即决策；决定车辆行驶意图的算法，对于静态障碍物到底往左还是往右，对于动态障碍物是减速还是避让或加速超车，决策算法决定了车辆的意图，这也是整个规划算法最难的部分
特点：决策算法会给一个车辆的行驶意图，会指导车辆避让、超车、左转、右转，但是决策不会给具体的运动建议，例如往左转多少度，车辆加速到多少；实际环境瞬息万变，因此决策算法需要较高的执行频率，一般为10hz，决策算法需要一定的稳定性，不允许在周围环境稳定时出现朝令夕改现象
3、运动规划算法：根据决策给出的行为意图在相关的时空中搜索出（或优化出）一条具有详细路径速度信息，并且满足各个约束条件的轨迹，并将此轨迹发给控制模块去跟踪，此轨迹长度一般在几米到几十米不等
特点：运功规划生成的轨迹是决策规划模块的最终输出，具有详细的路径速度的信息，执行频率与决策相同，为10Hz，同样，运动规划也要求具有一定的稳定性。
五次多项式是带约束的泛函
取极小值的解函数
1、凸优化与非凸优化
自动驾驶规划目标：算出一条满足约束的最优轨迹
然而，什么是“最优”？
指标：①平滑性；②舒适性；③尽可能短，耗时少。
约束：①轨迹连续性；②无碰撞；③遵守交规；④车辆动力学。
衡量轨迹质量用Cost function表示
假设（系数均为未知常数）
则Cost Function
J越小，意味着越平滑舒适，当然也要满足各种约束
数学问题：求解Cost Function在约束条件下的最小值问题
如何计算y=f(x)的最小值（在约束下）
回忆：高中时如何解y = f(x)在x∈[a,b]上的最小值（f(x)是连续可导函数）
①算出
②求出y’ = f'(x)
③令y’ = 0，解出f'(x)= 0的根，设为
④计算
与比较，
中最小的那个就是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本质上是离散约束空间的最优解（粗解）
非凸空间 –> 离散化 –> 粗解（base） –> 迭代 –> 最终解
对于一个非凸问题，如果采样越少，越容易收敛到局部最优，而不是全局最优
那么我采样点多一些，就会出现“维度灾难”
一维要采样100个点，二维要采样100*100 = 1w个点，三维可能要100w个点
所以非凸问题没有一个尽善尽美的解法，只能根据实际问题去调整
发表回复