快速安全追踪(FaSTrack)：确保动态系统的安全实时导航

问题：如何实施快速和安全的运动规划

实时自主运动和导航是很难的，特别是当我们关心安全性时。当我们的动力系统复杂，以及外部干扰（如风）和先验条件未知时，这变得更加困难。我们在这项工作中的目标是为了保证“鲁棒性“实时运动系统在动态系统导航过程中的安全。

在控制理论中，像Hamilton-Jacobi可达性分析这样的技术提供了严格的系统行为安全保证，同时还有一个最优控制器来达到一个给定的目标（见图1）。然而，一般来说，HJ可达性分析中使用的计算方法仅适用于可分解为低维系统或低维系统; 这是由于“维度的诅咒”。这意味着对于实时规划系统，我们无法处理大于二维的系统的安全轨迹计算问题。像汽车，飞机和四旋翼飞机这样的大多数现实世界的系统模型具有两个以上的维度，所以这些方法通常是难以对多维系统进行实时处理的。

另一方面，像快速探索随机树（RRT）和模型预测控制（MPC）等几何运动规划器可以通过使用简化的系统动力学模型或较小的规划范围进行实时规划。虽然这使得我们可以执行实时的运动规划，但是由此产生的轨迹可能过于简化，导致不可避免的碰撞，甚至可能是动态不可行的（见图1）。例如，想象一下骑自行车，跟随行人行走的路径。这条路通向一棵树，然后在最后一秒转90度。你不能让你的自行车如此急转弯，最终撞上树。通常，机器人专家通过假设障碍比规划中的实际情况略大。这很大程度上解决了这个问题，但仍然不能保证不发生。

那么我们如何将快速规划的实时性与慢速规划的安全性结合起来呢？

FIG1

图1.在左侧，我们有一辆高维车辆通过一个障碍跑道到达目标。计算最佳安全轨迹是一个缓慢而有时难以处理的任务，重新规划几乎是不可能的。在右边，我们简化了我们的车辆模型（在这种情况下，假设它可以直线连接点）。这使得我们可以很快地进行计划，但是当我们执行计划的轨迹时，我们可能会发现我们实际上不能确切地沿着路径行进，并最终崩溃。

解决方案：快速安全追踪(FaSTrack)

FaSTrack：快速和安全的追踪，实质上是“快速”运动规划者，类似RRT或MPC的同时保持实时性。FASTRACK可以让用户以简化的动力学模型实现快速运动规划，同时保持在一个形式安全预先计算的绑定在规划者的状态和实际的自治系统的运行时状态之间的最大可能距离。我们称这个距离为跟踪误差界限。这个预计算实现了了一个最优的控制查找表，它为自治系统提供最优的误差反馈控制器来执行实时计划。

fig2

图2. FaSTrack背后的想法是使用简化模型（蓝色）进行规划，但是预先计算跟踪误差界限，以捕捉由于模型不匹配和环境干扰（如风）导致的轨迹的所有潜在偏差，以及误差反馈控制器在这个界限内。然后，我们可以通过跟踪误差界限来增加我们的障碍，这确保了我们的动态系统（红色）保持安全。增加障碍物在机器人领域并不是一个新概念，但是通过使用我们的跟踪误差界限，我们可以考虑到系统动力学和干扰。

离线预计算

我们通过将问题视为规划者和跟踪者之间的追逃游戏来预先计算这个跟踪误差。规划者使用实时规划所需的真正自治系统的简化模型; 跟踪器使用更准确的真正的自治系统模型。我们假设追踪者即真正的自治系统，总是追求规划者。我们想知道在最坏的情况下最大相对距离（即最大跟踪误差）是多少：当计划者正在积极地试图逃避追踪者时。如果我们有这个界限的上限，那么我们知道运行时可能出现的最大跟踪误差。

图三

图3.具有复杂的真实系统动力学模型的跟踪系统跟踪计划系统，计划系统使用非常简单的模型进行计划。

因为我们关心最大跟踪误差，所以我们关心最大相对距离。因此，为了解决这个追求逃避游戏，我们必须首先确定两个系统之间的相对动态位置，方法是将规划者固定在原点上，并确定跟踪者相对于规划者的动态。然后，我们指定一个成本函数作为到这个原点的距离，即追踪器到规划器的相对距离，如图4所示。追踪器将尽量减少这个函数值，规划者将尽量使其最大化。随着时间的推移演变这些最佳轨迹，我们捕捉在这段时间内发生的最高成本。如果追踪者总是能够赶上计划者，那么这个值是会收敛到一个固定的值。

如图5所示，收敛值函数的最小不变水平集决定了跟踪误差界。此外，收敛值函数的梯度可以用来为跟踪器创建最优的误差反馈控制策略计划者。我们使用Ian Mitchell的Level Set Toolbox 和Reachability Analysis来解决这个差异化的游戏。关于优化的更全面的解释，请参阅我们最近的2017年IEEE决策和控制会议的文章。

gif4

gif5

图4和图5：在左边，我们展示了价值函数在成本函数初始化（到原点的距离）并根据差分博弈演化。在右边我们应该3D和2D切片这个值的功能。每个切片可以被认为是“候选跟踪误差界”。随着时间的推移，这些界限中的一些将变得不可行。收敛值函数的最小不变级集为我们提供了最紧密的跟踪误差界，这是可行的。

在线实时规划

在在线阶段，我们感知给定感应半径内的障碍物，并想象通过用闵可夫斯基和限制的跟踪误差来扩展这些障碍物。使用这些填充的障碍物，运动计划者决定下一个想要的状态。基于跟踪器和规划器之间的相对状态，从查找表中确定跟踪器（自治系统）的最优控制。自治系统执行最佳控制，并且重复该过程直到达到目标。这意味着运动计划者可以继续快速计划，只需增加障碍物并使用查找表进行控制，就可以确保安全！

gif6

图6. 10D近悬停四旋翼模型（蓝线）的MATLAB仿真“追求”使用RRT进行规划的3D规划模型（绿点）。当发现新的障碍（变红）时，RRT规划了一条新的道路。根据规划人员与自治系统之间的相对状态，通过查找表可以找到最优控制。即使RRT策划者突然转弯，我们也能保证跟踪误差范围（蓝框）。

通过元规划降低保守性

将安全跟踪问题作为规划者和跟踪者之间的追逃规则的一个后果是，由此产生的安全跟踪限制通常是相当保守的。也就是说，如果规划者总是被允许做出最糟糕的行为，那么追踪者不能保证它会接近规划者 。一个解决方案是在规划时间时使用多个规划模型，每个规划模型都有自己的跟踪误差界限。由此产生的“元计划”由每个规划者计算出的轨迹段组成，每个轨迹段用适当的最优控制器标记以跟踪规划者产生的轨迹。这在图7中示出，其中大的蓝色误差界限对应于被允许非常快速地移动的规划者，而小的红色界限对应于移动得更慢的规划者。

fig7

图7.通过考虑两个不同的规划器，每个规划器都有不同的跟踪误差界限，我们的算法能够找到一个保证安全的“元计划”，偏好精确度较低但速度较快的蓝色规划器，但恢复到更精确但较慢红色规划器在附近的障碍。这导致自然，直观的行为，最佳地交换规划保守与车辆操纵速度。

安全切换

做这项工作的关键是确保规划人员之间的所有转换都是安全的。这可能会有一点复杂，但主要思想是，如果我们可以保证A计算的不变集合包含在B中，那么两个计划人员之间的转换（称为A和B）是安全的。对于许多规划是真实情况，例如从图7中的蓝色边界切换到红色边界。但通常不是这样。一般来说，我们需要解决一个与FaSTrack中的原始游戏非常相似的动态游戏，但是我们想知道一组我们永远不会离开的状态，并且可以保证我们最终进入B的不变集合。通常，由此产生的安全切换界限（SSB）略大于A的跟踪误差界限（TEB），如下所示。

fig8

图8.具有较大跟踪误差的规划器与具有较小跟踪误差界的规划器之间的转换的安全切换边界一般大于大跟踪误差界，如图所示。

高效的在线元规划

要实时有效地做到这一点，我们使用经典RRT算法的修改版本。通常，RRT通过对状态空间中的点进行采样，并将它们与线段连接起来，形成一个以起点为根的树。在我们的例子中，我们用各个规划者生成的实际轨迹替换线段。为了找到达到目标的最短路线，我们更倾向于能够更快移动的规划者，先尝试他们，而如果更快的规划者失败了，他们只能采取行动迟缓的规划者。

但是，我们必须小心确保安全切换范围得到满足。这在元规划师决定转向更精确，移动速度更慢的规划者的情况下尤其重要，如上例所示。在这种情况下，我们实现了一个一步的虚拟回溯算法，其中我们使用开关控制器确保前面的轨迹段是无碰撞的。

实现

我们使用Open Motion Planning Library（OMPL）的低级别运动规划器，使用C ++ / ROS中实现了FaSTrack和Meta-Planning。模拟结果显示如下，有（右）和没有（左）我们的最佳控制器。正如您所看到的，线性反馈（LQR）控制器（左侧）就不会保证在跟踪误差范围内。

图9（http://bair.berkeley.edu/static/blog/fastrack/lqr_video.gif）和10（http://bair.berkeley.edu/static/blog/fastrack/opt_video.gif）.（左）标准的LQR控制器无法将四旋翼飞行器保持在跟踪误差范围内。（右）最佳的跟踪控制器使四旋翼飞行器保持在跟踪范围内，即使在计划的轨迹发生彻底的变化时也是如此。

它也适用于硬件！我们在开源的Crazyflie 2.0 quadrotor平台上测试。正如您在图12中看到的那样，即使在切换策划者时，我们也始终保持在追踪范围内。

F11

F12

图11和12.（左）由OptiTrack动作捕捉系统观察的Crazyflie 2.0四旋翼飞行器。（右）从元计划算法的硬件测试中定位轨迹。如图所示，即使在开始后大约4.5秒发生的规划师切换期间，跟踪系统始终处于跟踪误差范围内。

这篇文章是基于以下文件：

• FaSTrack: a Modular Framework for Fast and Guaranteed Safe Motion Planning Sylvia Herbert*, Mo Chen*, SooJean Han, Somil Bansal, Jaime F. Fisac, and Claire J. Tomlin Paper, Website
• Planning, Fast and Slow: A Framework for Adaptive Real-Time Safe Trajectory Planning David Fridovich-Keil*, Sylvia Herbert*, Jaime F. Fisac*, Sampada Deglurkar, and Claire J. Tomlin Paper, Github (code to appear soon)

我们要感谢我们的合着者; 开发FaSTrack是一个团队的努力，我们非常幸运拥有杰出的同事在这个项目上。