Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame-翻译解读

论文名：Frenet框架中动态街区场景的最优轨迹生成

作者：Moritz Werling,Julius Ziegler,Soren Kammel,and Sebastian Thrun
本文为个人学习和同大家交流，希望共同交流学习，文中有翻译不对的地方，请在评论区指出，谢谢！
原文链接：Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame

摘要

自动驾驶汽车对动态高速公路和内部城市场景的安全处理涉及到生成适应交通的轨迹问题。为了考虑到整体的自动驾驶系统实际要求，我们提出了一种semi-reactive(半反应性？？？)的轨迹生成方法，该方法可以和行为层(behavioral layer)紧密结合。
这个方法通过Frenet框架的最优控制策略实现了诸如速度保持、merging(合并？)、跟随、停止的long-term objectives(长期目标？),结合了反应性避撞。这种方法的能力在一个典型的告诉公路场景得到了证明。

1.介绍

A.动机

过去三十年见证了自动驾驶领域雄心勃勃的研究。随着自动驾驶车辆朝着处理现实道路交通，它们面临着有必须要明确考虑其他交通参与者的街道场景。这包括每天的驾驶操作，例如汇入车流，通过迎面而来的车辆，换道或者避免和其他车辆发生碰撞。在简化的场景下，例如在2007年DARPA城市挑战赛中，可以通过相当简单的启发式和保守估计来解决。然而，这些方法在从头到尾的交通和高速行驶时很快达到了极限，从而导致性能不佳，甚至发生事故。这就是轨迹概念发挥作用的地方，它明确考虑了在规划和执行层面的时间t。
所提出的方法采用这种策略，和以往的工作不同的是，它特别适合高速行驶，因为它产生速度不变的运动[2]，并将速度和距离控制传递给规划层面。此外，这个算法通过结合转向和刹车/加速来提供了reactive(被动？反应性？)避障。

B.相关工作

一些轨迹规划的方法已经被提出[11],[19],[2],[4]，它们找到一个连接起点和一个远的可能的目标状态的全局路径。然而，这些方法无法对其他交通的固有的不可预见性以及由此导致的不确定性进行建模，因为它们依赖对其它交通参与者在很长一段时间的运动精确预测。其它用于轨迹规划的方法遵循离散优化的方案(如[16],[1],[7])：通常通过对表述车辆运动的微分方程进行前向积分来计算一组有限的轨迹。从这个集合中，选择使给定成本函数最小的轨迹。为了生成轨迹集，选择参数模型，比如任意阶数的曲率多项式。虽然减少了求解空间并允许快速规划，但它可能引入次优。我们将在第二章中介绍这将会导致在弯道时超调和稳态偏差。在[9]中，通过快速搜索随机树算法模拟闭环系统对轨迹树进行采样[10]。该系统结合了很多启发式方法以采样偏差的方式来断言好的行为操作。一种方法是采用了一种与我们方法相似思想但是只考虑不受障碍物约束的自由问题的方法。在这里，气动系统的最优控制轨迹是在Galerkin base跨越的函数空间中找到的。
由于上述原因，为了，并且至少部分地克服这些在文献中描述的方法局限性，我们提出了一种局部方法(local method)，能够实现上游行为(长期目标)做出的高级决策，并在意外紧急情况做出反应性(reactive)紧急避障。将我们的方法和其它方案区别开来的一方面是直接遵循Bellman最优原则的非反应性动作(non-reactive maneuvers)的保证稳定性(时间一致性)。在这项工作中，我们坚持将反馈和规划严格解耦的策略。我们之前证明了通过轨迹跟踪反馈控制把导航任务分离为实时轨迹生成和随后的局部稳定（局部稳定怎么理解？）是有利的。这与通过将系统的观测状态直接反馈给规划阶段来闭环控制回路的其它一些方法形成对比。这项工作的重点将放在轨迹生成阶段，即生成在特定操纵模式下安全操纵车辆的norminal(名义)输入。

2.优化控制方法

将最优控制理论应用于轨迹生成并不新鲜。与众说周知的工作[13],[3]相比,我们的主要关注点不是优化特定的代价函数。相反，我们在用一种最佳可行的轨迹追踪问题，来利用在一段时间内选择最佳可行轨迹的一致性理论。有了这个，我们力求确保一旦找到最佳解，它将被保留（贝尔曼最优原则）。对于汽车，这意味着它在每个规划步骤中遵循先前计算轨迹的其余部分，因此提出了时间一致性。
这与[16],[1],[7]等方法形成对比，其中轨迹以参数方式表示。例如，通过假设系统输入或曲率是多项式，并且该集合是从参数空间中采样或对其优化以满足某些终端约束来生成的[7],[16]。通常，最优轨迹，就成本函数而言，不是由参数贯穿的函数空间的一部分（这句话怎么理解？）。因此，贝尔曼的最优性原则不成立，在下一次迭代中，将选择一条略有不同的轨迹。图1说明了这种时间一致性可以引起超调甚至不稳定。
虽然我们选择代价函数的主要标准是符合Bellman最优原则，trajectories minimizing（轨迹最小化 ??）仍然必须接近自动驾驶汽车所需的交通行为。因此，让我们口头描述一辆自动驾驶汽车在街道上行驶的理想行为：假设汽车对期望的车道有一定横向偏离，比如，由于最近开始变道或避障机动。汽车应该在其驾驶物理特性范围内返回到所需车道，并且在车里感到放松和舒适与让车回到期望车道的花费的时间取得最佳折衷。
图1：相同规划策略的两种不同瞬态行为取决与重新规划的频率：（顶部）具有可容忍瞬态的高重规划频率；（底部）低重规划频率会导致超调。∆T a 和∆T b 是规划频率的倒数，n i 是后续规划步骤的起点。
同时，必须以模拟方式在纵向中找到最佳折衷方案：假设汽车开的太快或太靠近前面的车辆，它必须明显减速但不能过度匆忙。从数学上讲，放松和舒适可以用jerk来最好描述，它由加速度随时间的变化率定义，其中所需的时间只是机动的T = t_end − t_start.
由于通常带约束的优化问题的解不限于特定的函数类型【注释3：如果你想象自动驾驶车辆正在被困在四辆移动的汽车之间，并且在同一方向上，这将迫使车辆的运动变成单一的可能解，如一个正弦曲线】。这个问题变得非常复杂，充其量只能用数值来解决。这就是为什么我们的方法仅在无限制(自由)优化问题的最佳解集合中搜索约束优化问题的合理近似，并选择满足限制的最佳方案。这反过来意味着，只要最佳解决方案是有效的（restrictions are then not active约束不活跃？），非反应轨迹的时间一致性就能得到保证。反应启发式的验证还有待模拟展示。

3.在Frenet框架下的运动规划

跟踪控制理论中一种众所周知的方法是Frenet Frame方法，它在特殊欧几里得群SE(2) := SO(2) × R2的作用是在这种方法下断言跟踪性能不变。在这里，我们将应用这种方法，以便能够针对不同任务结合不同横向和纵向代价函数，并模拟高速公路上人类的驾驶行为。如图2所示，移动参考坐标系由以下中心线的某条曲线的某一点的切向和法向矢量~tr, ~nr给出。该中心线表示沿着自由道路的理想路径，在最简化的情况下是道路中心，或者一条非结构化环境的路径规划算法路径[20]。而不是直接在笛卡尔坐标系x中解决轨迹生成问题，我们切换到建议的动态参考系，并寻找沿着中心线的根点-r和垂直偏移d生成一维轨迹，其关系如图2所示。

其中s代表中心线的弧长，tx nx是结果轨迹x(s(t), d(t))的切向和法向向量。
人类显然对加速度的横向和纵向的变化率的权重不同。由于向量对tx nx和ts ns在高速时几乎重合，我们考虑在Frenet坐标系下把d’’‘和s’’‘当作jerk。从[15]中我们还可以知道在一维问题中五次多项式是在时间间隔T=t1-t0里，起始状态p0=[p0,p0’,p0’’]和p1=[p1,p1’,p1’’]之间是以jerk-optimal连接的。更准确的说，它们最小化了jerk平方的时间积分的代价函数。

我们也将在我们的方法中使用这个结果：
命题1：给定t0处的起始状态P0=[p0,p0’,p0’’]和某个t1=t0+T处结束状态P1=[p1,p1’,p1’’]，最小化代价函数的解。

对于任意函数g和h，kj,kg,kp>0也是一个五次多项式。
证明：假设所提出问题的最优解不是五次多项式。它将在时间间隔Topt内连接两个点P0和P1.那么通过相同点和相同的时间间隔的五次多项式会除了相同的其它两个成本项外，总是导致更小的成本项p’’'2的从t0到t1的积分。这与最优解必须是五次多项式的假设相矛盾。

4.产生横向运动

A.高速轨迹

由于我们寻求最小化结果轨迹的jerk的平方，我们根据之前计算的轨迹选择优化起始状态D0=[d0,d0’,d0’’]，第6章，从而不会导致不连续问题产生，对于优化本身，因为我们要移动到中心线的平行线，我们让d1’=d1’’=0(最优控制术语中的目标流形)。此外，我们g(T)=T 和 h(d1)=d1^2，由此我们得到代价函数：

因为我们想要惩罚收敛缓慢的解和那些最终偏离中心d=0的解。请注意，此成本函数和后续中使用的函数不依赖车辆的速度（IV-B章节除外）。正如我们在第1 prop中所知道的，最优解是一个五次多项式。我们可以计算它的系数和T最小化（相当冗长的表达式）并检查它（结合最佳纵向轨迹s(t)）以防止碰撞。如果幸运的话，它是有效的，我们就完成了这个过程。如果不是，我们将不得不通过稍微修改T和d(t) s(t)的系数并且检查是否再次发生碰撞来寻找无碰撞的替代方案，或者说是“次优”轨迹，等等。
我们没有明确地计算最佳轨迹并修改系数以获得有效地替代方案，而是在第一步中生成，例如在文献【16】中。整个轨迹集：通过组合不同的多项式的终止条件di和Tj：

如图3所示，在仿真时间t=0，所有可能的操作都被充分覆盖。第二步，我们选择成本最低的有效轨迹。请注意，当我们沿着最佳轨迹（非反应性的长期目标）在每一步中继续前进时，与图1相比，剩余的轨迹将是下一步的最优解。一方面，这是因为我们选择绝对时间（在图3的模拟中每一秒）选择离散点。这样在每一步中，先前的最佳轨迹在下一步中可用，另一方面，我们处于无限制问题的正确（最优）函数类中。

图3：循环重新规划产生的最优横向运动，绿色为最优轨迹，黑色为有效的，灰色为无效替代方案。

B.低速轨迹

在更高的速度下，d(t)和s(t)可以独立选择，如上一节所述（不包括极端机动，在这种情况下，汽车上的横向力和纵向力的结合起着重要的作用）。然而，在极低的速度下，该策略忽视了汽车的非完整特性，因此由于无效曲率(s. Sec. VI)，大多数轨迹必须被否定。出于这个原因，行为层可以在低于某个速度阈值时切换到稍微不同的轨迹模式，根据纵向运动生成横向轨迹：

记住，我们的重点不是最小化某个成本函数，而是利用优化理论来一致性对生成的轨迹进行评价。由于d(s)的五次多项式(定义在中心线弧长s上)导致回旋样条曲线像平行策略取向偏离中心线小于pi/2，对于低速，我们坚持多项式，并将代价函数修改为：

根据Prop. 1, s上的五次多项式属于最优函数类。然后可以类似地将集合生成执行到具有起点D0 = [d0,d0;,d0’’]和各个终点:

图3. 由循环重新规划的产生的最佳横向运动，绿色是最佳轨迹，黑色是有效的，灰色是无效的替代方案。

5.纵向运动生成

与之前以时间或行驶距离为关键标准的工作相比，我们在这里将关注舒适性同时为高速行驶的安全做努力，因为平稳的运动更能适应交通流量。出于这个原因，我们还在优化问题中考虑了纵向jerk。

A.跟随，合并和停止

由于在某些位置保持距离、合并和停止需要轨迹，这些轨迹描述了从当前状态到纵向的、可能正在移动的目标目标位置S_target(t),我们生成一个纵向轨迹集，从起点S0 = [s0,so’,s0’’]开始，并根据不同的delta_s和Tj改变末端约束：

跟随
对于跟随，移动目标点可以从国际交通规则中导出，例如[14]，要求和前方车辆有一定的时间安全距离，成为恒定时间间隙法则，所以后车沿车道的理想位置由下式给出：

具有常数D0和tau以及前方车辆的位置Slv和速度Slv’。由于我们想推导出该点附近的替代轨迹，必须预测前车的运动，我们合理地假设：

进行时间上的积分：

tau时间后目标车状态：


图4.在每个重新规划步骤中，目标位置的最优纵向跟踪用蓝色表示，绿色表示最优轨迹，黑色表示有效方案，灰色表示无效方案。
合并和停止
和上述方式相同，我们可以定义目标点：

这使我们能够将自动驾驶汽车定位在Sa(t)和Sb(t)处的一对车旁边，然后在紧密合并机动过程中慢慢挤到两者之间。
由于红灯或者停止标志在交叉路口停车，我们定义：

B.车速保持

在许多情况下，比如没有车辆在正前方行驶时，自动驾驶汽车不一定要处于某个位置，但需要适应一个理想的速度Sd’=const。由行为层给出，类似于[15]中的变化计算(附加了所谓的s1的横截性条件)和Prop1，可以找到四次多项式来最小化代价函数：

对于给定的t0时刻的初始状态S0 = [s0,s0’,s0’’]和某个t1 = t0 + T时刻的末状态S1 = [s1’,s1’’]。这意味着，我们可以通过根据delat_si’和Tj改变末状态约束来生成四次多项式的最佳纵向轨迹集：

如图5所示：

图5. 最优速度适应s_d’ = 5.0m/s，绿色为最优轨迹，黑色为有效方案，灰色为无效方案。

6.结合横向和纵向轨迹

在组合横向和纵向轨迹集之前（在后续中记为T_lat和T_lon），每一个都要对照s’‘和d’‘的超大加速度值(上一节中的图中的灰色轨迹)进行检查。由于我们不专注于最大限度发挥车辆的物理特性，因此我们选择保守地选择它们，为反馈控制器留下足够的安全余量。然后将每组中的余数放在每个组合T_lat x T_lon中，如图6所示：

图6 结果轨迹设置在全局坐标系中保持车速：彩色地图可视化了反应层的成本增加，从红色到黄色是前瞻时间为3.0s，可替代的长期目标是灰色到黑色。由于3.0s范围内没有障碍物，选择了自由问题的最优轨迹（绿色，浅灰色），使车辆返回中心线并达到所需速度。
由于最佳有效轨迹描述了反馈控制器的跟踪参数，我们需要推导出x(t)的高阶信息，即航向theta_x(t)，曲率k_x(t)，速度v_x(t)和加速度a_x(t)。对于大多数设置，假设中心线在封闭形式不可用，而是由具有方向θr(s)、曲率 κr(s) 和弧长 κ′ r(s) 上的曲率变化的预采样曲线点表示，所需的插值使得它不可能从数值上获得高阶信息。因此，所要求的封闭形式变换的推导可以在App.Ⅰ中找到。曲率kx(t)用于排除超过汽车最大转弯半径的轨迹。在最后一步，每个轨迹的联合成本计算为加权C_tot = k_lat*C_lat + k_lon * C_lon.
至于碰撞检测，我们希望避免在其它障碍物附近的成本函数中添加启发式惩罚项，因为它们往往会导致复杂的参数调整以及不可预测的行为。相反地，我们为汽车的每一侧增加了一定的安全距离，并根据与其它类似[16]的障碍物的干扰做出zero/one的决策。我们在不增加安全距离的情况下防止汽车不必要地靠近其它障碍物地解决方案，即简单又有效：碰撞检查轮廓不断向时间范围扩展一点点，因此当我们靠近时，任何类型的障碍物似乎都会不断后退。每次我们使用新的参考作为中心线时，例如在初始化和换到期间，或者当我们在低速和高速轨迹之前切换时，我们必须将当前终点(x,θx,κx,vx,ax)(t0)投影到新的中心线上并确定相应的[s0,s˙0,s¨0,d0,d˙0,d¨0] 或 [s0,s˙0,s¨0,d0,d’0,d’'0]。出于这个原因，附录中的变换可以很容易地以封闭形式反转，除了s0，因为我们不限制中心线~r(s)为某个形状【6：对于直线或圆弧，存在闭式形式解】，然而，反演可以重新表述为最小化问题s = argmin kx - r(σ)k，为此存在有效地数值方法。

7.选择正确地轨迹

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

8.实验

该算法的第一个版本在自动驾驶驾驶车辆JUNIOR上以100ms的规划周期在没有障碍(长期目标)的情况下实现和测试。与跟踪控制器[8]结合生成的轨迹具有速度不变的时间和保证的时间一致性的平稳和受控行驶，如图6所示。
由于图7所示的真实世界高速公路场景的相关风险，我们用模拟的方法测试了轨迹生成方法的反应能力，并推迟了实际验证。为了展示算法的功能，我们禁用了行为层，这是为了防止车辆尽可能频繁地在反应层解决关键情况。因此，算法只永久通过中间车道的中心线，且期望速度高于平均车流量5.0m/s。在成本权重 k_lat ≈ k_lon 的情况下，汽车总是在领先汽车（未显示）的后面很好地行驶，因此，为了清楚起见，我们使用了 k_lat ≪ k_lon 。因此，汽车更喜欢超过速度慢得多的车辆，比如在t = 0.0s和7.75s之间。在t = 29.07 s到50.0 s之间，他们的减速也会引发鲁莽驾驶，展示了转向和刹车/加速的结合使用。

9.结论和未来的工作

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

10.参考文献

附录