利用内省学习和推理增强基于栅格的运动规划

编者按：目前机器学习已经广泛的应用在自动驾驶规划、控制算法的开发当中。但是车辆的工作环境复杂多变，具有较高的不确定性，加之建模的误差、环境噪声以及软件的性能和逻辑漏洞等，这些都对车辆的通行能力、学习算法的效率等提出了更高的要求，车辆的实际表现非常有可能超出由建模过程中的假设条件推导出的车辆运动安全边界，对行车安全造成很大的威胁。为了最大程度保证行车安全、提升系统的鲁棒性，对车辆进行异常监测是十分必要的。本文提出了一种对系统的正常状态进行学习、异常状态进行学习的方法，可以对算法的适应性、安全性等进行不同程度的提升。

本文译自：

《Enhancing Lattice-based Motion Planning With Introspective Learning and Reasoning 》

文章来源：

IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 6, NO. 3, JULY 2021

作者：

Mattias Tiger , David Bergström , Andreas Norrstig, and Fredrik Heintz

原文链接：https://arxiv.org/abs/2005.07385

摘要：基于栅格的运动规划是一种混合规划方法，规划由离散动作组成，同时也是物理上可行的轨迹。该规划同时考虑了离散和连续的方面，例如动作前提条件和配置空间中的无碰撞动作持续时间。安全运动规划依赖于经过良好校准的安全裕度来进行碰撞检测。轨迹跟踪控制器必须进一步能够在这个安全范围内可靠地执行运动，以便保持安全。在这项工作中，我们关注的是内省学习和推理对控制器的性能的影响。使用显式不确定性量化的机器学习技术学习不同动作的正常控制器执行，以便在安全关键应用中安全使用。通过提高模型的精度，可以降低安全边际，同时保持与以前相同的安全性。推理既可以验证学习到的模型保持安全的性能，也可以使用更准确的执行预测和更小的安全边际来在运动规划器中提高碰撞检查的有效性。该方法允许在正常情况下明确感知控制器的性能，并在异常情况下检测不正确的性能。利用仿真方法对四轴飞行器的三维非线性动力学进行了评价。

关键词：运动和路径规划，避免碰撞

Ⅰ 引言 
安全的运动规划是机器人在动态、非结构化和人为定制的环境中导航的必要条件，如在室内或城市环境中。在真实的动态环境中操作使内省功能变得重要，因为情况可以很容易地改变，超出合理的设计假设：硬件退化、建模错误、软件错误以及罕见的外部干扰，如极端天气或来自其他外界的意外对抗性攻击。从机器人的角度了解正常的运动执行是什么样子，并及时检测所执行的行为何时变得异常是非常重要的。基于栅格的运动规划是自动化车辆实际实现中最常用的运动规划技术之一。它的工作原理是将运动限制在有限数量的预先计算的运动基元上，这些运动基元在状态格上的点之间移动，并利用图搜索找到一个物理上可行的轨迹作为兼容的运动基元序列。这是一种适合于动态环境的技术和一种快速搜索方法来寻找可以实时执行最优轨迹，考虑舒适、安全和车辆约束。基于栅格的运动规划已经成功地在各种机器人上实现了。例如，最近的进展是，在实时重新规划新的具有多个动态障碍的无碰撞轨迹的挑战中。使用像度量时间逻辑(MTL)或者概率信号的时序逻辑(ProbSTL)这样的正式语言来评价正常行为，用于现代运行时验证框架，以执行运动计划的执行监控，是一个非常困难的挑战。机器学习可以用来补充正式的安全要求，学习的正常动作执行模型，例如机器人操作任务。

图1 当动作执行是安全至关重要的时候，对于像机器人这样的自主系统来说，机器人本身知道正常的执行是什么样子的（学习）以及当前的动作是正常的还是异常的（监控）是很重要的。

基于格的运动规划提供了一个有效学习和监控运动动作执行的机会，因为已经离散和定义良好的运动基元（动作）。在这篇文章中，我们提出了一种用于增强基于栅格的运动规划方法的一般方法：（1）正常的运动基元系统执行的学习模型 ，（2）使用学习的模型，提高碰撞检测的有效性 ，（3）有效的监控运动基元执行的异常情况。
由于碰撞检查和异常检测都是安全的关键，学习是使用机器学习技术（从贝叶斯机器学习的显式不确定性量化）完成的。对异常的监视并实时验证学习到的模型对于运动规划器中使用的碰撞检查仍然有效。第二节描述了给定动态障碍的现代基于网格的运动规划和控制方法。相关工作在第三小节中。我们方法中的学习、改进碰撞检查和监测运动原始执行的方法在第四节中提出，结果在第五节中，结论在第六节中。

Ⅱ 问题构造

A.运动规划

考虑一个被建模为时常非线性系统的机器人：

其中，代表机器人的状态； 是控制信号；机器人对其状态及其控制信号施加了物理约束。该机器人在2D或3D世界中运行 或者 。有一些区域被静态和动态的障碍所占据 ；自由空间是机器人所占据的区域由状态转换，在时刻没有与任何障碍物发生碰撞。

运动规划器的目标是产生一个可行的参考轨迹，使机器人从开始状态移动到目标状态。同时优化一个给定的性能度量，例如，在最小时间和平滑度之间进行权衡。考虑到这些障碍，参考轨迹也必须是无碰撞的。这个问题被称为动态运动规划问题(DMPP)：

B.基于栅格的运动规划

基于栅格的运动规划是DMPP 问题的一个可处理的近似，其中状态空间被离散为一个状态栅格，并构造了有限数量的平移不变运动基元（动作），以允许在栅格上的状态（节点）之间的运动。图搜索技术，如具有可接受启发式的A∗，可以用来寻找一个从到的有效的运动基元动作序列。一个运动基元动作是在栅格网格上从一个初始状态到一个最终状态的规划。上述的规划满足如下的条件：

运动基元是利用使用相同的损失函数的数值最优控制离线生成的，并被分配得到的目标函数值。图2显示了一些运动基元。

对DMPP的栅格近似是：

是由状态中的位置定义的平移矩阵，是一个在位置维数上的零对角线矩阵。 是规划中前个运动基元的结果平移，是规划中的前个运动基元的开始时间。

所得到的规划结果由一系列个运动基元动作组成，，。这个规划的结束时间是。参考轨迹是由平面中运动基元序列的顺序时空连接构成的。基于栅格的运动规划是完整的解决方案，他在分辨率极限下与DMPP等价。

图2 在总共104个基元中，26个运动基元的初始速度和最终速度为零。所有开始位置为

由运动规划器找到的参考轨迹由轨迹跟踪控制器执行，例如使用非线性模型预测控制器（MPC）。轨迹跟踪控制器的目标是让机器人以较小的跟踪误差跟踪期望的参考轨迹，同时保持接近前馈控制信号。

对当前时间点求解的连续时间非线性MPC问题表示为：

其中，设计参数、、均为正定权值矩阵，为预测范围。

C.碰撞检测

机器人占据了一个区域，取决于其在时刻的状态。我们想要找到一个参考轨迹，它在任何时间点是无碰撞的。

在应用的运动规划中存在着许多可能的不确定性来源。这些可能是由于例如建模误差、传感器噪声、噪声控制或其他因素的不可预测性。不确定性可以分为对状态的感知和对未来状态的可预测性，例如由于机器人的行动，机器人和它的环境。缓解不确定性的一种常见方法是使用安全裕度来减少不确定性引起的碰撞。这种安全裕度边界在本研究中考虑的是一个相对于机器人坐标系的区域，它扩展了机器人的空间占用率。

其中，是被机器人实际占据的区域；区域上的二进制算符被定义为：

另一种方法是扩展障碍区域：安全裕度可分为三个主要部分，

从机器人的角度、控制结果和在其他机器人的行为来反映了世界状态的不确定性。在存在不确定性的安全关键应用中，为了保证概率分布，必须保证概率安全性和概率可行性。一个安全裕度可以从这样的表示来构造约束碰撞的概率，例如每秒最多为0.01%，在文献中经常被称为机会约束。

第三个安全边际的组成部分对于基于栅格的方法很重要，但到目前为止在很大程度上被忽视了。由于建模误差、噪声源和硬件的限制，实际上期望轨迹跟踪控制器会跟踪参考轨迹是不合理的。即使使用精确的模型和没有噪声，精度也取决于运动原元序列，以及设计参数、、、的选择。这种跟踪误差往往同时有偏差和协方差，并直接影响。本文提出了一种系统地确定用于基于栅格的运动规划的方法，并且还可以在运行时验证确实是正确的概率。后者是为了确保在不断变化的情况下保持安全保障。

Ⅲ  相关工作

杂乱、复杂、动态环境中的实时运动规划问题在文献中受到越来越多的关注。

多分辨率栅格等进展使得基于栅格的方法适用于杂乱和复杂的环境。基于栅格的运动规划实现了最先进的（SOTA）性能，例如具有多个拖车的卡车，对于四轴飞行器的规划和重新规划，有动态和时间，在复杂的环境中实时的动态障碍。我们在第II-A节和第II-B节中概述了[3]的一般问题设置。我们通过提供详细的上下文来展示如何将我们提出的内省能力集成到基于栅格的运动规划中，并且我们的工作与[3]中的多分辨率栅格的方法完全兼容。[3]中的多分辨率栅格点方法又是基于[14]，[14]中的软约束可以包含在损失函数中。

通常的做法是放松的组成，并选择一个任意的大区域作为可能满足期望的安全约束的。这种方法中最常见的简化是使用像[15]这样的球形安全边际，由单个半径参数定义。这种做法虽然计算效率很高，但通常需要一个比实际需要的要大得多的，从而降低了碰撞检查的有效性。这可能是由于不同方向上的位置不确定性和位置偏差造成的，这两者都可能在强度和方向上随着时间的推移而变化（如强风）。先前的工作调查了的不同部分，并提供了概率基于和 的方法。

在[16]中，在不确定性下使用基于栅格的运动规划，在参考轨迹中明确考虑了状态不确定性。其中的一个结果是，在规划期间的每个时间点上， 都可以作为概率的基础。例如，它可以是时间点状态的99%概率区域。就其本身而言，这对应于一个安全边际，即机器人所占据的区域在范围内的可能性至少为99%。

如果有一个真实的目标域模拟器，它可以用来模拟其他个体的真实行为，以及这些个体与机器人之间的交互。这样的模拟器可以用于确定的不确定性，并且边际可以被调整以满足例如99.9%的无碰撞的概率。由于是基于其他个体的预测模型的，因此它适用于各种运动规划方法，包括基于栅格的方法。

在其他运动规划方法中已经考虑了安全裕度部分，比如那些基于快速扩展的随机树（RRT），其中过程噪声用于采样可能的运动规划执行。在[16]中，过程噪声影响参考轨迹周围的协方差，但不确定性是无偏的。

我们的贡献，除了提出的集成到基于栅格的运动规划，在基于栅格的范式之外是有用的。学习模型和运动基元的异常检测假设有一组固定的运动基元，我们可以观察它们的执行，如在SOTA中的四轴飞行器运动规划[18]，[19]。碰撞检查有效性的提高进一步假设了模型是对执行可变性的精确描述，这可能并不总是适用于搜索后的轨迹优化[4]，[18]。这将导致我们提出的方法也包括在不同的优化结果的变化。

Ⅳ 内省基于栅格的运动规划与控制

图3 学习基元的正常执行模型的说明。左：所有有效的三个运动基元规划，中间是（前一个，下一个）。中心：观察每个三个运动基元规划的执行情况。右：单峰分布的平均预测区域和95%概率区域（11），捕获了正常执行的预期可变性。

A.学习正常的基元执行

运动基元的执行，被机器人感知，是一个离散轨迹，带有时间点。设和表示一个关于的有效的前一个和下一个运动基元，则是一个有效的规划。对于每一个，让表示被观察到的原始的执行，一个表示每个有效的三运动基元序列 。所有基元的执行都具有相同的时间。请注意，这些时间点将不会被对齐，而数据点的数量可能在不同的执行之间略有不同。模型学习步骤的说明如图3所示。

从恢复执行基本的，我们假设一个具有对角协方差的非线性加性高斯噪声回归模型：

并在函数之前放一个高斯过程：

高斯过程[20]是一种在函数上的分布，在许多统计分析和回归任务中都非常成功，例如运动模式识别[21]。它是一个贝叶斯非参数模型，适用于建模轨迹和基于轨迹的运动模式[22]。高斯过程由平均函数和协方差函数定义。通过将GP调节在观察轨迹上，我们得到一个预测分布，对于每个时间点，

在所有状态维度上

其中，使用一个零均值函数，因为我们可以很容易地从数据中减去均值，

,，是一个恒等矩阵。因为是一个标量，那么是一个标量，是一个列向量，是一个方阵。为了简单起见，每个输出维度都被视为独立的，这相当于它们被一个单独的函数建模，每个函数都有一个单独的高斯过程先验。

使用(6)，现在可以在相同的时间间隔内对齐所有的三联基元执行。这组执行跨越了基元的多样性，我们期望未来的正常执行都与这个集合相似。

运动基元是通过由时间参数化的nD状态空间的一维曲线。因此，我们认为函数上的单峰分布（如单个GP）是一个合适的模型来表示[22]的执行可变性。

我们将运动基元执行模型表示为函数上的单峰分布：

其中，和由[22]给出。

这可以解释为具有高斯分布噪声的噪声样本的样本均值和样本方差。它是高斯过程混合物的高斯近似MoGPs。MoGPs是基于所有具有相同权重的GPs（每个三联基元都被观察到一次）。高斯近似允许MoGP在个别执行之外进行推广。运动基元状态执行模型如图3右边的第三个图所示，图4中的三维位置以及运动基元状态轨迹和观察到的执行。

图4 学习了一个运动基元的单峰分布。图中显示了残差w.r.t.减去运动基元的参考状态轨迹，即。红线是观测结果的单个高斯过程。单峰分布由蓝色区域定义，显示其99%概率区域，蓝线为其模态。

B.碰撞检测

学习到的运动基元模型表示机器人在执行运动基元时的状态。在高斯过程噪声假设下，其模态平均与运动原始参考状态轨迹相同或更准确。执行可变性被明确地表示为状态上随时间变化的概率密度。

对于根据多元高斯分布分布的随机变量，将概率为P的中心概率区域（PR）定义为[23]

是概率的具有个自由度的卡方分位数函数。对于一个对角线矩阵简化为一个轴对准的椭球体，

每个轴的轴长由给出。设是运动规划(4)中的一个运动基元，对应于运动基元和开始时间 。设 ，它限制在0和之间。运动基元的模型是一个维的多元分布，在每个时间点处具有对角协方差矩阵。安全裕度现在可以基于概率给定概率，

C.异常检测

一旦我们学习了运动基元模型，我们就想使用它们来检测运动基元的异常执行。基元的执行可变性模型上的概率区域描述了我们期望机器人在给定时间点处于的状态。例如，[7]在执行监控中使用概率间隔来实现机器人的安全。

有几种可能的方法来使用概率区域来定义什么是正常的行为。一种简单的方法是检查机器人是否离开该区域，如果离开，将该执行定义为异常。然而，由于我们选择的区域不包含所有的概率，我们仍然期望机器人偶尔离开概率区域。

更准确地说，如果我们选择一个99%的PR，机器人预计将在1%的时间点上离开这个间隔。如果机器人开始更频繁地离开概率区域，这可能是一个错误的指示。我们将离开概率区域的速率定义为失败率，并将其建模为一个随机变量。我们将考虑以下参数：

我们将我们之前关于的假设编码为一个分布，将模式设置为，例如同时仍然允许一些不确定性：

与和一起使用，其中为先验强度。

鉴于我们知道，观察到的失败的数量遵循一个二项分布：

因为贝塔分布是二项分布的一个共轭先验，的后验也是：

使用的后验值，有可能评估大于预期的概率：

其中在大多数统计库中都是一个固定时间的标准函数，

这个故障率高于的概率，然后可以被监测和阈值。在这项工作中，我们使用了的概率阈值和的先验强度，但这些值可以进行微调，以选择精度和召回率之间的权衡。

Ⅴ 实验和结果

为了评估我们提出的方法，我们考虑了一个模拟的DJI Matrice 100四轴飞行器（图5），一个常用的商业研究平台。

图5 模拟了DJI Matrice 100四轴飞行器。

A.运动规划和控制

使用了与[3]中相同的DJI Matrice 100中相同的非线性模型。我们使用一个基于[24]中的工作的非线性MPC控制器，并使用ACADO [25]来生成一个有效的实现来求解(5)。使用ACADO（图2），使用与非线性MPC控制器相同的目标，总共生成了104个运动基元。使得运动基元具有初始状态和最终状态：

运行一组三联基元执行可能会导致一个三联基元执行干扰下一个执行。为了避免瞬态噪声在三联体之间传播，每个三联基元规划都在三联基元之前和之后添加动作，这样整个规划总是从静止状态开始。

B.学习

对于运动基元模型，采用具有平方指数协方差函数的高斯过程先验，

其中为每个输入维具有长度尺度的对角矩阵，为信号方差。这些，连同对角线噪声协方差，是这个层次贝叶斯模型的超参数。

我们通过最大化边际对数似然值（经验贝叶斯）来从数据中估计超参数，

其中，被定义为（9-10）中，是一个常数。

我们研究了学习到的运动基元执行模型的预测均值与观测轨迹的接近程度，并将其与运动基元参考状态轨迹与观测轨迹的接近程度进行了比较。在表I中，第一行显示了观察到的执行和参考状态轨迹或已执行的运动基元之间的RMSE。

该表显示了几个均方根误差（RMSE）。第1行：运动基元参考状态轨迹与观察到的同一运动基元执行之间的误差。第2行：运动基元执行模型的平均预测与观察到的同一运动基元执行之间的误差。第3行：原始参考轨迹与执行该运动的模型的平均预测之间的误差。

作为执行轨迹的点预测，平均预测明显比参考状态轨迹更准确，如下行所示。这样做的原因是，与学习到的模型相比，单个基元的参考状态轨迹对执行轨迹（跨状态，而不是时间）的平均值有更大的偏差。在图4中可以看到一个示例。表一第三行比较了学习到的运动基元模型和基元的参考状态轨迹的平均预测。表一第三行比较了学习到的运动基元模型和基元的参考状态轨迹的平均预测。这提供了额外的证据，表明与运动原模型本身相比，学习运动原模型模型是一个系统地更好的执行平均预测器。

C.碰撞检查

文献中的标准方法是使用一个球体，捕获 ，,的聚合，单个半径参数对时间不变。我们使用这个基线，并构造了两个更紧密的边界，（每个基元不同）和（也随着时间而变化）。为了与我们提出的变异性模型进行有意义的比较，我们通过假设以原始参考轨迹为中心的观测值具有高斯似然性来概率地建立基线。对于、中的，

其中对于各自的基线是：

平均归一化面积（平均，相当于），相对于进行归一化（对所有维度和所有运动基元使用单一的安全边际半径）。

将每个维度和IV-B中定义的分开。安全裕度 ，给定的概率为

在表Ⅱ中，将基线与学习到的运动基元模型进行了比较。是所有基元中最大的99%概率区域，每个维度，以参考轨迹为中心。是特定于单个基元的，这允许它更小。是时变的，允许在观测结果允许的情况下进一步减小半径。

图6显示了一个比较一个运动基元的，和学习模型 的例子。图7显示了图6在绿色垂直线上的解剖图。为了仍然确保安全，例如，观察到的执行情况应该仍然在以内，区域必须更大，并有更大的偏差。

当以平均值为中心时，对称的安全裕度达到最小值。表Ⅱ显示，与基线相比，使用学习到的变异性模型的面积减少较大。即使与时变半径相比，当使用学习模型时，面积也减少了3倍以上。

图6 （浅灰色包络）、（深灰色包络）、（蓝线和蓝色包络）和观察到的单一运动原体i (c.f的执行（红色）的比较。图4）。和与它们的平均值（参考状态轨迹）具有相同的黑线。没有显示，以便使其他细节更精细的可见。

图7 图6在处的残余图（用垂直的绿线表示）。（黑点和深灰色包络）和（蓝点和蓝色包络）与观察到的单一运动基元i的执行（红点）一起描述。和没有显示出来，以使其他元素的更精细的细节可见。

D.异常检测

该实验共使用了104个运动基元。除了图2中的26个基元外，其他基元的初始速度和/或最终速度均为非零。对于每个基元，执行20个随机但不同的有效三联基元组合并进行记录。每个基元的前10个三联基元组合用于学习每个基元的模型，即一个训练集。以下10个被用于检测异常检测，即测试集。

将异常行为定义为离开概率区域的朴素方法与所提出的使用后验的方法进行了比较，使用。

对于方法，先验参数被设置为和。后验值的计算包括了最后一秒的观测结果，这意味着我们设置了时间窗秒。对于每个时间点，我们计算故障率大于的概率。如果在任何时间点的概率大于的阈值，则整个执行被归类为异常，否则为正常。

两种方法都使用从训练集学习到的运动基元模型给出的概率区域，然后在测试集上进行测试。图8显示了每种方法和数据集组合的混淆矩阵。方法的性能略优于朴素方法，同时仍然保持着完美的精度。这表明，我们可以通过调整的阈值和时间窗的大小来提高召回率，而不会损失精度。

图8 使用中心99.9%-probability体积的所有基元的异常检测的混淆矩阵。要求所有的观察结果都在这个体积内，或者使用后验，要求在1s窗口内个体异常小于99.9%的比例可能高于0.01%。第一行：在模型训练集上的性能。第二行：对以前看不见的执行的性能，来自相同的基元，但不同的三联基元组合。

Ⅵ 结论和未来工作

在本文中，我们提出了一种用于自动驾驶的在线概率、多模态、多目标跟踪算法。我们的模型学习融合2D相机图像和3D激光雷达点云特征。然后，这些融合的特征被用于学习有效的权重，以将深度特征距离与Mahalanobis距离相结合，从而获得更好的数据关联。我们的模型还学习以数据驱动的方式管理跟踪循环周期。我们在NuScenes [9]和KITTI [10]数据集上验证了我们提出的方法，我们的方法在定量和定性上都优于使用相同目标检测器的最先进的基准方法。

参考文献