Frenet坐标系下动态街道场景的最优轨迹生成

0 前言

有两个主要算法已经在实际中使用：
（1）大多数研究组采用插值来解决规划问题，如奥迪、斯坦福最近演示中使用了回旋曲线，贝塞尔以及多项式曲线也被其他研究组使用。主要原因是在结构化环境中增强映射可以提供所需路点，在平滑性、连续性、车辆约束、速度以及舒适性方面提供了保证；
（2）图搜索技术是实际应用中第二个比较常用的算法，尤其是状态格。选择该技术主要原因是保证了舒适性安全性并且满足了车辆约束条件，可以实时快速搜索最优路径。
当前主要挑战是动态环境中的实时运动规划问题，实际中有许多不明意图的其他交通参与者，如何获取它们的状态并且进行准确的预测会减少规划层的压力，并且在有限时间内生成多重动态障碍的自由碰撞轨迹仍然是一个尚未解决的问题。

由于规划阶段是在感知与控制之间的环节，当前的研究在开发新的路径规划算法时既考虑了感知的不确定性又考虑了控制约束。路径规划最新发展旨在正确处理动态环境下数据采集过程中的不确定性，这需要有一个更好的实时环境理解，提高规划能力，以防止感知的不确定所带来的危险情况。从控制的角度来看，必须考虑多种约束从车辆运动学动力学限制到乘客的舒适感。最近的发展旨在规划阶段考虑其中一些限制，从而实现顺利以及可实现的轨迹，减少控制阶段的限制。下一步应该专注于开发能够将感知不确定性与控制约束融合在一起的算法。

并且将驾驶员加入控制回路是汽车自动化研究的新趋势，这带来了新的挑战，即生成安全平稳可实现轨迹时，将感知不确定性、控制约束以及驾驶员知识进行多重融合。

论文解读

论文题目：Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame

1 摘要

（1）提出了一种semi-reactive的轨迹生成算法，可以很好得集成进行为决策层。Lattice Planner方法通过最优控制策略，结合Frenet坐标系实现速度保持，塞车，跟随以及停车等目标。适用于低速场景以及高速场景的自动驾驶规划算法。
（2）在无约束优化问题的最优解集合中寻找合理的近似来解决受约束的优化问题，并选择最佳解决方案，以满足约束条件
（3）高速横纵向互相解耦；低速则不同

2 Introduction

2.1 Motivation

传统的算法存在一些局限性，难以适用于动态复杂的环境以及高速场景。作者提出的方法：
（1）很好的适应于公路场景，并且能够生成保持速度不变（指变道、并道等操作与行驶速度独立）的驾驶动作以及将速度和距离控制传递给规划层面。
（2）通过结合转向、制动/加速来实现reactive的障碍物避障

2.2 Related work

① [11], [19], [2], [4]: 使用一个连接起点和一个远的可能的目标状态的全局轨迹，但这类方法没有考虑自动驾驶车辆行车过程中各类不确定性，依赖于其他交通参与者的未来行为轨迹的精准预测。

② [16], [1], [7]:使用了离散优化的方法：通过对车辆动力学微分方程的前向积分来获得有限轨迹库(控制空间的离散化)。利用轨迹库，使用cost函数进行评价、打分，作局部路径规划。轨迹的类型可以有很多种形式。但是这种方式可能会非最优。而且这种方式可能会产生超调甚至不稳定性，如下图所示

重规划频率高的轨迹可以避免或减少重规划频率低的轨迹所遇到的超调甚至是不稳定问题。

③ [9]: 通过RRT模拟闭环系统对轨迹树进行采样[10]

④ [17]：与本文类似的思想，但没有考虑障碍物。提到了Galerkin base这个概念

（1）Galerkin base：Galerkin基是一种基函数，可以在函数空间中描述一个系统的动态行为。在优化控制中，使用Galerkin基来表示系统的状态和控制空间，以便找到最优的控制轨迹。简单来说，Galerkin基是一种数学方法，用于将一个系统的动态行为表示为一组基函数的线性组合，从而简化系统的建模和控制。

（2）Bellman最优性原理：动态规划的基本原则之一，如果我们在一个问题中找到了最优决策序列，则这个序列中的每个决策都是这个问题的最优决策，无论之前的决策序列如何。这个原理是动态规划算法的基础，将复杂问题逐步分解为简单问题，并进行逐步求解。

3 最优控制方法

作者使用在轨迹生成中引入最优控制的思想，用以保证轨迹的时间一致性。

通常来说，采用代价函数的方式可能不会遵守Bellman最优性原理，在下一次规划的时候，规划出的轨迹会有不同。那么本文是如何做到的呢？

本文的方法符合Bellman最优性原理，是因为在轨迹生成过程中，将轨迹跟踪问题优化为最优控制问题，并选择一个符合最优性原理的代价函数。
最优性原理：当一个最优解被找到后，它将一直被保留，直到问题的某些方面发生变化。
本文的方法：一旦找到最优解，车辆就会在每个规划步骤中沿着预先计算的轨迹的剩余部分行驶，从而保证了轨迹的时间一致性。

• 在最优控制方面，主要是遵循Bellman的最优化原则去选取cost function，轨迹在最小化cost的基础上也能最大程度的接近车辆理想的开车形式。
• 在解决一般受限制的优化问题时，由于问题不限于某个函数类，因此问题变得非常复杂，最多只能进行数值求解。作者的方法是用无约束优化问题下求得的最优解序列去近似受约束的优化问题。

理想的自动驾驶车辆运动行为：在偏离车道后，能够很好地平衡舒适度（jerk）以及回到期望位置的时间（$t_{end}-t_{start}$）

这意味着一旦找到最佳解决方案是有效的（约束条件不再活跃），non-reactive轨迹的时间一致性就得到保证。

4 在Frenet坐标系下的运动规划

轨迹跟踪控制里比较好的方法是Frenet坐标系方法

• $SE(2):=SO(2)$ X $R^2$特殊欧式子群里有很好的跟踪性能

特殊欧式子群SO(2)是指二维旋转群，它由所有二维旋转矩阵组成，形式为：

特殊欧式子群SE(2)是指二维欧氏群，它由所有二维刚体运动矩阵组成，包括平移和旋转，形式为：

其中$\theta$表示旋转角度，x和y表示平移量。二维欧氏群可以用于描述机器人在平面上的运动。

• Frenet坐标系下的规划问题中，参考线十分重要。参考线可以是道路中心线、或者是非结构化道路下path planning产生的粗轨迹。

基于动态参考系分别生成横纵向上的1维轨迹。相对关系如下所示：


本文采用五次多项式作为轨迹，可以达到最小化jerk的效果，构建了经典的BVP/OBVP问题。

起点：$P_0=[p_0,\dot{p_0},\ddot{p_0}]$，起点时间：$t_0$
终点：$P_0=[\dot{p_1},\ddot{p_1}]$，终点时间：$t_1=t_0+T$
代价函数是一个五次多项式：
$C=k_j{J}_t+k_{t}g(T)+k_{p}h(p_1)$
其中g和h是任意的函数，$k_j,k_t,k_p>0$

5 侧向运动的生成

在最优控制领域中，“目标流形”是一个术语，它指的是在控制系统中，控制输入所需达到的状态变量集合。在优化控制问题中，目标流形是优化问题的一部分，它描述了系统必须遵循的约束条件。在论文中，作者将$\dot{d_1}=\ddot{d_1}=0$作为目标流形，以便汽车沿着中心线平行移动

5.1 High Speed Trajectories

高速时采用横纵向解耦，独立计算的方式。
起始状态由前一段轨迹计算得到，从而保证连续性：$D_0=[d_0,\dot{d_0},\ddot{d_0}]$。为了使轨迹平行于center line，终末状态$\dot{d_0}=\ddot{d_0}=0$。再另$g(T)=T$，$h(d_1)=d_1^2$（前者为了加速收敛，后者为了减小与center line的偏离）
得到的代价函数：

轨迹生成的流程：
绿色轨迹：最佳轨迹；黑色轨迹：有效轨迹；灰色轨迹：无效轨迹

（1）不是直接计算出轨迹，再调整相关参数。作者首先生成了一个轨迹库，包含终末状态$d_i$以及经历时间$T_j$：

（2）选出最低成本有效轨迹
每个周期规划出来的轨迹会沿着上一个周期规划出来的最优轨迹继续规划。这样就能够符合Bellman最优性原理以及达到时间一致性。

5.2 Low Speed Trajectories

对于低速场景，横纵向解耦的方式忽略了车辆的非完整性约束，所以轨迹可能不有效。低速时的相对关系如下所示：

对于低速场景，可以在behavioral层设置一个速度阈值，当速度低于这个阈值时基于纵向运动生成横向轨迹。
低速场景下的代价函数：

轨迹库的生成和高速情况类似，但是要注意$d^{\prime }$是对s求导。

6 纵向运动生成

横向规划主要将时间/距离作为主要的评判标准，此处的评判标注主要是comfort和safety，所以引入了纵向的jerk作为优化问题的考虑。
如下图纵向规划的过程：

在每个重新规划步骤中，目标位置的最佳纵向跟踪轨迹用蓝色表示，其中绿色为最佳轨迹，黑色为有效轨迹，灰色为无效轨迹

对于跟车、并道以及停车等场景，需要有一个目标状态$s_{target}(t)$作为引导，在每一次的周期中，更新末端约束（会有不同的$△s_i$以及$T_j$）


代价函数：$C_t=k_j{J}_t+k_{t}T+k_s[s_l-s_d{]}^2$

6.1 Following 跟车

constant time gap law：国际交通规则中要求车辆与前车保持一定时间间隔的安全距离，该距离会随着速度的变化而变化。这个时间间隔被认为是一个常数，因此被称为"constant time gap law”。这个规则的目的是确保在高速公路等高速道路上行驶的车辆之间保持安全距离，以确保行车安全。

其中前车的一些数据都是需要通过感知预测来获得的，其中我们假设前车的加速度保持不变，${\ddot{s}}_{lv}(t)={\ddot{s}}_{lv}(t_0)=const$。对时间积分得：

然后再得到目标状态的剩余变量

6.2 Merging and Stopping 并道和停车

并道的目标点定在两车之间。
停车场景则是如下：

6.3 Velocity Keeping 定速

针对前方无车的情况，代价函数如下：$C_v=k_j{J}_t(s(t))+k_{t}T+k_{\dot{s}}[\dot{s_1}-\dot{s_d}{]}^2$
通过用$△\dot{s}$，$T_j$来改变终末状态的约束，来生成四次多项式的轨迹库：

7 横向和纵向轨迹相结合

① 先对轨迹进行加速度限幅检查，再进行合并
② 通常情况下，参考线不是close form的，而是由预先采样得到的点集进行表示（航向角${\theta }_r(s)$，曲率$k_r(s)$，曲率的导数${\dot{k}}_r(s)$），无法用数值求解的方式得到更高阶的信息。

close form是一个数学概念，指的是可以用有限的基本算术运算和函数运算，以及有限次求导和积分等基本数学运算得到的解析式表示的数学表达式。

③ 最后轨迹的联合代价通过给横纵向添加不同的权重系数得到：

④ 减少不必要的side-pass的措施：碰撞检查的范围随着时间扩张一点点

⑤ 当遇到需要变更参考线的情况时，需要将终末端点$(x,{\theta }_x,k_x,v_x,a_x)$投影到新的参考线上的起点$[s_0,\dot{s_0},\ddot{s_0},d_0,\dot{d_0},\ddot{d_0}]$，$s_0$除外，因为没有限定特定的参考线形状。但是可以通过构建优化的问题进行求解。

在全局坐标中设置的用于保持速度的结果轨迹：彩色地图可视化了反应层的成本增加，从红色到黄色的3.0s前瞻，以及从灰色到黑色的长期目标的替代方案。由于在3.0s范围内没有障碍物，因此选择自由问题的最佳轨迹（绿色，浅灰色），该轨迹将车辆引导回中心线和期望速度。

8 选择正确的轨迹 && 实验验证

高速公路轨迹生成足以将所有交通场景分为合并、跟随另一辆车，保持特定车速，停在特定点以及它们的所有组合，这些场景大部分时间是相互冲突的。在控制理论中，超车控制是一种众所周知的技术，它根据一个控制方案在多种控制策略里进行选择，最保守的控制策略通常通过最大算子或者最小算子来实现。该技术已在控制级别(自适应巡航控制)的众多自动驾驶汽车中成功实施，但据我们所知，并非如我们在此提出的那样在轨迹生成级别上实施。在任何时候，横向轨迹集与根据$Sec.VI$的每个活动纵向轨迹生成的模式的组合。然后将各主动模式的联合代价函数$C_{tot}$最小的无碰撞轨迹与其它轨迹进行比较，最终将最小初始jerk值$s^{\prime \prime \prime }(t_0)$的轨迹传递给跟踪控制器。主动模式的典型组合是保持速度保持和跟随(自适应巡航控制，稀疏交通状态下的车道变换)、合并（密集交通中的车道变化）以及速度保持和停止（具有交通灯的交叉路口）。

模拟高速公路场景：中心线由中间车道给出，期望速度明显高于交通流量。在$t=0.0s$和$7.75s$之间以及$t=29.07s$和$50.0s$之间显示了两个完全反应性通过机动，其中预测障碍物运动$T_{hor}\le 3.0s$。

9 结论和未来的工作

为了用自动驾驶汽车处理动态街道场景，我们提出了一种具有最优控制的轨迹生成问题的解决方案，我们通过结论性实验对其进行了说明。派生的策略有效地实现了在存在动态和静态障碍物的情况下道路驾驶所需的所有操作。由此产生的操作的特点是一致、有效、舒适且安全地融入不断变化的交通流里，并且可以通过一小组直观的正交参数进行调整。轨迹生成可以直接嵌入到行为层中，命令算法进行抽象输入，例如所需速度、要跟随的汽车的位置或换道意图。
然而，由于优化范围有限，该方法并不旨在减轻做出有远见的决策的行为层。 新的合适的启发式方法，例如在恒定时间间隙或保守汇流检查，可以最大限度地减少在反应级别处理的危险情况的数量。算法更好地集成到行为层中并且可以更精准地预测其他交通参与者，我们可以进一步推进我们的测试车辆在开放道路交通中，这将揭示在那里的接受程度。

10 Frenet到笛卡尔坐标的转换