Online Safe Trajectory Generation For QuadrotorsUsing Fast Marching Method and Bernstein Basis Polyn

基于快速行进法和Bernstein基多项式的四旋翼在线安全轨迹生成

摘要

在本文中，我们提出了一种用于未知环境下自主导航的在线四旋翼运动规划框架。基于机载状态估计和环境感知，我们采用基于快速行进的路径搜索方法，在地图的欧几里德符号距离场（ESDF）引起的速度场上找到路径，以实现更好的时间分配。我们通过对环境的路径膨胀，为四旋翼机生成飞行走廊。我们使用Bernstein多项式基将轨迹表示为分段Bézier曲线，并将轨迹生成问题表示为典型的凸程序。通过使用Bézier曲线，我们能够完全在安全区域内约束轨迹的位置和高阶动力学。

将所提出的运动规划方法集成到定制的轻型四旋翼平台中，并通过在未知的杂乱室内和室外环境中呈现完全自主导航来验证。我们还将轨迹生成代码作为开源包发布。

I、介绍

近年来，微型飞行器（MAVs），特别是四旋翼飞行器，在各种应用中引起了越来越多的关注。由于其机动性、灵活性和灵活性，四旋翼机能够在复杂环境中快速安全地飞行，同时避免任何意外碰撞。在本文中，我们提出了一种四旋翼运动规划方法，用于在未知环境中在线生成安全且动态可行的轨迹。我们将所提出的运动规划模块集成到基于完全自主视觉的四旋翼系统中，并在未知的复杂室内和室外环境中进行在线实验，以验证我们方法的鲁棒性。

给定来自机载传感器的测量值，逐步构建运动规划任务的配置空间以表示环境。为了在线生成四旋翼的安全和平滑轨迹，我们之前的工作[1]和其他工作[2]，3]已经提出提取配置空间中的自由空间，并通过使用一系列凸形来表示自由空间。然后将轨迹生成问题表述为通过凸优化将轨迹限制在凸形状内。然而，这些方法存在两个问题。第一个问题是分段轨迹的时间分配，选择不当的时间分配很容易产生低质量的轨迹。第二个问题是如何在硬约束下有效地约束自由空间内的整个轨迹及其在可行空间内的导数。在本文中，我们在基于欧氏符号距离场（ESDF）的速度场中使用快速行进方法[4]来搜索时间索引路径。与基准方法相比，所得到的时间分配适应环境中的障碍物密度，并实现了更好的轨迹质量。为了解决第二个问题，我们使用Bernstein基代替传统的单项式多项式基，将轨迹表示为分段Bézier曲线。通过优化，保证了弹道的安全性和高阶动力学可行性。我们总结我们的贡献如下：

1）一种基于快速行进的方法应用于基于欧几里德符号距离场（ESDF）的速度场，用于搜索时间索引路径，该路径为轨迹优化提供合理的时间分配。

2）一种轨迹优化框架，用于使用Bernstein基生成保证平滑、安全和动态可行的轨迹。

3）在完整的四旋翼平台上实时实现所提出的运动规划方法和系统集成。本文介绍了在未知室内和室外复杂环境中的自主导航飞行，并将发布源代码。

我们在第二节中回顾相关文献。我们的路径查找方法和轨迹优化方法在第三节中和第四节中分别详细介绍。关于所提出的方法和四旋翼系统设置的实施细节，请参见第五节。在第六节、 给出了基准测试结果。室内和室外的实验结果表明，在未知环境中完全自主飞行。论文在第七节中总结。

II、相关工作

运动规划模块在自主机器人系统中起着重要作用，并建立在系统的顶层，以协调机器人在复杂环境中的导航。粗略地说，运动规划问题可以分为前端可行路径搜索和后端轨迹优化。

对于前端路径查找，已经提出并应用了从基于采样[5]到基于搜索[6]的方法。LaV alle[5]开发了基于采样的快速探索随机树（RRT）方法。在该方法中，从配置空间中随机抽取样本，并引导树朝向目标生长。Karaman和Frazzoli[7]提出了基于渐进最优抽样的方法，包括PRM*、RRG和RRT*。在这些方法中，随着样本的增加，解最终收敛到全局最优。基于搜索的方法将配置空间离散化，并将寻路问题转化为图搜索。该图可以在3D空间或高阶状态空间中定义。最近的典型方法包括JPS[8]、ARA*[6]和混合A*[9]。通常，由于车辆的动态限制和路径本身的平滑性差，前端发现的路径不能由车辆直接执行。因此，后端优化模块需要参数化到时间t的路径并生成平滑的轨迹。

时间参数化或所谓的时间分配过程主导了连续空间中轨迹的参数化，并被认为是导致轨迹优化次优的主要原因之一。实现良好时间分配的一种方法是使用梯度下降迭代细化分配的时间[10]。但是迭代地重新生成轨迹的成本太高，并且无法实时使用该方法。在我们之前的工作[1]中，我们根据分割点之间的欧几里德距离为每条轨迹分配时间，并依赖于飞行走廊的重叠区域来调整时间分配。其他人也在时间分配中使用一些启发式[3]。在本文中，我们使用快速行进方法将时间分配紧密结合到路径搜索中，而不是基于独立前端路径搜索所搜索的离散路径来分配时间。我们在地图的ESDF引起的速度场上使用快速行进方法，以搜索自然参数化的时间和适应障碍物密度的路径。在[11]中，提出了一种名为快速行进广场（FM2）的类似方法，其中快速行进方法使用了两次。在第一波传播中，波前来自地图中的所有障碍物，并建立了势场。基于该势场，再次从起点调用快速行进，以搜索到目标点的路径。然而，第一波扩展需要花费太多时间来完成，尤其是在密集环境中，这使得实时使用变得困难。

提出了许多方法来优化路径以生成平滑的轨迹。基于梯度的方法，如CHOMP[12]将轨迹优化问题表述为非线性优化，而不考虑安全性和平滑性的影响。Mellinger等人[13]首创了一种最小捕捉轨迹生成算法，其中轨迹由分段多项式函数表示，轨迹生成问题被表述为二次规划（QP）问题。Chen等人[2]、Liu等人[3]和我们之前的工作[1]都雕刻了一条飞行走廊，该走廊由简单的凸形区域组成，并将轨迹限制在走廊内。通过在违反极值上迭代添加约束并重新求解[1，2]中的凸程序，实现了安全性和动态可行性。然而，通过这种方式，只有在多次迭代之后才能获得可行的解决方案。另一种方法是沿着轨迹采样时间实例，并在其上添加额外的约束[3]。这将使问题的复杂性严重恶化，因为随着四旋翼的速度增加，必须密集地添加约束。在本文中，我们使用Bernstein基来表示分段轨迹。利用Bernstein多项式基的性质，直接在整个轨迹上实施安全和动态可行性约束。

III、基于快速行进法路径搜索方法

A、快速行进法

快速行进（FM）方法[4]作为Level Set方法[4]的一种特例，是一种模拟波前传播的数值方法，广泛应用于图像处理、曲线演化、流体模拟和路径规划。快速行进法通过假设波阵面以速度f在其法线方向上传播来计算波首次从源到达点的时间，则波前的演变由Eikonal微分方程描述：

 其中T是到达时间的函数，x是位置，f（x）描述了不同位置x处的速度。[4]中介绍了离散空间中方程1的近似解，为了简洁起见，我们在本文中保留了关于该解的细节，请读者参考[4]。

对于路径搜索，我们可以定义机器人导航的速度图。在模拟从起点扩展的波之后，获得地图中每个点的到达时间。通过沿到达时间从目标点到起点的梯度下降方向回溯路径，我们获得了具有最小到达时间的路径。这是在路径搜索中应用快速行进方法的主要思想。与其他基于势场的方法不同，快速行进方法没有局部最小值，并且如[14]中所证明的那样是完整和一致的。理论上，它具有与A*相同的O（Nlog（N））时间复杂度。

对于离散空间中使用的传统图形搜索方法，如Dijkstra或A*，搜索离散路径，然后将其参数化为时间t。这可能导致后端轨迹优化的次优，因为仅给定路径长度，很难将路径参数化为合理的时间。然而，通过快速行进法搜索的路径自然地被参数化为t。因为路径是基于速度场中的到达时间而不是距离的度量的最优路径，并且路径上的每个节点都与从源点传播的模拟波的首次到达时间相关联。

B、距离场中的快速行进

为了找到具有适当分配时间的路径，我们需要建立一个合理的速度场。我们认为，四旋翼在有密集障碍物的环境中降低速度是很自然的，因为四旋翼必须更频繁地改变姿态以避免碰撞。

同样，应该允许四旋翼在稀疏环境中快速移动。换句话说，根据与障碍物的距离来分配四旋翼的允许速度是合理的。因此，欧几里德有符号距离场（ESDF）是设计速度图的理想参考。在本文中，我们使用移位双曲正切函数tanh（x）作为速度函数：

 其中d是ESDF中位置x处的距离值，e是欧拉数，vm是最大速度。速度函数f的图如图2（b）所示。当距离值较小时，即在接近障碍物的位置，该位移双曲正切函数缓慢增长，并逐渐接近预定义的最大速度vm。在占领区，速度为0，这意味着波前不能扩展到这些区域。图2显示了通过快速行进法发现的速度场和路径。

 为了加快波前的传播，可以通过启发式函数[15]来驱动快速行进方法，以减少扩展节点的数量。在我们的实现中，由于快速行进应用于速度场，因此可以通过计算到达目标的最短时间h（x）=d来估计允许的启发式h（x∗（x） \/vm，其中d∗（x） 是从x到目标点的欧几里德距离。

C、飞行走廊生成

在获得时间索引的最小到达路径后，我们提取环境中的空闲空间，以形成用于后端优化的飞行走廊（第四节）。我们希望充分利用自由空间，因为找到解空间和获得最优解对于轨迹生成同样重要。因此，我们的方法是初始化飞行走廊并扩大它。对于路径中的每个节点，可以通过查询ESDF轻松获得节点位置到最近障碍物的距离。通过这种方式，在每个节点上，我们获得了一个安全空间球体，以对抗最近的障碍物，并将飞行走廊初始化为球体的内接立方体。然后，我们通过查询轴对齐方向x，y，z上的相邻网格来放大每个立方体，直到其可能具有的最大自由体积。步骤如图3所示。由于我们将飞行走廊初始化为一系列密集节点，一些节点共享相同的放大立方体，这是不必要的，但增加了后端优化的复杂性。因此，我们修剪走廊中所有重复的立方体。路径搜索、初始走廊生成和走廊膨胀的管道如图4所示。

 在本文中，飞行走廊膨胀具有与[2]中类似的目的。然而，我们不使用引入额外维护成本的八叉树地图数据结构，而是在更通用的图结构体素网格上进行。此外，与[3]中提出的方法相比，我们只在轴对齐的方向上膨胀飞行走廊，因此我们不需要迭代地进行距离查询并节省计算成本。

IV、安全动态可行轨迹生成

由分段多项式组成的轨迹在x，y，z的每个维度μ中被参数化为时间变量t。在本文中，我们使用类似于[16，17]的伯恩斯坦多项式基代替传统的单多项式基，并将轨迹表示为分段Bézier曲线，这是B样条曲线的一种特殊情况。

A、伯恩斯坦基分段轨迹公式

Bernstein多项式基定义为：

 其中n是基的阶数（n~i）是二项式系数。由伯恩斯坦基组成的多项式称为Bézier曲线，其形式如下：

 其中[c0j，c1j，…，cnj]表示为cj是Bézier曲线第j条的控制点集。与单项式基多项式相比，Bézier曲线具有几个特殊性质：

        1）端点插值属性。Bézier曲线始终从第一个控制点开始，在最后一个控制点结束，并且从不经过任何其他控制点。

        2）固定间隔属性。参数化为变量t的Bézier曲线定义在t上∈ [0, 1].

        3）凸包属性。Bézier曲线B（t）由一组控制点组成，这些控制点ci完全限制在由所有这些控制点定义的凸包内。

        4）霍图特性。Bézier曲线B（t）的导数曲线B（1）（t）称为速度图，它仍然是控制点由c（1）i=n·（ci+1）定义的Bézier曲面− ci），其中n是度。

实际上，对于Bézier曲线，控制点可以被视为基础的权重，基础也可以被视作为控制点的权重。由于Bézier曲线是在固定间隔[0，1]上定义的，因此对于每条轨迹，我们需要一个比例因子s来将参数时间t缩放为该段的任意分配时间。因此，x，y，z的一维μ中的m段分段Bernstein基轨迹可以写成如下：

 

其中cji是轨迹的第j段的第i个控制点。T1、T2、…、Tm是每个段的结束时间。总时间T=Tm− T0.s1，s2，…，sm是应用于Bézier曲线每段的比例因子，以将时间间隔从[0，1]放大到时间[Ti−1，Ti]分配在一个段中。此外，在实践中，在每条曲线的位置上乘以比例因子可以获得优化程序更好的数值稳定性。

成本函数是第k阶导数平方的积分。在本文中，我们最小化了沿轨迹的加急，因此k为3。目标如下：

 这确实是在二次公式pT Qop中。p是包含x、y、z的所有三个维度上的所有控制点cij的向量，而Qo是目标函数的赫森矩阵。假设在μ维中，轨迹的第j段表示为fμj（t），区间[0，1]中的无标度Bézier曲线为gμj（t），如Equ所示。4.表示（t−Tj）\/sj=τ。然后导出第j段μ维的最小加加速度目标函数，并将其写为：

 

其仅取决于n度和尺度sj，并且可以按照经典的最小快照公式[13]来编写。为了简洁起见，我们保留了Qo的细节。

B、强制约束

对于分段轨迹生成，我们必须强制执行一些约束，以确保轨迹的平滑性、安全性以及动态可行性。对于每一条Bézier曲线，其高阶导数可以由相应的低阶控制点的线性组合表示，其表示为

 其中l是导数的阶数，n是伯恩斯坦基的阶数。

1）航路点约束：四旋翼需要经过的航路点，例如起始位置和目标位置以及n阶导数。通过在相应的控制点上设置相等约束，直接实施航路点约束。对于固定的lth（l≤ n） μ维的阶导数d（l）μj存在于轨迹的第j段（航路点）的开始处，我们得到：

 2）连续性约束：轨迹必须在两件（0≤ φ ≤ k− 1). 连续性约束通过在两条连续曲线中的相应控制点之间设置相等约束来实现。对于第j条和第（j+1）条曲线，我们有：

3） 安全约束：由于Bézier曲线的凸包特性，如属性3所示，整个轨迹被限制在由其所有控制点形成的凸包内。因此，我们可以添加安全约束，以强制一个段的所有控制点位于III-C中生成的相应立方体内。对于轨迹的一段，由于放大的立方体是一个凸多边形，所有控制点都在其内部，因此这些控制点的凸包肯定在立方体内部，这意味着轨迹的整个段都被限制在立方体内。通过在控制点上添加边界限制，对第j段的控制点应用安全约束：

 其中β+μj和β− μj是对应的第j个立方体的上边界和下边界。

4）动态可行性约束：与安全约束类似，利用属性3和4，我们可以对整个轨迹的高阶导数实施硬约束。添加约束以约束每个维度μ中的曲线导数。在本文中，四旋翼的速度和加速度被约束在[v−m、 v+m]和[a−m、 a+m]以使生成的轨迹动态可行。对于第j段的控制点，我们有：

航路点约束和安全约束直接表示为每个路段中优化变量的可行域（cj∈ Ωj）。连续性约束和高阶动力学约束被重新表述为线性等式约束（Aeqc=beq）和线性不等式约束（Aiec≤ bie），这里c＝[c1，c2，…，cm]。然后将轨迹生成问题重新表述为：

 这是一个凸二次规划（QP），并且可以通过一般的现成的凸求解器在多项式时间内求解。

V、实验细节

A、系统设置

本文提出的规划方法1在C++11中使用通用凸解算器Mosek2实现。飞行实验在自主开发的四旋翼平台上进行（图6（b））。运动规划、状态估计和控制模块运行在双核3.00 GHz Intel i7-5500 U处理器上，该处理器具有8 GB RAM和256 GB SSD。单眼鱼眼映射模块在具有256个内核的英伟达TX1上以10Hz运行。

四旋翼系统的系统结构如图6（a）所示。为了为映射模块提供较大的视野，我们使用235◦ 鱼眼镜头并在其上裁剪图像以覆盖水平FOV。输出的密集图被馈送到规划层的前端路径搜索模块。在此基础上，搜索当前四转子状态与目标位置之间的路径。然后对路径进行优化，以生成时间参数化的平滑、安全和动态可行的分段轨迹。然后使用轨迹服务器接收轨迹并将其转换为控制命令。我们使用几何控制器[18]跟踪生成的轨迹，姿态控制留给四旋翼上的DJI A3自动驾驶仪。

 B、为运动规划稠密建图

我们利用我们之前的单目视觉惯性状态估计和密集映射系统[19]来构建四旋翼平台的感知层。前视鱼眼相机，大视场235◦ 用于为反馈控制和单目密集映射提供6-DoF状态估计结果。我们的密集映射模块通过截断符号距离融合（TSDF）将每个关键帧的深度映射融合到全局映射中。作为一种改进，修改了采样深度范围，以适应通货膨胀大小，以避免异常值，这提高了实验的成功率。此外，在这项工作中，利用一个简单的深度图滤波过程来消除每次密集深度图提取后的深度不一致。

C、规划策略

对于机载应用，欧几里德距离场的构建通常是整个运动规划管道的瓶颈。因此，在实践中，我们采用局部缓冲策略，仅在等于映射结果范围加上缓冲长度的局部范围内构建欧几里德有符号距离场。我们之前的工作[20]中揭示的另一个实际问题是，当观测不充分时，单目鱼眼映射模块可能会输出异常值，从而阻碍找到可行路径。这个问题在距离相机较远的地方以及没有明显的运动立体声基线时尤为明显。解决这个问题的一种方法是使用双视界规划策略，如图7所示。我们定义了一个执行视界，并仅检查该视界内当前轨迹的碰撞状态。这种策略是有意义的，因为相机在其附近的地方总是有更多的信息观察，并且映射模块在较短的距离内更有信心。在导航过程中，如果在此执行范围内发生任何碰撞，将触发重新规划机制，并生成到目标的新轨迹。

VI、结果

A、比较与分析

存在利用自由空间来形成凸形飞行走廊并在其中生成轨迹的其他方法。[21]中的方法使用IRIS（通过半定规划的迭代凸区域膨胀）获得最大走廊，并使用组合优化在几个凸区域内生成轨迹。然而，生成轨迹需要几秒钟，不适合实时机载使用。因此，我们不将所提出的方法与此方法进行比较。

Chen的方法[2]、Liu的方法[3]和本文提出的方法都是用于四旋翼运动规划的系统级方法。与这两项工作相比，我们的方法利用速度场中的快速行进方法来提供自然时间索引路径，而不是基于某些启发式搜索路径和分配时间。此外，我们利用Bernstein多项式基直接获得安全性和动态可行性，这避免了Chen程序[2]中的迭代优化和Liu基于样本的程序[3]中的碰撞风险。在这里，我们将所提出的方法与陈氏方法在轨迹质量（目标值）、轨迹长度、轨迹平滑度和运行时间方面进行了比较。通过在10个障碍物数量大于100的40米×40米的随机森林中随机选择至少60米外的起始位置和目标位置，生成100条轨迹（如图8所示）。由于轨迹的速度对目标值有很大影响，我们将两种方法的平均速度设置为相等。最大速度设置为2m/s，未设置加速度限制。结果如表VI-A所示，具有大致相似的平均速度，所提出的方法实现了较低的目标成本，以及平均和最大加急。快速行进法倾向于找到比A*更长的路径，因为度量是场中的速度，而不是路径长度。这一结果表明，所提出的方法生成的轨迹比陈的方法更长，如表所示。此外，随着平均速度的增加，Chen方法的计算成本也增加。这是因为随着平均速度越来越接近极限（2m/s），通过迭代添加约束来限制它变得更加困难，以这种方式进行了更多的迭代。

 B、室内自主飞行

室内自主飞行是在先前未知的环境中进行的，随机设置障碍物，以验证我们提出的方法。快照如图1（A）所示。在飞行过程中，环境感知和轨迹生成都是在线执行的，没有任何关于环境的事先信息。在实验中，密集映射模块输出5m×5m的局部密集环境感知，缓冲区长度设置为1.5m。我们地图的分辨率定为0.2米。Bézier曲线的阶数n设置为10。我们将所有未知区域视为无碰撞区域。在每次飞行中，目标位置被发送到四旋翼机，并生成到目标的初始轨迹。根据V-C中所示的策略触发重新计划机制。室内飞行的代表性演示如图9所示。

C、室外自主飞行

室外实验是在未知的复杂环境中进行的。图10给出了几次户外飞行的记录。图10（c）给出了一次飞行的完整地图和轨迹。系统配置、参数设置和计划策略与室内实验VI-B相同。路径搜索、走廊生成和轨迹生成的平均计算时间分别为2.04ms、37.17ms和26.85ms。注意，轨迹生成中的时间成本主要取决于Bézier曲线的阶数，因为约束的数量与未知变量的数量成线性比例。我们请读者参考视频，了解更多关于户外飞行的试验和细节。

VII、总结和未来工作

在本文中，我们提出了一种用于四旋翼自主导航的在线运动规划框架。该方法采用基于快速行进的路径搜索方法，在适应环境的速度场中找到时间索引路径。基于路径生成并膨胀飞行走廊，以充分利用环境中的自由空间。最后，我们使用基于优化的方法，利用Bernstein多项式基生成具有硬约束的安全和动态可行轨迹。我们将运动规划、状态估计和密集映射模块集成到我们自主开发的四旋翼平台中，并在室内和室外未知环境中展示完全自主的飞行，以验证我们的方法。

在本文中，我们使用的凸包特性是约束轨迹及其导数的保守方法，如VI-a所示。这意味着轨迹的解空间是飞行走廊的子集，并且随着多项式次数的增加，渐近地接近无限接近飞行走廊。在实践中，这是问题的可行性和复杂性之间的权衡。未来，我们计划对Bernstein和单项式多项式基以及相应的优化方法进行更多的分析和比较。我们还打算在大规模或动态环境等极端情况下挑战我们的四旋翼系统。此外，我们计划将我们的运动规划框架扩展到四旋翼群。

总结

该论文利用快速行进法搜索出初始路径，然后将路径的初始立方体膨胀至最大空间，以获得飞行走廊，再利用贝塞尔曲线规划出带有硬约束（束缚在飞行走廊中）的轨迹。