Swarm of micro flying robots in the wild (二)

讨论

在这项工作中，提出并验证了一种基于高级个体智能实现群体智能的模块化和分层系统，其中所有无人机都是通过感知环境并规划局部最优轨迹的能力开发的。该框架之所以采用，是因为我们专注于现实世界非结构化领域的完全自主导航，而不需要先验知识，同时满足TEEM(如引言中所讨论的)。

从模拟基准到真实世界的实验，我们成功地展示了软件和硬件中的TEEM。所提出的导航底层能力与进一步结合精心设计的特定于任务的要求兼容。例如，在编队飞行中，可能没有预先定义的形状，因此无人机必须只彼此靠近，这使得它们的行为就像鸟群一样。此外，根据任务要求，首选的形状可以自适应地变化。轨迹规划器能够避开多个动态障碍物，如图8F所示，虽然障碍物的未来路径并不完全已知，因为它们是由用户实时控制的。

在这里，我们过分强调严格的完整性和最优性证明来实现现实世界的性能，因为由复杂的现实世界交互模块组成的整个系统已经被证明很难严格数学化。幸运的是，由于现实世界的随机性，我们的方法自然缓解了多智能体文献(51，52)中的死锁(由于资源分配挑战而失效)，更多地关注完备性和最优性的一些问题，在我们的轨迹表示中异步触发规划，以及高度的自由。同样，在正常情况下，其他轨迹规划方法 (53) 也几乎没有发生死锁，除非在具有特殊环境几何形状的情况下，例如在狭窄和长管内。

总之，我们提出的规划器遵循目标排序方案，其中用户可以在任务期间随时给出目标。该方案可以无缝地连接到高级决策和任务分配模块，其输出始终是每个机器人的首选目标（54、55）。此外，所提出的多目标优化轨迹规划器允许高级模块专注于任务抽象，而不必担心安全和动态可行性等常见要求。

材料和方法

系统架构

遵循单群方法，自然构建了一个去中心化方案，其中每个无人机都配备了完全自主权以实现最大导航质量。系统架构如图 2B 所示。轨迹广播网络是个体之间唯一的联系。因此，该系统比以前的工作 (33) 耦合较小，后者需要稳定的链连接。映射模块基于概率映射（7），显示出鲁棒性和效率。无人机去除模块深度去除其他目击无人机的像素，干扰映射。基于VIO的定位模块和提出的漂移校正算法计算六自由度无人机状态。控制器模块命令无人机精确跟踪计划的轨迹。生成高质量的轨迹的规划模块是实现TEEM的核心，因此在“轨迹表示”到“动态避障”部分和补充材料中进一步详细介绍。硬件模块在“手掌大小的无人机硬件”部分介绍。无人机移除模块，该模块以其他无人机的轨迹确定三维边界框为信任区域，详见补充材料的S8部分。

轨迹表示

在这里，时空轨迹规划是使用新开发的轨迹表示为MINCO(最小控制)(56)实现的，该表示是为多直升机等差分平坦系统设计的(4)。MINCO 的最新进展是它为用户将轨迹的时空参数解耦，为此设计了线性复杂度操作，便于时空变形。MINCO分段轨迹的参数是每个片段的(i)持续时间，以及(ii)每对连通块之间的相邻路径点，其中M为分段数。然后，MINCO轨迹时刻t的三维点由操作M定义

                        (1)

根据 (56) 中的“最优性条件”部分，对于集成器链动力学（本工作中 s = 3），MINCO 轨迹默认为 2s-1 程度  多项式样条，具有恒定边界和给定 {q, T} 的最小控制工作量。它的控制努力优化由

                (2)

给出，其中t∈[t0，tM]是当前轨迹的域。请注意，平滑度最大化是通过控制努力最小化来完成的，因为我们使用抖动控制系统模型。

此外，MINCO 在将给定参数 {q, T} 转换为具有线性复杂度 O(M) 的多项式系数 c 和时间轮廓 Tp 方面取得了进展。更具体的对应关系可以表示为

M(T)c = b(q),Tp = T           (3)

其中 和 是任何 T ≻ 0 的非奇异带状矩阵，如 (56) 所示。通过带状 PLU 分解恢复轨迹具有线性复杂度。多项式系数的梯度也线性传播到MINCO参数。这意味着一旦获得 {c, Tp} 上目标的部分梯度，它们就可以有效地传播到 {q, T}，然后将优化可以直接应用于 MINCO。补充材料中提供了 M、b 和 MINCO 更多特征的详细信息。具体来说，在台式计算机上通过等式每个多项式分段从给定 {q, T} 到 {c, Tp} 的计算时间大概是1us 。

约束转录

多直升机的微分平坦度(4)意味着它们的运动规划可以在低维平滑轨迹上执行，如MINCO。为了实现平滑的运动和高效的飞行，我们分别定义了平滑度和时间两个指标，然后最小化它们的加权和。决策变量是 MINCO 参数 q 和 T。开始状态和终端状态是固定的，以确保连续性。可行性要求轨迹满足车辆的动力学约束，避免障碍物。平坦度使得通过限制轨迹速度、加速度和加速度的大小来加强动力学约束成为可能。通过变形轨迹形状来实现避障。

沿轨迹的连续时间约束由无限多个不等式组成。为了处理难度，我们提出了一种用于约束转录的两步过程。首先，受 (40) 的启发，约束是通过具有足够惩罚权重的惩罚函数的积分来实现的。其次，每个积分都由沿时间线等距样本的有限总和进行评估。问题最终成为一个不受约束的问题，可以更有效地解决 (57)。对于时间 t ∈ [t0, t M], t M − t0 = sum(T) 内的等式约束 H 和不等式约束 G 下的时间依赖性目标 J 的优化，这种两步转换可以写成

 为简单起见，我们省略了方程式 6 和 7中的参数 q、T 和 t。在这些方程中，和 是用户定义的权重，具有适当的大条目。ti = t0 + (t M−t0)i/ k 表示有限数量的采样时间戳，其中k + 1 等于样本数，是积分评估的区间值。如果 J 中的任何积分都具有封闭形式的表达式，例如平滑度和总时间，则应使用分析结果。等式 7 可以写成用户友好的形式

 其中 J x 是各种惩罚术语，即任务规范，是相对权重。下标 x = {s, t, d, o, w} 表示平滑度 (s)、总时间 (t)、动态可行性 (d)、避障 (o)、群体避碰 (w) 等。

求解方程7 如果使用原始分段多项式轨迹，则计算复杂度很高，因为密集矩阵反演是不可避免的。相比之下，MINCO 在“轨迹表示”部分的线性复杂度操作大大减少了每次迭代中的计算开销。除了紧凑的参数表示外，总收敛速度加快了几个数量级。另一方面，由于约束被转换为目标，因此通过“分层安全保证”部分中描述的后处理来保证可行性。

 轨迹规划过程

 所提出的轨迹规划器运行如下。第 1 步。用户或软件给出了一个全局目标位置。第2步。规划器沿着到目标的方向在预定义的局部规划距离内选择一个局部目标，然后用初始猜测开始迭代。步骤3。在每次迭代中，求解器返回热启动优化的求解轨迹。步骤4。计算总惩罚j和梯度，然后返回步骤3之前发送回求解器。步骤5。返回并执行从当前位置到满足任务要求的局部目标的局部轨迹。第6步。在给定的周期(总是一秒或几秒)之后，或者当轨迹与新感知的障碍物发生碰撞时，通过返回步骤2来重新激活规划。重复此过程，直到无人机到达目标。在这项工作中，我们使用了一个开源的L-BFGS求解器(58)，它属于下一次迭代下一个候选轨迹的准牛顿优化方法的类别。

通用处罚

最大化平滑度

根据方程式 2，平滑度惩罚 Js 定义为平方 s 阶导数的积分:

           (9)

这个积分可以通过解析计算，因为MINCO轨迹可以表示为根据公式3的分段多项式。

最小化总时间

在大多数情况下，较短的飞行时间是可取的(1)，因此我们还最小化了加权总飞行时间

动态可行性

对于差分平面多直升机，通过限制轨迹导数的大小来保证动力学可行性。在我们的工作中，如果这些导数超过物理阈值，我们通过添加惩罚来限制速度、加速度和加速度的幅度，

 其中 vm、am 和 jm 分别是速度、加速度和加速度的大小的最大允许幅度；这里的 ti 和后面的部分遵循等式7中的相同定义。 我们直接将 Jd,v, Jd,a 和 J d,j 相加，因为它们具有相似的幅度，尽管具有不同的单元。

避障

避障是在杂乱现实世界构建的无序障碍物图上执行的。在我们的工作中，我们将障碍物建模为平面 ，其中我们将平面的障碍物一侧视为占用，另一侧视为自由 (14)。这里，s ∈ ℝ3 是平面上的一个点，v ∈ ℝ3 是指向自由边的法向量。然后，对于任何点 ，它与障碍物的距离定义为

                                                                                                  (15)

这是一个高度简化的障碍表示，但在我们之前对单个无人机导航的工作（14）中，它在保真度和计算开销之间显示了适当的平衡。{s, v} 生成的详细描述可以在补充材料中找到。具体来说，在台式计算机上生成平面通常需要 0.1 毫秒，因此足以在机载执行。根据 d0 的定义，如果且 惩罚障碍物清除,然后，避障惩罚 Jo 表示为

                     (16)

其中 do 是 p(t) 的函数，因此 Jo 是 MINCO 轨迹的函数

从单到群

群飞行进一步要求个体之间没有碰撞。在现实世界的使用中，我们假设机器人可以使用带宽受限的无线通信。因此，MINCO的紧凑参数在无线网络中广播。通过知道邻近机器人的轨迹，规划器可以准确地评估短期内任何时间戳的群分布和相对速度，通常为几秒钟。这个持续时间足以满足安全性，因为无人机总是在当前轨迹的末尾重新规划一个新的轨迹。互相回避也被表述为惩罚 Jw，当两个无人机太近时，它会产生很高的成本。因此，对于包含U架无人机的蜂群中的第u架无人机，Jw定义为

其中 和 =分别是第 u 架无人机和第 k 架无人机的轨迹。 和  之间的偏移量将它们对齐到相同的全局时间。Cw，称为群间隙，是两架无人机之间的最小安全间隙。矩阵 E ≜ diag (1,1,1/c) 将欧几里得距离转换为椭球距离，z 轴的小轴，以减轻转子的下洗风险。接下来，优化问题仍然不受约束，因此可以有效地解决。

编队期望

编队被定义为局部帧F中的固定顶点，它相对于世界帧移动和旋转。为了保持编队中，分配给顶点的每架无人机根据其他轨迹计算出的其他运动规划其轨迹。在接收到长期目标后飞行开始，编队需要严格沿直线l移动，连接当前位置和目标。F的x轴平行于l。然后，通过将编队形状与当前蜂群分布拟合来确定帧原点。这种编队推理可以在不久的将来检索编队位置，为每架无人机提供引导路径 g(t) 进行轨迹规划。然后编队惩罚 定义为

             (20)

为了加强连续性，我们假设轨迹定义域之外的均匀运动。

多视图跟踪和视频

为了在避开障碍物的同时记录参与者，RGB 相机必须在参与者和深度相机指向无人机飞行的方向。为了避免障碍物，我们将深度相机与无人机速度对齐。因此，调整深度相机视图的约束定义为(i)将轨迹速度ṗ(t)与预测参与者速度vp对齐

                         (21)

为了使参与者保持在具有适当大小的图像中，我们 (ii) 在参与帧 S 中强制执行首选无人机的位置 S ，它给出

                                 (22)

其中  是一个变换矩阵，它将  转换为世界帧。结合以上部分直接给出多视图跟踪惩罚

                             (23)

通过 CSI（相机串行接口）连接的 RGB 相机的视频使用 OpenCV（60）实现的 JPEG（联合摄影专家组）标准（59）压缩并存储在流中。对象检测依赖于 YOLOv5 (61) 神经网络，除了其他默认参数外，模型深度设置为 0.33，层通道设置为 0.50。它使用 NVIDIA TensorRT (62) 加速。所有上述软件都需要大量的计算资源和高带宽输入输出。使用先前已知的对象高度估计对象的相机帧上的 z 轴，并通过卡尔曼滤波器 (63) 进一步过滤其全局位置。

动态避障

从分散轨迹规划的角度，以与其他移动无人机相同的方式处理预测轨迹的动态障碍物。因此，避免动态障碍物仍然遵循“从单到群”部分中提出的公式，除了根据障碍物形状和体积的不同 E 和 Cw 值。在“Swarm playground”部分，使用标准自行车模型来模拟和预测动态障碍物的运动。

定位和漂移校正

关于总是需要高精度和鲁棒性的实际实现，我们使用VIO准确地获得精确和高频状态估计。然而，在没有外部定位设施的情况下，累积漂移是不可避免的，这可能会导致远距离紧凑飞行过程中的机器人间碰撞。为了分别估计和正确的定位漂移，我们利用相对距离测量以及它们从接收轨迹计算的位置。也采用了机载超宽带传感器测量相对距离(64)。对于包含 U 架无人机的蜂群中的第 u 架无人机，其在位置  处到第 k 架无人机的距离测量为，然后我们最小化总距离测量误差

 为了获取第u架无人机的位置，其中为最后一次漂移估计校正的最新里程计。注意，增加了的正则化项，以避免非唯一或不稳定的解，例如，当U = 2时，整个球面满足最小化。利用数值优化问题求解，也作为初始值。为了进一步平滑里程计，同时提高精度，我们估计了缓慢变化的漂移，并对它应用低通滤波器，而不是直接使用优化的。然后将漂移添加到VIO的最新里程计中，以产生校正的定位。

此外，具有真实位置的站式设施也可以合并到优化中，以确保全局一致性，如“密集互相回避评估”部分所示。请注意，全局帧最初需要大致一致；否则，非线性优化缺乏可靠的初始化。除了提高定位精度外，这种方法几乎没有额外的通信负担，因为其他无人机位置是根据轨迹计算的，UWB与轨迹广播网络共享不同的射频。在我们的系统解决方案中，我们使用VINS(视觉惯性导航系统)(5)作为VIO，Ceres求解器(65)进行优化。对我们真实世界实验和相应框图的有效性的评估在补充材料中给出。

分层安全保证

由于安全约束被转移到惩罚中，求解器的输出轨迹仍然是不可行的，因此需要进行轨迹规划后的后检查。如果违反了安全约束，规划器会增加其权重，然后进行另一次试验以提高找到满意解决方案的可能性。如果经过多次试验后仍然不可行，则终止当前规划，规划器在激活下一个重新规划之前等待 10 毫秒。然而，每个规划后的后检查只保证了该时间点的可行性，因为地图发生了变化，因此安全检查过程不断检查背景中的碰撞。一旦检测到不安全，这个过程就会立即激活重新规划。如果该试验失败，预测碰撞时间低于阈值，则生成紧急停止轨迹。停止后，计划尝试再次开始。这种回退保证了最严重的情况下的安全性，然后恢复任务。

手掌大小的无人机硬件

有实验都是在我们设计的、组装和发布的 114 毫米轮基微型平台上进行的，这里硬件列表为 (66)。平台的总重量小于 300 g，包括一个提供 11 分钟飞行时间的 100 克电池。无人机由以下五个子系统组成，如图 2A 所示。

1) 电源和运动套件。串联连接了3000mAh容量和7.4-V电压的两个LiPo电池，使用了4个6000kv无刷电机(模型1404)，具有3英寸、三叶螺旋桨，其推力重量比为2.4。采用具有15-A最大电流的四一电子速度控制器。螺旋桨安装在机身的底部，因此，强烈的下洗流不会直接吹到车身上。这样的设计根据我们的实验提高了飞行时间，悬停中使用的平均功耗约为 120 W。

 2)低级控制单元。建立了尺寸为16 mm × 32 mm的纳米级飞行控制单元(FCU)，运行PX4 Autopilot (67)。PX4标准的硬件由STM32 H7 MCU (68)和BMI088 IMU (69)组成，带有8GB内存卡进行日志记录。在这里，我们省略了除IMU之外的所有传感器，因为我们使用VIO运行定位，而不是依赖于气压计、磁力计或GPS。该单元负责低级角度控制和将IMU数据发送到高级导航单元。

3)高级导航单元。该单元运行所有本地化、规划、高级控制和其他特定于任务的代码，因此需要足够的计算性能。在我们的系统中，我们使用 NVIDIA Xavier NX (70)，这是一种强大的计算机，用于具有六核 CPU、384 核 GPU 的嵌入式和边缘系统，8 GB RAM。在我们的实验中，除了多视图视频之外，CPU 和 GPU 用法都低于 40%，这允许相当大的计算储备以获得额外的潜在用途。

4)传感器。我们解决了基本的传感器设置，以在保持高精度的同时小型化无人机的大小。我们使用来自FCU的灰度和深度相机Intel Realsense D430 (71,72)和IMU。D430 相机输出深度图像进行映射和立体灰度图像进行定位。超宽带模块是一个Nooploop LTPS (73)，内部有一个DW1000无线电芯片(74)。

5)无线通信模块。我们测试并实现了两种拓扑结构:(i)使用单接入点TP-LINK TL-XVR6000L路由器的星形形状，EDIMAX EW-7822UCL (75) USB WiFi适配器和(ii)通过PCIe(外围组件接口表达)接口使用AzureWave AW-CB375NF (76)的分散点对点自组织网络。第一个结构显示出更高的带宽，而第二个结构更适合补充材料中评估的大规模群体规模。