[video width="640" height="360" mp4="https://www.atyun.com/uploadfile/2017/12/videoplayback.mp4"][/video]

首先观看→https://www.youtube.com/watch?v=KcJJOI2TYJA

问题:快速和安全的运动规划
实时自主的运动规划和导航是很困难的，尤其前提是在是否具备安全性的时候。当出现复杂的动态系统，外部干扰(如风)和未知的环境时，这就变得更加困难了。在这项工作中，我们的目标是使现有的实时运动规划器具有鲁棒性，以保证在动态系统的导航过程中安全。

在控制理论中，有像哈密顿-雅可比可达性分析这样的技术，提供了系统行为的严格安全保证，以及达到给定目标的最优控制器(见图1)。然而，一般而言，哈密顿-雅可比可达性分析中使用的计算方法仅可用于可分解和/或低维度系统;这是由于“维度的诅咒”。这意味着我们不能为超过两个维度的系统处理安全的轨迹。由于大多数现实世界的系统模型，如汽车、飞机和四轴飞行器都有两个以上的维度，这些方法在现实中通常是难以处理的。

• 哈密顿-雅可比可达性分析：http://ieeexplore.ieee.org/abstract/document/1463302/

另一方面，像快速遍历随机树(RRT)和模型预测控制(MPC)这样的几何运动规划器可以通过使用简化的系统动态模型和/或短期规划的视野（horizon）在实时执行规划。虽然这让我们能够进行实时的运动规划，但最终的轨迹可能过于简单，导致不可避免的碰撞，甚至可能是动态的并不可行的(见图1)。例如，设想你在骑自行车，沿着人行道追踪行人的轨迹。这条路径引导你径直骑向一棵树，然后在最后一秒的时候，你想以90度的转弯避开这棵树。但是你的自行车做不到这么大的转弯，所以你会撞到树上。一般来说，机器人专家通过假设障碍比计划中的实际情况略大一些，从而缓和了这一问题。这大大提高了不碰撞的可能性，但仍然不能提供足够的保证，并可能导致意外的碰撞。

那么，我们如何将快速规划的速度与缓慢规划的安全保证结合起来呢?

 

图1：在左边，我们有一个高维度的飞行器，它通过一个障碍路线进入一个目标。计算最优安全轨道是一个缓慢而有时较为棘手的任务，而重新规划几乎是不可能的。在右边，我们简化了我们的飞行器模型(在这种情况下，假设它可以在点上的直线上移动)。这使我们能够非常迅速地做出规划，但是当我们执行规划的轨迹时，我们可能发现我们不能真正地沿着这条路径走，最终会发生碰撞。

解决方案:FaSTrack
FaSTrack（Fast and Safe Tracking），翻译过来是“快速而安全的跟踪”。FaSTrack是一种工具，它本质上是像RRT或MPC这样的快速运动规划器，但却同时保持实时的性能。FaSTrack允许用户以简化的动态方式实现快速运动规划，同时以预先计算的边界的形式保持安全，这是在规划器状态和实际的自治系统状态之间的最大可能距离。我们称这个距离为跟踪误差界（tracking error bound）。该预算法还能得到一个最优的控制查找表（lookup table），为自治系统提供最优的误差反馈控制器，使其能够实时地进行在线规划。

图2：FaSTrack的目的是使用简化模型(蓝色)，但是预先计算一个跟踪误差界，这个跟踪误差界捕捉了由于模型不匹配和环境扰动（例如，风）而导致的所有可能的偏差，以及在这个范围内的一个误差反馈控制器。然后，我们可以通过跟踪误差界来增加我们的障碍，这保证了我们的动态系统(红色)仍然是安全的。在机器人学社区中，增加障碍不是一个新概念，但是通过使用我们的跟踪误差界，我们可以考虑系统的动态和干扰。

离线预先计算
我们通过将问题看作是规划器和跟踪器之间的一种追踪行为，来预先计算这个跟踪错误。规划器使用一个真正的自治系统的简化模型，这是实时规划的必要条件;跟踪器使用了一个更精确的真实自治系统模型。我们假设跟踪器（真正的自治系统）总是在追赶规划器。我们想知道在最坏的情况下，最大的相对距离(即最大的跟踪误差)是什么:当规划器正积极地试图避开跟踪器时。如果在这个边界上有一个上限，那么我们就知道在运行时可能发生的最大跟踪误差。图3：跟踪系统，采用复杂的真实系统动态跟踪，以一个非常简单的模型进行计划。

因为我们关心的是最大的跟踪误差。因此，为了解决这一问题，我们必须首先确定两种系统之间的相对动态，方法是在原点固定规划器，并确定跟踪器相对于规划器的动态。然后，我们将成本函数指定为该原点的距离，即跟踪器的相对距离，如图4所示。这个跟踪器将尝试把这个成本最小化，并且规划器试图将它最大化。随着时间的推移，我们不断进化出这些最优的轨迹，我们捕捉到了在这段时间内发生的最高成本。如果跟踪器总能最终赶上规划器，那么这个成本就会一直集中在固定成本（fixed cost）上。

聚合的价值函数的最小不变的水平集提供了跟踪误差界的判断，如图5所示。此外，聚合的价值函数的梯度可以创建一个最优的错误反馈控制策略，以便跟踪器能够追赶规划器。我们使用了伊恩米切尔的水平集工具箱和可达性分析来解决这个微分对策（differential game）。关于优化的更详细的解释，请参阅我们最近在2017年IEEE会议上发表的关于决策和控制的的论文。

• 伊恩米切尔的水平集工具箱：http://www.cs.ubc.ca/~mitchell/ToolboxLS/


• 论文：https://arxiv.org/abs/1703.07373

图4

图5

在图4，我们展示了价值函数在成本函数上(距离原点)的初始化，并根据微分对策的变化进行展开。在图5，我们应该对这个价值函数进行3D和2D切片。每个切片都可以被认为是“候选的”跟踪误差界”。随着时间的推移，其中一些界限变得不可能继续存在。聚合的价值函数最小的不变水平集为我们提供了最严格的、可行的跟踪误差界。

在线实时规划
在在线阶段，我们在一个给定的感知范围内感知障碍，想象一下带有一个明可夫斯基和（Minkowski sum）的跟踪误差来扩展这些障碍。使用这些填补的障碍物，运动规划器决定它的下一个想要的状态。基于跟踪器和规划器之间的相对状态，跟踪器(自治系统)的最优控制由查找表确定。自治系统执行最优控制，不断地重复工程，直到达到目标为止。这意味着运动规划器可以继续快速的做出规划，并且通过简单的增加障碍和使用查找表来控制是可以确保安全的！

图6：用MATLAB模拟的一个10D近似悬停的四轴飞行器（quadroter）模型(蓝色线)“追踪”一个正在使用RRT进行规划的三维规划模型(绿点)。随着新的障碍被发现(转变为红色)，RRT计划为目标制定一条新的路径。基于规划和自治系统之间的相对状态，可以通过查找表找到最优控制。即使RRT规划器突然转向，我们也保证会在跟踪误差界(蓝盒子)中。

通过元规划减少保守性
在规划器和跟踪器之间制定安全跟踪问题的一个后果是，由此产生的安全追踪通常相当保守。也就是说，如果规划器总是被允许做最坏的行为，跟踪器不能保证它会与规划器接近。一个解决方案是使用多个规划模型，每个规划模型都有自己的跟踪误差。由此产生的“元规划”（meta-plan）由每个规划器计算出的轨迹段组成，每个规划器都用适当的最优控制器来跟踪规划器生成的轨迹。这在图7中得到了说明，蓝色的误差界与一个可以快速移动的规划器相对应，小的红色边界对应于一个移动速度较慢的规划器。

图7：通过考虑两个不同的规划器，每个规划器都有不同的跟踪误差界，我们的算法能够找到一个有保证的、安全的“元规划”，它更喜欢那些不那么精确但速度更快的蓝色规划器，但它会恢复为在障碍附近的更精确但速度较慢的红色规划器。这就产生了一种自然的，直观的行为，最优权衡是将规划器的保守主义和飞行器的机动速度联系起来。

安全转换
实现这一工作的关键是确保规划器之间的所有过渡都是安全的。这可能有点复杂,但主要的思想是,如果我们能保证计算为A的不变集（invariant set）包含在计算为B的不变集中，两个规划器（称之为A和B）之间的过渡是安全的。对于许多成对的规划器来说这是真实的,比如在图7中，从蓝色边界转换到红色边界。但通常情况并非如此。一般来说,我们需要解决一个非常类似于在FaSTrack中的那个原始的动态对策,但我们想知道的是我们永远不会离开的集的状态,并且从这之中我们可以保证我们在B的不变集内部结束。通常,由此产生的安全转换界(SSB)比跟踪误差界(TEB)更大,如下所示。

图8：对于一个大跟踪误差界和小跟踪误差界规划器之间的过渡，安全切换通常比跟踪误差界还要大，如图所示。

高效的在线元规划
为了有效地做到这一点，我们使用了经典RRT算法的改良版。通常，RRT的工作方式是在状态空间中采样点，并将它们与线段连接起来，形成一棵扎根在起始点的树。在我们的例子中，我们将线段替换为单个规划器所产生的实际轨迹。为了找到通往目标的最短路径，我们倾向于那些能够更快地行动的规划器，首先尝试它们，如果更快的那个规划器失败的话，它们只会求助于行动缓慢的规划器。

不过，我们必须小心确保安全的转换界是令人满意的。这对于元规划器决定向更精确、更慢移动的的规划器过渡的情况尤其重要，就像上面的例子一样。在这种情况下，我们实现了一种单步虚拟回溯算法，在此算法中，利用转换控制器能够确保前一段轨迹段不受碰撞。

图9：一个标准的LQR控制器不能将四轴飞行器保持在跟踪误差界内。

图10：最优的跟踪控制器使四轴飞行器保持在跟踪界限内，即使在规划的轨道中剧烈变化。

它也适用于硬件！我们在开源的Crazyflie 2.0四轴飞行器平台上进行了测试。正如你在图12中所看到的，我们可以在任何时候都保持在跟踪界限内，即使是在转换规划器的时候。

图11(左):一个由OptiTrack运动捕捉系统观察到的Crazyflie 2.0四轴飞行器。图12(右)：在元规划算法的硬件测试中的位置跟踪。如图所示，即使是在开始后大约4.5秒时规划器转换的情况下，跟踪系统始终处于跟踪误差界内。