【论文笔记】ego_planner_swarm

文章名：EGO-Swarm: A Fully Autonomous and Decentralized Quadrotor Swarm System in Cluttered Environments

【单位】浙大fastlab

【期刊】ICRA2021

摘要：本文提出了一种基于机载资源的多机器人自主导航的分布式异步系统解决方案。在基于梯度的局部规划框架下建立规划系统，通过将碰撞风险确定为非线性优化问题的惩罚项来实现碰撞避免。为了提高鲁棒性和避开局部极小值，引入了一种轻量级拓扑轨迹生成方法。然后，agent使用不可靠的轨迹共享网络在几毫秒内生成安全、平滑和动态可行的轨迹。利用深度图像中的agent检测校正agent之间的相对定位漂移。通过仿真和实际实验验证了该方法的有效性。源代码已发布以供社区参考。

1.引言：

        四旋翼的敏捷性使该机器能够在未知环境中进行单智能体自主导航，在开放或已知领域进行多智能体精确编队控制。 然而，很少有工作将它们两者结合起来展示任何能够导航共享相同未知空间的四旋翼集群的真实世界系统，尤其是仅在机载处理的情况下。 在未知环境中部署多个四旋翼飞行器的困难包括但不限于障碍物参数化的重要性、感知范围有限、通信不可靠和带宽受限以及定位不一致导致的定位漂移。 一些相关的工作，例如 [1, 2]，将能够避障的现实世界四旋翼群推动得更远。 然而，对于大多数先前的工作，在运动捕捉系统或纯仿真中，上述困难总是被忽略，限制了其算法在实践中的应用。

        本文提出了一种系统化的解决方案，可以在存在上述困难的情况下，为四旋翼机群在杂乱的环境中实现高性能飞行。 此外，它不需要外部定位和计算或预先构建的地图。所提出的EGO-Swarm系统是我们先前工作的一个扩展，它是基于无ESDF梯度的LOcal规划器（EGO-Planner）的扩展，它为未知环境下单个四旋翼的机载局部规划奠定了坚实的基础。扩展包括拓扑规划和机间避碰两部分。如图3所示，非凸配置空间可能导致不期望的行为，例如动力学不可行或拥挤的导航。因此，像拓扑规划这样的策略来避开局部极小值是有益的。基于EGO Planner中的碰撞代价公式，前端拓扑路径搜索是隐式进行的，因此几乎不需要计算。通过在目标函数中加入群碰撞的加权惩罚，实现了分布式机间避碰。通过比较未来某一时刻的agent分布和正在优化的轨迹来评估这个惩罚函数。为了尽量减少数据传输并允许不可靠的通信，使用广播网络来共享轨迹。为了纠正相对定位漂移，可以将其增加精度到半米，我们比较了目标agent的观测结果和轨迹评估的预测结果。

        提出了真实世界的实验来验证我们提出的群系统。 据我们所知，这是在未知杂乱环境中完全自主分散的四旋翼集群的第一个系统解决方案，这意味着感知、规划和控制集成到机载系统中。 与几种 SOTA 方法的比较显示了计算效率和鲁棒性。 本文的贡献总结如下：

（1）我们扩展了以前的工作EGO - Planner，提出了一种新的、鲁棒的拓扑规划方法，几乎不需要额外的计算。

（2）我们提出了分布式和异步的无人机集群框架，它对不可靠的通信和定位漂移不敏感。

（3）将该方法集成到一个全自动的四旋翼系统中，并发布了硬件和软件，供社区参考

2.相关工作

A.单个无人机的局部规划

        基于梯度的运动规划是四旋翼局部规划的主流。在将局部规划问题描述为无约束非线性优化的开创性工作[4,5]的基础上，提出了一系列工作[6]–[10]。他们使用各种参数化方法，包括多项式和B样条，考虑轨迹的平滑性、可行性和安全性。最近，我们提出了一个名为EGO_Planner[3]的单四旋翼导航系统，它使用更紧凑的环境表示法进一步减少了计算时间。这就是本文所基于的工作。

B.拓扑规划

        拓扑规划用于避开局部最小值。基于源自复分析 [11] 的二维表面中的homology等价关系，Rosmann 等人 [12] 提出了一种使用 Voronoi 图和基于采样的前端以及 TEB 本地规划器 [13] 作为后端的独特拓扑轨迹规划方法。然而，3-D 中的homology等价关系要简单得多。为了捕捉独特的有用路径，Jaillet 等人 [14] 构建可见性变形路图，其编码比homotogy类的代表性路径更丰富、更相关的信息。基于 [14],Zhou 等人 [15]通过提出有效的拓扑等效检查来实现实时拓扑规划。我们扩展了 EGO-Planner 以进一步加速拓扑规划的前端。

C.分布式无人机集群

        例如，[16]-[20] 中提出了去中心化方法。 利用速度障碍来保证质点机器人 [16]、完整agent [17] 和非完整agent [18] 的无碰撞轨迹。 刘等人[21] 提出了一种分散和异步的无人机规划策略，以避免静态/动态障碍物和机间碰撞。 虽然这些算法是通过仿真验证的，但没有集成传感、测绘、规划能力。实验结果已在 [19, 20] 中显示。  [19] 可以在没有外部障碍的情况下实现多机点对点过渡，并且 [20] 依赖于临时规划优先级。 然而，它们都没有在外场环境中实现完全自主。

3.基于梯度的局部规划隐式拓扑轨迹生成

        在这一部分中，我们首先介绍了我们之前在EGOPlanner[3]上的工作，提出的swarm系统就是基于这一工作。然后对提出的拓扑规划策略进行了说明。

A.无ESDF图的梯度局部规划器

        作为基于梯度的局部规划器，EGO-Planner 将轨迹生成公式化为一个非线性优化问题，该问题在平滑度 Js、碰撞 Jc、动态可行性 Jd 和终端进度 Jt 之间进行权衡。优化问题，决策变量来自用于参数化轨迹的均匀 B 样条 Φ 的控制点 Q ，由下式给出

其中 ,下标 λ 表示相应的权重。J项可分为两类：最小误差和软单边约束。最小误差项Js和Jt,将决策变量L  (Q) 的线性变换与期望值D之间的总误差最小化，由下式给出：

软单边约束项Jc和Jd, 惩罚超过特定阈值的决策变量，表示为如下。

其中参数S、n和影响 [13] 中所述的单侧约束近似精度。根据惩罚类型选择变换和参数。

        在 EGO-Planner 中，我们根据每个 Q 独立拥有的环境信息提出了一种新的障碍物距离估计方法。 由几个 {p, v} 对参数化的信息是从周围的障碍物中高度抽象出来的，其中 p 表示障碍物表面的锚点，v 表示从障碍物内部到外部的安全方向，如图 4a 所示。 则第 i 个控制点 Qi 到第 j 个障碍物的障碍物距离 dij 定义为

        {p, v} 对生成和轨迹优化过程如图 4a 和 4b 所示。 首先，给出一个简单的初始轨迹Φ，不管碰撞。 然后搜索连接Φ的碰撞段两端的安全路径Γ。之后，从到产生向量,以及p定义在障碍物表面。利用产生的对，规划器最大化并且返回优化后的轨迹。由于文章篇幅有限，这里我们只对EGO-Planner的基本思想做一个简单的描述。 详细解释见[3]。 

 

  图4:关于EGO-Planner和拓扑轨迹生成的阐述。(a)安全path已经搜索到，然后对已经生成。(b)优化器已找到一条安全轨迹，满足大于一个常值。(c)产生新的 对，其中新的v是原来v的负方向。(d)另一条不同的轨迹生成。

 （B）隐式拓扑轨迹生成

        [14, 15] 中的分析表明，广泛使用的homotopy概念不足以捕获 3D空间中的候选轨迹，如图 5 所示。 因此，Jaillet 等人[14] 在 3D 空间中提出了一种更有用的关系，称为可见性变形 (visibility deformation)，以及 Zhou 等人[15] 进一步提取了称为统一可见性变形 (UVD) 的 VD 子集，它可以实现实时操作。 然而，我们在本文中仍然使用术语拓扑规划，因为之前的工作没有歧义。 满足 UVD 的轨迹被认为是同胚的homeomorphic。  [15]中定义的UVD是：

定义1：两条轨迹τ1(s)，τ2(s)，

         传统的拓扑规划方法[12]-[15],]由拓扑不同的路径搜索和后端优化组成，主要关注于在不同的homotopy中找到多个初始路径。与这些方法不同，该方法通过将v反转为来构造不同方向的距离场。然后是搜索过程

4.飞机集群导航

A.机间避碰

        agent-k考虑其他agents的存在，如图6所示。不同于文献[22]，在这里我们忽略了障碍物和动力学限制（这部分已经在第三节A部分讨论过）。类似于避障和动力学可行性的惩罚函数，我们将第k个agent的集群机间避碰的惩罚函数定义为软单边约束，如下式：        

图6：自身无人机通过接收到的周围无人机在同一时刻的轨迹，进行对比来产生自身的轨迹。 

把权重项加到方程1中去，形成1个总优化问题：

         任何轨迹参数化方法包含一个从决策变量映射到轨迹上点的过程。本文用阶均匀B样条参数化轨迹，

B. 定位漂移补偿

        由于在未知环境中的个体定位（没有可靠的高频闭环），漂移在飞行期间累积。Xu等人 [23] 提出了一种具有额外UWB距离测量的无人机群状态估计方法，实现了精确的协同定位。然而，我们更关注于穿越障碍环境，并且必须为其他应用程序预留计算和通信资源。因此，在文献 [23] 的启发下，提出了一种简化且轻量的相对漂移估计方法，该方法通过比较从接收其他agent的轨迹评估的预测位置和从观测者的深度图像测量的位置来实现。当轨迹跟踪误差可忽略不计，并且任何两个可能发生碰撞的agent中至少有一个agent看到另一个agent时，此策略有效。因此，我们使用来自 [24] 的控制器进行精确跟踪，并使用广角相机来减少丢失agent的可能性。

        漂移消除程序如下。 在评估agent-i的当前位置后，确定以 为中心、半径为 R 的球形信任区域，其中 R 是一个经验参数，指示从实验估计的典型漂移的上限。然后将S映射到当前捕获的深度图像上，即满足的区域S  ⊂ R2    

其中s  ∈ S ，s ∈ s，K和Tc w是相机本征和外在矩阵，z是S沿主光轴与光学中心的偏差。S  是椭圆圆锥曲线，需要复杂的计算才能获得。因此，我们采用近似轴对齐的椭圆s ¯ 而不是精确的s 。没有必要精确定义信任区域，因为它只是一个经验区域。

        然后我们将 内的每个点投影到世界框架中，并收集属于 S 的点，从而得到一个点簇 P ⊂ S。然后将智能体观察 P 的位置视为 P 的中心（第一个原始矩）， 即

         如果 P 仅包含相应agent的观察而没有任何不相关的对象，则等式 9 成立，这是不能保证的。然而，由于每个智能体都规划了与附近物体有间隙的轨迹，因此 Equ.9 大部分时间都成立。添加了其他标准以提高agent检测的鲁棒性，例如像素数、P 的第二个中心矩、当前测量值与先前测量值的偏差等。更严格的标准会增加假阴性率，但由于定位是无害的漂移变化缓慢。最后，将和 P 之间的误差馈送到滤波器，然后从中获取估计的漂移。

C.agent从深度图中移动

        我们使用占用网格图来存储静态障碍物，并使用深度图像进行地图融合。移动agent在第四节-A 中得到处理。因此，记录移动的agents并把它们在建图过程中视为障碍物是不必要的，甚至是有害的。为了消除移动物体的影响，我们从深度图像中屏蔽并去除在第四节-B中检测到的agent的像素，如图 7 所示。除此之外，覆盖大部分视野的移动物体会对 VIO 造成干扰。因此，灰度图像上的agent也被移除，对相应的深度图像使用相同的掩码。这里使用的agent检测标准不那么严格，因为错误的正值比错误的负值更有害。

图7：左：VIO漂移被估计在从接收轨迹评估的预测位置和信任区域内的深度图像中的观察位置之间。右：将观察到的agent掩蔽以避免影响建图。

5.系统架构

        系统架构如图 8 所示，其中包含单个agent和多agent通信系统的详细架构。

A.单agent导航系统

        单个agent系统，包括硬件和软件设置，基于我们之前的工作 EGO-Planner [3]，带有一个额外的模块，可以补偿 VIO 漂移并移除图像上的被看到的agent。 所有组件都集成到一个自组装的 250 毫米轴距四旋翼飞机中。 对于未知环境中的轨迹生成，使用局部地规划器。 当当前轨迹与新发现的障碍物发生碰撞，或者agent接近当前轨迹的末端时，就会激活规划。 

B.通信框架

        两个网络用于连接系统，一个是广播网络用于共享轨迹，另一个链网络，用于同步时间戳和管理启动序列。

1）广播网络： 一旦一个agent生成一个新的无碰撞轨迹，它就会立即广播给所有agent。然后其他agent接收并存储此轨迹，用于在必要时为自己生成安全轨迹。 这种闭环策略在连接稳定且延迟可忽略不计的理想情况下正常工作。 但是，这在实践中并不能保证。 因此，我们提出了两种方法来减少碰撞的可能性。

       首先，在网络负载下以给定频率广播一个轨迹。这不会造成计算负担，因为包含三维航路点和其他参数的典型轨迹的大小小于0.5kb。相比之下，Bluetooth等现代无线网络可以达到1 Mbps以上的速度。其次，每个agent在从广播网络接收到轨迹时立即检查碰撞，并且如果检测到潜在碰撞，则生成新的无冲突轨迹。该策略可以解决，当多个agent在非常接近的时刻生成轨迹时由于延迟或数据包丢失没接收到其他agent的轨迹。此外，还考虑了计算复杂度随agent数量的增加而增加的问题。在规划之前，每个agent将其当前位置与接收到的周围agent的轨迹进行比较，其中任何超出规划范围的轨迹都将被忽略。

2)链网络：连接稳定的链式网络用于时间戳同步和系统启动管理。在系统启动时,agents以一个预定义的顺序生成轨迹。每个agent生成自身最初的轨迹在收到通过更高的优先级的链式网络发来的其他agents的轨迹之后。这种策略避免了在系统启动时由于同步轨迹生成造成的混乱，因为此时agent没有来自其他agent的轨迹信息。

6.benchmark

我们在英特尔酷睿 i7-9700KF CPU 上进行了关于拓扑规划、在空阔和杂乱空间中的集群规划的广泛基准比较。

A.拓扑规划

        我们在候选轨迹数和前端拓扑路径搜索的计算时间两方面比较了所提出的 EGO-Swarm 与 Fast-Planner [15] 的拓扑规划性能。 如图 9 所示，EGOSwarm 找到的候选轨迹较少，这意味着找到全局最优的概率较低，但比 [15] 快两个数量级。 由于 Fast-Planner 通过 PRM [25] 图搜索、路径缩短和路径修剪找到拓扑不同的路径，与所提出的隐式拓扑路径搜索方法相比，这些方法耗时但具有更高的自由度。

图9： 拓扑路径搜索的比较。与 Fast-Planner 相比，本文提出的规划器发现的候选拓扑不同的轨迹更少，但消耗的计算更少。请注意，显示的时间仅用于前端局部最小值发现，而显示的轨迹经过优化以获得更好的可视化。

B.集群规划

1)在空旷空间中：我们将所提出的方法与DMPC  [19] 、ORCA  [16] 和RBP  [20] 进行了比较，从飞行距离、飞行时间、每个agent的碰撞次数和计算时间几个方面进行了比较。除最大速度和加速度外，每种比较方法都使用默认参数。如图10所示，八个agent在一个圆上执行交换变换。表I中的结果是所有agent的平均值。tcal用“*”标记，因为我们记录的计算时间是规划所有agent的整个轨迹的总时间(对DMPC和RBP而言)，而对于ORCA和EGO -Swarm，它是每个agent的局部重新规划时间。

         表I 和图10 表明 RBP 倾向于生成安全但保守的轨迹，因为构建凸的相对安全飞行走廊 [20] 显著压缩了解空间。  DMPC 专为分布式部署而设计。 然而，它需要准确和高频的姿态通信，这在现实应用中无法保证。 高效的规则使 ORCA 更新更快。 但是，使用速度作为控制命令使其与四旋翼等三阶系统不兼容。 碰撞的风险也限制了它的应用。 相比之下，所提出的方法生成最短的无碰撞、非保守轨迹，计算速度快。 因此，它可以实现四旋翼的实时应用。

2）在充满障碍物的空间中：我们仿真了从地图一侧到另一侧的10架无人机，速度限值为2米/秒，四旋翼半径为0.2米。图2是0.42个障碍物每平方米的仿真环境。每个Agent独立感知环境，构建的局部地图以不同的颜色显示。结果总结在表II中，其中是平均飞行距离，是飞行试验期间最接近障碍物的距离。设计了一种逆点对点转换，使围绕地图中心的机间碰撞避免项不可避免。在这种情况下，属于群的每个agent都可以规划平滑和安全的轨迹。

3）可测性分析：我们在以直线排列的agents飞到 50 米外的随机目标点的情况下评估计算性能。如图 11 所示，由于第五节-B1中的按需碰撞检查策略，时间复杂度随着agent数量的增加而逐渐趋于平稳。

7.真机实验

A.室内

        我们在 1.5m/s 的速度限制下展示了几个室内实验，如图 12 所示。 路径的颜色表示每个四旋翼的大致飞行时间。 顶部显示了三个四旋翼在空阔空间中执行循环交换，因此需要相互避免碰撞。 在中间，四旋翼飞机在飞行过程中避开障碍物，一个接一个地通过狭窄的门。 在下图中，我们设置了一个由一些垂直和水平障碍物组成的更杂乱的环境。 三个四旋翼飞机设法在这个环境中导航。

 B.室外

        如图1所示，在树木相隔约2米的森林中，三个四旋翼开始在森林中，并设法到达外部森林的目标位置。速度限制设置为 1.5m/s。 为了进一步强调相互避碰，我们颠倒了目标位置相对于起始位置的顺序，使得相互回避不可避免，就像我们在第六节-B2中所做的那样。 有关实验的更多信息，请参阅代码并在 github上观看我们附加的视频。

8.总结和未来工作

        在本文中，提出了一种仅使用机载资源在未知杂乱环境中进行多机器人导航的系统解决方案。 基准比较表明其计算时间短，轨迹质量高。 真实世界的实验验证了它的稳健性和效率。 未来，我们将致力于实现精准共定位和现实世界动态避障。 主要挑战在于对其他无人机的不精确观察以及不可靠的运动物体检测和预测。 最后，我们希望为机器人社区提供一个完整的四旋翼群在复杂环境中的自主导航解决方案。