EGO Swarm翻译

摘要

Ⅰ 介绍

Ⅱ 相关工作

A . 单四旋翼局部规划

B . 拓扑规划

C. 分布式无人机集群

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

A.无需ESDF梯度的局部路径规划

B.隐式拓扑轨迹生成

Ⅳ 无人机集群导航

A 机间避碰

B. 定位漂移补偿

C. 从深度图像中去除agent

Ⅴ 系统结构

A . 单个agent的导航系统

B .通信框架

Ⅵ 基准

A. 拓扑规划

B. 集群规划

1. 空旷环境中。

2. 在障碍物丰富的环境中。

3. 可扩展性分析

摘要

本文提出了一种分布式异步系统解决方案，用于多机器人在未知障碍物丰富的场景中仅利用机载资源进行自主导航。

在基于梯度的局部规划框架下建立规划系统，通过将碰撞风险制定为非线性优化问题的惩罚来实现避碰。

为了提高鲁棒性和避免局部极小值，结合了一个轻量级拓扑轨迹生成方法。然后，智能体使用不可靠的轨迹共享网络能在几毫秒内生成安全、平滑且动态可行的轨迹。

通过使用深度图像中的智能体检测校正智能体之间的相对定位漂移。通过仿真和实际实验验证了该方法的有效性。

注：agent为智能体

Ⅰ 介绍

四旋翼的灵活(敏捷性)使该机器能够在未知环境中执行单智能体自主导航，并在开放或已知领域中执行多智能体精确编队控制。

(针对的问题)然而，很少有工作将两者结合起来，以呈现任何能够导航共享相同未知空间的四旋翼机群的真实系统，尤其是仅使用机载处理的系统。在未知环境中部署多个四旋翼机的困难包括但不限于障碍物参数化的重要性、有限的传感范围、不可靠且带宽有限的通信以及定位不一致导致的定位漂移。

一些相关的工作，例如[1，2]，将现实世界中的四旋翼群推得更远。然而，在以往的大多数工作中，上述困难在运动捕捉系统或纯仿真中总是被忽视，限制了其算法在实际中的应用。

本文提出了一种系统的解决方案，在上述困难存在的情况下，使四旋翼无人机群能够在杂乱的环境中进行高性能探索。此外，它不需要外部定位和计算，也不需要预先构建地图。该系统名为EGO Swarm，是我们之前工作的扩展，基于无ESDF梯度的局部规划器（EGO planner）[3]，它为未知环境中单个四旋翼的机载局部规划奠定了坚实的基础。

这个扩展由拓扑规划和交互(机间)避碰两部分组成。

如图3所示，非凸配置空间可能导致不期望的行为，例如动力学不可行性或拥挤导航。

因此，一种避免局部极小的策略，如拓扑规划，是有益的。

基于EGO Planner中的碰撞代价公式，前端拓扑路径搜索是隐式进行的，因此几乎不需要计算。通过在目标函数中加入群碰撞的加权惩罚，实现了分布式机间避碰。通过比较未来某一时刻的agent分布和正在优化的轨迹来评估此惩罚。为了尽量减少数据传输并允许不可靠的通信，使用广播网络共享轨迹。

为了纠正相对定位漂移，可以将其增加精度到半米，我们比较了目标agent的观测结果和轨迹评估的预测结果。

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

我们扩展了先前的工作EGO Planner提出一种几乎不增加额外计算量的新型鲁棒拓扑规划方法。
我们提出了一个分布式和异步四旋翼集群框架，它对不可靠的通信和定位漂移不敏感。
所提出的方法被集成到一个完全自主的四旋翼系统中，并发布了硬件和软件供社区参考1。

Ⅱ 相关工作

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]依赖于临时规划优先级。然而，它们都没有在外场环境中实现完全自主。

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

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

A.无需ESDF梯度的局部路径规划

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

其中r = {s，c，d，t}，下标λ表示相应的权重。J项可以分为两类:最小误差和软单边约束。最小误差项Js和Jt使决策变量L(Q)的线性变换和期望值D之间的总误差最小化，公式如下：

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

其中参数S、n和ε,影响单边约束近似精度，如[13]所述。根据惩罚类型选择变换L(.)和参数。由于论文篇幅有限，这里省略了L(.)的确切形式，可以在[3]中找到。

在EGO-Planner中，我们提出了一种新的障碍物距离估计方法，该方法利用了每个Q独立拥有的环境信息。由几个{p,v}对参数化的信息是从周围障碍物中高度抽象的，其中p表示障碍物表面的锚点，v表示从该障碍物内部指向外部的安全方向，如图4a所示:

然后，第i个控制点Qi到第j个障碍物的障碍物距离dij定义为：

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

B.隐式拓扑轨迹生成

[14, 15] 中的分析表明，广泛使用的homotopy概念不足以捕获3D空间中的候选轨迹，如图 5 所示。

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

定义1:由s ∈ [0，1]参数化且满足τ1(0) = τ2(0)，τ1(1) = τ2(1)的两个轨迹τ1(s)，τ2(s)，属于同一个UVD类，若对所有s，直线τ1(s)τ2(s)无碰撞。

传统的拓扑规划方法[12]-[15]由拓扑上不同的路径搜索和后端优化组成，主要关注于在不同的homotopy中找到多个初始路径。与这些方法不同，所提出的方法通过将v反转为vnew:= v，来构建不同方向的距离场。然后，搜索过程在障碍物表面单独确定新的锚点pnew，如图4c所示。它们构成新的一对{pnew，vnew}，这导致不同的局部最小值。注意，没有采用显式路径搜索，但是分别通过p和pnew的任何一对路径自然在这两点上违反Def.1。随后，在不同的线程中并行优化不同的轨迹，如图4d所示。执行具有最低成本的轨迹。

Ⅳ 无人机集群导航

A 机间避碰

gent-k考虑其他agents的存在，如图6所示。

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

其中，(t那些)轨迹时间跨度内的全局开始和结束时间。c是用户定义的agent许可。E := diag(1，1，1/c)，c > 1将欧氏距离转换为z轴上主轴较短的椭球距离，以缓解向下运动的风险。将加权的Jw，k加到等式上。(1)产生每个agent的总优化问题：

何轨迹参数化方法包含一个从决策变量映射到轨迹上点的过程(5).。本文使用pb阶均匀B样条将轨迹参数化，这为位置评估提供了矩阵表示[9]

这里Mpb+1是一个常数矩阵，当t属于节点跨度(tm，tm+1]时，由pb，s(t)=(t-tm)/Δt确定。

B. 定位漂移补偿

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

漂移消除程序如下。在评估agent-i的当前位置φi(tnow)之后，确定以φi(tnow)为中心、半径为r的球形信任区域S ⊂ R3，其中r是指示根据实验估计的典型漂移的上限的经验参数。

然后在S’⊂ R2满足的区域，s被映射到当前捕捉的深度图像。

其中s’∈ S’，s ∈ S，K和Tcw是相机的内参和外参矩阵，z是S沿主光轴与光学中心的偏差。S’是需要复杂计算才能获得的椭圆圆锥曲线。因此，我们采用近似的轴对齐椭圆S’，而不是精确的S’。没有必要精确地定义信任区域，因为它只是一个经验域。然后我们将S’内的每个点投影到世界框架中，收集属于S的点，得到一个点簇P ⊂ S .然后将agent观测p的位置作为P的中心(第一个原始矩)，即：

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

C. 从深度图像中去除agent

我们使用占用栅格地图存储静态障碍物，使用深度图像进行地图融合。移动agent在第四节-A 得到处理，因此，在地图构建中记录移动agent并将其视为障碍是不必要的，甚至是有害的。为了消除运动物体的影响，我们从深度图像中屏蔽并去除在第四节-B中检测到的agent的像素，如图 7 所示。

除了覆盖大部分视图的移动物体对VIO是干扰之外。因此，对于相应的深度图像，也使用相同的遮罩来去除灰度图像上的agent。此处使用的病原体检测标准不太严格，因为假阳性比假阴性更有害。

为了消除移动物体的影响，我们从深度图像中屏蔽并去除在第四节-B中检测到的agent的像素，如图 7 所示。除此之外，覆盖大部分视野的移动物体会对 VIO 造成干扰。因此，灰度图像上的agent也被移除，对相应的深度图像使用相同的掩码。这里使用的agent检测标准不那么严格，因为错误的正值比错误的负值更有害。

Ⅴ 系统结构

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

A . 单个agent的导航系统

单个agent系统，包括硬件和软件设置，是基于我们以前的工作EGO-Planner [3]，带有一个额外的模块来补偿VIO漂移和去除图像上的被看到的agent。对于未知环境中的轨迹生成，使用局部规划器。当当前轨迹与新发现的障碍物发生碰撞，或者agent接近当前轨迹的末端时，规划被激活。

B .通信框架

两个网络连接系统，即共享轨迹的广播网络和同步时间戳并管理顺序启动的链式网络。1)广播网络:一旦agent生成新的无碰撞轨迹，就立即广播给所有agent。然后，其他agent接收并存储该轨迹，用于在必要时为自己生成安全轨迹。这种闭环策略在连接稳定且延迟可以忽略的理想情况下工作正常。然而，这在实践中并不能得到保证。因此，我们提出两种方法来减少碰撞的可能性。

首先，在网络负载下以给定频率广播一个轨迹。这不会导致计算负担，因为包含三维航迹点和其他参数的典型轨迹的大小小于0.5KB。Bluetooth等现代无线网络可以达到1 Mbps以上的速度。第二，一旦从广播网络接收到轨迹，每个agent就检查碰撞，如果检测到潜在的碰撞，则生成新的无碰撞轨迹。这种策略可以处理当多个agent在接近的时间生成轨迹，而由于延迟或分组丢失而没有接收到其他agent的轨迹的情况。

此外，还考虑了计算复杂度随着agent数量的增加而增长的情况。在规划之前，每个agent将其当前位置与接收到的周围agent的轨迹进行比较，其中任何超出规划范围的轨迹都将被忽略。

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

Ⅵ 基准

基准比较是在使用i7-9700KF CPU的模拟中进行的。参数设置，规划视野设为7.5m，λs = 1.0，λc = λw = λt = 0.5，λd = 0.1。地图分辨率为0.1米。每秒触发一次重新规划，否则会发生碰撞。该设置在模拟实验和真实实验中都可以实现。

A. 拓扑规划

我们比较了提出的EGO-Swarm与Fast-Planner [15]在候选轨迹数和前端拓扑路径搜索的计算时间方面的拓扑规划性能。如图9所示。

EGOSwarm找到的候选轨迹更少，这意味着找到全局最优的概率更低，但比[15]快两个数量级。因为Fast-Planner通过PRM [25]图搜索、路径缩短和路径修剪来找到拓扑上不同的路径，与所提出的隐式拓扑路径搜索方法相比，这是耗时的，但是具有更高的自由度。

B. 集群规划

1. 空旷环境中。

我们在飞行距离(dfly)、飞行时间(tfly)、每个agent的碰撞次数和计算时间(tcal)方面将所提出的方法与DMPC [19]、奥卡[16]和RBP [20]进行比较。除了最大速度和加速度之外，每个比较方法都使用默认参数。如图10所示

八个agent在一个圆上执行交换转换。表I中的结果是所有agent的平均值。

tcal用“*”标记，因为我们记录的计算时间是规划所有agent的整个轨迹的总时间(对DMPC和RBP而言)，而对于ORCA和EGO -Swarm，它是每个agent的局部重新规划时间。

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