Chapter-4 结构化道路上的单一车辆决策与规划方法

结构化道路与非结构化道路场景的重要区别是，车辆在结构化道路中行驶时受到指引线(Reference Line)的引导，于是可以在Frenet坐标系中进行路径决策规划。

1. Reference Line

1.1 参考线生成

参考线的生成依赖一系列航路点(waypoint)的位置信息，据此可以将参考线生成任务转化为规划一条尽量抵近各航路点的平滑路径的任务。可以建立最优化控制问题模型：

$$J=w_1\sum _{k=1}^{N_w}(x(\frac{t_{f}k}{N_W})-x_{wk}{)}^2+w_1\sum _{k=1}^{N_w}(y(\frac{t_{f}k}{N_W})-y_{wk}{)}^2+w_2\sum _{k=1}^{N_w}{\omega }^2(\frac{t_{f}k}{N_W})+w_3\sum _{k=1}^{N_w}{a}^2(\frac{t_{f}k}{N_W})\text{\hspace{1em}(1-1)}$$

约束包括边界值约束(首末路径点的位置约束和航向角约束)，车辆运动学约束、车辆在不同时刻和waypoint的距离偏差约束等。但由于车辆的运动学特性约束，可能会不满足和waypoint的距离偏差的约束，会存在着优化问题无解的情况。因此将问题的轨迹优化简化为路径优化问题。

质点运动模型将车辆后轴中心点(x,y)视为能够在平面上自由移动的质点，此时如果$x(s),y(s)$充分平滑，那么$(x(s),y(s))$对应的参数化轨线也会充分平滑，在道路场景中一般能够对应着易被车辆跟踪的平滑路径。为了保证平滑，要求$x(s),y(s)$三阶可微。将$x^{\prime \prime \prime }(s)$设置为分段常值形式的不连续函数，$x^{\prime \prime }(s)$是$x^{\prime \prime \prime }(s)$的积分，而$x^{\prime }(s)$是$x^{\prime \prime }(s)$的积分，最终可以由$x^{\prime \prime \prime }(s)$计算得到$x(s)$，因此将$x_i^{\prime \prime \prime }(s),y_i^{\prime \prime \prime }(s)$作为优化问题的决策变量，给定$i=0$时刻的边界值，约束每段线段的末端与对应waypoint的距离相隔不远：

$$\begin{gathered} \epsilon \le x(i\Delta s)-x_{kw}\le +\epsilon ，i=1,\cdots ,N-1 \\ \epsilon \le y(i\Delta s)-y_{kw}\le +\epsilon ，i=1,\cdots ,N-1\text{\hspace{1em}(1-2)} \end{gathered}$$

设计优化问题的代价函数为x,y一阶导数、二阶导数和三阶导数的加权和。和Apollo中的优化问题类似。

1.2 Frenet坐标系及弊端

Frenet坐标系以参考线的切线方向设为横轴、以参考线的正交方向为纵轴方向，消除因道路走势曲折而带来的建模困难问题。但其也存在缺陷：

• Frenet坐标系忽视参考系的曲率，所规划轨迹无法如实反应车辆的运动学能力；
• Frenet与笛卡尔坐标系转换关系无法做到精准无误。

2. 决策轨迹的生成

文章介绍了基于T-S -L分层采样的离散型解空间，一个为$(N_S{N}_L{)}^{N_T}$个互不相同的解构造，使用DP算法在解空间中计算最优决策轨迹。

代价函数的设计要体现安全性、舒适性、高效性。其中，安全性反映车辆的碰撞程度、潜在碰撞风险以及安全行驶习惯的违背程度；舒适性反映车辆横向、纵向速度变化幅度，以及当前决策周期的结果与上一周期结果的延续性；高效性则与车辆同行效率有关。代价函数用以反映备选决策轨迹的优劣，将轨迹的安全性、舒适性和高效性等多种因素叠加成一个标量，具有不直观的缺陷，因此代价函数的设计需要保证惩罚层次和逻辑是合理的。对于待选决策轨迹的运动学可行性的惩罚力度应设置的最大，其次才是安全的保障，最有才是舒适性和效率的追求。在不同场景中可能需要使用不同的配置参数，甚至是不同的代价函数子式。

3. 轨迹规划问题的构建与求解

3.1 隧道化建模

由于碰撞躲避约束的打规模以及非线性包含着大量冗余的部分。可以采用隧道化建模的方法简化问题。

将矩形车体用两个半径为R的圆形均匀覆盖，结合决策轨迹对应的车辆状态可以确定两个圆心的运动轨迹$P_r$和$P_f$，于是问题可以转为化$P_r$和$P_f$至少和障碍物保持R距离的问题。让$P_r$在决策轨迹上，可以得到$P_r$的隧道化的约束范围，即为橙色区域。因此可以构建简单边界的不等式约束：
$$\begin{gathered} x_{r-lb-k}\le x_r(t_k)\le x_{r-ub-k} \\ {y}_{r-lb-k}\le y_r(t_k)\le y_{r-ub-k} \\ {x}_{f-lb-k}\le x_f(t_k)\le x_{f-ub-k} \\ {y}_{f-lb-k}\le y_r(t_k)\le y_{f-ub-k}\text{\hspace{1em}(3-1)} \end{gathered}$$
然而上述约束中的$x_r(t),y_r(t)，x_f(t)，y_f(t)$和质点的关系是非线性的，并且车辆运动学约束也是非线性的，可以将其改造为外罚多项式，例如${\int }_0^{t_f}(x^{\prime }(\tau )-v(\tau )cos\theta (\tau ))d\tau$，可以将优化问题改造为全是线性约束。

3.2 轨迹规划在线求解能力保障方

3.2.1 离散化精度渐变方案

在最优化控制问题中，精度渐变方案是指完整的时间域$[0,t_f]$不均匀地分割为若干时间子区间，其中近期子区间长度较远期子区间长度短。与均匀离散化方案相比，非均匀离散化能够在问题规模不变的前提下使得时刻值较小的时间值片段上的模型精度更高。

在道路场景中有诸多静态障碍物和移动障碍物，一般认为感知模块对运动前方较远处障碍物的追踪、识别不够准确；另一方面。对于周围非协同车辆的预测轨迹在未来较远时刻的预测结果更不可靠。由于感知和预测信息的置信度随着时间衰减，将完整时域$[0,t_f]$均匀离散化无法体现未来不同时刻的感知预测置信度差异，因此离散化精度渐变方案在道路行车任务中意义明确。

3.2.2 多完成度冗余计算方案

轨迹规划问题未必总能求解成功，一是命题本身有解但求解器存在瑕疵而计算失败，二是命题本身无解。轨迹规划失败会使车辆处于危险状态，因此应布置一些更简单的行车任务，寄希望于得到“退而求其次”的解。

3.2.3 基于在线查表的紧急避险方案

在道路场景中，如果行驶前方近距离突然出现显著变化(例如突然出现行人)，可能导致正常轨迹规划命题及冗余方案全部无解，则需要一种“应激性”轨迹的方案。