Enable faster and smoother spatiotemporal trajectory planning for autonomous vehicles in constrained

基于代价的轨迹规划方法有三种流行的模型：基于采样、基于图搜索、基于优化。它们都有各自的缺点，基于采样的方法计算代价高，基于图搜索的方法分辨率低，基于优化的方法缺乏全局意识。本文框架保持了基于图搜索的方法和基于优化的方法的优势，从而实现了在约束动态环境下更快更平滑的时空轨迹规划。
该方法首先基于有向无环图构造时空驱动空间，并利用改进后的 $A\ast$算法有效地搜索时空轨迹。然后以搜索结果为参考，设计局部凸可行驾驶区域，并应用MPC，综合考虑车辆运动学和运动障碍，进一步优化轨迹。

引言

基于采样的方法首先将随机或确定性的采样点连接起来，并通过碰撞检测和性能评价在一系列候选点中选择期望的轨迹。利用在动作空间中绘制的采样点来生成时空轨迹。
kuwata采用闭环系统提高标准RRT的稳定性，通过低级控制器的仿真保证了规划的可行性。麦克沃兰提出基于确定性采样的方法在预定的加速度范围内采样，产生了jerk-bounded。gutianyu通过在短期内确定性采样的多项式速度剖面上进行分支，进一步提高了时空轨迹的平滑性。然而，在小样本量足够的情况下，基于采样的方法可以产生远离全局最优的轨迹。否则，由于维度灾难，规划是棘手的和耗时的。此外，碰撞检测实际上没有考虑由时间参数化的动态障碍，可能导致时空轨迹的可行性较差。
基于图搜索的方法在离散图上在一定的性能要求下搜索期望的轨迹。最初，这些搜索方法只能处理静态的障碍。但目前，越来越多的研究工作都在关注于时空轨迹的生成，并考虑了未来的动态障碍。由piv托拉科等人提出的state lattice是应用最广泛的由运动学上可行的运动primitives和时间信息组成的离散图。在这种state lattice中，利用经典的图搜索方法可以得到时空轨迹。Dijkstra算法被广泛用于寻找最短轨迹。$A\ast$是Dijkstra算法的扩展，是一种快速启发式搜索算法，也是许多实现的基础，如Junior（混合A*）[17]和Boss（$AD\ast$）。为了在不断变化的环境中更有效地进行搜索，提出了$D\ast$[19]和$D\ast$Lite[20]利用之前的信息重新规划轨迹。虽然基于图搜索的方法生成了接近全局最优的轨迹，但其高阶导数仍然是不连续的。此外，state lattice的创建需要大量的时间，导致规划效率较低。
基于优化的方法涉及数学问题的求解，以获得具有多个约束的最小代价或目标函数。轨迹被迭代地细化，直到满足终止或收敛条件[22]。柴田等[23]采用势场方法生成基于速度矢量的避障轨迹。他们通过一种称为拟牛顿的数值优化方法来确定速度势函数的最优参数值。Keller等[24]用定时弹性带（TEB）函数表示时空轨迹，并在约束动作空间中对其进行优化。他们将这个问题表述为一个非线性最小二乘问题，并使用黎文堡-马夸特算法来解决它，这是一种数值优化方法。Gao等人[25]应用了一种基于局部优化的方法，称为模型预测控制（MPC）的方法来平滑一个粗糙的预定义轨迹。他们通过顺序二次规划（SQP）使代价函数最小化，其中问题被划分为一系列具有线性化约束的二次规划（QP）子问题。然而，基于优化的方法往往是耗时的，特别是对于非凸的环境约束。
总的来说，目前关于轨迹规划的研究工作提供了深刻的进展，但仍有局限性。基于采样的方法可以通过探索整个空间和寻找全局最优值来缓解局部最优性问题。然而，它遭受了维数灾难和高计算代价，导致分辨率次优。基于图搜索的方法可以有效地生成接近全局最优的轨迹，但通常不是曲率连续的，不满足模型执行轨迹的可行性。基于优化的方法可以保证模型信息的可行性，并通过几次迭代找到局部最优，在计算上是可接受。然而，需要预先确定的参考轨迹，并且需要适当的设计和初始化。
因此，本文提出了一种由两个模块组成的混合规划框架，该框架保留了基于图搜索的局部最优性和基于优化的局部细化方法，从而实现了在受约束的动态环境下更快更流畅的时空轨迹规划。该方法首先基于有向无环图（DAG）构建时空驱动空间，并利用改进的$A\ast$算法有效地搜索时空轨迹。然后以搜索结果为参考，设计局部凸可行驾驶区域，并应用MPC，综合考虑车辆运动学和运动障碍，对轨迹进行进一步优化。

提出的方法

为了在受约束的动态环境下有效地生成光滑的时空轨迹，本文提出了一种混合规划方法。其框架包括两个过程：使用基于搜索的方法生成参考轨迹和使用基于优化的方法进行局部细化和平滑：
1、用基于搜索的方法生成参考轨迹。行驶图由三维（3D）时空图表示，而不是由DAG构造的一般网格图表示。动态障碍在未来的位置可以在这样的驾驶地图中得到说明。然后，利用改进后的A*算法在动作空间中进行搜索，根据设计的性能要求，有效地得到最佳的无碰撞轨迹。
2、采用基于优化的方法来平滑参考轨迹。由于离散作用空间和状态空间的存在，参考轨迹不够光滑。因此，我们采用MPC方法进行了进一步的优化。然而，动态障碍的存在通常会给自我车辆带来非凸可行的驾驶区域。它增加了计算的难度，影响了整个规划的效率。然后，提出了凸可行集的重构方法，以保证快速优化。引入运动学模型，保证轨迹的高阶导数是连续的，与车辆运动学一致。总体上，优化后的轨迹足够平滑，跟踪性能好，控制代价合理。

参考轨迹生成

本节介绍了参考轨迹的生成，其中包含了在动态环境中指向目的地的时空信息。首先利用DAG建立了以三维时空图所代表的驾驶图。然后，利用改进后的$A\ast$算法在动作空间中进行搜索，有效地得到驾驶图上的最佳无碰撞轨迹。
时空图建设
时空轨迹不仅提供了位置载体，而且还提供了运动学信息，使车辆控制器具有良好的跟踪性能。为了在存在移动障碍物的情况下生成一个合理的轨迹，它们的位置必须通过时间进行参数化。然而，由于缺乏时间信息，基于图搜索的算法通常不考虑其在未来的位置。因此，在网格图中添加一个时间线，然后利用DAG建立三维时空图。
DAG由点和指向从一个点到另一个点的边组成，沿着它，任何点都不可能环回自身。这种拓扑排序中的点序列表示车辆状态随时间的变化，即时空轨迹。然后，具有连接相邻点的有向无循环边的网格图就是DAG。
首先，通过将环境空间划分为有限多个相同大小的正方形，建立了二维（2D）空间网格图。自车被认为是一个占据其几何中心所在的网格的质点。其他车辆所占据的网格考虑到车辆的大小，以便与自车的横向和纵向距离足够大，以避免碰撞。为了提高搜索效率，有一些关于消除无用方形的初步工作。即使有最大的转向角度，自车也不可能不碰撞这些区域：例如，跟随邻近车辆和目的地之前的圆形区域，被描绘成灰色阴影部分。这些区域的半径，即自我车辆的最小转弯半径，与车速的方向相切。
二维空间网格图沿着添加的时间线进行扩展，得到了一个三维时空图。它的多个图层彼此平行，几条有向边在不同的时间步长上将相邻图层中的两个状态连接起来。随着时间的推移，边缘呈单一方向，符合DAG的要求。由几条实蓝线连接的点序列说明了用时间步长$\Delta t$离散的时空轨迹。这样的轨迹同时包含了位置信息和时间信息。具体来说，它可以表示特定时间的停止，而对于一般的网格图是不可能的。
在三维时空图中，动态障碍在不同时间步长下的位置被描述为浅绿色部分。假设周围车辆未来的位置由轨迹预测给出，这是决策系统中的另一个重要模块。然后，在未来，所提出的规划方法可以考虑动态不可行区域，而经典的基于网格图的基于图形搜索的方法只考虑每个搜索步骤中的静态障碍。因此，没有碰撞的参考轨迹在较长的时间范围内是可行的。由于离线建立的三维时空图具有良好的适宜性和可伸缩性，因此从场景到场景只指定了不可行的方块。不可行的方块可以迅速消除，从而使障碍物数量的增加对整体处理时间影响不大。
此外，所建立的三维时空图不仅包含车辆状态s的信息，还包含动作$A$的信息，用蓝色定向线表示。其在平面$XOT$中的投影的斜率表示横向速度$v_x=\Delta l_x/\Delta t$，而其在$YOT$平面中投影的斜率表示纵向速度$v_y=\Delta l_y/\Delta t$。它在平面$XOY$上的投影与轴$Y$之间的角度近似于偏航角。这些动作是根据所确定的邻近状态来计算的。然后，如果确定了动作空间（蓝色实线和虚线），则可以将可能的状态从当前状态展开。请注意，可以采用不同的曲线来在相邻的图层上连接两个状态。由于假设时间步长足够短，所以假设速度是常数。然后，本文使用直线，将自车的空间轨迹表示为浅蓝色部分。
轨迹搜索算法
引入了一种基于图搜索的方法$A\ast$算法，以其效率高来寻找最佳参考路径。它通常处理带有固定障碍的情况。网格配置空间的基础$A\ast$算法仅限于空间轨迹。然后，本文对$A\ast$算法进行了改进，以适用于所建立的三维时空图。
在本文中，车辆状态是横向和纵向位置$x$、$y$、速度$v$和偏航角$\theta$的集合，表示为 s t = { x t , y t , v t , θ t } {{\rm{s}}_t} = \{ {x_t},{y_t},{v_t},{\theta _t}\} st​={xt​,yt​,vt​,θt​}。离散的动作空间包括可能的加速度和偏航角，表示为
A = { A , Θ ∣ A = ⋃ i a i , Θ = ⋃ i θ i } {\cal A}{\rm{ = \{ }}A,\Theta |A = \bigcup\nolimits_i {{a_i},} \Theta = \bigcup\nolimits_i {{\theta _i}} {\rm{\} }} A={A,Θ∣A=⋃i​ai​,Θ=⋃i​θi​}。展开式为：
$${\theta }_{t+1}={\theta }_t+{\theta }_i$$
$$v_{t+1}=v_t+a_i\times \Delta t$$
$$x_{t+1}=x_t+v_{t+1}\times \Delta t\times \cos {\theta }_{t+1}$$
$$y_{t+1}=y_t+v_{t+1}\times \Delta t\times \sin {\theta }_{t+1}$$
在搜索过程中，每个时间步长的closed set不仅存储被选择到所需轨迹的节点，还存储自车目前无法到达的节点。而open set存储了等待被检查的候选节点。$A\ast$算法通过一个估计的路径代价函数来评估open set中的每个点。$$\mathrm{Cos}t(s_t)=J(s_t)+H(s_t)$$
其中，$J(s_t)$是从初始状态移动到状态$s_t$的累积代价，而$H(s_t)$是一个启发式函数，它估计从$s_t$移动到目的地的代价。$A\ast$算法将启发式信息引入到路径代价函数中，提供了车辆状态的方向扩展，减少了要扩展的节点总数。
在每次迭代中，选择open set代价最低的节点进行展开。然后，将其从open set中删除，并在下一步中添加到closed set中。参考轨迹通过重复选择生成，直到选择目标点。本文中的搜索空间是动作空间$\mathcal{A}$而不是状态空间$s$，这大大减少了搜索面积。节点在动作空间$\Theta$中沿着离散的偏航角展开，并根据从动作空间$\mathcal{A}$中的加速度计算出的速度确定下一个位置。

轨迹平滑

平滑的时空轨迹会导致自动驾驶汽车较小的跟踪误差和较高的驾驶舒适性。如前一节所述，改进后的$A\ast$算法在某些性能要求下可以有效地找到最佳轨迹。然而，状态和动作空间都是高度离散的，这使得时空轨迹不够平滑，使自车可以跟随。因此，采用基于车辆运动学模型的MPC进行进一步优化，确保其高阶导数连续的，与车辆运动学一致。实时应用的挑战是如何处理动态约束，因为混合车流中的移动障碍通常会给自我车辆带来非凸可行的驾驶区域。本文介绍了以往关于重构凸可行集以加速解决优化问题的研究。详细可参考[1]。

[1]The convex feasible set algorithm for real time optimization in motion planning.

凸可行集设计
自车的整个可行集$\Gamma$是超集${\Gamma }_i$的交集
$$\Gamma :=\bigcap _i^N{\Gamma }_i$$
其中，${\Gamma }_i$是依赖于周围各车辆约束的可行集。基于搜索结果的增强的车辆状态被表示为参考状态${\text{X}}^r$。根据每个约束${\mathcal{F}}_i(X^r)\in {\Gamma }_i$，一个凸可行超集属于可行集。然后，计算了给定参考状态下的整个凸可行集
$$\mathcal{F}({\text{X}}^r)=\bigcap _{\mathrm{i}}{\mathcal{F}}_i({\text{X}}^r)$$
其中，参考轨迹${\text{X}}^r$周围的每个凸可行超集可以根据之前工作启发的以下规则构造：
1、如果${\Gamma }_i$是凸的，
$${\mathcal{F}}_i({\text{X}}^r)={\mathrm{\Gamma }}_{\mathrm{i}}$$
2、如果${\mathrm{\Gamma }}_{\mathrm{i}}$的补集是凸的，则它对障碍物的有符号距离函数$\varphi$可以设计为凸的
$${\varphi }_{\mathrm{i}}(X)\ge {\varphi }_{\mathrm{i}}(X^r)+\Delta {\varphi }_{\mathrm{i}}(X^r)\times (X-X^r)$$
然后定义凸可行超集为
F i ( X r ) : = { X ∣ ϕ i ( X r ) + ∇ ϕ i ( X r ) × ( X − X r ) ≥ 0 } {{\cal F}_i}({X^r}): = \{ X|{\phi _i}({X^r}) + \nabla {\phi _i}({X^r}) \times (X - {X^r}) \ge 0\} Fi​(Xr):={X∣ϕi​(Xr)+∇ϕi​(Xr)×(X−Xr)≥0}
3、如果${\mathrm{\Gamma }}_{\mathrm{i}}$和补集$R\backslash {\mathrm{\Gamma }}_{\mathrm{i}}$都不是凸的，函数${\varphi }_i$既不是凸也不是凹的。采用辅助函数作为
$${\tilde{\varphi }}_i(X):={\varphi }_i(X)+\frac{1}{2}(X-X^r{)}^T{H}_i(X-X^r)$$
其中${\tilde{\varphi }}_i(X)$是凸的，所以
$${\tilde{\varphi }}_i(X)\ge {\tilde{\varphi }}_i(X^r)+\nabla {\tilde{\varphi }}_i(X)\times (X-X^r)={\varphi }_i(X^r)+\nabla {\varphi }_i(X)\times (X-X^r)$$
然后定义凸可行超集为
F i ( X r ) : = { X ∣ ϕ i ( X r ) + ∇ ϕ i ( X ) × ( X − X r ) ≥ 1 2 ( X − X r ) T H i ( X − X r ) } {{\cal F}_i}({X^r}): = \{ X|{\phi _i}({X^r}) + \nabla {\phi _i}(X) \times (X - {X^r}) \ge {1 \over 2}{(X - {X^r})^T}{H_i}(X - {X^r})\} Fi​(Xr):={X∣ϕi​(Xr)+∇ϕi​(X)×(X−Xr)≥21​(X−Xr)THi​(X−Xr)}
参考轨迹的动态障碍约束都可以重构为上述三种凸可行集，保证了以下优化步骤中的高效率。
轨迹优化方法
利用MPC对参考轨迹进行了优化，并将问题表述为：
$$\underset{U_n}{\min }\sum _{n=1}^T{\omega }_1\times {\Vert X_n-X_n^r\Vert }_Q^2+{\omega }_2\times {\Vert U_n\Vert }_R^2$$
$$s.t{X}_0=X^{start},X_N=X^{t\arg et}$$
$$X_n=X_{n-1}+F(X_{n-1},U_{n-1})\times \Delta t$$
$$U_{\min }\le U_n\le U_{\max }$$
$$\varphi (X_n)\ge 0,\forall n=1,...,N-1$$
其中，$T$为预测视界，$X^{start}$表示初始位置，$X^{target}$表示终点，$X^{start}$，$X_n^r$表示时空轨迹的参考状态，$U_n$表示限制为（$U_{min}$，$U_{max}$）的控制输入，$Q$和$R$是状态和控制输入的每个分量的加权矩阵，${\omega }_1$和${\omega }_2$是在两个不同目标上的权重，$F(\cdot )$是车辆状态空间模型，$\varphi (\cdot )$表示每个状态的状态约束。将非线性函数$F(X_n,U_n)$展开为参考点周围的泰勒级数
$$F(X_n,U_n):=F(X_n^r,U_n)+Ja{c}_F(X_n,U_n)\times (X_n-X_n^r)$$
其中，$ja{c}_F(\cdot )$是$F(\cdot )$的雅可比矩阵。然后，将状态空间模型线性化为
$$X_n=X_{n-1}+F(X_{n-1},U)\times \Delta t$$
$$=A_n{X}_{n-1}+B_n{U}_{n-1}+C_n$$
其中，$A_n$、$B_n$和$C_n$是定常矩阵。如果动态约束$\varphi$是凹的，则解决问题是耗时的。利用前面介绍的凸可行集构造方法，将问题重新表述为
$$\underset{U_n}{\min }\sum _{n=1}^T{\omega }_1\times {\Vert X_n-X_n^r\Vert }_Q^2+{\omega }_2\times {\Vert U_n\Vert }_R^2$$
$$s.t{X}_0=X^{start},X_N=X^{t\arg et}$$
$$X_n=X_{n-1}+A_n{X}_{n-1}+B_n{U}_{n-1}+C_n$$
$$U_{\min }\le U_n\le U_{\max }$$
$$X_n\in \mathcal{F}(X_n^r),\forall n=1,...,N-1$$