Planning Dynamically Feasible Trajectories for Quadrotors Using Safe Flight Corridors in 3-D Complex

在三维复杂环境中使用安全飞行通道规划四旋翼的动态可行轨迹

摘要

关于使用凸优化导出分段多项式轨迹以控制微分平面系统以及微型飞行器三维飞行的应用，有大量文献。在这项工作中，我们提出了一种使用安全飞行走廊（SFC）概念将轨迹生成公式化为二次规划（QP）的方法。SFC是凸重叠多面体的集合，它模拟自由空间，并提供从机器人到目标位置的连接路径。我们导出了一种有效的凸分解方法，该方法从使用快速图搜索技术获得的分段线性骨架构建SFC。SFC在QP中提供一组线性不等式约束，允许实时运动规划。由于机器人传感器的范围和视野有限，我们开发了一个“重新规划地平线”框架，该框架在局部地图的有限足迹内规划轨迹，通过重新规划过程不断更新轨迹。对于大而杂乱的地图，重新规划过程需要50到300毫秒。我们展示了我们方法的可行性、完整性和性能，并在使用四旋翼的模拟和物理实验中应用于高速飞行。

I、介绍

微型飞行器（MAV）在障碍物杂乱环境中的生存是一个具有挑战性的问题，它不仅需要MAV检测障碍物，还需要规划和执行无碰撞和动态可行的轨迹。在本文中，我们提出了一种实时有效生成这些安全平滑轨迹的算法。我们使用该算法作为四旋翼快速安全导航系统的基础（图1）。

已经表明，对于差分平坦系统，轨迹生成问题可以公式化为二次规划（QP）[1]。轨迹可以参数化为时间上的k阶多项式[2]。在[3]–[5]中，使用混合整数方法解决了生成无碰撞轨迹的问题。由于求解MILP/MIQP需要几秒到几分钟[3]–[5]，因此已经开发了其他方法来移除整数变量并求解QP，这要快得多[6]–[9]。轨迹可以以封闭形式求解[6]，但需要多次迭代才能生成无碰撞轨迹，尤其是当地图复杂时。[7] 需要OctoMap[10]表示，并在自由空间中生成一系列轴对齐的立方体以生成轨迹。凸自由空间的这种公式不是通用的，只有当障碍物是长方体时才有效。在[11]中，作者分析了通过障碍物场的高速导航，但他们没有考虑到非平凡的机器人动力学，其结果可能不适用于MA Vs。[12]中的框架基于先验路径来修正轨迹，但它需要精确的先验地图，这是在未知环境中实际导航的限制。

我们采纳了这些相关工作中的一些想法，并基于我们之前的工作[13]，[9]提出了一种鲁棒有效的解决方案，以实时生成轨迹。我们的管道使用来自快速图形搜索算法的线性分段路径来指导地图的凸分解，以找到安全飞行走廊（SFC）。SFC是一个凸连通多面体的集合，它对地图中的自由空间进行建模，并且可以作为QP中的线性不等式约束来进行轨迹优化。受[14]启发，我们开发了一种新的凸分解方法，以使用椭球体构造SFC。使用该管道生成轨迹的总时间足够短，因此我们将其与最近的地平线规划（RHP）框架一起使用，以构建具有映射和状态估计的导航系统。我们假设机器人能够通过非线性控制器跟踪我们生成的轨迹[15]。我们通过仿真和真实世界实验验证了系统在部分感知的复杂环境中避免碰撞的鲁棒性。

为了保证安全，使用了停止策略[13]。我们算法的三个主要区别优势可以概括为：

1） 快速计算

2）高速轨迹生成

3）安全性和完整性

与我们在[9]中的先前工作相比，我们提高了规划速度，提出了更通用和有效的分解方法，并测试了具有更大行进距离和更高飞行速度的管道。本文的概要如下：在第二节中，我们描述了弹道生成的技术方法；在第三节中，我们分析了算法的计算开销和效率；实验结果见第四节；见解和结论见第五节。

II、技术方法

我们的自主系统的总体架构如图2所示。在本节中，我们主要讨论前四个组件，通过这些组件，我们可以得出控制MAV达到目标所需的轨迹。快速路径规划和凸分解的源代码可以在https://github.com/sikang/JPS3D中找到。git和https://github.com\/sikang\/DompositUtil.git。

A、路径规划

环境被表示为占用网格，该占用网格可以由诸如激光测距仪、立体摄像机或RGB-D传感器之类的传感器数据构建。然后可以使用图搜索算法在网格中找到有效的无冲突路径。随机方法，如RRT∗ 和PRM在概率上是完全的，这意味着如果存在最优路径，则无法保证找到最优路径所需的时间。此外，当我们需要频繁重新规划时，它们的随机行为使算法的性能无法预测。基于搜索的算法，如Dijkstra和A∗, 另一方面，它们的分辨率是完整的，但当与大型地图一起使用时，它们用于找到最佳路径的计算时间是一个限制。跳点搜索（JPS）[16]通过在统一成本网格图中进行规划来解决这个问题。由于我们使用具有统一体素的三维网格图，JPS可以应用于我们的问题。JPS删除正在搜索的节点的邻居，并可能减少A*的运行时间 一个数量级。为了将JPS用于三维体素图，我们将[16]中提出的二维算法扩展到三维。我们提出了三维体素网格的修剪规则，如图3和4。如[16]中所定义的，自然邻居是指修剪后剩余的节点集。对于那些由于障碍而无法修剪的邻居，我们称之为强制邻居。

 

递归修剪和跳转过程的详细信息可以在[16]中找到。图4中提出的修剪是在检查所有情况和保持算法简单性之间的折衷：我们添加了比所需更多的邻居（三个强制邻居的情况），但更容易检查（即更有效）。JPS提供了与A* [16] 相同的完整性和最优性保证，唯一的限制是统一成本网格的假设，这适用于我们的情况。我们的3-D JPS显著加快了规划的运行时间（表I中的第6列），这使得在我们的RHP框架内运行轨迹生成成为可能。

 B、安全飞行走廊构造

构成障碍物的一组点（环境的3D网格地图表示中所占的体素）表示为O。自由空间中从起点到目标的分段线性路径P表示为P=<P0→ P1→ · · · → Pn>，其中pi是自由空间中的点，pi→ pi+1是自由空间中的有向线段。我们在P中的每个线段周围生成一个凸多面体，以构造有效的SFC。第i条线段表示为Li=？圆周率→ π+1？。注意每个Li生成的凸多面体为Ci。这些凸多面体覆盖的空间构成了安全飞行走廊。我们将这些凸多面体的集合表示为SFC（P）={Ci|i=0，1，…，n− 1}. 图5是路径P和相应SFC（P）的典型示例。构造SFC的标准是两个连续的多面体Ci和Ci+1需要在包含pi+1的非空集合中相交。这确保了SFC的连续性。

为了从Li生成凸多面体Ci，我们描述了两个过程：（1）“查找椭球体”，它首先拟合Li周围的椭球体；（2）“查找多面体”，它从切平面到一系列扩张椭球体构造多面体Ci。为了减少计算时间，我们添加了一个边界框来限制Li周围的空间，其中我们考虑了障碍物。此外，我们提出了一个收缩过程，以确保非点机器人无碰撞。在以下小节中，我们将介绍这些程序的详细信息。为了简单起见，我们删除了下标“i”，并简单地使用L，C来表示相应的线段和多面体。

1） 步骤1–查找椭球体：在此步骤中，我们找到一个包含线段L且不包含O的任何障碍点的椭球体。椭球体描述为

 

对于R3中的椭球体，E是一个3×3对称正定矩阵，表示球体（？≤ 1).E可以分解为E=RT-SR，其中R是将椭球轴与地图轴对齐的旋转矩阵，S=diag（a，b，c）是对角比例矩阵，其对角元素代表椭球半轴的相应长度。d表示椭球的中心。在不失一般性的情况下，我们假设≥ b、 一个≥ c、 我们的目标是找到给定线段L和障碍物O的E，d。

这个椭球体分两步计算：首先，我们收缩初始球体以导出最大椭球体（两个轴长度相等的椭球体）；第二，我们沿着第三轴“拉伸”这个椭球体，得到最终的椭球体。在第一步中，初始椭圆体是一个以L中点为中心的球体，其直径等于L的长度。假设椭圆体的x轴长度固定并与L对齐，我们减小其他两个轴的长度，直到球体不包含障碍物。这是通过从ξ的中心搜索O中最近的障碍物来完成的。图6从二维角度显示了收缩过程。

最大球体在p？其与线段L一起限定了球体的~x-~y轴的平面。接下来，我们拉伸椭球体的~z轴的长度，使其等于a，以形成新的初始椭球体。c的实际值可以通过使用图6所示的类似过程找到另一个最近点来确定。

 

2） 步骤2–查找多面体：将上一步骤中找到的椭球表示为ξ0，它在pc0=p处接触障碍点∗. 在这一点上，椭圆体的切平面创建了包含椭圆体的半空间H0={p|aT0p<b0}。在计算H0之后，我们移除O中位于H0之外的所有障碍物（称之为剩余障碍物集合，Orem ain），并“扩张”椭球体（保持其纵横比恒定），直到它与另一个障碍点pc1接触，该点称为ξ1，新的椭球体创建新的半空间H1。继续此过程以获得半空间序列H0、H1、··、Hm。这些m+1个半空间的交集给出了凸多面体，C=？mj=0Hj={p|AT p<b}，其中aj和bj分别是矩阵A的第j列和向量b的元素。

图7显示了椭球体膨胀的一个例子。在每次扩张迭代中，椭球ξj在点pcj处接触障碍物。算法1显示伪代码。定义第j个半空间Hj的超平面是pcj处ξj的切线，计算如下

 到目前为止，我们能够在给定障碍物O的情况下为L生成多面体C。我们在路径P的每个单独线段上应用此方法，以获得安全飞行走廊，SFC（P）＝{Ci|i=0，1，…，n− 1} （图5）。由于原始椭球体位于相应的多面体内部，我们可以保证线段L也位于多面体内部。因此，整个路径P被保证在SFC（P）内。

3）边界框（Bounding Box）：如图所示，当为每个线段L构造多面体C时，该算法需要搜索O中的所有点至少两次，以检查与膨胀椭球体的相交。这是一个昂贵的过程。我们通过在L周围添加边界框来减少要检查的点的数量，从而只搜索其中的障碍点。这个过程节省了大量的计算时间，也防止了轨迹偏离原始路径太远。L的边界框由6个矩形组成，使得边界框的轴与L对齐，并且从每个面到L的最小距离为rs。如果MA V的最大速度和加速度为vmax，amax，则施加在安全半径上的条件为rs≥ v 2max2a max。图8显示了应用边界框的典型结果。生成的SFC包含类似的半空间，如图5所示。

4） 收缩：我们将机器人建模为半径为rr的球体，并在原始地图M中扩展占据的体素，以生成配置空间Me，从而我们能够将机器人视为规划的单个点。当为Me中规划的路径P构建SFC时，u s i n g Me可以生成窄椭球和多面体[图9（a）和（b）]。为了避免这种糟糕的SFC，我们使用原始映射M生成SFC，并将SFC缩小机器人半径rr，以确保安全。收缩过程通过将每个支撑超平面沿其法线推rr来应用。当我们将障碍物和每个超平面之间的距离增加rr时，该过程确保了收缩SFC的安全性，但也可能排除可能导致安全飞行走廊不连续的部分路径[图9（d）]。为了保证连续性，我们必须确保线段L位于收缩多面体C？内？。为此，我们修改了算法1：对于任何半空间Hj∈ C（C是原始多面体），我们检查从L到Hj超平面的最小距离d（L，Hj）。如果d（L、Hj）<rr，我们调整超平面的法线，使d（L）=rr（H？j是调整后的半空间）。新半空间H的超平面？j也穿过Hj与扩张椭球的交点[图9（e）]。

 C、轨迹优化

在本节中，我们介绍了使用生成的SFC生成最小快照轨迹的方法。我们在之前的工作中采用了类似的轨迹优化公式[9]。假设SFC包含n个凸多面体，整个轨迹由n个多项式组成，第i个多项式位于第i个多面体Ci内。因此，最小捕捉轨迹的凸优化可以形成为QP，其对机器人的开始和结束状态的约束如下

 这里，矩阵Ai、bi对应于第i个多面体Ci。轨迹Φ（t）组成如下

 

这里我们使用分段多项式[17]来描述轨迹Φ（t）。因此，Φi（t）、d dtΦi（t）、d2 dt2Φi（d）、d3 dt3Φi（e）表示时间t时的期望位置、速度、加速度和加急度，这是非线性控制器[15]计算用于控制四旋翼的期望力和动量的输入。上述方程中的Δti表示每个多项式的时间，Δti=ti+1− 图10显示了受SFC限制的分段多项式轨迹的示例。

 

Δti或时间分配的估计会显著影响生成的轨迹。由于每个SFC都包含一个有效的路径P，所以朴素的时间分配方法是使用梯形速度分布将该路径P映射到时域。使用初始时间分配求解（3）可能会导致速度、加速度或加速度超过MAV最大阈值的轨迹。与[6]类似，我们根据（5）修改Δti以调整时间分配，从而使用Δt？我可以被机器人跟踪。将生成轨迹的最大速度、加速度和加速度表示为vmax、amax、jmax，相应的阈值表示为“vmax”、“amax”、“jmax”。第一单元用于防止修改正确的时间分配。

 在我们的优化过程中，我们使用基于样本的方法来限制每个多项式，其细节可以在[2]中找到。此外，为了确保飞行安全，我们总是假设静止的末端状态，速度、加速度和加速度为零。

D、 最近的范围规划

对于在未知环境中使用局部感知的MAV导航，我们使用最近地平线规划（RHP）来连续生成轨迹，直到机器人达到最终目标。如本节所述。一、 RHP是后退地平线控制的一种变体，人们在固定的未来时间间隔内解决最优控制问题[18]，[19]。我们没有求解固定的时间间隔，而是将规划地平线定义为最长距离dr，该距离在我们的后退地平线框架中受到感知范围的限制。一旦我们规划了从起点到目标的路径，我们只使用机器人半径为dr的路径的一部分来生成轨迹Φ。机器人仅在短时间内执行Φ，我们称之为执行水平Te，因此下一个重新规划时期的轨迹生成的开始状态由Φ（Te）决定。我们选择Te，以确保生成轨迹的时间小于Te，从而机器人一旦完成当前轨迹Φ的执行，就能够遵循新的轨迹。换句话说，当机器人在当前时期执行轨迹时，我们开始生成下一时期的轨迹。由于执行时间Te大于生成轨迹所需的时间，因此当机器人完成当前轨迹的执行时，它总是能够转移以跟踪新轨迹。图11显示了RHP的示例。

在某些情况下，如果规划器无法找到路径或由于时间分配不好导致轨迹优化失败，则无法实现下一个重新规划时期的轨迹。我们利用[13]中所述的停止策略，在故障发生时使机器人停止，在机器人自身稳定后，我们继续使用相同的轨迹生成管道搜索新的轨迹。我们可以在全局或局部地图中规划轨迹，但对于我们的实验，我们使用局部地图。使用最后几个传感器读数构建局部地图，而全局地图需要完整的SLAM解决方案来校正状态估计中的漂移。与全局地图相比，局部地图更容易实现，并且足以避免障碍物，但是缺乏全局信息使得规划器全局不完整，并且容易受到类似于死胡同的环境的影响。

III、分析

A、 与IRIS的比较

用于生成无碰撞凸区域的现有算法[5]，[14]需要正确选择种子和难以从真实传感器数据获得的障碍物的几何表示。在他们的过程（IRIS）中，通过凸优化求解最大椭圆体需要很长时间。对于图12所示的地图，IRIS算法需要大约110毫秒，而我们的算法只需要4.8毫秒。事实上，在IRIS中生长椭球体的种子选择非常重要，这也使得实时运行IRIS进行分解变得更加困难。

B、运行时间分析

我们使用四个不同的地图，通过生成数百条轨迹来测试算法的运行时间。这四张地图分别命名为“随机街区”、“多层”、“森林”和“室外建筑”。我们在每个地图中以一定的密度采样目标，并手动选择开始。图13显示了这些地图和生成的结果。选择这些地图是因为它们对于现实世界中遇到的不同环境（即2.5D、全三维、随机分散的复杂障碍物和真实世界数据）是典型的。

为了评估算法的计算开销，我们将整个轨迹生成分为三个部分：路径规划、凸分解和轨迹优化。表I显示了在i7-4800MQ处理器上生成如图13所示轨迹时每个组件的时间成本。对于路径规划，我们比较了两种不同的方法：∗ 和JPS来显示使用JPS对运行时间的影响。从结果中可以看出，我们能够在几百毫秒内生成轨迹，这对于在2-3 Hz下重新规划来说足够快。

C、完备性

在这一小节中，我们讨论了局部地图内的算法完整性：如果存在一个达到地图分辨率的轨迹，是否会找到轨迹。由于SFC的构建从线段开始，它至少会产生一组包含这些线段的凸区域。在这种情况下，优化的可行集总是包含轨迹Φ是具有静态开始和结束状态的多项式的解。对于初始非静态动态导致轨迹优化失败的其他情况，车辆将遵循现有无碰撞轨迹或执行停止策略。最终，车辆将在悬停模式下停止，如果存在通向最终目标的路径，我们可以从静止状态始终生成轨迹。总之，我们的算法是完整的，因为我们使用的路径规划算法是完整。当使用全局地图（例如，图13）时，保证了完整性。然而，如果我们只有局部地图，全球完整性是不可能实现的，机器人可能会陷入死胡同。

D、飞行速度

在本节中，我们通过无量纲参数分析自主飞行的速度。我们通过最大加速度amax（受车辆推重比限制）和最大速度vmax（受空气阻力限制）来描述MAV模型。这两个参数反映了四旋翼的飞行速度。对于不同的平台，由于其不同的硬件配置，我们通常具有不同的amax和vmax值。计划水平dr（受感应范围限制）和执行水平Te（受机载计算能力限制）是影响RHP中飞行器飞行速度的两个独立变量。它们可以通过标准化无量纲化为：

 可以使用两个参数来评估飞行速度：达到目标的总时间T和最大速度vmax。假设总距离为dgoal，我们可以使用以下符号计算标称飞行时间和最大速度：

 

我们使用这些无量纲参数绘制了在模拟中使用三个不同机器人的测试结果（图14）。我们可以得出结论，通过设定一个大的规划范围，可以实现快速飞行。然而，在实际的实验中，计划视界受到感知范围的限制，在一定阈值之后不会增加飞行速度。执行范围也受到机载计算的限制，例如，在表I中，重新规划的最大时间成本需要0.47秒，这为Te设置了一个下限。

为了测试高速避障，我们通过在一个区域内随机散射N个凸形障碍物来模拟环境。典型环境如图15所示。通过40米传感范围的模拟V elodyne Puck VLP-16，机器人能够在这片森林中达到19.2米/秒的最大速度，并在14.3秒内达到200米宽的目标。

IV、实验结果

我们将建议的导航管道应用于图1所示的四旋翼平台。我们使用MSCKF算法的立体版本[20]进行状态估计，并使用V elodyne VLP-16构建局部地图。所有计算都在板载Intel NUC计算机（双核i7）上执行。图17显示了户外场景中的几个实验，其中机器人对环境没有任何先验知识。给定相对于初始机器人位置的目标，我们的系统可以成功地到达目标并返回，而不会碰到任何障碍物。在图17所示的运行中，车辆以5 m/s的速度行驶。

在测试1中，机器人成功地避开了具有复杂三维几何结构的树木和灌木丛。在测试2中，由于森林茂密，机器人决定绕着森林飞行，而不是穿过森林。在测试3中，机器人避开树木、森林和建筑物，机器人行驶的总距离约为1公里。我们的轨迹是平滑的，并受到速度、加速度和加速度阈值的约束，这有助于减少基于视觉的状态估计中的误差：返回到起始位置后，位置的一般漂移小于1%。

当我们将最大加速度设置为相对较小（3 m/s2）时，机器人能够密切跟踪生成的轨迹。图16显示了测试1期间控制器的性能，我们可以看到误差小于0.2 min位置。

V、结论

高速自主导航对MA V来说是一个挑战，因为（a）必须将动力学约束纳入运动规划；（b） 用于规划的有限计算资源；（c） 由于传感器传感范围有限，机器人只能访问世界的局部地图。在这封信中，我们描述了一种轨迹生成算法，该算法仅基于机载传感和计算，实时导出动态可行的无碰撞轨迹，并在Receed Horizon Planning框架下，当传感器提供新信息时，实时更新这些轨迹。我们研究了速度和安全性之间的权衡，以及诸如（a）Te、t h e执行范围、（b）l、不同四旋翼环境中的传感器感应范围等参数的影响。对整个系统和子系统之间平衡的研究将确定阻碍导航速度的限制因素，并指导未来研究如何调解这些因素。

总结

生成飞行走廊的方式是首先利用JPS搜索出三维安全路径，然后以两个顶点生成椭圆，然后生成凸多面体，在凸多面体中生成轨迹。