EGO-Swarm原文翻译

admin管理员组

文章数量:1597475

EGO-Swarm: A Fully Autonomous and Decentralized Quadrotor Swarm System in Cluttered Environments

Abstract

This paper presents a decentralized and asynchronous systematic solution for multi-robot autonomous navigation in unknown obstacle-rich scenes using merely onboard resources. The planning system is formulated under gradient-based local planning framework, where collision avoidance is achieved by formulating the collision risk as a penalty of a nonlinear optimization problem. In order to improve robustness and escape local minima, we incorporate a lightweight topological trajectory generation method. Then agents generate safe,smooth, and dynamically feasible trajectories in only several milliseconds using an unreliable trajectory sharing network.Relative localization drift among agents is corrected by using agent detection in depth images. Our method is demonstrated in both simulation and real-world experiments. The source code is released for the reference of the community.

该论文提出了一种针对未知障碍物丰富场景下的多机器人自主导航问题的分散化和异步化的系统性解决方案，仅仅利用机载资源。规划系统采用基于梯度的局部规划框架，通过将碰撞风险形式化为非线性优化问题的惩罚项来实现碰撞避免。为了提高鲁棒性和避免陷入局部最优解，我们采用了一种轻量级的拓扑轨迹生成方法。然后，智能体利用一个不可靠的轨迹共享网络仅需几毫秒的时间生成安全、平滑和动态可行的轨迹。智能体通过使用深度图像中的智能体检测来纠正智能体之间的相对定位漂移。我们的方法在仿真和实际实验中得到了验证。源代码已经向社区发布，供参考使用。

I. INTRODUCTION

The agility of quadrotors enables this machine to perform single-agent autonomous navigation in unknown environments and multi-agent precise formation control in open or known fields.

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

However, few works combine both of them to present any real-world system which is capable of navigating quadrotor swarms sharing the same unknown space,especially with only onboard processing.

然而，很少有作品将两者结合起来，呈现出任何能够在共享同一未知空间的四旋翼机群中导航的真实世界系统，尤其是只有机载处理的系统。

The difficulties in deploying multiple quadrotors in unknown environments include but not limit to nontriviality of obstacle parameterization, limited sensing range, unreliable and bandwidth limited communication, and positioning drift caused by inconsistent localization.

在未知环境中部署多个四旋翼机的困难包括但不限于障碍物参数化的不平凡性、有限的传感范围、不可靠和带宽有限的通信，以及定位不一致导致的定位漂移。

Several related works, e.g. [1, 2], push the realworld quadrotor swarms capable of obstacle avoidance much further.

一些相关工作，例如[1，2]，进一步推动了现实世界中能够避障的四旋翼机群。

However, above-mentioned difficulties are always neglected under motion capture systems or in pure simulation for most of the previous works, limiting the application of their algorithms in practice.

然而，在以往的大多数工作中，在运动捕捉系统或纯仿真中，上述困难总是被忽略，限制了其算法在实践中的应用。

This paper proposes a systematic solution that enables high-performance traveling in cluttered environments for quadrotor swarms in the presence of these above-mentioned difficulties.

本文提出了一种系统的解决方案，在存在上述困难的情况下，使四旋翼机群能够在杂乱的环境中进行高性能飞行

In addition, it requires no external localization and computation or a pre-built map. The proposed system named EGO-Swarm is an extension of our previous work ESDF-free Gradient-based local planner (EGO-Planner) [3],
which lays a solid baseline for onboard local planning of a single quadrotor in unknown environments.

此外，它不需要外部定位和计算，也不需要预先构建的地图。所提出的名为 EGO Swarm 的系统是我们之前工作的扩展， ESDF-free Gradient-based local planner（EGO planner）[3]，它为未知环境中单个四旋翼机的机载局部规划奠定了坚实的基础。

The expansion consists of two parts, topological planning, and reciprocal
collision avoidance.

扩展由拓扑规划和互惠的避障构成。

As depicted in Fig.3, non-convex configuration space may lead to undesired behaviors, such as dynamical infeasibility or crowded navigation.

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

Therefore,a strategy such as topological planning to escape local minima is beneficial.

因此，诸如拓扑规划之类的策略来逃避局部极小值是有益的。

Based on the collision cost formulation in EGO-Planner, the front-end topological path search is done implicitly, therefore requiring nearly no computation

基于 EGO Planner 中的碰撞代价公式，前端拓扑路径搜索是隐式完成的，因此几乎不需要计算

Decentralized reciprocal collision avoidance is achieved by adding a weighted penalty of swarm collision to the objective function.

通过在目标函数中加入群体碰撞的加权惩罚，实现了分散的相互碰撞避免。

This penalty is evaluated by comparing the agent distribution at a pried of future time with the being optimized trajectory.

这种惩罚是通过将未来某一时刻的智能体分布与正在优化的轨迹进行比较来评估的。

In order to minimize data transmission and allow unreliable communication, a broadcast network is used to share trajectories

为了最大限度地减少数据传输并允许不可靠的通信，使用广播网络来共享轨迹

To correct relative localization drift, which can increase to half a meter, observations of witnessed agents and predictions from trajectory evaluations are compared.

为了校正可能增加到半米的相对定位漂移，比较了目击智能体的观测结果和轨迹评估的预测结果。

Real-world experiments are presented to validate our proposed swarm system. To the best of our knowledge, this is the first systematic solution for fully autonomous decentralized quadrotor swarms in unknown cluttered environments, which means perception, planning, and control are integrated into the onboard system.

给出了真实世界的实验来验证我们提出的群体系统。据我们所知，这是第一个在未知杂乱环境中实现完全自主分散四旋翼机群的系统解决方案，这意味着感知、规划和控制集成到机载系统中。

Comparisons with several SOTA methods show the computation efficiency and robustness. The contributions of this paper are summarized as follows:

与几种 SOTA 方法的比较表明了计算的有效性和鲁棒性。本文的贡献总结如下：

• We extend our previous work EGO-Planner to propose a novel and robust topological planning method with nearly no extra computation.

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

• We propose a decentralized and asynchronous quadrotor swarm framework which is insensitive to unreliable communication and localization drift.
  我们提出了一种分散的异步无人机群框架，该框架对不可靠的通信和定位漂移不敏感。

• The proposed method is integrated into a fully autonomous quadrotor system with hardware and software released for the reference of the community1.

  将所提出的方法集成到一个完全自主的四旋翼系统中，并发布硬件和软件供社区参考 。

II. RELATED WORKS

A. Single Quadrotor Local Planning

Gradient-based motion planning is the mainstream for quadrotor local planning. Based on the pioneering works [4, 5] that formulate the local planning problem as unconstrained nonlinear optimizations, a series of works [6]–[10] are proposed.

基于梯度的运动规划是四旋翼局部规划的主流。基于将局部规划问题表述为无约束非线性优化的开创性工作[4，5]，提出了一系列工作[6]–[10]。

They consider smoothness, feasibility, and safety of the trajectory using various parameterization methods, including polynomial and B-spline.

他们使用各种参数化方法，包括多项式和 B 样条，来考虑轨迹的平滑性、可行性和安全性。

Recently, we propose a single-quadrotor navigation system named EGO-
Planner [3], which further reduces computation time using a more compact environment representation. This is the work on which this paper is based.

最近，我们提出了一种名为 EGO-Planner 的单四旋翼导航系统[3]，它使用更紧凑的环境表示进一步减少了计算时间。这就是本文所基于的工作。

B.Topological planning

Topological planning is used to escape local minima.Based on the homology equivalence relation in 2-D surfaces originated from complex analysis[11], Rosmann et al.[12] present a trajectory planning method in distinctive
topologies using Voronoi and sampling-based front-ends and TEB (Timed-Elastic-Bands) local planner [13] as back-ends.

拓扑规划是用来逃避局部极小的。基于复分析[11]中二维曲面上的同源等价关系，Rosmann 等人。[12] 提出了一种与众不同的拓扑结构轨迹规划方法使用 Voronoi 和基于采样的前端以及 TEB（定时弹性带）局部规划器[13]作为后端。

However, homology equivalence relation in 3-D is far more trivial.To capture distinctive useful paths, Jaillet et al. [14]construct visibility deformation roadmaps that encode richer and more relevant information than representative paths of the homotopy classes.

然而，三维中的同源等价关系要琐碎得多。为了捕捉独特的有用路径，Jaillet 等人[14]构建了可见性变形路线图，该路线图编码的信息比同源类的代表路径更丰富、更相关。

Based on [14], Zhou et al. [15] enable real-time topological planning by proposing an efficient topology equivalence checking. We extend EGO-Planner to accelerate the front-end for topological planning further.

在[14]的基础上，周等人[15]通过提出一种有效的拓扑等价性检查，实现了实时拓扑规划。我们扩展了 EGO Planner，以进一步加速拓扑规划的前端。

C. Decentralized Drone Swarm(去中心化无人机群)

Decentralized approaches have been proposed in, e.g.,[16]–[20]. Velocity obstacles is leveraged to guarantee collision-free trajectories for point robots [16], holonomic agents [17], and non-holonomic agents [18].

[16]-[20]中提出了分散方法。利用速度障碍来保证点机器人[16]、完整智能体[17]和非完整智能体[18]的无碰撞轨迹。

Liu et al.[21] propose a decentralized and asynchronous planning
strategy for drones to avoid static/dynamic obstacles and inter-vehicle collisions.

刘等人[21]提出了一种分散的异步规划无人机避静态/动态障碍物和车辆碰撞的策略。

While those algorithms are validated through simulations without integrating sensing, mapping, and planning capabilities

而这些算法是在没有集成传感、测绘和规划能力的情况下通过仿真进行验证的

Experimental results have been shown in [19, 20]. [19] enables multi-vehicle point-to-point transitions without external obstacles, and [20] relies on adhoc planning priority. However, none of them achieve full autonomy in field environments.

实验结果如[19，20]所示。[19] 能够在没有外部障碍的情况下实现多车辆点对点规划，并且[20]依赖于特定规划优先级。然而，它们中没有一个在实地环境中实现完全自主。

III. IMPLICIT TOPOLOGICAL TRAJECTORY GENERATION OF GRADIENT-BASED LOCAL PLANNING

In this section, we introduce our previous work on EGO-Planner [3] at first, on which the proposed swarm system is based. Then the proposed topological planning strategy is explained.

在本节中，我们首先介绍了我们之前在 EGO Planner[3]上的工作，所提出的群系统是基于该工作的。然后对所提出的拓扑规划策略进行了说明。

A. An ESDF-Free Gradient-based local planner

As a gradient-based local planner, EGO-Planner formulates trajectory generation as a non-linear optimization problem that trades off smoothness J s J_s Js​, collision J c J_c Jc​, dynamically feasibility J d J_d Jd​, and terminal progress J t J_t Jt​. The optimization problem, with decision variables from control points Q of a uniform B-spline Φ used to parameterize the trajectory, is given by

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

min ⁡ Q = J E G O = Σ λ r J r \begin{equation} \min_{Q} = J_{EGO} = \Sigma \lambda_r J_r \end{equation} Qmin​=JEGO​=Σλr​Jr​​​

where r = { s , c , d , t } r = \{s,c,d,t\} r={s,c,d,t} and the subscripted λ \lambda λ indicates the
corresponding weights. Terms of J J J can be divided into
two categories: minimum error and soft barrier constraint.
Minimum error terms J s J_s Js​ and J t J_t Jt​, which minimize the total
error between a linear transformation of decision variables
L ( Q ) L(Q) L(Q) and a desired value D D D, are formulated as

其中 r = { s ， c ， d ， t } r=\{s，c，d，t\} r={s，c，d，t}，下标的 λ \lambda λ表示相应的权重。 J J J的条款可分为两类：最小误差和软屏障约束。最小误差项 J s J_s Js​和 J t J_t Jt​，使总数最小化决策变量线性变换之间的误差 L （ Q ） L（Q） L（Q）和期望值 D D D被公式化为

J r = ∑ Q ∈ Φ ∣ ∣ L ( Q ) − D ∣ ∣ n n \begin{equation} J_r = \sum_ {Q \in \Phi} ||L(Q) - \mathcal{D}||^n_n \end{equation} Jr​=Q∈Φ∑​∣∣L(Q)−D∣∣nn​​​

Soft barrier constraint terms J c J_c Jc​ and J d J_d Jd​, which penalize decision variables exceeding a specific threshold T \mathcal{T} T, are generally expressed as

软屏障约束项 J c J_c Jc​和 J d J_d Jd​惩罚超过特定阈值 T \mathcal{T} T的决策变量，通常表示为

J r = ∑ Q ∈ Φ { ∣ ∣ L ( Q ) − ( T − ϵ ) S ∣ ∣ L ( Q ) > ( T − ϵ ) 0 L ( Q ) ≤ ( T − ϵ ) \begin{equation} J_r = \sum_{Q \in \Phi} \begin{cases} &||\frac{L(Q) - (\mathcal{T} - \epsilon)}{S}|| \qquad & L(Q) > (\mathcal{T} - \epsilon)\\ &0 \qquad & L(Q) \leq (\mathcal{T} - \epsilon) \end{cases} \end{equation} Jr​=Q∈Φ∑​{​∣∣SL(Q)−(T−ϵ)​∣∣0​L(Q)>(T−ϵ)L(Q)≤(T−ϵ)​​​

where parameters S S S, n n n, and ϵ \epsilon ϵ affect unilateral constraint approximation accuracy as described in [13]. Transformation L ( ⋅ ) L(·) L(⋅) and parameters are chosen according to penalty types. The exact form of $L(·) $is omitted here due to limited paper length and can be found in [3].

其中参数 S S S、 n n n和 ϵ \epsilon ϵ影响[13]中所述的单边约束近似精度。转换 L ( ⋅ ) L(·) L(⋅)和参数是根据惩罚类型选择的。由于文章长度有限，这里省略了$L(·) $的确切形式，可以在[3]中找到。

In EGO-Planner, we propose a novel obstacle distance estimation method from environment information possessed by each Q Q Q independently. The information parameterized by several p , v {p,v} p,v pairs is highly abstracted from surrounding obstacles, where p p p denotes an anchor point at the obstacle surface and v v v represents a safe direction pointing from inside to outside of that obstacle, as illustrated in Fig.4a.

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

Then the obstacle distance d i j d_{ij} dij​ of i t h i^{th} ith control point Q i Q_i Qi​ to j t h j^{th} jth obstacle is defined as

d i j = ( Q i − p i j ) ⋅ v i j \begin{equation} d_{ij} = (Q_i - p_{ij})\cdot v_{ij} \end{equation} dij​=(Qi​−pij​)⋅vij​​​

p , v {p,v} p,v pair generation and trajectory optimization procedure are shown in Fig.4a and 4b. First, a naive initial trajectory Φ \Phi Φ is given, regardless of collision. Then a safe path Γ \Gamma Γ connecting two ends of the colliding segment of Φ \Phi Φ is searched.

然后 p ， v {p，v} p，v对的生成和轨迹优化过程如图4a和4b所示。首先，给出了一个天真的初始轨迹 Φ \Phi Φ，而不考虑碰撞。然后搜索连接 Φ \Phi Φ碰撞段两端的安全路径 Γ \Gamma Γ。

After that, vector v v v is generated from Φ \Phi Φ to Γ \Gamma Γ and p p p is defined at the obstacle surface. With generated p , v {p,v} p,v pairs, the planner maximizes d i j d_{ij} dij​ and returns an optimized trajectory

之后，从 Φ \Phi Φ到 Γ \Gamma Γ生成向量 v v v，并在障碍物表面定义 p p p。使用生成的 p ， v {p，v} p，v对，规划器最大化 d i j d_{ij} dij​并返回优化的轨迹

Here we only give a simplified description of the basic idea of EGO-Planner due to the limited article length. Detailed explanations can be found in [3].

由于文章篇幅有限，这里我们只简单介绍一下EGO Planner的基本思想。详细解释见[3]。

B. Implicit Topological Trajectory Generation

Analysis in [14, 15] reveals that the widely used homotopy concept is insufficient to capture candidate trajectories in 3-D cases, as shown in Fig.5. Therefore, Jaillet et al. [14]propose a more useful relation in 3-D space named visibility deformation (VD), and Zhou et al. [15] further extract a
subset of VD called uniform visibility deformation (UVD),which enables real-time operation. However, we still use the term topological planning in this paper as previous works do without ambiguity.

[14，15]中的分析表明，广泛使用的仿射概念不足以在三维情况下捕捉候选轨迹，如图5所示。因此，Jaillet等人[14]在三维空间中提出了一种更有用的关系，称为可见性变形（VD），Zhou等人[15]进一步提取了VD的子集称为均匀可视变形（UVD），这使得能够进行实时操作。然而，我们在本文中仍然像以前的工作一样使用拓扑规划这个术语，没有歧义。

Trajectories satisfying UVD are considered homeomorphic. UVD defined in [15] is

满足UVD的轨迹被认为是同胚的。[15]中定义的UVD是

Definition 1: Two trajectories τ 1 ( s ) \tau_1(s) τ1​(s), τ 2 ( s ) \tau_2(s) τ2​(s), parameterized by s ∈ [ 0 , 1 ] s \in [0,1] s∈[0,1] and satisfying τ 1 ( 0 ) = τ 2 ( 0 ) \tau_1(0) = \tau_2(0) τ1​(0)=τ2​(0), τ 1 ( 1 ) = τ 2 ( 1 ) \tau_1(1) = \tau_2(1) τ1​(1)=τ2​(1), belong to the same UVD class, if for all s s s, line τ 1 ( s ) τ 2 ( s ) τ1(s)τ2(s) τ1(s)τ2(s)is collision-free.

定义1：两个轨迹 τ 1 ( s ) \tau_1(s) τ1​(s)， τ 2 ( s ) \tau_2(s) τ2​(s)参数化为 s ∈ [ 0 , 1 ] s\in[0,1] s∈[0,1]并且满足 τ 1 ( 0 ) = τ 2 ( 0 ) \tau_1(0)=\tau_2(0) τ1​(0)=τ2​(0)， t a u 1 ( 1 ) = τ 2 ( 1 ) _tau_1(1)=\tau_2(1) t​au1​(1)=τ2​(1)，属于同一UVD类，如果对于所有的 s s s，线 τ 1 ( s ) τ 2 ( s ) τ1(s)τ2(s) τ1(s)τ2(s)是无碰撞的。

Traditional topological planning methods [12]–[15], which
consist of topologically distinct path search and back-end
optimization, focus mainly on finding multiple initial paths in distinct homotopy.

传统拓扑规划方法[12]-[15]，其中由拓扑上不同的路径搜索和后端组成优化，主要集中于在不同的同伦论中寻找多个初始路径。

Unlike those methods, the proposed method constructs distance fields in different directions by inverting v v v to v n e w : = − v v_{new }:= −v vnew​:=−v.

与这些方法不同，所提出的方法通过将 v v v反转为 v n e w : = − v v_{new }:= −v vnew​:=−v来构造不同方向的距离场。

Then a search process determines a new anchor point p n e w p_{new} pnew​ at the obstacle surface alone vnew, as depicted in Fig.4c. They constitute a new pair p n e w , v n e w {p_{new},v_{new}} pnew​,vnew​, which leads to a different local minima.

然后，搜索过程在障碍物表面单独确定一个新的锚点 p n e w p_{new} pnew​，如图4c所示。它们构成了一个新对 p n e w , v n e w {p_{new},v_{new}} pnew​,vnew​，这导致了不同的局部极小值。

Note that no explicit path search is adopted, but any pair of paths through p and pnew respectively violates Def.1 at these two points naturally. Subsequently, distinctive trajectories are optimized in parallel in different threads, as illustrated Fig.4d. The trajectory with lowest cost is executed.

注意，没有采用显式路径搜索，但通过 p p p和 p n e w p_{new} pnew​的任何一对路径在这两个点上自然地违反了Def.1。随后，在不同的线程中并行优化不同的轨迹，如图4d所示。执行成本最低的轨迹。

IV. DRONE SWARM NAVIGATION

A. Reciprocal Collision Avoidance

Let x k ( t ) ∈ X ⊂ R 3 x_k(t) \in X \subset \mathbb{R}^3 xk​(t)∈X⊂R3 be the position state of the agent k k k among K K K agents at time t t t. X k f r e e ( t ) ⊂ X \mathcal{X}^{free}_k(t) \subset \mathcal{X} Xkfree​(t)⊂X is the free region in the state space of agent k considering the existence of other agents. Thus, X k f r e e ( t ) : = X \ { i ∈ Z \ k , i ≤ K ∣ x i ( t ) } \mathcal{X}^{free}_k(t) := \mathcal{X} \backslash \{i \in \mathbb{Z} \backslash k,i \leq K|x_i(t)\} Xkfree​(t):=X\{i∈Z\k,i≤K∣xi​(t)} and the valid trajectory Φ k \Phi k Φk satisfies for any t t t within the domain of Φ k \Phi k Φk, Φ k ( t ) ∈ X k f r e e ( t ) \Phi k(t) \in \mathcal{X}^{free}_k(t) Φk(t)∈Xkfree​(t), as depicted in Fig.6. Unlike [22], here we ignore obstacles and dynamic constraints which have already get tackled in Sec.III-A.

设 x k ( t ) ∈ X ⊂ R 3 x_k(t) \in X \subset \mathbb{R}^3 xk​(t)∈X⊂R3是智能体 k k k在 K K K个智能体中在时间 t t t的位置状态 X k f r e e ( t ) ⊂ X \mathcal{X}^{free}_k(t) \subset \mathcal{X} Xkfree​(t)⊂X是在考虑其他智能体存在的情况下，智能体k的状态空间中的自由域。因此， X k f r e e ( t ) : = X \ { i ∈ Z \ k , i ≤ K ∣ x i ( t ) } \mathcal{X}^{free}_k(t) := \mathcal{X} \backslash \{i \in \mathbb{Z} \backslash k,i \leq K|x_i(t)\} Xkfree​(t):=X\{i∈Z\k,i≤K∣xi​(t)} 并且有效轨迹 Φ k \Phi k Φk满足 Φ k \Phi k Φk域内的任何 t t t。 Φ k ( t ) ∈ X k f r e e ( t ) \Phi k(t) \in \mathcal{X}^{free}_k(t) Φk(t)∈Xkfree​(t)。如图6所示。与[22]不同，这里我们忽略了第II-A节中已经解决的障碍和动态约束。

Similar to the penalty on obstacle collision and dynamical infeasibility, we formulate the penalty function J w , k J_{w,k} Jw,k​ of swarm collision avoidance for agent k k k as soft barrier constraint

类似于对障碍物碰撞和动力学不可行的惩罚，我们将智能体 k k k的群体防撞惩罚函数 J w ， k J_{w，k} Jw，k​公式化为软障碍约束

J w , k = ∑ i ∫ t = t s t e { d k , i ( t ) 0 d t ， d k , i ( t ) < 0 d k , i ( t ) ≥ 0 d k , i ( t ) = ∣ ∣ E 1 / 2 [ Φ k ( t ) − Φ i ( t ) ] ∣ ∣ − ( C + ϵ ) \begin{equation} \begin{aligned} J_{w,k} = \sum_i \int^{t_e}_{t=t_s} \begin{cases} d_{k,i(t)} \\ 0 \end{cases}dt， \begin{matrix} d_{k,i}(t) < 0\\ d_{k,i}(t) \geq 0 \end{matrix}\\ d_{k,i}(t) = \left|\left|\mathbf{E}^{1/2}\left[\Phi_k(t) - \Phi_i(t)\right]\right|\right| - (\mathcal{C} + \epsilon) \end{aligned} \end{equation} Jw,k​=i∑​∫t=ts​te​​{dk,i(t)​0​dt，dk,i​(t)<0dk,i​(t)≥0​dk,i​(t)= ​ ​E1/2[Φk​(t)−Φi​(t)] ​ ​−(C+ϵ)​​​

where i ∈ Z \ k i \in \mathbb{Z}\backslash k i∈Z\k, i ≤ K i \leq K i≤K, t s t_s ts​ and t e t_e te​ are global start and end time within the time span of trajectory Φ k ( t ) \Phi k(t) Φk(t). C \mathcal{C} C is user-defined agent clearance. E : = d i a g ( 1 , 1 , 1 / c ) , c > 1 \mathbf{E} := diag(1,1,1/c),c > 1 E:=diag(1,1,1/c),c>1 transforms Euclidean distance into ellipsoidal distance with shorter principal axes at z-axis to relief downwash risk.

其中 i ∈ Z \ k i \in \mathbb{Z}\backslash k i∈Z\k， t s t_s ts​ 和 t e t_e te​ 是轨迹 Φ k ( t ) \Phi k(t) Φk(t)全局的开始和结束时间 . C \mathcal{C} C是用户定义的智能体清除。 E : = d i a g ( 1 , 1 , 1 / c ) , c > 1 \mathbf{E} := diag(1,1,1/c),c > 1 E:=diag(1,1,1/c),c>1 将欧几里得距离转换为主轴在z轴较短的椭球距离，以缓解下洗风险。

Adding the weighted J w , k J_{w,k} Jw,k​ to Equ. (1) yields the total optimization
problem for each agent

将加权后的 J w , k J_{w,k} Jw,k​添加到Equ. (1) 产生总体优化每个智能体的问题

min ⁡ Q = J + J E G O + λ ω J ω \begin{equation} \min_{Q} = J + J_{EGO} + \lambda_\omega J_{\omega} \end{equation} Qmin​=J+JEGO​+λω​Jω​​​

Any trajectory parameterization method containing a mapping from decision variables to points on the trajectory is applicable to Equ. (5). This paper parameterizes trajectory using a pb-degree uniform B-spline, which gives the position evaluation a matrix representation [9]

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

Φ ( t ) = s ( t ) T M p b + 1 q m s ( t ) = [ 1 s ( t ) s 2 ( t ) … s p b ( t ) ] T q m = [ Q m − p b Q m − p b + 1 Q m − p b + 2 … Q m ] T \begin{equation} \begin{aligned} \Phi(t) &= s(t)^T M_{p_b + 1}q_m\\ s(t) &=\begin{bmatrix} 1 & s(t) & s^2(t) & \dots & s^{p_b}(t) \end{bmatrix}^T\\ q_m &= \begin{bmatrix} Q_{m-p_b} & Q_{m-p_b + 1} & Q_{m-p_b + 2} & \dots &Q_{m} \end{bmatrix}^T \end{aligned} \end{equation} Φ(t)s(t)qm​​=s(t)TMpb​+1​qm​=[1​s(t)​s2(t)​…​spb​(t)​]T=[Qm−pb​​​Qm−pb​+1​​Qm−pb​+2​​…​Qm​​]T​​​

where M p b + 1 M_{p_b+1} Mpb​+1​ is a constant matrix determined by p b p_b pb​, s ( t ) = ( t − t m ) / Δ t s(t) =(t −tm)/\Delta t s(t)=(t−tm)/Δt when t t tbelongs to the knot span ( t m , t m + 1 ] \left(tm,tm+1\right] (tm,tm+1].

其中 M p b + 1 M_{p_b+1} Mpb​+1​ is a 常量矩阵由 p b p_b pb​定义, s ( t ) = ( t − t m ) / Δ t s(t) =(t −tm)/\Delta t s(t)=(t−tm)/Δt其中 t t t属于节点时间段 ( t m , t m + 1 ] \left(tm,tm+1\right] (tm,tm+1].

B. Localization Drift Compensation(定位漂移补偿)

As a result of individual localization in unknown environments (no reliable and high-frequency loop-closure),drift accumulates during the flight.

由于在未知环境中的个体定位（没有可靠的高频回路闭合），漂移在飞行过程中积累。

Xu et al. [23] propose a state estimation method for aerial swarms with extra UWB distance measurements and achieve accurate collaborative
localization.

Xu et al. [23]提出了一种具有额外UWB距离测量的航空机群状态估计方法，并实现了精确的协作定位。

However, we focus more on traversing obstacle-rich environments and have to reserve computation and communication resources for other applications.

然而，我们更多地关注于穿越障碍物丰富的环境，并且必须为其他应用保留计算和通信资源。

Therefore, inspired by [23], a simplified and lightweight relative drift estimation method is proposed by comparing the predicted position evaluated from received agents’ trajectories and the measured positions from depth images of witnessed agents. This strategy works when the trajectory tracking error is negligible and at least one of any two agents that might collide see the other.

因此，受[23]的启发，通过比较从接收到的智能体轨迹中评估的预测位置和从目击智能体的深度图像中测量的位置，提出了一种简化且轻量级的相对漂移估计方法。当轨迹跟踪误差可以忽略不计，并且任何两个可能碰撞的只能体中至少有一个看到另一个时，这种策略就起作用了。

Therefore, we use the controller from[24] for precise tracking and use wide-angle cameras to reduce the possibility of losing agents.

因此，我们使用[24]中的控制器进行精确跟踪，并使用广角相机来减少丢失智能体的可能性。

The drift elimination procedure is as follows. After evaluating the current position Φ i ( t n o w ) \Phi_i(t_{now}) Φi​(tnow​) of agent i i i, a spherical trust region S ⊂ R 3 \mathcal{S} \subset \mathbb{R}^3 S⊂R3 centered at Φ i ( t n o w ) \Phi_i(t_{now}) Φi​(tnow​) with radius R \mathcal{R} R is determined, where R \mathcal{R} Ris an empirical parameter indicating the upper bound of typical drift estimated from experiments.

漂移消除程序如下，评估完智能体 i i i当前位置 Φ i ( t n o w ) \Phi_i(t_{now}) Φi​(tnow​)后，一个半径为 R \mathcal{R} R以 Φ i ( t n o w ) \Phi_i(t_{now}) Φi​(tnow​) 为球心球形置信区间 S ⊂ R 3 \mathcal{S} \subset \mathbb{R}^3 S⊂R3被确定。其中 R \mathcal{R} R 是表征由实验得出的典型漂移上限的一个经验值。

S \mathcal{S} Sis then mapped to the currently captured depth image, which is the region S ′ ⊂ R 2 \mathcal{S}′ \subset \mathbb{R}^2 S′⊂R2 satisfying

然后 S \mathcal{S} S被映射到当前捕捉到的深度图像, 使得 S ′ ⊂ R 2 \mathcal{S}′ \subset \mathbb{R}^2 S′⊂R2 满足

z [ s ′ T 1 ] = K T W c [ s T 1 ] T \begin{equation} z\begin{bmatrix} s'^T & 1 \end{bmatrix} = \mathbf{K}\mathbb{T}^c_W \begin{bmatrix} s^T & 1 \end{bmatrix}^T \end{equation} z[s′T​1​]=KTWc​[sT​1​]T​​

where s ′ ∈ S ′ s′ \in S′ s′∈S′, s ∈ S s \in S s∈S, K K K and T w c T^c_w Twc​ are camera intrinsic and extrinsic matrices, z z zis the deviation of s s s along the main optical axis from the optical center. S ′ S′ S′ is an elliptical conic section which requires complex calculation to acquire.

其中 s ′ ∈ S ′ s′ \in S′ s′∈S′, s ∈ S s \in S s∈S, K K K 和 T w c T^c_w Twc​是相机的内参矩阵和外参矩阵， z z z是 s s s沿主光轴与光学中心的偏差。 S ′ S′ S′是一个需要复杂计算才能获得的椭圆圆锥截面

Therefore, we adopt an approximate axis-aligned ellipse S ˉ ′ \bar{S}' Sˉ′ instead of the exact S ′ S′ S′. There is no necessity to precisely define the trust region since it is just an empirical region.

此，我们采用近似轴对齐的椭圆̄ S ˉ ′ \bar{S}' Sˉ′，而不是精确的 S ′ S' S′。没有必要精确定义信任区域，因为它只是一个经验区域。

Then we project each point within S ˉ ′ \bar{S}' Sˉ′ into the world frame and collect points belonging to S S S, which results in a point cluster P ⊂ S \mathcal{P} \subset \mathcal{S} P⊂S.Then the position of agent observation P \mathbf{P} P is regarded as the center (first raw moment) of P \mathcal{P} P

然后我们投影 S ˉ ′ \bar{S}' Sˉ′ 中的每一个点到世界坐标系，然后收集属于 S S S的点云，记为
P ⊂ S \mathcal{P} \subset \mathcal{S} P⊂S。然后被观察的智能体的位置 P \mathbf{P} P被认为在 P \mathcal{P} P的中心(期望)。
P = μ 1 ′ ( P ) \begin{equation} \mathbf{P} = \mu_1'(\mathcal{P}) \end{equation} P=μ1′​(P)​​

which is Equ. (9) holds if P \mathcal{P} P contains only the corresponding agent’s observation without any unrelated object, which is not guaranteed.

如果 P \mathcal{P} P 只包含相应智能体的观察而没有任何不相关的对象，则Equ. (9)成立，这是无法保证的。

However, since each agent plans trajectories with a clearance to nearby objects, Equ. (9) holds for most of the time. Additional criteria are added to improve the robustness for agent detection, such as the number of pixels, the second central moment of P \mathcal{P} P, the deviation of the current measurement to previous ones, etc. Stricter criterion increases falsenegative rate, but it is harmless since localization drift alters slowly. Finally, the error between Φ i ( t n o w ) \mathbf{\Phi}_i(t_{now}) Φi​(tnow​) and P \mathbf{P} P is fed to a filter, from where the estimated drift is then acquired.

然而，由于每个智能体规划轨迹都和周边的物体有一定间距，因此Equ. (9)大部分时间成立。我们添加了额外的标准来提高智能体检测的鲁棒性，例如像素数、 P \mathcal{P} P的第二中心矩、当前测量值与以前测量值的偏差等。更严格的标准会增加假阴性率，但由于定位漂移变化缓慢，这是无害的。最后， Φ i ( t n o w ) \mathbf{\Phi}_i(t_{now}) Φi​(tnow​)和 P \mathbf{P} P之间的误差被馈送到滤波器，然后从滤波器获得估计的漂移。

C. Agent Removal from Depth Images

We use occupancy grid map to store static obstacles, and depth images for map fusion. The moving agents are taken care of in Sec.IV-A. Therefore, recording moving agents and treating them as obstacles in the map building are not necessary, even harmful. To eliminate the influence of moving objects, we mask and remove the pixels of agents detected in Sec.IV-B from the depth images, as illustrated in Fig.7. Except that, moving objects covering the majority of the view are interference to VIO. Therefore, agents on gray-scale images are removed as well using the same mask for the corresponding depth images. The criterion for agent detection used here is less strict since false positives are more harmful than false negatives.

我们使用占用网格地图来存储静态障碍物，并使用深度图像进行地图融合。移动智能体在第IV-A节中得到了处理。因此，记录移动智能体并将其视为地图构建中的障碍物是没有必要的，甚至是有害的。为了消除运动物体的影响，我们从深度图像中屏蔽并去除在第IV-B节中检测到的智能体的像素，如图7所示。除此之外，覆盖大部分视图的移动对象都是对VIO的干扰。因此，对于相应的深度图像也使用相同的掩模来去除灰度图像上的因素。这里使用的检测标准不那么严格，因为假阳性比假阴性更有害。

V. SYSTEM ARCHITECTURE

The system architecture is depicted in Fig.8, which contains the detailed architecture for a single agent and the multi-agent communication system.

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

A. Navigation System of A Single Agent

The single agent system, including hardware and software setups is based on our previous work EGO-Planner [3], with an extra module that compensates VIO drift and removes witnessed agents on images. For trajectory generation in unknown environments, the local planner is used. Planning is activated when the current trajectory collides with newly discovered obstacles, or the agent is getting close to the end of the current trajectory.

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

B. Communication Framework

Two networks connect the system, i.e., a broadcast network sharing trajectories, and a chain network to synchronize timestamps and manage the sequential startup.

两个网络连接系统，即共享轨迹的广播网络和同步时间戳和管理顺序启动的链网络。

• Broadcast Network: Once an agent generates a new collision-free trajectory, it is broadcast immediately to all agents. Other agents then receive and store this trajectory used to generate safe trajectories for themselves when necessary. This closed-loop strategy works properly in ideal situations where the connection is stable and the latency is negligible. However, this is not guaranteed in practice. Therefore, we propose two methods to reduce the possibility of collision.

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

  First, one trajectory is broadcast at a given frequency under network capacity. This does not cause computation burden since a typical trajectory containing 3-D waypoints and other parameters are less than 0.5KB in size. By contrast, modern wireless networks such as Bluetooth2 can reach a speed above 1Mbps. Second, each agent checks collision as soon as a trajectory is received from broadcast network, and if potential collisions are detected, a new collision-free trajectory is generated. This strategy can tackle when multiple agents generate trajectories at a close time without receiving others’ trajectories due to latency or packet losses.

  首先，在网络容量下以给定频率广播一个轨迹。这不会造成计算负担，因为包含3-D路点和其他参数的典型轨迹的大小小于0.5KB。相比之下，像蓝牙2这样的现代无线网络可以达到1Mbps以上的速度。其次，一旦从广播网络接收到轨迹，每个智能体就检查碰撞，如果检测到潜在的碰撞，则生成新的无碰撞轨迹。该策略可以解决多个智能体由于延迟或数据包丢失而在不接收其他智能体的轨迹的情况下近距离生成轨迹的问题。
  

  In addition, the growth of computational complexity with the increase of agent number is considered. Before planning, each agent compares its current position with received surrounding agents’ trajectories, where any trajectory outside the planning range will be ignored.

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

• Chain Network: A connection-based stable chain network is used for timestamp synchronization and system startup management. At system startup, agents generate trajectories in a predefined order. Each agent generates its initial trajectory after receiving trajectories from agents with higher priority through the chain network. This strategy avoids chaos caused by simultaneous trajectory generation during system startup since agents have no information of other trajectories at that time.

  链网络：基于连接的稳定链网络用于时间戳同步和系统启动管理。在系统启动时，智能体按照预定义的顺序生成轨迹。每个智能体在通过链网络从具有更高优先级的智能体接收到轨迹之后生成其初始轨迹。该策略避免了系统启动期间同时生成轨迹所引起的混乱，因为智能体当时没有其他轨迹的信息。

VI. BENCHMARK

Benchmark comparisons are presented in simulation using an i7-9700KF CPU. For parameter settings, the planning horizon is set to 7.5m. λ s = 1.0 \lambda_s = 1.0 λs​=1.0, λ c = λ w = λ t = 0.5 \lambda_c = \lambda_w = \lambda_t = 0.5 λc​=λw​=λt​=0.5, λ d = 0.1 \lambda_d = 0.1 λd​=0.1. Map resolution is 0.1m. Re-planning is triggered every second or a collision is predicted. This setting is implemented in both simulation and real-world experiments.

在使用i7-9700KF CPU的模拟中进行了基准比较。对于参数设置，规划范围设置为7.5m。 λ s = 1.0 \lambda_s = 1.0 λs​=1.0， λ c = λ w = λ t = 0.5 \lambda_c = \lambda_w = \lambda_t = 0.5 λc​=λw​=λt​=0.5， λ d = 0.1 \lambda_d = 0.1 λd​=0.1。地图分辨率为0.1m。每秒触发一次重新规划，或者预测会发生碰撞。此设置在模拟和真实世界的实验中都可以实现。

A. Topological Planning

We compare the topological planning performance of the proposed EGO-Swarm with Fast-Planner [15] in terms of candidate trajectory number and computation time for front-end topological path search. As is shown in Fig.9, EGO-Swarm finds fewer candidate trajectories, which means a lower probability of finding the global optimal, but is faster than [15] by two orders of magnitude. Since Fast-Planner finds topologically distinct paths by PRM [25] graph search, path shortening, and path pruning, which are time-consuming but with a higher degree of freedom, compared to the proposed implicit topological path search method.

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

B. Swarm Planning

In Empty Space:
We compare the proposed method with DMPC [19], ORCA [16], and RBP [20], in terms of flight distance( d f l y d_{fly} dfly​), flight time( t f l y t_{fly} tfly​), collision times per agent, and computation time( t c a l t_{cal} tcal​)). The default parameters, except for maximum velocity and acceleration, are used for each compared method. As depicted in Fig.10, eight agents perform swap transitions on a circle. The results in Tab.I are the average of all agents. tcal is marked by ‘*’, since the computation time we record for offline methods, DMPC and RBP, is the total time to plan all agents’ whole trajectory, while for ORCA and EGO-Swarm, it is the local replanning time for each agent.

我们将所提出的方法与DMPC[19]、ORCA[16]和RBP[20]在飞行距离（ d f l y d_{fly} dfly​）、飞行时间（ t f l y t_{fly} tfly​）、每个智能体的碰撞次数和计算时间（ t c a l t_{cal} tcal​）方面进行了比较。除了最大速度和加速度外，每个比较方法都使用默认参数。如图10所示，八个智能体在一个圆上执行交换转换。表一中的结果是所有智能体的平均值。tcal用“*”标记，因为我们记录的离线方法DMPC和RBP的计算时间是规划所有智能体的整个轨迹的总时间，而对于ORCA和EGO Swarm，它是每个智能体的本地重新规划时间。

Tab.I and Fig.10 state that RBP tends to generate safe
but conservative trajectories, since the construction of convex relative safe flight corridor [20] significantly compresses the solution space. DMPC is designed for distributed deployment. Nevertheless, it requires accurate and high-frequency pose communication, which can not be guaranteed in real-word applications. Efficient rules make ORCA update fast. However, using speed as control commands makes it incompatible with third-order systems, such as quadrotors. The risk of collision also limits its application. By contrast, the proposed method generates the shortest collision-free, non-conservative trajectories with fast computation. Therefore, it enables real-time applications for quadrotors.

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

2)In Obstacle-rich Environments:
We simulate ten drones flying from one side of the map to the other with a speed limit of 2m/s and the quadrotor radius of 0.2 meters. Fig.2 is a simulation snapshot of 0.42 o b s t a c l e s / m 2 0.42 \quad obstacles/m^2 0.42obstacles/m2. Each agent senses the environment independently, and the constructed local maps are displayed in different colors. The results are summarized in Tab.II, where dfly is the average flight distance, dsafe is the closest distance to obstacles during the flight tests. An inverse point-to-point transition is designed to make reciprocal collision avoidance inevitable around the map center. In this scenario, each agent belonging to the swarm manages to plan smooth and safe trajectories.

我们模拟了十架无人机以2米/秒的速度和0.2米的四旋翼半径从地图的一侧飞到另一侧。图2是 0.42 o b s t a c l e s / m 2 0.42 \quad obstacles/m^2 0.42obstacles/m2的模拟快照。每个智能体独立地感知环境，并且构建的局部地图以不同的颜色显示。结果总结在表II中，其中 d f l y d_{fly} dfly​是平均飞行距离， d s a f e d_{safe} dsafe​是飞行测试期间距离障碍物最近的距离。设计了一种反向点对点导航，以使地图中心周围的避障不可避免。在这种情况下，属于群体的每个智能体都能规划出平稳安全的轨迹。

2)Scalability Analysis: We evaluate the computation performance in a scenario where agents arranged in a straightline fly to random target points 50 meters away. As is depicted in Fig.11, due to the on-demand collision check strategy in Sec.V-B.1, time complexity gradually flattens out with the increase in the number of agents.

我们评估了在一个场景中的计算性能，其中排列在直线上的智能体飞到50米外的随机目标点。如图11所示，由于第V-B.1节中的按需冲突检查策略，时间复杂性随着智能体数量的增加而逐渐趋于平缓。

VII. REAL-WORLD EXPERIMENTS

1)Indoor: Indoor experiments are presented at a speed limit of 1.5m/s, as in Fig.12. The top one shows three quadrotors perform a circle swap with reciprocal collision avoidance. In the middle one, quadrotors manage to pass through a narrow door one after another. In the bottom figure,
the environment is more cluttered. Three quadrotors manage
to navigate across this environment.

室内实验是在1.5m/s的速度限制下进行的，如图12所示。上图显示了三个四旋翼机在相互避免碰撞的情况下进行圆周交换。在中间的一个房间里，无人机一扇接一扇地穿过一扇狭窄的门。在下图中，环境更加杂乱。三个四旋翼机管理在这个环境中导航。

2)Outdoor: In a forest where trees are spaced about 2 meters apart as shown in Fig.1, three quadrotors start in the forest together and manage to reach the target position outside the forest. The velocity limit is set to 1.5m/s. To further emphasize reciprocal avoidance, we reverse the order of the goal position relative to the start position, making reciprocal avoidance unavoidable, like what we do in Sec.VI-B.2. For more information about the experiments, please refer to the codes and watch our attached video on github3.

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

VIII. CONCLUSION

In this paper, a systematic solution for multi-robot navigation in unknown cluttered environments using only onboard resources is proposed. Benchmark comparisons demonstrate its short computation time and high trajectory quality. Real-world experiments validate its robustness and efficiency.

本文提出了一种仅使用机载资源在未知杂乱环境中进行多机器人导航的系统解决方案。基准比较表明其计算时间短，轨迹质量高。现实世界的实验验证了它的稳健性和效率。

本文标签： 原文EGOswarm