Online Quadrotor Trajectory Generation and Autonomous Navigationon Point Clouds 点云上的在线四旋翼轨迹生成和自主导航

点云上的在线四旋翼轨迹生成和自主导航

摘要

在本文中，我们提出了一种直接在点云上在线生成自主四旋翼飞行安全轨迹的框架。考虑到四旋翼机在未知环境中运行，我们使用三维激光测距仪进行状态估计，同时构建环境的点云图。基于增量构建的点云地图，我们利用KD树中快速最近邻搜索的特性，采用基于采样的路径查找方法，在三维空间中生成具有安全保障的飞行走廊。然后使用在二次约束二次规划（QCQP）中制定的轨迹生成方法来生成完全约束在走廊内的轨迹。我们的方法在100毫秒内运行，适合在线重新规划。我们将所提出的规划方法与基于激光的状态估计和映射模块相结合，并演示了在未知室内和室外环境中的自主四旋翼飞行。

1、介绍

微型飞行器（MA-V），尤其是四旋翼飞行器，由于其在人类或其他地面车辆无法接近或危险的复杂环境中的卓越机动性，已越来越受到安全和安保任务的关注。在搜索和救援任务中，四旋翼机应能够在线生成并执行从起始位置到目标位置的平滑安全轨迹，同时避免意外障碍。在本文中，我们建立在用于大规模未知环境中状态估计和映射的最先进的基于激光的解决方案[1]的基础上，并提出了一种直接在点云上操作的在线轨迹生成方法，以实现安全的空中导航。我们将所有模块集成到一个完整的系统中，并提供在线实验结果。

给定车辆状态的估计，已经提出了许多方法来将车载传感器生成的距离测量值转换为全球地图。代表性方法包括体素网格[2]、八角图[3]、高程图[4]等。这些方法中的每一种在特定环境中都有优缺点。例如，体素网格对于小体积的细粒度表示很好，但存储复杂度缩放很差。当表示具有大型开放空间的环境时，Octomap是内存高效的，其代价是昂贵的地图结构维护。立面图对于表示主要由垂直墙组成的人造结构很有用，但在表示自然场景时效率较低。在本文中，我们选择直接在全球注册的点云上规划轨迹，而不是使用后处理的地图。这绕过了昂贵的地图绘制过程，并实现了对不同环境的最佳适应性，这是搜索和救援任务的关键要求之一。如Mellinger等人[5]所述，四旋翼系统具有差分平坦度特性，可以将全状态空间减小到三维位置和偏航角及其导数。因此，具有有界导数的三维位置的平滑分段多项式轨迹可以由适当设计的几何控制器跟随[6]。我们利用这一特性，设计了一种使用凸优化直接在点云上生成无碰撞平滑轨迹的新方法。我们的方法采用了一种随机飞行走廊生成方法，该方法利用KD树上的快速邻居搜索[7]，然后是基于优化的多项式轨迹生成器。我们的贡献总结如下：

（1）一种基于采样的快速探索随机图（RRG）方法，结合A*路径查找算法，用于查找由多个重叠的自由空间球组成的无碰撞飞行走廊。

（2）一种基于二次约束二次规划（QCQP）的轨迹生成方法，在安全保证的情况下在飞行走廊内运行。

（3）实时实现，100毫秒内在线地图更新和轨迹（重新）生成，实现未知复杂环境的在线导航和探索。

（4）与状态估计和映射模块集成，并在未知的室内和室外环境中进行在线实验

我们在第节讨论相关文献。二、 并在第节中介绍了系统的总体架构。三、 基于激光的状态估计和映射是我们方法的先决条件，在第节中介绍。四、 我们的走廊生成和轨迹优化方法详见第节。V和第。VI。在第七节给出了实现细节、仿真和实验结果。论文在第八节中总结。

2、相关工作

机器人运动/轨迹规划有大量算法和应用，从基于采样的方法[8]到基于优化的方法[5]。这里，我们概述了与四旋翼轨迹规划任务相关的方法。

[9]中的方法将固定最终状态和自由最终时间控制器与快速探索随机树*（RRT*）方法相结合，以确保渐近最优性。通过这种方式，可以导出最优轨迹的闭式解。Karaman和Frazzoli[10]提出了RRG方法，这是RRT算法的一个扩展，因为它不仅将新样本连接到最近的节点，而且还连接到球内的所有其他节点。Bry和Roy等人提出了另一种结合RRG和信念路线图的方法[11]，其中引入了部分排序来权衡信念和距离。

基于控制的方法也可用于解决规划和避障问题。[12] 考虑了线性四旋翼模型，并提出了线性二次高斯（LQG）障碍物集来表示导致碰撞的目标状态。只要目标状态不在此集合内，就可以生成安全控制命令。[13]中提出了一种用于类似目的的后退地平线控制策略。

Mellinger等人[5]首创了一种最小快照轨迹生成算法。有大量的后续工作。[14]中提出了一种混合整数二次规划（MIQP），以在异构四旋翼团队中强制避免静态障碍和其他代理。[15]中提出了具有封闭形式解的无约束二次规划（QP），其中RRT*用于找到可行的直线路径，然后是基于航路点的轨迹。如果轨迹与障碍物相交，则添加额外的中间航路点，并再次生成轨迹。

由几何约束辅助的轨迹生成最近变得流行起来。在[16]中，通过半定规划迭代区域膨胀（IRIS）（一种最近开发的凸分割方法）被用于用凸区域逼近自由空间。然后使用平方和（SOS）编程将平滑轨迹分配给这些凸区域。Chen等人[17，18]提出了一种在线方法，该方法利用八面图数据结构中的有效操作来在线生成飞行走廊。二次规划用于生成安全和动态可行的轨迹。

我们的方法属于同一类几何约束轨迹优化方法，只是我们建议绕过映射过程，直接对点云进行操作。在[19]中，提出了一种以点云为输入的路径规划算法。该方法利用张量投票来重建表面并找到地面车辆导航的局部平坦区域。然而，当我们在全三维空间中生成轨迹时，该算法与我们的算法有根本不同。

3、概述

我们首先介绍了自主四旋翼系统的总体架构（图2）。V elodyne VLP-161 3-D激光雷达的距离测量用于姿态估计[1]和生成全球注册点云图。在此过程中，使用基于ICP的广义方法进行帧间6自由度运动估计。该增量运动被用作帧到图对齐的第二阶段的初始猜测，以实现全局一致性，也使用基于ICP的算法。KD树用作地图存储和最近邻搜索的数据结构。为了获得用于反馈控制的高速率状态估计，使用扩展卡尔曼滤波器（EKF）将基于激光的姿态估计与IMU和声纳测量融合。

使用点云，采用基于采样的方法生成无碰撞飞行走廊。在该方法中，我们将自由空间与具有局部最大半径的球的形状进行采样和连接。使用KD树结构可以非常有效地进行半径搜索。因此，走廊是一系列相连的球体。使用飞行走廊作为几何约束，使用基于优化的方法生成完全适合走廊的无碰撞分段多项式轨迹。走廊和轨迹生成可以在100毫秒内完成，从而使在线轨迹（重新）生成成为可能，以避免意外障碍。最后，使用反馈控制器进行轨迹跟踪。

4、基于激光的状态估计和映射

我们现在介绍了基于激光的状态估计和点云映射方法的实现。我们的方法来自[1]，它为能够利用我们的轨迹生成方法的完整导航系统奠定了感知基础。

我们基于激光的状态估计器的流水线如图3所示。我们从每个360度3-D距离测量中提取边缘和表面。执行广义迭代最近点（ICP）扫描匹配算法以在扫描Si和Sj之间进行姿态估计，步骤总结如下：

对于Si边缘上的每个3-D点，我们搜索Sj中最近的两个点。从点（x0i）到直线（由x1j和x2j形成）的距离（ds）计算如下

 对于Si表面上的每个3-D点，我们搜索Sj中不共线的最近三个点，并计算从点（x0i）到表面（由x1j、x2j、x3j形成）的距离（ds），如下所示：

 我们堆叠与每个特征点的距离，并使用Levenberg-Marquardt方法[20]最小化直线和曲面的距离之和，以获得6自由度相对姿态。

我们对两次扫描使用相同的ICP算法（i=j− 1） 并且扫描到地图（Sj是全局点云地图）对准以获得6-DOF激光姿态。scanto扫描对齐的结果用作scanto地图对齐的初始猜测，然后将当前扫描的点添加到地图M中。此外，我们强调了点云地图的两个特征：

1） 我们通过限制总点数来保持恒定的计算复杂性。我们限制了点云可以传播的体积，并限制了点的空间密度。这将生成与UA V一起滑动的局部地图

2） 我们动态更新地图中每个点的权重（w），以清除移动对象和异常值测量。在第i次扫描时，我们可以如下定义点xki的权重更新函数：

其中α和β是控制重量减少和增加速率的参数。δ是一个小数字，用于确定是否观察到相同的点。λH和λL是权重的上限和下限，在该点处，该点被视为从地图中永久添加或删除。重复观察到的点最终将在地图中固定，而接收到很少观察的点（如移动对象上的点或异常值）将在多次更新后删除。点云图的快照如图4所示。

 我们对两次扫描使用相同的ICP算法（i=j− 1） 并且扫描到地图（Sj是全局点云地图）对准以获得6-DOF激光姿态。scanto扫描对齐的结果用作scanto地图对齐的初始猜测，然后将当前扫描的点添加到地图M中。此外，我们强调了点云地图的两个特征：

1） 我们通过限制总点数来保持恒定的计算复杂性。我们限制了点云可以传播的体积，并限制了点的空间密度。这将生成与UA V一起滑动的局部地图

2） 我们动态更新地图中每个点的权重（w），以清除移动对象和异常值测量。在第i次扫描时，我们可以如下定义点xki的权重更新函数：

其中α和β是控制重量减少和增加速率的参数。δ是一个小数字，用于确定是否观察到相同的点。λH和λL是权重的上限和下限，在该点处，该点被视为从地图中永久添加或删除。重复观察到的点最终将在地图中固定，而接收到很少观察的点（如移动对象上的点或异常值）将在多次更新后删除。点云图的快照如图4所示。

为了实现反馈控制所需的高速率状态估计。我们使用EKF将激光姿态估计与IMU和声纳测量融合。为了补偿每个传感器中的延迟，我们采用了Lynen[21]提出的环形缓冲方法。在我们的滤波器中，IMU的加速度和角速度测量被用作过程模型，而LiDAR和声纳的里程测量都被用作6自由度姿态和高度的相对测量模型。我们的三维激光雷达只有30度的垂直覆盖，因此在仅包含垂直结构的环境中，它很容易在高度估计中失败。添加一个向下的声纳解决了这个问题。

5、点云上的飞行走廊生成

A、基于KD树的环境表示

我们的基于激光的状态估计算法是利用KD树[7]数据结构进行快速最近邻点搜索的。具有N个点的平衡KD树可以在O（Nlo gN）时间内构造，空间复杂度为O（kN），M个最近邻搜索的时间复杂度为0（Mlo gN。对于状态估计，KD树对于点云配准非常有用。但是，从规划的角度来看，最近邻居搜索也可以用于查找三维空间中任意未占用位置与其地图中最近点（障碍物）之间的距离。该距离表示相对于该位置的安全半径，从该半径可以生成球形状的安全区域。请注意，KD树是一个静态数据结构，这意味着将点动态插入KD树将使其不平衡。因此，每次更新地图时，我们都会重建KD树。然而，如图13所示，重建具有80000个点的KD树所需的时间对于实时应用来说是可以接受的。

B、球形安全区域图的生成

如第V-A，可以通过KD树中的快速最近邻搜索来找到地图中任意点的安全半径。基于这一特性，我们采用基于随机抽样的方法来生成连接起点和目标点的道路。首先，我们使用RRG构建一个随机采样图，其中顶点是球形安全区域的中心，边是安全区域之间的连通性。然后，飞行走廊由一系列相连的球形安全区域组成，其中区域的位置、大小以及它们之间的连通性由该RRG确定。

如算法1所示。图G仅用起始节点ns初始化。图中的节点n表示具有两个属性的自由空间球，即3-D位置n.c和半径n.r。当随机采样新位置cr时，最近的（c，G）在图中找到其最近的节点nc。我们的算法确保生成一个图，其中边是球形安全区域之间的无碰撞连接。intersect（）函数生成从cr到nc的光线。c、 并返回光线与nc球体的交点。我们将此点设置为新节点nn.c的中心。我们执行半径搜索半径搜索（nn.c，M），以找到以nc为中心的最大球形安全区域。c与点云图M.I f nn进行对比。如果r足够大，则节点将被添加到图中并与nc连接。我们还使用G内的最近邻居找到nn邻居处的所有节点。如果重叠体积（可以非常方便地以闭合形式计算）大于允许四旋翼安全通过两个球的安全余量，我们将其与nn连接。

由于每次采样一个新点时，我们都会创建一个仅以最近节点的球体为中心的新节点，因此图的扩展速率会根据障碍物的稀疏程度而变化，这使得它对于具有大量自由空间的环境特别有效。RRG基于预定义的迭代极限N停止。

C、飞行走廊的生成

有了安全区域的连通图，我们可以利用基于搜索的算法（如A*搜索方法）在图上找到从起始位置到目标位置的路径。请注意，由于起始位置和目标位置可能包含在许多重叠的安全区域中的一个区域中，因此起始区域和目标区域可能有多种选择。我们选择使用启发式方法来选择目标区域作为欧几里德度量中最接近起始位置的区域。A*发现的飞行走廊如图5所示。注意，我们这里使用的算法具有渐近最优性的特征。随着安全区域样本数量的增加，它最终将密集地填充整个自由空间，这相当于在均匀且精细离散的体素网格上运行基于搜索的算法。

 六、基于优化的安全保障弹道生成

在本节中，我们提出了基于优化的方法，用于生成受安全飞行走廊约束的平滑轨迹（第五节）。由于四旋翼模型的差分平坦度特性，四旋翼系统{x，y，z，˙x，˙y，˙z，φ，θ，ψ，p，q，r}的全状态空间减少到三维位置，偏航角{x，y，z，ψ}及其产生轨迹的导数。几何跟踪控制器[6]可用于反馈控制。由于我们的四旋翼上有一个360度激光雷达，我们跳过了偏航角的规划。

A、多项式轨迹生成

由分段多项式组成的轨迹被参数化为x，y，z的每个维度μ中的时间变量t。一维的M段轨迹可以写成如下：

 其中pij是轨迹的第i段的j阶多项式系数。T1、T2、…、TM是每个段的结束时间，总时间T=TM− T0。多项式系数通过沿轨迹最小化第kφ导数的成本函数来计算。

成本函数是第k个φ导数的平方的积分。在本文中，我们最小化了沿轨迹的Jrek，因此第kφ为3。在本文中没有像[5]中那样公式化每个维度的成本函数，而是将所有x、y、z维度中的系数耦合到一个单独的方程中：

目标函数可以用二次公式pT Qop表示，其中p是一个向量，包含x、y、z三个维度上的所有多项式系数（aij），Qo是目标函数的Hessian矩阵。为了简洁起见，我们省略了成本函数的详细构造。我们还必须实施一些约束，以确保轨迹的平滑性和安全性：

1） 航路点约束：如果需要，我们可以确定航路点（例如起始位置和目标位置）及其第一个kφ-1导数dik。如果在Ti时刻存在固定航路点，我们有：

 2） 连续性约束：轨迹必须在两个多项式段之间的连接点处的所有k阶导数处连续，其中0≤ k≤ kφ− 1:

 3） 安全区域之间的相交：在每个多项式段结束时，轨迹的位置必须在连接的球形安全区域内：

 其中Ti（i=1，2，…，M）是分段多项式的结束时间，ci是球中心，ri是半径。

第一和第二约束可以重新表述为线性等式约束（Aeqp=beq），而第三约束必须以二次不等式约束形式（pT Qip+aip≤ bi）。因此，轨迹生成问题可以重新表述为二次约束二次规划（QCQP）问题：

 在实践中，高阶多项式系数中的小数值误差可能导致整个轨迹中的严重误差。因此，我们采用Richter等人[22]提出的方法，其中计算轨迹段每个结束时间的导数，然后映射到原始多项式系数。此外，我们使用Mellinger等人提出的时间归一化方法[5]，以避免每个段中的大时间值导致的赫森矩阵中的大数值误差。

B、对段连接点调整的影响

如Chen等人[17]所示，具有大重叠区域的飞行走廊可以为多项式段之间的过渡引入大空间。这提供了隐式调整时间分配的机会。因此需要更大的重叠区域，因为它们为优化提供了更大的自由度以降低轨迹成本。在本文中，我们将两个连续球之间的连接区域充气成一个新球。如图6所示，绿色球体是膨胀的连接区域，直径为两个相交球体的弦。因此，我们可以将二次约束修改为

 其中cj和rj是充气球的中心和半径。此修改不仅为调整线段连接点增加了更多空间，而且将二次约束的数量减半。

C、实施安全和动态约束

轨迹必须完全约束在飞行走廊内，以确保避免碰撞。我们还需要施加最大速度和加速度约束，以确保动力可行性。在上一节中，我们在每个路段结束时强制执行了道路内剩余的轨迹，现在我们应该确保它在任何其他时间都不会超出道路。我们采用Chen的方法[17]，这证明了在多项式极值处迭代添加的有限数量的点约束导致完全有界的轨迹。在我们的算法中应用这种方法的缺点是，我们可能会将极值约束到原始重叠的安全区域，而不是膨胀的连接区域。通过这样做，可以减少对路段连接点调整的影响，从而产生不太理想但仍然安全可行的轨迹。

七、结果

本节介绍了室内和室外自主飞行的模拟结果和实验。我们的轨迹生成模块在C++11中使用通用凸解算器Mosek2在免费学术许可证下和Eigen中的稀疏矩阵库实现。模拟是在配备2.20 GHz Intel Core i55200U CPU和8 GB RAM的笔记本电脑上完成的。飞行实验在配备Intel NUC（双核CPU i5-4250U，1.30 GHz，16GB RAM）的DJI M100四旋翼机（图1）上进行。模拟是用我们大学的一张已知地图完成的，该地图是由一个离线地图软件构建的。自主飞行实验仅使用在线传感器测量在未知环境中进行。在线构建的点云地图如图所示。10和11。模拟和实验中的采样数设置为5000，根据图13，实验中的取样时间是可接受的。

A、模拟结果

模拟中使用的三维点云图由Altizure3使用无人机从高空收集的数据构建。该地图包含40多万个点，水平方向覆盖160米，垂直方向覆盖40米。将四旋翼机提升至30米，以模拟近距离搜索和救援任务中的实际场景。在图7（a）中，在我们的标准笔记本电脑上，总长度为95.138米的无碰撞轨迹在74.3毫秒内计算。图7（b）显示了另一种情况，其中轨迹长度为60.884米，计算时间为60.8毫秒。仿真表明，我们的算法在大规模环境中足够快，并且计算复杂度与旅行距离相关。

 B、自主室内飞行

室内实验在我们的实验室中使用随机放置的障碍物进行（图1（a））。四旋翼被命令飞行通过多个预定义的航路点。状态估计、映射和轨迹生成都是在线执行的，没有任何关于环境的事先信息。以20 Hz运行的Velodyne 3-D激光雷达在每次扫描中输出15000点。状态估计以与传感器相同的速度运行，而点云以10 Hz更新。地图更新后，四旋翼检查当前轨迹的碰撞状态，必要时重新生成。考虑到四旋翼的尺寸，我们设置了一个安全余量，并为飞行走廊中的每个安全区域预留0.15米的半径，以考虑控制误差。在实验中，三个目标被依次发送到四旋翼机，由于在线检测障碍物，产生了四个轨迹（重新）（图8和图9）。

总结

生成飞行走廊的方式是利用RRG搜索图得出路径，并且对顶点膨胀得到最大安全半径的圆，边来连接圆的顶点。