学习笔记13--路径-速度分解法之汽车速度规划

本系列博客包括6个专栏，分别为：《自动驾驶技术概览》、《自动驾驶汽车平台技术基础》、《自动驾驶汽车定位技术》、《自动驾驶汽车环境感知》、《自动驾驶汽车决策与控制》、《自动驾驶系统设计及应用》。
此专栏是关于《自动驾驶汽车决策与控制》书籍的笔记.

2.汽车局部轨迹规划

2.3 路径-速度分解法

当局部路径规划给定了一条或若干条选出的路径曲线后，运动规划模块需要解决的后续问题是在此局部路径的基础上加入与速度相关的信息，这一问题称为速度规划；
速度规划的目标是在给定的局部路径曲线上，在满足反馈控制的操作限制及符合行为决策的输出结果这两个前提下，将路径点赋予速度及加速度信息；速度规划主要考虑的是对动态障碍物的规避；
常见的速度规划方法：
- 对路径指定线加速度来生成速度：线加速度可以是某一常数，或是由比例-微分控制器来生成；
- 样条插值：在给定时间内通过一段给定路径的速度生成问题；其解决方案是将时间域划分为若干区间，使用速度关于时间的三次样条函数来插值，此方法容易产生加速度变化率较大的问题；
- 函数拟合：直接用速度关于路径长度的二次多项式来生成速度；
- 目标时刻点法：速度规划部分先根据对障碍物未来运动状态的预测，在规划路径与时间这两个维度构成的二维图中标记障碍物在未来一段时间内所占据的区域；以目标汽车当前车速匀速通过规划路径所需的时间为基准，根据一定原则创建一组目标时刻点，在此二维图中以目标时刻点为目标点搜索产生一组速度规划方案；
- QP算法：此方法引入 $S - T$ 图概念，把自动驾驶汽车速度规划归纳为 $S - T$ 图上的搜索问题进行求解； $S - T$ 图是一个关于给定局部路径纵向位移和时间的二维关系图；任何一个 $S - T$ 图都基于一条已给定的轨迹曲线；根据自动驾驶汽车预测模块对动态障碍物的轨迹预测，每个动态障碍物都会在这条给定的路径上有投影，从而产生对于一定 $S - T$ 区域的覆盖；
QP算法实例说明：
1. 障碍物预测
  
  当环境中出现移动障碍的时候，移动障碍会在未来某些时刻 $t_1、t_2、t_3)$ 占据已经规划好的路径；如果目标汽车仍然以当前车速匀速行驶，有可能会在未来的某一时刻与移动障碍相碰撞；
  根据移动障碍当前所处位置 $x_{0_{obj}},y_{0_{obj}})$ 、速度 $v_{0_{obj}},v_{0_{obj}})$ 、加速度 $a_{0_{obj}},a_{0_{obj}})$ ，可以预测经过时间 $t$ 后移动障碍物所处位置 $x_{t_{obj}},y_{t_{obj}})$ ：
  
  \tag{32} ${x_{t_{o bj}} = x_{0_{o bj}} + v x_{o bj} \times t + \frac{1}{2} \times a x_{o bj} \times t^{2} y_{t_{o bj}} = y_{0_{o bj}} + v y_{o bj} \times t + \frac{1}{2} \times a y_{o bj} \times t^{2} (32)$
  矩阵形式：
  $[x0objvxobj12×axobjy0objvyobj12×ayobj]\tag{33}$
  $Y=AX\tag{34}$
  得到 $t$ 时刻移动障碍所处位置 $Y$ 后，根据移动障碍轮廓计算其覆盖区域 $Q$ ：
  $Y\Rightarrow{Q}\tag{35}$
  障碍物覆盖区域与规划轨迹的交集示意图：
  
  计算 $Q$ 与规划路径 $S$ 的交集 $\Delta{S}$ ： $\Delta{S}=Q\bigcap{S}$ ， $\Delta{S}$ 与 $t$ 一一对应；

$S - T$ 图生成

S-T图是已经规划完成的路径纵向位移与时间之间的二维关系图；

假如移动障碍物经过了规划路径，那么因为移动障碍物在一段时间内占据了规划路径，在S-T图中会有一块覆盖区域；

图4.24中， $S_{goal}$ 为规划路径的长度，绿色虚线代表目标汽车以当前车速匀速沿规划路径行驶，用时为 $t$ ，可以发现在某些时刻目标汽车和移动障碍物将会同时出现在期望路径的某个位置，意味着目标汽车将与移动障碍物碰撞；红色实线代表目标汽车从当前时刻开始以最大加速度加速通过期望路径，用时记为 $t_{min}$ ；
S-T栅格化
为了有利于在S-T图中进行路径搜索，需要对S-T图进行离散化、网格化；根据需要确定采样周期和规划路径的路径点间隔，如下图所示：

从当前时刻开始根据采样周期针对未来某一时刻 $t_k$ 计算移动障碍所处位置 $Y_k$ ，其中 $k$ 为 $1 ～ c$ 的自然数， $c$ 的值根据移动障碍物离开规划路径的时刻确定； $t_k$ 和 $t_{k+1}$ 之间的时间差为采样周期；
$X_k= [1tkt2k]$
，Y_k= ，k\in(1,c)且c\in{N}\tag{36} $X_{k} = ⎣ ⎡ 1 t_{k} t_{k}^{2} ⎦ ⎤ ， Y_{k} = [x_{t_{k}} y_{t_{k}}] ， k \in (1, c) 且 c \in N (36)$
$Y_k=AX_k，Y_k\Rightarrow{Q_k}，\Delta{S_k}=Q_k\bigcap{S_{xy}}\tag{37}$
最终得到 $\Delta{S_k}，\Delta{S_k}$ 中包含在时刻 $t_k$ 移动障碍在采样后的期望路径点上的投影；将 $t_k$ 与 $\Delta{S_k}$ 相对应的标记在网格化后的S-T图上，即为S-T搜索图;
创建目标时刻点

创建S-T搜索图的目的在于为搜索算法提供搜索空间，从 $t_a$ 与 $t_{min}$ 中选择较小的值记为 $T_{t_{min}}$ ，从 $T_{t_{min}}$ 开始向后等间隔产生一组时刻点，这些时刻点(包括 $T_{t_{min}}$ )用 $T_{t_r}(r\in{N})且r≠0$ 表示，数目由移动障碍在S-T图中占据的区域而定； $T_{t_r}$ 称为目标时刻， $T_{t_r}$ 的集合记为 $T_T$ ， $T_T$ 称为目标时刻集；

同时产生与之数量匹配的一组 $T_{s_r}$ ， $T_{s_r}$ 的大小与 $S_{goal}$ 相等，称为目标路径点，这一组 $T_{s_r}$ 的集合记为 $T_S$ ，称为目标路径点集； $T_T$ 与 $T_S$ 中的元素一一对应，且对应了S-T图上 $T_S$ 线上的一组点，这组点称为目标时刻点；
搜索产生路径

结合上游行为决策输出的信息，速度规划可以灵活设置障碍物体周边的代价，达到调整速度方案的目的；当上游决定针对物体 $a$ 进行抢先决策时，在S-T图上物体 $a$ 运动路径上方的网格的代价可以调成偏小；同时，为了避免任何潜在的碰撞，所有动态障碍物体的路径经过的网格的代价都要调大；

基于路径搜索算法针对每一个S-T候选目标点，在S-T禁忌搜索空间中搜索产生从起点到目标点的路径，如下图所示：
速度平滑

通过路径搜索算法在S-T禁忌搜索图中找到一条从起点到目标点的路径时，由于对原有S-T图的离散采样，得到的路径也是曲折且离散；为了获得平滑的路径曲线，利用多项式回归分析对离散路径点进行拟合，此外，QP算法亦可用于速度平滑过程；

速度规划轨迹平滑结果如下图所示：
1. 多项式回归介绍
  - 统计学中定量地找出两种或两种以上变量之间的对应关系称为回归分析；
  - 根据自变量的个数、因变量的类型和回归线的形状等信息将回归分析的方法分为：线性回归、逻辑回归、多项式回归、逐步回归、岭回归、套索回归等；
  - 按照自变量和因变量之间的关系类型，回归分析分为：线性回归分析和非线性回归分析；研究一个因变量与一个或多个自变量间多项式的回归分析方法，称为多项式回归；如果自变量只有一个时，称为一元多项式回归；如果自变量有多个时，称为多元多项式回归；多项式回归的优势：仅仅通过改变自变量的次数构建多项式去逼近数据点，不存在其他复杂的函数；
  - 一元 $m$ 次多项式回归方程：
    $\hat{y}=b_0+b_1x+b_2x^2+\dots+b_mx^m\tag{38}$
  - 二元二次多项式回归方程：
    $\hat{y}=b_0+b_1x_1+b_2x_2+b_3x_1^2+b_4x_2^2+b_5x_1x_2\tag{39}$
  - 对回归方程的拟合函数建立损失函数，用于评价拟合程度的好坏，损失函数的值越小，则拟合程度越好；当损失函数为最小值时，则对应的参数 $\theta$ 为最优参数，即此时拟合达到最优；
  - 在回归分析中常用最小二乘法构建损失函数，最小二乘法亦称最小平方法，是通过求拟合函数值与给定数据之间平方和最小的方式，使得函数逼近给定数据，得到最优的函数方程；
  - 求最小损失函数值的方式有两类：一类是利用最大似然和最小二乘法等对损失函数求导且令导数为零，直接得到解析解；另一类是基于迭代算法，迭代优化求解；第一类方法要求解逆矩阵，对于数据量小的问题可以快速有效得到解析解，但对数据量大的问题求解逆矩阵需要消耗大量计算内存；
2. 多项式回归在速度平滑中的应用
  
  以基于5次曲线的多项式为例，对其对应5次多项式的系数进行迭代寻优，从而获取平滑后的速度曲线：
  
  在对S-T图中的路径点进行5次曲线拟合得到速度规划方案时，需要考虑目标汽车在 $t$ 为0时当前时刻目标汽车的速度值和加速度值及S-T图中创建的目标时刻点；即利用5次曲线对S-T图中速度规划路径点的拟合，是一个包含等式约束的优化问题；
  
  当通过路径搜索算法在S-T图中搜索产生路径后，可以获取一组路径点 $(t_i,s_i)(i-0,1,2,\dots,m)$ ；用于速度规划路径点的5次拟合函数为：
  $s(t)=k_0+k_1t+k_2t^2+k_3t^3+k_4t^4+k_5t^5$
  构建损失函数：
  $J(k_0,k_1,k_2,k_3,k_4,k_5)=\frac{1}{2m}\sum_{i=1}^m(h_k(t_i)-s_i)^2$
  在速度规划中，目标汽车在轨迹上所处的位置用 $s$ 表示，速度用 $s^{'}$ 表示，加速度用 $s^{''}$ 表示，则在时刻 $t$ ,目标汽车的车速为：
  $s'(t)=k_1+2k_2t+3k_3t^2+4k_4t^3+5k_5t^4$
  目标汽车加速度为：
  $s''(t)=2k_2+6k_3t+12k_4t^2+20k_5t^3$
  已知 $t = 0$ 时初始状态下目标汽车在规划轨迹上所处位置、汽车速度、汽车加速度为已知量，即：
  $s(0)=k_0=s_{t=0}，s'(0)=k_1=s'_{t=0}，s''(0)=2k_2=s''_{t=0}$
  目标时刻点信息，即存在：
  $s(t_r)=k_0+k_1t_r+k_2t_r^2+k_3t_r^3+k_4t_r^4+k_5t_r^5=s_t$
  联立可得：
  
  $⎩ ⎨ ⎧ k_{0} = s_{t = 0} k_{1} = s_{t = 0}^{'} k_{2} = \frac{1}{2} s_{t = 0}^{''} k_{3} = \frac{s _{t}}{t _{r}^{3}} - \frac{s _{t = 0}}{t _{r}^{3}} - \frac{s _{t = 0}^{'}}{t _{r}^{2}} - \frac{s _{t = 0}^{''}}{2 t _{r}} - k_{4} t_{r} - k_{5} t_{r}^{2}$
  损失函数表示为关于 $k_4,k_5$ 的表达式，即：
  $J(k_4,k_5)=\frac{1}{2m}\sum_{i=1}^m(h_k(t_i)-s_i)^2$
  $k_4，k_5$ 的梯度分别为：
  $\frac{\partial}{\partial{k_4}}J(k_4,k_5)=\frac{\partial}{\partial{k_4}}\frac{1}{2m}\sum_{i=1}^m(h_k(t_i)-s_i)^2=\frac{1}{m}\frac{1}{2m}\sum_{i=1}^m[(h_k(t_i)-s_i)(t_i^4-t_rt_i^3)]$
  
  $\frac{\partial}{\partial{k_5}}J(k_4,k_5)=\frac{\partial}{\partial{k_5}}\frac{1}{2m}\sum_{i=1}^m(h_k(t_i)-s_i)^2=\frac{1}{m}\frac{1}{2m}\sum_{i=1}^m[(h_k(t_i)-s_i)(t_i^5-t_rt_i^3)]$
  
  梯度下降寻优过程是不断更新 $k_4,k_5$ ；
  
  求最小损失函数值的过程：
  
  $k_{4} = k_{4} + α \frac{1}{m} i = 1 \sum m [(h_{k} (t_{i}) - s_{i}) (t_{i}^{4} - t_{r} t_{i}^{3})] k_{5} = k_{5} + α \frac{1}{m} i = 1 \sum m [(h_{k} (t_{i}) - s_{i}) (t_{i}^{5} - t_{r} t_{i}^{3})]$
  当损失函数值收敛于允许误差之内时，认为找到最优解，此时得到的 $k_4,k_5$ 可以最终确定对给定的路径点拟合程度最好的拟合函数方程；

相关阅读:
Java开发：反射机制
PHP去除字符串前或后的字符或空格
搞懂TypeScript的类型声明
保障新能源园区安全无忧：可燃气体报警器校准检测的必要性探讨
nacos服务注册源码过程阅读
VSCode安装图文详解教程
kubernetes使用NFS存储卷---csi-driver-nfs
百度全景数据采集与分析
Django--Laboratory drug management and early warning system
windows10下编译32位和64位webrtc(m77)静态库

原文地址：https://blog.csdn.net/qq_39032096/article/details/126078458