ego-swarm论文阅读

本文是将高飞老师等人的这篇论文翻译了一遍，顺便跟着读一下，只是记录一下自己读的论文，无其他见解，原文链接如下：
https://ieeexplore.ieee.org/abstract/document/9561902

摘要

在未知的复杂环境中，仅使用机载资源进行多机自主导航的分散式异步系统解决方案。该规划系统在基于梯度的局部规划框架下制定，通过将碰撞风险建模为非线性优化问题的惩罚实现避撞。采用一种轻量轻量级的拓扑轨迹生成方法提高鲁棒性和跳出局部最小值。agents使用不可靠的轨迹共享网络在仅几毫秒内生成安全、平滑和动态可行的轨迹。利用深度图中的agent检测修正agents之间的相对定位漂移。

前言

在未知环境中部署多架四旋翼飞行器的难点包括但不限于障碍物参数化的nontriviality（非平凡性？）、无人机有限的感知范围、不可靠和带宽有限的通信以及定位不一致导致的定位漂移。
这里的nontriviality指障碍物形状的多样性、环境的动态性和不确定性、不同空间尺度的适应性、计算的复杂性、实时性要求以及传感器数据融合的挑战。
ego-swarm是ego-planner的扩展，基于无ESDF梯度的局部规划，并且不需要外部定位和计算或预先构建的地图。ego-swarm主要包括拓扑规划和相互碰撞避免两部分，基于 EGO-Planner 中的碰撞成本公式（ego-planner后续进行了解），通过在目标函数中添加群体碰撞的加权惩罚来实现分散的相互碰撞避免。通过将预测的未来时间的代理分布与正在优化的轨迹进行比较来评估这种惩罚。为了最大限度地减少数据传输并允许不可靠的通信，使用广播网络来共享轨迹。为了纠正可能增加到半米的相对定位漂移，对witnessed agents的观察结果和轨迹评估的预测进行比较。
*补充：ESDF（Euclidean Signed Distance Field，欧几里得符号距离场）提供了每个空间点到最近障碍物的距离信息，并指示点是在障碍物内还是在自由空间内，通过计算空间中每个点到最近障碍物的距离来帮助路径规划，ESDF方法的缺点是计算复杂度高，在大规模环境中计算耗时，并且对动态障碍物处理复杂。无ESDF是指在路径规划和导航中，不依赖于欧几里得符号距离场进行障碍物避让和路径生成的局部规划方法，避免了构建和维护ESDF的计算开销，可能提高实时性和效率，更容易处理动态障碍物和实时更新。

相关工作

A.单无人机局部规划

基于梯度的运动规划是四旋翼飞行器局部规划的主流。主流的研究方法使用各种参数化方法，包括多项式和B样条，来考虑轨迹的平滑性、可行性和安全性。ego-planner使用更紧凑的环境表示来减少计算时间，ego-swarm基于该工作开展。

B.拓扑规划

拓扑规划方法通过构建和利用图来表示环境的拓扑结构，其中节点（vertices）代表关键位置，边（edges）代表这些位置之间的可通行路径，可用于避免局部最小值问题。ego-swarm扩展了ego-planner来进一步加速拓扑规划的前端。

C.分散式无人机集群

用速度障碍（Velocity Obstacle, VO）来保证点机器人的无碰撞轨迹，速度障碍是一种用于移动机器人、无人驾驶汽车和其他自主系统中的避障和导航方法。速度障碍方法主要用于动态环境中，通过计算速度空间中的障碍区域，帮助自主系统避开移动障碍物，确保安全导航。
速度空间是每个物体（如机器人或车辆）的速度表示为二维或三维的向量，速度空间中的每个点表示物体可以采取的一种速度。
速度障碍是在速度空间中，导致机器人在未来某个时刻与障碍物发生碰撞的速度集合，如果机器人当前速度位于这个障碍区域内，那么它将在未来与障碍物碰撞。
通过计算机器人与障碍物之间的相对速度确定哪些速度组合将导致碰撞。或者在速度空间中定义一个锥形区域，表示所有会导致碰撞的速度，如果障碍物在机器人相对于障碍物的速度方向上，那么机器人就有可能与障碍物碰撞。

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

利用人工势场法，为环境中的每个点定义一个势场值，由目标位置的吸引势场和障碍物的斥力势场组成。梯度表示势场值的变化率和方向，指向势场增加最快的方向，可以通过计算势场函数的偏导数得到。通过计算势场的梯度，机器人可以确定在当前点的运动方向，并沿着梯度的负方向移动。

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

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

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

软障碍约束项 Jc 和 Jd 惩罚超过特定阈值 T 的决策变量，通常表示为：

其中参数 S、n和 epsilon 影响单边约束逼近精度，根据惩罚类型选择变换L(·)和参数。L(·)的具体形式可以在参考文献[3]中找到。（后续再阅读ego-plannerl论文）
在EGO-Planner中，根据每个Q独立拥有的环境信息提出了一种新颖的障碍物距离估计方法。由几个{p，v}对参数化的信息是从周围障碍物中高度抽象的，其中p表示障碍物表面的锚点，v表示从障碍物内部指向外部的安全方向，如图1所示。第 i 个控制点 Qi 到第 j 个障碍物的距离 dij 定义为：

图1：EGO-Planner 和拓扑轨迹生成的图示。 (a) 搜索绕过碰撞轨迹的安全路径，然后生成 {p, v} 对。 (b) 优化器找到满足 d = (Qi − p) · v 大于常数值的安全轨迹。（c） 生成一个新的 {p, v} 对，其中 vnew := -v 且 pnew 在障碍物的另一侧。 (d) 生成满足{pnew，vnew}对约束的不同轨迹。
{p，v}对生成和轨迹优化过程如图1a和b所示。首先，给出一个简单的初始轨迹 Φ，无论碰撞如何。然后搜索一条连接Φ碰撞段两端的安全路径Γ。之后，生成从 Φ 到 Γ 的矢量 v，并在障碍物表面定义 p。通过生成的 {p, v} 对，规划器最大化 dij 并返回优化的轨迹。

B.隐式拓扑轨迹生成

广泛使用的同伦概念不足以捕获 3D 情况下的候选轨迹，如图2所示。因此，Jaillet 等人提出了 3-D 空间中更有用的关系，称为可见性变形（VD），进一步提取VD的一个子集，称为均匀可见变形（UVD），从而实现实时操作，满足UVD的轨迹被认为是同伦的。UVD定义如下：
定义1：两个轨迹 τ1(s), τ2(s)，由 s ∈ [0, 1] 参数化，满足 τ1(0) = τ2(0), τ1(1) = τ2(1)，属于同一 UVD 类，如果对于所有 s，线 τ1(s)τ2(s) 是无碰撞的。
注：（1）同伦是拓扑学中的概念，用于描述两条路径在空间中的连续变形。如果两条路径可以通过连续变形（不打破、不切断）转换为彼此，那么它们是同伦等价的。在路径规划中，同伦类用于区分不同的路径类型。例如，在二维平面上，绕过障碍物的路径可以分为不同的同伦类。
（2）可见性变形（Visibility Deformation）是一种在路径规划中用于处理障碍物和自由空间的技术。它利用环境中可见性的变化来指导路径生成和优化，通过逐步改变路径或环境中的一些参数，使得路径能够避开障碍物并保持在自由空间中。

图2：属于同一 3-D 同伦的四个轨迹处于不同的局部最小值
传统的拓扑规划方法由拓扑不同的路径搜索和后端优化组成，主要侧重于寻找不同同伦的多个初始路径。与传统方法不同，ego-swarm所提出的方法通过将 v 反转为 vnew := -v 来构造不同方向的距离场。然后搜索过程单独在障碍物表面vnew处确定新的锚点pnew，如图1c所示。它们构成了一个新的对 {pnew, vnew}，这导致了不同的局部最小值。这里没有采用显式路径搜索，但是任何一对分别经过p和pnew的路径在这两点上自然违反了定义1。随后，在不同线程中并行优化不同的轨迹，如图 1d 所示。执行成本最低的轨迹。
注：显式路径搜索方法通过明确地构建和评估候选路径，需要先构建搜索图，再使用搜索算法搜索路径，如A星和Dijkstra算法，隐式路径搜索方法不依赖于预先构建的搜索图或显式表示的路径，使用动态和增量的方法（路径逐步生成和优化，算法在探索过程中动态调整路径，如RRT算法）来探索环境，逐步生成和优化路径。

无人机集群导航

A.无人机避碰

设 x_k(t) ∈ X ⊂ R3 为 t 时刻 K 个智能体中智能体 k 的位置状态。 Xfree_k (t) ⊂ X 是考虑到其他智能体的存在，智能体 k 的状态空间中的自由区域。因此， Xfree_k (t) := X {i ∈ Z\k, i ≤ K|xi(t)} 并且对于 Φk 域内的任何 t，有效轨迹 Φk 满足，Φk(t) ∈ Xfree_k (t)，如图3所示。

图3：无人机自身通过将距离与相同轨迹时间的周围无人机接收到的轨迹进行比较来生成轨迹。
与障碍物碰撞和动态不可行性的惩罚类似，我们将智能体 k 的群体碰撞避免惩罚函数 J_w,k 公式化为软障碍约束：
注：软障碍约束通过优化问题中的惩罚项来引导路径避开障碍物，而不是将障碍物视为绝对不可穿越的区域。这种方法可以提高路径规划算法的灵活性和鲁棒性

其中i ∈ Z\k, i ≤ K,ts 和 te 是轨迹 Φk(t) 时间跨度内的全局开始和结束时间。C 是用户定义的代理权限。E := diag(1, 1, 1/c), c > 1 将欧几里德距离转换为 z 轴主轴较短的椭球距离，以减轻下洗风险。将加权J_w,k添加到优化问题表达式中得到每个智能体的总体优化问题：

任何包含从决策变量到轨迹上的点的映射的轨迹参数化方法都适用于上面的避碰惩罚函数。该文使用 pb 级均匀 B 样条对轨迹进行参数化，这为位置评估提供了矩阵表示：

其中M_pb+1是由 pb 决定的常数矩阵，当 t 属于节点跨度(tm, tm+1]时，s(t) = (t - tm)/delta_t。

B.定位漂移补偿

由于在未知环境中进行单独定位（没有可靠的高频环路闭合），定位漂移会在飞行过程中累积。本文更关注穿越障碍物的环境，并为其他应用预留计算和通信资源。通过比较从接收到的智能体轨迹评估的预测位置和从目击智能体的深度图像测量的位置，提出了一种简化且轻量级的相对漂移估计方法。当轨迹跟踪误差可以忽略不计并且可能发生碰撞的任意两个智能体中至少有一个能够看到另一个时，该策略就有效。因此，我们使用[D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), Shanghai, China, May 2011, pp. 2520–2525.]中的控制器（基于反馈线性化和PID控制设计的控制器）进行精确跟踪，并使用广角摄像头来减少丢失智能体的可能性。
漂移消除过程如下：在评估智能体 i 的当前位置 Φi(tnow) 后，确定以 Φi(tnow) 为中心、半径为 R 的球形信任区域 S ⊂ R3，其中 R 是经验参数，指示通过实验估计的典型漂移的上限。然后将 S 映射到当前捕获的深度图像，即满足 S′ ⊂ R2 的区域：

其中 s′ ε S′、s ε S、K 和 Tc_w 分别为相机内参和外参矩阵，z 为 s 沿主光轴与光心的偏差。S’是椭圆圆锥曲线，需要复杂的计算才能获得。因此，我们采用近似轴对齐椭圆 S’~而不是精确的 S′，此外没有必要精确定义信任区域，因为它只是一个经验区域。
然后我们将 S′~内的每个点投影到世界框架中并收集属于 S 的点，从而产生点集 P ⊂ S。然后将智能体观测点P的位置视为点集P的中心（第一原始矩），即：

如果 P 只包含相应智能体的观测而没有任何不相关的对象，则上式成立，这是不能保证的。然而，由于每个智能体都规划了与附近物体有间隙的轨迹，因此上式大部分时间成立。添加额外的标准来提高智能体检测的鲁棒性，例如像素数量、P 的第二中心矩、当前测量值与先前测量值的偏差等。更严格的标准会增加假阴性率（false negative rate，系统报告智能体不在信任区域内，尽管它实际上是在的。这种情况增加了因为误报而忽略实际安全状态的风险。），但由于定位漂移变化缓慢，因此它是无害的。最后，Φi(tnow) 和 P 之间的误差被馈送到滤波器，然后从中获取估计的漂移。

C.从深度图像中去除智能体

使用占用网格地图来存储静态障碍物，并使用深度图像进行地图融合。移动智能体在 A 节中处理。因此，记录移动主体并将其视为地图构建中的障碍物是没有必要的，甚至是有害的。为了消除移动物体的影响，论文从深度图像中屏蔽并删除了 B 节中检测到的智能体像素，如图 4 所示。除此之外，覆盖大部分视野的移动物体会对 VIO （结合视觉和惯性测量单元数据的方法）产生干扰。因此，灰度图像上的智能体也使用对应深度图像的相同掩模（掩模表示图像中智能体占据的区域）被移除。这里使用的智能体检测标准不太严格，因为误报比漏报危害更大。

图4：左：VIO定位方法根据接收的轨迹评估的预测位置与信任区域内深度图像中的观察位置之间的漂移被估计。右：观察到的代理被屏蔽以避免影响映射。

系统架构

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

A.单智能体导航系统

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

B.通讯框架

两个网络连接系统，即共享轨迹的广播网络和用于同步时间戳并管理顺序启动的链网络。
1）广播网络：一旦一个智能体生成了新的无碰撞轨迹，它就会立即广播给所有智能体。然后其他代理接收并存储该轨迹，用于在必要时为自己生成安全轨迹。这种闭环策略在连接稳定且延迟可以忽略不计的理想情况下可以正常工作。然而，这在实践中并不能得到保证。因此，该论文提出两种方法来减少碰撞的可能性。
首先，在网络容量下以给定频率广播一条轨迹。这不会造成计算负担，因为包含 3D 航路点和其他参数的典型轨迹的大小小于 0.5KB。相比之下，蓝牙2等现代无线网络可以达到1Mbps以上的速度。其次，每个智能体在从广播网络接收到轨迹后立即检查碰撞，如果检测到潜在的碰撞，则生成新的无碰撞轨迹。该策略可以解决多个智能体在接近的时间生成轨迹而由于延迟或数据包丢失而无法接收其他智能体的轨迹的问题。
此外，还考虑了计算复杂度随着智能体数量的增加而增加。在规划之前，每个智能体将其当前位置与接收到的周围智能体的轨迹进行比较，其中任何超出规划范围的轨迹都将被忽略。
2）链网：基于连接的稳定链网络用于时间戳同步和系统启动管理。系统启动时，智能体按预定义的顺序生成轨迹。每个智能体通过链式网络接收来自更高优先级智能体的轨迹后生成其初始轨迹。该策略避免了系统启动期间同时生成轨迹而造成的混乱，因为智能体当时没有其他轨迹的信息。