Online Generation of Collision-Free Trajectories for Quadrotor Flight in Unknown Cluttered Environme

未知杂乱环境中四旋翼飞行无碰撞轨迹的在线生成

摘要

我们提出了一种在线方法，用于在杂乱环境中生成自主四旋翼飞行的无碰撞轨迹。我们考虑了现实世界中的场景，即四旋翼空中机器人配备了有限的感知能力，并在最初未知的环境中运行。在飞行过程中，使用机载传感器逐步构建基于八叉树的环境表示。利用八叉树数据结构中的有效操作，我们能够以在线方式生成由大型重叠三维网格组成的自由空间飞行走廊。然后，一种新的基于优化的方法生成平滑的轨迹，这两种轨迹都完全在安全飞行走廊内，并满足更高阶的动力学约束。我们的方法在一台速度适中的计算机上，在几秒钟内计算出有效的轨迹，从而允许在线重新生成轨迹，以应对新的障碍。我们建立了一个完整的四旋翼试验台，包括机载传感、状态估计、映射和控制，并集成了所提出的方法，以显示在复杂未知环境中的在线导航。

I、介绍

我们解决了在线生成无碰撞轨迹的问题，使四旋翼在未知的杂乱环境中从初始状态飞行到最终状态。我们考虑的情况是，机载感知是有限的，环境中的障碍物只能在飞行过程中被绘制出来。这要求生成的轨迹满足三个重要要求：首先，对于四旋翼飞行器，轨迹必须平滑且动态可行；第二，必须保证整个轨迹（而不是有限数量的控制点）无碰撞；第三，所有的传感、地图构建和轨迹规划都必须以在线方式完成，这样四旋翼机才能对飞行过程中观察到的新障碍做出反应。

四旋翼机具有复杂且欠驱动的动力学特性，因此很难明确考虑其完整状态。幸运的是，如Mellinger和Kumar[1]的最新结果所示，四旋翼在位置和偏航角方面不同程度地平坦。换句话说，四旋翼的完整状态可以表示为x、y、z位置和偏航角的瞬时值及其导数的函数。因此，任何平滑且具有有界导数的轨迹（其中有界导数由机械配置和电机动力学中的限制规定）都可以由四旋翼跟随。

我们遵循[1]中的最小捕捉成本函数，并使用分段多项式来确保轨迹的平滑性，然后使用我们的方法来确保动态可行性（第V-D节）。这满足第一个要求。我们的主要贡献是通过我们的两步轨迹生成方法来解决第二个需求，该方法将整个轨迹（第五节）限定在自由空间飞行走廊内，其中走廊使用基于八叉树的地图表示（第四节）中的有效操作在线生成。第三个要求是通过我们的方法能够在几分之一秒内计算有效的轨迹来确保的（图10）。我们能够以在线方式应用我们的方法，在飞行过程中逐步构建地图。

我们的轨迹生成方法与机载传感、状态估计、映射和控制的多传感器四旋翼试验台集成在一起，形成一个完整的导航系统。给出了模拟和真实世界在线实验的结果，以演示速度高达1.8m/s的自主飞行（第六节）。与最先进的方法相比，我们还表现出了优异的性能[2]。据我们所知，我们是第一个展示四旋翼机在安全保证的情况下在杂乱的未知环境中平稳导航的公司（图12）。

II、相关工作

关于机器人运动规划和轨迹生成主题的现有工作非常广泛，涵盖了机器人操纵、自动驾驶、自主飞行等领域的广泛应用。在有限的可用空间内对现有工作进行回顾的任何尝试都是不完整的，但我们讨论了与空中机器人自主飞行最相关的最新进展。

在杂乱环境中运行的无人机的轨迹规划面临的挑战在于难以建模大量可能非凸形障碍物或确定可穿越空间。早期的研究考虑了简化的凸情况，并提出了混合整数线性规划（MILP）方法来编码障碍物信息[3]–[6]。在[7]中，基于[1]中关于最小捕捉轨迹生成的开创性工作，作者提出使用混合整数二次规划对非凸障碍物的面进行建模。与直接建模障碍物不同，[8]建议找到覆盖自由空间的大型凸段[9]，然后使用混合整数规划执行同时的段分配和安全轨迹生成。然而，随着障碍物或自由空间区域数量的增加，所有这些混合整数方法的计算复杂性可能很快变得难以控制。

为了生成具有合理计算复杂性的轨迹，局部[10]和反应[11]方法也被广泛研究，但这些方法的缺点是缺乏全局最优性。基于航路点的方法，其中轨迹生成是由位置约束产生的，对于降低计算成本也是非常有利的。在初始步骤中，可以使用基于搜索的[12]或随机化的[2]算法找到无碰撞航路点路径。然后将航路点路径转换为时间参数化轨迹[2、12、13]或直接控制输入[14]。该类别中值得注意的工作是[2]，其中使用RRT*[15]获得无碰撞路径，然后使用二次规划（QP）生成平滑轨迹。然而，当平滑轨迹偏离原始路径时，可能会发生碰撞。[2] 建议增加中间航路点，以使轨迹更接近安全航路点路径。然而，路线点的迭代添加导致轨迹具有更高的控制成本。它也不能保证需要多少额外的航路点。[12]中提出了另一种基于航路点的方法，该方法依赖于c空间扩展的启发式来防止碰撞，因此无法提供安全保证。

另一方面，我们的方法与[8]中提出的混合整数方法享有相同的无冲突保证，但运行速度更快（与分钟相比，只需几分之一秒）。并非使用凸区域来表示自由空间，我们在基于八叉树的环境表示中使用有效的操作来在线生成无碰撞飞行走廊。我们使用有效的二次规划来找到具有保证安全性的平滑轨迹。事实上，在受限环境中，我们的方法比[2]中基于航路点的方法运行得更快，如第节所示。VI-A，因为RRT*很难找到穿过狭窄走廊的有效路径。

III、方法论概述

图2显示了整个系统管道的概况。三维环境使用基于内存高效八叉树的[16]结构表示（第IV节-a）。给定目标位置，基于搜索的算法对由一系列相连的自由空间三维网格组成的虚拟飞行走廊进行初始化（第IV-C节）。使用八叉树数据结构中的有效操作（第IV-D节），将这些网格扩展到最大体积的飞行走廊。然后使用第V节中描述的方法生成完全符合飞行走廊的平滑轨迹。请注意，三维网格可能具有不同的大小，代表飞行走廊的不同宽度。轨迹生成器可以生成网格内任何位置的运动。这符合我们的直觉，即四旋翼机应该在自由空间网格内有更大的机动自由度，以降低其控制成本，而不是坚持单一的最短路径。整个管道只消耗四旋翼机上的几分之一秒，从而能够对未知环境中的障碍物做出实时反应。

 

IV、虚拟飞行走廊生成

A、基于八叉树的环境表示

当来自机载传感器的障碍物信息流入时，可以使用八叉树数据结构方便地表示飞行环境[16]。树结构中的每个节点代表一个三维网格。可变大小的自由空间网格的集合构成了环境的总自由空间。注意，大量的自由空间可以由几个大网格有效地表示。

假设地图体积可以划分为最多m个分辨率最高的网格。其树结构的深度为h=O（log（m））。为了考虑飞行系统的实际尺寸，我们在将每个新障碍点插入地图时将其膨胀到固定尺寸的立方体体积。利用八叉树结构的懒惰原理[16]，我们实现了更新立方体积的复杂性为O（h）。

B、走廊搜索的连接图

我们使用标准A∗ 用于初始化飞行走廊的算法。这要求除了基于八叉树的地图之外，还维护所有自由空间网格的连通图G。每个自由空间网格将与G中的一个顶点相关联。G中的无向边用于建模相邻网格之间的连通性。当新的障碍物添加到地图中时，必须相应地修改图表。在地图上添加了一批新的障碍点后，我们对连接图进行更新。对于障碍点的每次插入，可以创建O（h）个新网格，并且可以修改O（h）个现有网格的占用值。因此，要添加或删除的图顶点的数量由O（h）限定。

图中的边表示两个相邻自由空间栅格之间的连接。在将新顶点添加到地图中时，必须创建新边。在不失一般性的情况下，我们可以将一条边与两个连接的网格中较小的网格相关联。在八叉树结构中，较小网格的一个面完全用于连接。这表明，在每次插入障碍物时，边的数量相对于新添加的自由空间网格的数量呈线性增加，其再次由O（h）限定。

在实践中，我们发现我们的方法对于使用映射频率为5Hz的激光测距仪进行环境的增量映射足够有效。

C、基于搜索的飞行走廊初始化

我们将八叉树中所有自由空间网格的集合表示为C={c1，c2，…}，它对应于连通图G中的顶点集合。网格表示为：

 

其中l0ci和l1ci分别代表第l维度上第i个网格的下边界和上边界。我们使用标准A∗ 找到从起始网格到目标网格的最小成本网格路径的算法。我们将边缘成本设置为网格中心之间的欧几里德距离。A*的启发式函数搜索算法也方便地定义为网格中心到目标位置之间的距离。

我们能够获得由顺序连接的自由空间网格组成的网格路径Cp=（Cp 1，··，Cp M）作为初始飞行走廊。下一步（第IV-D节）将对初始飞行进行膨胀，以提供更大的飞行空间。由于自由空间可以通过八叉树中的少量网格有效地表示，因此可以在线完成飞行走廊的初始化，且计算量非常小。

D、 最大体积飞行走廊生成

使用基于搜索的方法（第IV-C节）获得的初始飞行走廊保证了安全，正如我们先前的工作所证明的[17]。然而，由于八叉树数据结构中的确定性网格划分，它不能代表最大的可用自由空间，这可能会将大的开放空间分割成小的区域。为了提高性能，我们开发了一种方法，将初始走廊中的每个网格膨胀到局部最大体积，同时仍然确保走廊无碰撞。我们的方法利用八叉树数据结构中的有效操作。我们定义了障碍物指示函数g:R3×2×R3×2→ ｛0，1｝为：

 其中c∈ R3×2表示走廊网格的原始立方体空间，如（1）所示，d∈ R3×2为膨胀方向，r∈ R1是膨胀距离。

由于g（·）相对于膨胀距离r不减这一简单事实，我们可以使用二进制搜索来确定最大可能的膨胀距离r，从而使c+r·d无障碍。在八叉树结构中，关于立方体体积是否无障碍的单个检查的复杂性为O（h）。二进制搜索迭代O（log（d r r？e）），其中r？是地图网格的最佳分辨率。给定充气方向d，求最大无障碍体积的总复杂度为O（h·log（d r r？e））。

我们使用一系列不同的膨胀方向在初始走廊中的每个网格上应用这种二元搜索。首先，通过设置

 然后，我们按照连接网格的方向膨胀网格。最后，网格从其六个面中的每个面展开。

 

这样，初始走廊中的小网格可以转换为大的重叠体积，如图3（a）和图4所示。重叠体积有利于调整隐式时间优化的路段过渡点（第V-C节）。我们将由重叠和顺序索引网格组成的充气走廊表示为Cf=（Cf 1，··，Cf M）。

 V、带有走廊约束的轨迹生成

在本节中，我们着重于使用有效的二次规划方法生成完全适合飞行走廊（第四节）的安全轨迹。该方法最初是在我们的先前工作[17]中提出的，我们通过调整分段过渡点来改进隐式时间优化。我们遵循四旋翼轨迹生成的最小捕捉公式[1]，这表明四旋翼具有不同的平坦度特性。这使得能够使用分段多项式轨迹来指定三维位置和偏航角{x，y，z，ψ}及其导数的平面输出，用于平滑四旋翼轨迹跟踪[1，2]。由于四旋翼的对称形状，我们专注于获得避免碰撞的平移运动。偏航角的规划和有限传感器视野的处理将作为未来工作。

A、多项式轨迹生成

M段N阶分段多项式轨迹将顺利通过飞行走廊Cf（第IV-D节）。给定轨迹的开始时间和所有段的结束时间（t0，t1，t2，··，tM），整个轨迹的一维（出{x，y，z}）可以写成：

 其中aij是第i段的j阶多项式系数。我们的目标是找到一条最小化第N个φ导数平方积分的轨迹：

 

目标函数（5）可以写成pTHp，其中p=[p1T，··，pM T]T是所有分段的多项式系数的向量，H是赫森矩阵。为了简洁，我们省略了它的结构。我们将证明走廊（第V-B节）和动力学（第V-D节）约束都可以表示为线性等式（Aeqp=beq）或不等式（Alqp≤ blq）约束。结果，轨迹生成问题可以写成二次规划问题（QP）：

 为了确保轨迹段之间的平滑过渡，我们在第一个Nφ− 1段过渡时间（t1，··，tM−1）的导数。

在实践中，高阶系数中的小数值误差可能导致轨迹的大变化。因此，我们利用Richter等人[2]中提出的方法来处理这个问题，其中计算了段端点导数，而不是多项式系数，来解决这个问题。

B、强制走廊约束

Cf中第i个网格和第i+1个网格之间的过渡应发生在网格i和网格i+1的重叠区域内的时间ti。数学上，我们可以写出线性不等式约束（Alqp≤ blq），以确保安全过渡：

 其中l∈ {x，y，z}，fi（·）是轨迹的第i个多项式段。图4（b）显示了这种类型的约束。

然而，（7）仅确保在网格过渡（ti）时，轨迹保持在道路边界内。它不会在任何其他时间对弹道的安全性提供任何保证。为了实现这种保证，我们提出了一种新的算法，通过考虑多项式的极值来强制整个轨迹的网格边界。我们首先在（6）中仅使用网格过渡约束（7）来求解QP，并保持初始解。考虑第k次迭代时的第i个轨迹段。我们检查每个维度l∈ ｛x，y，z｝，无论N− N阶多项式的1个极值违反了网格边界约束（图5）。我们在违反极值fki（tejk）处添加约束点，并在第（k+1）次迭代中为求解QP添加裕度δp：

 

 其中fki（·）是通过第i个网格的第i个多项式段，tejk是第j次边界破坏的时间，所有这些都是在第k次迭代时测量的，δp是恒定的位置裕度。这些约束点可以用线性不等式形式（Alqp≤ blq）。

注意，只有在找到多项式系数后才能检查极值，并且当前迭代中的极值在下一次迭代中可能不是极值。然而，该算法通过向QP添加更多约束点来迭代，直到多项式中没有极值违反网格边界约束。我们在定理1中表明，只要速度有界，就只需要有限数量的约束点来实施网格边界约束。与[1]中使用的密集约束点方法相比，这节省了大量的计算能力。

定理1：如果轨迹的导数是有界的，则可以生成拟合在网格边界内的多项式轨迹，并且边界上有有限数量的约束点，边界上有边界δp。

证明：为了简洁起见，我们只考虑了违反上限的情况，但这可以很容易地推广到违反下限的情况。设f:t→ y是指定[0，tf]之间的轨迹段的一维的N阶标量多项式。该段的上边界为y1b。约束点可以写成y1c=y1b−δp，其中δp是正裕度。由于f（0）和f（tf）在y1b到（7）内有界，并且f（t）的导数也有界为˙ym，因此∀吨∗ ∈ [0，tf]，k d f dt | t=t∗k≤ ˙ym。

考虑一个边界破坏情况（图5），其中轨迹破坏了两个相邻现有约束点（tl，y1c）和（tr，y2c）之间的边界y1b。它必须满足f（tl）≤ y1c，f（tr）≤ y1c和f（te）≥ y1b，其中f（te）是落在区间[tl，tr]内的f（t）的极值之一，∃吨∗ ∈ [tl，te]使得d f dt | t=t∗ = f（te）−f（tl）te公司−热释光≥ y1亿−y1立方英尺−tl=δp te−由于导数是有界的，我们有

 这表明

 同样，我们可以证明tr− 特≥ δp˙ym和：

 如上所示，只有当两个相邻的约束点至少相隔2·δp˙ym时，才会发生网格边界冲突。因此，我们只需要添加最多d∆t\/（2·δp\/˙ym）e约束点将轨迹完全约束在边界内。

根据（11），任何打破网格边界的极值将与现有约束点相距至少δp\/˙ym。在我们的方法中，我们迭代地添加违反网格边界的极值作为约束点。因此，所有约束点至少由δp\/˙ym分隔。如定理1所示，我们最终将有足够的约束点来消除所有网格边界冲突。在实践中，我们发现我们的迭代方法非常有效，几乎所有的边界违反情况都在几次迭代中被消除，与定理1所示的最坏情况相比，约束点要少得多。

C、对路段过渡点调整的影响

如（7）和图4（b）所示，多项式段之间的过渡可以发生在两个相邻走廊网格之间的重叠区域内的任何位置。事实上，如图3（a）所示，并且在几乎所有的实验结果（第VI节）中显示，我们的走廊生成步骤（第IV-D节）产生了具有大重叠的大自由空间区域。以这种方式，重叠区域内段长度的变化充当了隐式时间优化。我们大量重叠的走廊网格允许进行非平凡的调整，以降低轨迹的成本。我们的方法嵌入到QP中，并且不引入额外的计算。这对于[1]和[2]中提出的迭代时间优化方法是有利的。

D、实施高阶动态约束

第V-B节中提出的方法也可用于将轨迹的高阶导数绑定到预定义的最大值，以确保四旋翼轨迹的动态可行性。例如，飞行通过网格时的速度可以通过在速度剖面上违反边界的极值上增加约束来限制。如果需要，也可以绑定高阶导数。所有动态约束都可以写成线性不等式形式（Alqp≤ blq）。注意，多项式的N阶导数将是一个常数，并且绝对有界。因此，（N− 1） 可以从下向上限制第0阶导数。这支持定理1中的有界导数要求。

VI、结果

通过两次模拟和两次真实飞行实验验证了我们的方法。我们的轨迹生成方法是在C++11中使用带有-O3优化选项的g++编译器实现的。此外，我们使用Eigen中的稀疏线性代数库和开源二次规划求解器OOQP（面向对象的二次规划软件）[18]。边界裕度δp、δv和δa分别设置为0.005 m、0.05 m/s和0.05 m/s2。

A、与最先进方法的比较

我们在[2]中实现了最先进的在线轨迹生成方法，并在仿真中将其与所提出的方法进行了比较。在[2]中，轨迹生成是通过RRT的航路点进行的∗-基于规划。该比较是在具有3.20 GHz Intel Core i54570 CPU的台式计算机上进行的。RRT公司∗ 使用开放运动规划库（OMPL）1中的模块。我们将RRT步长设置为0.2m。

地图大小为40×40×10 m3。我们基于八叉树的地图具有0.2×0.2×0.2 m3的最佳网格分辨率。我们为地图随机生成160个虚拟柱，并对其进行膨胀以获得实际配置空间。所有方法的最大速度和线性加速度分别为3.0 m/s和3.0 m/s2。这些值还用于计算简单的加速-减速启发式之后的分段持续时间。我们也在我们的方法（第V-D节）中约束了这些动力学约束。尽管障碍物以二维方式生成，但这些方法在三维配置空间中工作以生成三维轨迹。

图6显示了使用三种轨迹生成方法进行的四次模拟的结果。该图有1.0 m2的单元格供视觉参考。蓝色立方体是实际的障碍物，而透明的白色立方体是膨胀的障碍物。绿色轨迹由Richter等人的方法[2]生成，蓝色轨迹来自我们之前的方法[17]，该方法没有走廊网格膨胀，红色轨迹来自所提出的方法。最初的飞行走廊显示为浅绿色透明立方体，而最终的飞行走廊则显示为白色。在四次模拟中，所有方法生成的轨迹长度约为26、28、46和64米。

 如图7所示，RRT∗ Richter等人的方法[2]需要大量时间来在受限环境中找到有效路径，并且需要许多额外的约束点来确保轨迹无碰撞。另一方面，我们的方法需要更少的计算时间，并通过施工保证安全。此外，如表1所示，我们的方法以更低的成本生成轨迹（定义见（5））。即使没有走廊通胀，正如我们之前的方法[17]，轨迹成本也优于Richter等人[2]中的方法。通过完整的走廊生成管道（第四节）和路段持续时间调整（第五节C节），所提出的方法大幅降低了轨道成本，在线性能极具竞争力。

 B、未知环境中的模拟飞行

我们给出了模拟四旋翼机在未知环境中航行的结果。在此模拟中，我们将感应范围限制为3米。只有当四旋翼接近障碍物时，才能发现障碍物。不断检查生成的轨迹是否可能发生碰撞。如果当前轨迹与任何障碍物相交，将生成新轨迹。实验期间发生了13次重复生成，图8中显示了一些示例。每个轨迹生成的计算时间如图9所示。

 C、杂乱室内环境中的自主飞行

我们展示了四旋翼机在杂乱未知的室内环境中的自主导航，所有计算都在船上完成。我们的实验测试台如图1所示。我们将3 m/s和2 m/s2设置为平台的动态约束。图11显示了实际测试环境。使用机载激光测距仪探测障碍物。每次扫描的平均值为200点，然后以5 Hz的频率将扫描向下采样到地图中。每次扫描的平均地图更新时间为0.014秒，最大飞行速度为1.4m/s。如图.10、12和13，在实验过程中发生了五次轨迹重新生成，每一次的时间都小于20ms。这表明我们的方法适用于未知环境中的实时避障。

D、户外环境中的自主导航

如图1所示，我们还在自主室外飞行实验中测试了我们的方法。所有系统参数与室内实验（第VI-C节）相同，速度达到1.8m/s。由于所有障碍物都可以通过一次激光扫描检测到，因此在该实验中没有轨迹重新生成。走廊初始化（第IV-C节）、走廊生成（第IV-D节）和轨迹生成（第V节）分别只需0.003、0.013和0.051秒。

VII、结论和未来工作

我们提出了一种在线方法，用于在杂乱环境中生成自主四旋翼飞行的无碰撞轨迹。我们的方法在一台速度适中的计算机上以几分之一秒的时间计算无碰撞轨迹，从而允许在未知环境中进行实时操作。我们建立了一个完整的四旋翼试验台，包括机载传感、状态估计、映射和控制，并集成了所提出的方法，以显示在复杂未知环境中的在线导航。我们的方法还与最先进的方法进行了比较，并显示出优异的性能。这项工作的扩展可能包括高速下的大规模实验。我们还对在线跟踪和避免移动物体的问题感兴趣。

总结

生成飞行走廊的方式是首先利用A*算法找到成本最小的路径，将该路径作为初始走廊，再将初始走廊膨胀到最大体积。