自动驾驶决策规划（三）——动态规划

1、前置知识

参考系模块：

https://blog.youkuaiyun.com/TalentRoxy_/article/details/144135733?spm=1001.2014.3001.5501

2.基本原理

2.1决策规划总体概述

1. 参考线生成： 利用定位和导航信息生成平滑的参考线，作为路径规划的基准。参考线可以避免直接使用导航路径带来的过长和不平滑问题，提高路径规划的效率和精度；
2. 障碍物投影： 将静态障碍物投影到以参考线为坐标轴的 Frenet 坐标系中，便于后续的路径规划。Frenet 坐标系可以简化路径规划问题，将二维平面问题转换为参数化曲线问题；
3. 决策算法： 根据障碍物的位置和参考线信息，决定车辆绕行障碍物的方向，即开辟最优凸空间。决策算法需要考虑障碍物的大小、形状、速度等因素，以及车辆的性能限制和安全约束；
4. 规划算法： 在凸空间中搜索出最优的路径，保证车辆安全且平稳地绕过障碍物。规划算法需要考虑路径的平滑度、曲率、长度等因素，以及车辆的动力学特性和控制能力；
5. 后处理： 从规划轨迹中选择一个点，将其坐标转换为笛卡尔坐标系，输出给控制模块进行跟踪。后处理需要考虑轨迹的平滑度、曲率、长度等因素，以及车辆的动力学特性和控制能力。

2.2参考线

参考线是解决导航路径过长并且路径不平滑的方案，提供光滑的freret坐标轴.过长的路径不利于坐标转换且障碍物的投影可能不唯一。

2.3规划起点

首先，假设以定位得到的host_X以及host_y投影到referenceline，得到sl坐标（0，L0），以此点为路径规划的起点这么做肯定是错误的，因为考虑控制是不完美的，他的控制效果绝对会有误差，以下面图2-3-1为例子，假设规划是以100m为周期进行执行的，绿色的点为上一个周期规划后车应该所在的位置，但是由于控制是不完美的总是会出现误差，所以红色点表示这个周期内车的实际位置，假设以这个点为坐标原点进行规划，那么规划出来的路线为红色曲线，同样的由于控制是不完美的，在一个周期后，车的实际位置为橙色点。那么从控制的方面看来，规划的轨迹就是割裂不完整的，不连续性，导致会出现车身异常抖动的情况。

图2-3-1

正常做法应该如图2-3-2所示，首先需要判断current_location与pre_location规划结果是否误差过大：

若是过大的话，那么控制完全没有跟上，就以当前current_location投影到referenceline,以(0，L0)作为规划起点；

若是误差不大，设在本周起规划开始的时间为T，一个周期后的时间为T+100ms，假设以T为起点进行规划，那么在[T,T+100]这段时间，由于规划未计算完成，直到T+100ms后，绿色的本周期规划轨迹才计算好，那么控制跟踪的是哪段轨迹？答案是上个周期的轨迹，那么从控制的角度看来，轨迹是不连续不平滑的，会导致车身异常抖动；那如果假设是以T+100为规划起点，如图2-3-3所示，那么从控制的角度看来，规划的轨迹就是连续平滑的。

图2-3-2

图2-3-3

2.4 最优路径

在确定规划起点以后，接下来的任务就是以规划起点为坐标远点结合障碍物在referenceline的投影，搜索出一条最优路径，即保证设计出的路径平滑且连续，与障碍物保持适当距离，并且不远离参考线。

在传统的方案中，最优路径往往可以通过二次规划解决，即设定一定的代价函数规划出最优路径，但此类避障问题的二次规划为非凸问题，cost function有多个极小值点，并没有一个完美的解决方案。

而解决这个问题，提出一个方案，如图2-4-1所示，将空间点进行离散化，动态规划启发式搜索出一个粗解，开辟出一个凸空间，在以基础上二次规划出最优的解。

图2-4-1

2.5 动态规划

在确定起点，将空间点进行离散化后，如图2-5-1所示，之间的采样距离为1m，将(S0,L0),(S11,L11)两个点用五次多项式进行连接，同理将其他两点进行连接，为(S0,L0)-(S13,L13)这条路径，同理找出不同路径，那么再找到这条路径后，如何评价这条路径的优劣呢？用cost funciton进行评价，如图2-5-2所示，cost function有三个代价组成，分别为平滑代价，障碍物距离代价以及参考线距离代价。平滑代价即与直线的相似度，直线最短，越像直线越平滑，

障碍物距离代价，即距离障碍物d范围内，代价为0；在(d1，d2)内线性增大；在(0，d1)内为无穷大，可根据实际情况将代价函数设成反比例函数。

参考线距离代价，即路径上每个点到参考线的距离之和。

图2-5-1

图2-5-2

那么，对每条路径都计算出相应代价之后，问题就转变为找图的最短路径问题，采用动态规划的方案进行求解，如图2-5-3所示，对于(P0,P12)这条路径，无非有三条路径，在其中选择最短的一条记为L，现假设有另外几个点P13 P23 P33，如图2-5-4所示，那么求解(P0,P13)这条最短路径，无非就是前面以及计算出的最短路径L加上cost(p12,p13)或者L加上cost(p22,p23)或者L加上cost(p22,p23)，同理对于ij也是一样，那么每次我们只需要计算前面一条最短路径再加上最后几个点的代价即可。

图2-5-3

图2-5-4

2.6 轨迹拼接

当已经规划完成后，规划直接发一条带有时间链的轨迹给控制，规划以100ms为一周期进行规划，而控制以10ms为一周期进行控制，很明显控制的频率比规划快得多，那么当规划直接发了一条带有时间链的轨迹给控制时，控制就可以根据当前控制周期的时间，对轨迹进行搜索，找到当前时间的x，y等等数据，输出给控制算法去跟踪，那么问题来了，这个发给控制的时间是相对时间还是绝对时间？答案是绝对时间，因为对于做拼接，做控制而言，绝对时间更加的直观方便。比如说，本周期在16：00：10进行规划，首先获得本周期的时间，并且直接查找上一时刻规划的本周期起点的点，计算误差并进行规划，对于控制，在16：00：13时刻，查找规划发给控制的轨迹链对应16：00：13时刻的数据并输出给控制算法进行跟踪。