无人驾驶决策规划技术理论（一）

本博客记录学习李柏老师所著的决策规划技术理论与实践中的关键知识，想以此作为自己学习的框架，方便自己日后回顾。

1 非结构化场景决策规划方法

场景：港口、停车场的泊车附近区域、偏僻道路、野外
特色：障碍物位置杂乱无章

决策规划 = 行为决策 + 轨迹规划

2 泊车轨迹规划任务建模

2.1 泊车轨迹规划任务

任务：要求生成一条时空轨线，使得车辆能够从给定起始姿态运动至既定的终止位姿。这条时空轨线应符合车辆运动规律，且避让环境中的障碍物。
在这里插入图片描述

2.2 基于最优控制问题模型的轨迹规划任务描述

首先需要明确的是，车辆不是质点，车辆存在的空间也不属于一维空间，车辆的运动规律更加复杂，环境限制约束的表现形式也更加丰富。但是，最优控制问题仍旧是描述轨迹规划任务最直接、最准确、最通用的方式。无人驾驶的最优控制问题由四部分组成：（1）车辆运动学约束（2）边值约束（3）碰撞躲避约束（4）代价函数多项式

如下所示OCP，求解下面命题可获得时间片段 $0,t_f]$ 上的控制信号 $a (t)$ ，使车辆在最短的时间内从状态 $x = 0$ 迁移到 $x = 6$ 。
$J=t_f \\ s.t. \ \frac{dx(t)}{dt}=v(t) \\ \frac{dv(t)}{dt}=a(t) \\ |v(t)|≤v_{max} \\ |a(t)|≤a_{max} \\ 0≤x(t)≤6 \\ \forall t∈[0,t_f]$

2.3 车辆运动学约束

车辆运动学约束：描述车辆在二维平面上的运动能力。
在车速不高的情况下，二自由度模型能够符合实际需求，该模型遵循的假设：
（1）车辆行驶于平坦路面，忽略车辆在垂直于路面方向上的运动
（2）忽略车辆在行驶中受到的空气阻力以及地面侧向摩擦力
（3）车轮与地面始终保持良好的滚动接触
（4）车辆为刚体，忽略车身悬架结构的影响

以后轮为参考点的单车模型：
在这里插入图片描述
车辆在惯性坐标系X-Y中的运动过程受到以下微分方程组的制约：
$\begin{array}{c} \frac{\mathrm{d}}{\mathrm{d} t}\left[\begin{array}{c} x(t) \\ y(t) \\ v(t) \\ \phi(t) \\ \theta(t) \end{array}\right]=\left[\begin{array}{c} v(t) \cdot \cos \theta(t) \\ v(t) \cdot \sin \theta(t) \\ a(t) \\ \omega(t) \\ v(t) \cdot \tan \phi(t) / L_{\mathrm{w}} \end{array}\right] \\ t \in\left[0, t_{f}\right] . \end{array}$

车辆内在的机械特性对应着状态/控制变量的容许作用区间，一般包括：
$\begin{array}{c} -\Phi_{\max } \leqslant \phi(t) \leqslant \Phi_{\max } \\ v_{\min } \leqslant v(t) \leqslant v_{\max } \\ a_{\min } \leqslant a(t) \leqslant a_{\max } \\ -\Omega_{\max } \leqslant \omega(t) \leqslant \Omega_{\max }, t \in\left[0, t_{f}\right] . \end{array}$

对于行驶平稳性要求较高的轨迹规划任务，如需 $a (t)$ 变化平缓，应额外补入关于 $a (t)$ 的微分变量 $j er k (t)$ ，并对 $j er k (t)$ 限幅。

轨迹规划任务的本质是寻找一个非线性系统的某一组控制变量 $u (t) = [a (t), w (t)]$ ，使得系统在受限条件下实现状态的最优迁移——最优控制问题正是精准、直接描述此类任务的方式

2.4 边值约束

在车辆起始时刻t = 0，应指定车辆所处的运动状态，即 $\phi(0), a(0), \omega(0), x(0), y(0), \theta(0)]=\left[p_{1}, p_{2}, p_{3}, p_{4}, p_{5}, p_{6}, p_{7}\right]$ ，其中 $p_{1}, p_{2}, p_{3}, p_{4}, p_{5}, p_{6}, p_{7}]$ 对应车载传感器记录/推算的当前时刻t=0的运动状态客观实际情况。
在车辆运动的终止时刻 $t_f$ ，也要对车辆运动状态进行限制。在泊车任务中，一般要求车辆最终稳定地停泊下来。
$v(t_f)=a(t_f)=w(t_f)=0$ ，如果明确要求车辆的前轮在终止时刻回正，应施加约束 $\Phi(t_f)=0$ 。在工程实践中最常见的终止时刻状态约束是要求车辆停泊在某一矩形区域内，相当于间接对 $[x(t_f),y(t_f),\theta(t_f)]$ 提出了限制要求。

在这里插入图片描述
① 首先根据后轴中心点(x,y)与各个顶点的相对位置得到各顶点坐标。
$\begin{array}{l} \left.A=\left(A_{x}, A_{y}\right)=\left[x+\left(L_{\mathrm{w}}+L_{\mathrm{f}}\right) \cdot \cos \theta-0.5 L_{\mathrm{b}} \cdot \sin \theta, y+\left(L_{\mathrm{w}}+L_{\mathrm{f}}\right) \cdot \sin \theta+0.5 L_{\mathrm{b}} \cdot \cos \theta\right]\right]\\ \begin{array}{c} B=\left(B_{x}, B_{y}\right)=\left[x+\left(L_{\mathrm{w}}+L_{\mathrm{f}}\right) \cdot \cos \theta+0.5 L_{\mathrm{b}} \cdot \sin \theta, y+\left(L_{\mathrm{w}}+L_{\mathrm{f}}\right) \cdot \sin \theta-0.5 L_{\mathrm{b}} \cdot \cos \theta\right] \\ C=\left(C_{x}, C_{y}\right)=\left(x-L_{\mathrm{r}} \cdot \cos \theta+0.5 L_{\mathrm{b}} \cdot \sin \theta, y-L_{\mathrm{r}} \cdot \sin \theta-0.5 L_{\mathrm{b}} \cdot \cos \theta\right) \\ D=\left(D_{x}, D_{y}\right)=\left(x-L_{\mathrm{r}} \cdot \cos \theta-0.5 L_{\mathrm{b}} \cdot \sin \theta, y-L_{\mathrm{r}} \cdot \sin \theta+0.5 L_{\mathrm{b}} \cdot \cos \theta\right) \end{array} \end{array}$
② 建立描述点在矩形内部的一般条件。某点P=(x,y)处于某一矩形内部的约束条件可以一般性地写成如下形式：
$\left\{\begin{array}{c} a \cdot x+b \cdot y+c_{1}<0 \\ -a \cdot x-b \cdot y-c_{2}<0 \\ m \cdot x+n \cdot y+q_{1}<0 \\ -m \cdot x-n \cdot y-q_{2}<0 \end{array}\right.$

在给定矩形区域 $P_1P_2P_3P_4$ 顶点坐标的前提下，将终止时刻的区域限制约束建立为：
$\text { PointInRect }\left(\chi, P_{1} P_{2} P_{3} P_{4}\right), \forall \chi \in\left\{A\left(t_{f}\right), B\left(t_{f}\right), C\left(t_{f}\right), D\left(t_{f}\right)\right\} \text {. }$

该式是针对状态变量 $x(t_f),y(t_f),\theta(t_f)$ 的非凸非线性约束。注意不要简单地将边值约束 $\theta(t_f)=\theta_f$ ，在稍微复杂的泊车场景中，不能提前预判车辆绕行多少圈才能抵达终止位姿，因此针对 $\theta(t_f)=0$ 的限制实际上是 $\theta(t_f)=2k\pi,k∈Z$ ，为了避免在约束条件中混入整数变量k，将姿态角度的终止时刻点约束写成：
$\begin{array}{l} \sin \left[\theta\left(t_{f}\right)\right]=\sin \theta_{\mathrm{f}} \\ \cos \left[\theta\left(t_{f}\right)\right]=\cos \theta_{\mathrm{f}} \end{array}$

2.5 碰撞躲避约束

碰撞躲避约束要求车辆在整个运动时域避免与环境中任何静止、移动障碍物发生碰撞。
（1）障碍物的表达形式：凸多边形，即激光雷达反射点云的最小凸包。
（2）某点处于某一凸多边形外部的约束建模，考虑描述点 $P = (x, y)$ 位于具有n个顶点的凸多边形 $Q_1,Q_2,...,Q_n$ 外部。

在这里插入图片描述
if-else 不适合构建通用的约束条件。一旦在最优控制问题中显式地掺入if-else语句，实质上相当于引入了函数导数不连续的隐患，为最优控制问题求解带来巨大困难。
三角面积法：将点P与凸多边形每两个相邻顶点分别组成三角形，并将这些三角形面积叠加，如果面积之和大于凸多边形，则P处于凸多边形外部，否则在轮廓或内部。点P处于凸多边形 $Q_1,Q_2,...,Q_n$ 外部的约束条件如下：
$S_{\triangle P Q_{\mathrm{n}} Q_{1}}+\sum_{k=1}^{n-1} S_{\triangle P Q_{k} Q_{k+1}}>S_{\square Q_{1} Q_{2} \ldots Q_{n}},$
$S_{\triangle P Q_{k} Q_{k+1}}=\frac{1}{2} \cdot\left|x \cdot y_{Q k}+x_{Q k} \cdot y_{Q(k+1)}+x_{Q(k+1)} \cdot y-x \cdot y_{Q(k+1)}-x_{Q k} \cdot y-x_{Q(k+1)} \cdot y_{Q k}\right|.$

在这里插入图片描述
碰撞躲避约束规模庞大、复杂程度高，包含大量对三角函数求取绝对值并将绝对值项加和的复杂操作，具有强非凸非线性，绝对值运算也属于if-else条件建模语句，不利于最优控制问题的求解。事实上，多边形之间的碰撞躲避约束无可避免地会引入if-else语句，因为多边形轮廓在顶点处均有突变。

2.6 代价函数

从符合硬性约束的众多轨迹中选出最令人满意的那一条作为轨迹规划的最终结果。这种筛选轨迹的指标式就是代价函数。
① 期待泊车尽早完成： $J_1=t_f$
② 期待泊车车速尽量按照三角函数波动： $J_{2}=\int_{0}^{t_{f}}[v(t)-\sin (t)]^{2} \cdot \mathrm{d} t .$
③ 期待泊车车速变化尽量小： $J_{3}=\int_{0}^{t_{f}}[a(t)-0]^{2} \cdot \mathrm{d} t .$
④ 期待车辆前轮在整个泊车运动过程中转动速度缓慢： $J_{4}=\int_{0}^{t_{f}}[\omega(t)-0]^{2} \cdot \mathrm{d} t .$
实际求解的轨迹会在满足硬性约束的前提下尽量将其最小化
⑤ 期待车辆按照某一既定路径行驶： $J_{5}=\int_{0}^{t_{f}}\left\{[x(t)-t)^{2}+(y(t)-\sin t]^{2}\right\} \cdot \mathrm{d} t .$
⑥ 期待车辆在严格避障基础上进一步做到适当远离障碍物，避免因闭环控制偏差、外界扰动等因素导致碰撞。
在这里插入图片描述
将代价函数多项式加权汇总，得到完整的代价函数：
$J=w_1*J_1+w_2*J_2+...+w_6*J_6$
较大的权重意味着在整个代价函数中对该部分的最小化效用更加重视。

完整的最优控制问题：
min J：
$车辆运动学约束：\begin{array}{c} \frac{\mathrm{d}}{\mathrm{d} t}\left[\begin{array}{c} x(t) \\ y(t) \\ v(t) \\ \phi(t) \\ \theta(t) \end{array}\right]=\left[\begin{array}{c} v(t) \cdot \cos \theta(t) \\ v(t) \cdot \sin \theta(t) \\ a(t) \\ \omega(t) \\ v(t) \cdot \tan \phi(t) / L_{\mathrm{w}} \end{array}\right] \\ t \in\left[0, t_{f}\right] . \end{array} \\\begin{array}{c} -\Phi_{\max } \leqslant \phi(t) \leqslant \Phi_{\max } \\ v_{\min } \leqslant v(t) \leqslant v_{\max } \\ a_{\min } \leqslant a(t) \leqslant a_{\max } \\ -\Omega_{\max } \leqslant \omega(t) \leqslant \Omega_{\max }, t \in\left[0, t_{f}\right] . \end{array} \\ 边值约束：[v(0), \phi(0), a(0), \omega(0), x(0), y(0), \theta(0)]=\left[p_{1}, p_{2}, p_{3}, p_{4}, p_{5}, p_{6}, p_{7}\right] \\ v(t_f)=a(t_f)=w(t_f)=0 \\ \Phi(t_f)=0 \\ \text { PointInRect }\left(\chi, P_{1} P_{2} P_{3} P_{4}\right), \forall \chi \in\left\{A\left(t_{f}\right), B\left(t_{f}\right), C\left(t_{f}\right), D\left(t_{f}\right)\right\} \text {. } \\碰撞躲避约束：S_{\triangle P Q_{\mathrm{n}} Q_{1}}+\sum_{k=1}^{n-1} S_{\triangle P Q_{k} Q_{k+1}}>S_{\square Q_{1} Q_{2} \ldots Q_{n}}$