Swarm-LIO2: Decentralized, Efficient LiDAR-inertial Odometry for UAV Swarms论文翻译一

---

摘要

Swarm-LIO2论文翻译二

原文：原文
代码：github

摘要—空中群体系统在合作探索、目标跟踪、搜索与救援等多个方面具有巨大潜力。高效、准确的自我状态和相互状态估计是完成这些群体任务的关键前提，但这仍然是具有挑战性的研究课题。本文提出了Swarm-LIO2：一种完全去中心化、即插即用、计算高效且带宽高效的激光雷达-惯性里程计，适用于空中群体系统。Swarm-LIO2采用去中心化的即插即用网络作为通信基础设施，仅交换带宽高效且低维的信息，包括身份、自我状态、相互观测测量和全球外部变换。为了支持新队友参与者的即插即用，Swarm-LIO2自动检测潜在队友无人机并初始化时间偏移和全球外部变换。为提高初始化效率，提出了新颖的基于反射率的无人机检测、轨迹匹配和因子图优化方法。在状态估计方面，Swarm-LIO2在高效的ESIKF框架内融合激光雷达、IMU和相互观测测量，同时仔细补偿时间延迟并建模测量，以增强准确性和一致性。此外，所提出的ESIKF框架在激光雷达退化的情况下利用全球外部进行自我状态估计，或者在其他情况下与自我状态估计共同优化全球外部变换。为了增强可扩展性，Swarm-LIO2在ESIKF中引入了一种新颖的边际化方法，防止随着群体规模的增加而导致计算时间增长。大量仿真和真实世界实验表明，该方法对大规模空中群体系统和复杂场景具有广泛的适应性，包括GPS受限场景、相机或激光雷达的退化场景。实验结果展示了厘米级的定位精度，优于其他先进的单无人机系统的激光雷达-惯性里程计。此外，多种应用展示了Swarm-LIO2作为各种空中群体任务可靠基础设施的潜力。

图 1. 一个由五架无人机组成的空中群体系统在野外飞行，其中 Swarm-LIO2 作为自我和相互状态估计器。

---

一、介绍

近年来，多机器人系统，特别是空中群体系统，在合作探索、目标跟踪、搜索与救援等多个领域展现出了巨大的潜力。得益于其卓越的团队协作能力，群体系统能够在复杂场景中完成各种任务，即使在单个机器人面临退化环境时也能有效运作。对于单个机器人系统，成熟的状态估计技术提供了准确的自我状态估计，作为各种自主任务（如轨迹规划和运动控制）的关键前提。对于机器人群体系统，状态估计同样发挥着重要作用，其中每个机器人需要估计自身无人机的状态（即自我状态估计）以及其他队友无人机的状态（即相互状态估计）。准确和稳健的自我状态和相互状态估计对机器人群体协作完成任务至关重要。
在过去几十年中，已采用多种传感器和设备来实现机器人群体系统的可靠状态估计。GPS和RTK-GPS通常用于户外环境中的自我定位，这在之前的研究中已有报道。对于GPS受限的环境，运动捕捉系统和基于锚点的超宽带（UWB）系统已被用于多机器人系统的状态估计。这些方法通常依赖于固定的地面站，导致系统集中化，容易出现单点故障。最近的研究中，摄像头因其轻巧设计、低成本和丰富的色彩信息而成为多机器人系统中的流行选择。这些基于摄像头的系统通常与惯性测量单元（IMU）和无锚点UWB结合使用，以提供更稳健的状态估计。然而，摄像头容易受到照明不足的影响，且缺乏直接的深度测量，导致在计算三维测量时的计算复杂度较高。尽管补充的无锚点UWB可以提供距离测量，但它容易受到多路径效应和环境障碍物遮挡的影响，从而降低整体系统的准确性。近年来，三维激光雷达（LiDAR）传感器因其能够在各种照明条件下提供直接、准确的长距离三维测量而获得了越来越多的关注。虽然传统的机械旋转LiDAR通常价格昂贵且体积较重，但激光雷达技术的最新进展已经推出了成本效益高且轻便的LiDAR，这些LiDAR适合部署在移动机器人上，特别是无人机（UAV）上。这些LiDAR不仅使得基于激光雷达的无人机自主导航系统得以开发，而且为群体系统中的状态估计开辟了新的可能性。
基于上述LiDAR的优势，本文旨在开发一种完全去中心化、即插即用、计算高效且带宽友好的状态估计方法，适用于基于LiDAR-惯性测量的空中群体系统。完全去中心化意味着整个系统的通信硬件到算法软件的任何模块中都不存在主控代理，从而避免了单点故障。即插即用意味着代理可以自动加入群体，并在任务前或任务进行中与其他队友轻松协作。由于无人机的有效载荷容量有限，系统还必须具备计算高效性和带宽高效性，以应对计算资源和网络带宽的显著限制。

我们将空中群体状态估计的任务分解为两个关键的在线模块：初始化和状态估计。在初始化模块中，每个无人机需要检测和识别可能的新队友无人机，并与找到的队友进行时间偏移和全球外部变换的标定。为实现简单而有效的队友检测，每个无人机上附加了反光带，使得队友无人机可以通过激光雷达的反射率测量轻松检测。该队友检测在每次激光雷达扫描测量中实时进行，从而能够在任务进行中实时检测新队友无人机。对于每个检测到的新无人机，通过轨迹匹配获取其身份和全球外部变换，同时通过因子图优化迅速标定网络上找到的其余队友无人机的全球外部变换。此外，通过去中心化的Ad-Hoc网络交换低维数据，可以高效、完全去中心化地实现队友监测和时间标定。

在状态估计模块中，群体系统中的每个无人机实时执行稳健且精确的自我状态估计以及相互状态估计。估计每个无人机中所有队友的完整状态计算量大，因此我们建议在每个无人机上仅估计自我状态，同时针对队友精细化全球外部变换。自我状态和全球外部变换在误差状态迭代卡尔曼滤波器（ESIKF）框架内高效估计，通过紧密融合激光雷达点云测量、惯性测量单元（IMU）测量和观察到的队友位置（即相互观察测量），这些测量通过精心的测量建模和时间补偿得到了增强。在每次状态估计步骤中，我们将所有未在激光雷达中观察到的队友无人机的外部状态进行边缘化。该状态边缘化以及退化评估方法防止了状态维度（因此计算复杂性）随着群体规模的增长，有效提升了我们系统对更大规模群体的可扩展性。

本文是我们之前工作的一项扩展，提出了群体激光雷达-惯性里程计的一般框架。与之前的工作相比，本文提出了五项关键扩展：

1. 因子图优化：用于高效的队友识别和全球外部标定，显著降低了群体初始化的复杂性和能耗。具体而言，具有N个无人机的群体初始化所需的飞行次数从O(N)减少到O(1)。

2. 新颖的状态边缘化策略和激光雷达退化评估方法：减轻计算负担，增强群体的可扩展性。边缘化将状态估计复杂性的增长率从立方降低到次线性。

3. 详细的测量建模和精心设计的相互观察测量的时间补偿：用于补偿由于不同无人机之间的传感器异步测量导致的时间不匹配。

4. 全面的仿真和实际实验：验证了Swarm-LIO2的有效性，即支持大规模群体（在现实世界中测试5个无人机，如图1所示，在仿真中测试40个无人机），在退化场景中表现稳健，并允许群体规模的动态变化，能够在线加入或退出任何队友无人机。

5. 开放源代码实现：提出的系统称为Swarm-LIO2，包括算法的源代码和我们空中平台的硬件设计。

二. 相关工作

在本节中，我们首先回顾机器人群体系统状态估计的主流框架。然后讨论现有的群体初始化方法，这是群体状态估计的核心模块。

A. 机器人群体的状态估计

在过去几年中，多机器人系统蓬勃发展，提出了一些优秀的协作SLAM方法，用于实现地面机器人群体系统的稳健自我状态估计和一致性映射。Chang等人提出了一种多机器人SLAM系统，其中每个机器人将其单机器人里程计结果和构建的子地图发送到集中式基站，以执行回环闭合检测和联合位姿图优化。

在机器人群体系统中，主流的状态估计方法大致可以分为两类：基于中心化的和基于分散化的。在[37]中的轮式机器人平台上，安装了Velodyne LiDARs和完整的移动LiDAR扫描系统。类似地，在[39]中，中心化的计算机需要接收所有机器人的信息，以实现闭环校正和全局优化，并用于数据收集的中心化平台。在[39]中包含了摄像头和Ouster LiDAR。这些上述的中心化系统容易因单点故障导致系统崩溃，促进了分散化方法的发展。

在[40]中，Lajoie等人提出了一种分散式的多机器人SLAM方法，每个机器人都能够仅依靠自身的计算资源完成任务。在[38]、[40]中，紧凑的描述符得以交换，这显著降低了基于地图信息的交流需求和群体扩展时的通信负担。上述的所有方法可分为基于环境特征的方法，这些方法的明显弱点在于其对特征丰富的环境依赖性较强，且通信带宽相对较低。

相较于地面机器人，由于航空载具的有效负载和续航能力的限制，轻量级的传感器配置、更高效的算法和低带宽的通信更加适用于空中群体系统。为了满足这些要求，一些空中群体系统利用视觉惯性测距（VIO）来实现自我估计，这比基于激光雷达的方法更加轻量，且不需要高计算成本。

尽管独立的VIO易于采纳，但这些方法在群体任务中往往受限于漂移、低精度和长距离运行问题。为了缓解这些VIO漂移，诸如在[43]中提到的方案，通过在群体中共享位置信息来进行漂移补偿。在[20]、[27]、[31]中，MUVIO被纳入系统中作为摄像头的补充传感器，用于改善群体中的相对距离约束。

然而，基于视觉的方法在低可见度环境中效果较差，激光雷达提供了许多新的机会。激光雷达的长程深度测量能力能显著提升状态估计的准确性，但其高通信负担和高计算成本对群体系统的扩展性提出了挑战。

B. 群体初始化

机器人群体系统初始化的关键部分通常包含机器人检测、机器人识别和全局外参校准。

机器人检测

基于学习的机器人检测方法被广泛用于基于视觉的群体系统中。例如，Nguyen 等人 [45] 提出了一个基于视觉惯性测量的 UAV 本地化系统，其中 MAVNet 被用来检测其他队友机器人。Xu 等人 [20] 提出了一个使用 YOLOv3-tiny 进行队友机器人检测的视觉惯性 UWB 相互状态估计系统。这些基于学习的检测方法通常需要预先标定网络训练，从而增加了时间和计算消耗。在 [51] 中，每个机器人都配备了一个标记，以检测动态障碍物，该过程既耗费记忆又费时。同样地，在我们之前的工作 [36] 中，将反射带附加到每个机器人（UAV）上并利用 LiDAR 传感器来检测队友机器人。这种检测方法简单有效，避免了繁琐的网络训练。

识别和全局外参校准

在完全去中心化的系统中，通常情况下，每个 UAV 都会在其自身的全局框架中估计状态。因此，在检测到可能是队友的物体后，每个机器人需要识别其他队友，并校准相应的全局外参变换。在现有文献中，全局外参通常通过测量 UAVs 之间的距离来离线校准 [27, 28]，从而得到粗略的全局外参值。随着群体规模的增加，即使全局外参参数中的小偏差也会导致显著的漂移。在 [20, 21, 45] 中，全局外参参数在线估计，然而准确的初始化估计是必要的。Tian 等人在 [38] 中通过构造一个截断最小二乘问题来校准外参变换，这部分利用了回环闭合结果和里程计估计。该方法需要环境信息交换，导致较高的通信负载。Chang 等人 [37] 提出了一个在 3D 空间中使用已知 3D 坐标的标签来执行全局外参校准的系统。然后，每个机器人上的 LiDAR 传感器会对三个标记进行分割，并通过最小化所有标记位置三元组之间的距离来计算全局外参。相比于 [37]，我们将反射带附加在无人机上，而不是在环境中，这支持在起飞区域外的初始化。与一次性校准 [27, 28] 或依赖良好初值的在线估计方法 [20, 21, 45] 不同，我们利用我们之前工作中提出的轨迹匹配方法 [36] 和因子图优化来校准全局外参参数，并在后续的群体状态估计中不断优化。整个过程完全自动化，不需要初始值。

符号	解释
田/日	在状态流形上的封装“boxplus”和“boxminus”操作。
$t_{i,k}$	第 k 次无人机 i 的激光雷达测量的时间戳。
$x_{i,k},{\hat{x}}_{i,k},{\hat{x}}_{i,k}$	在时间戳 $t_{i,k}$ 时，无人机 i 的真实、预测、更新状态。
${\hat{x}}_{i,k}$	在时间戳 $t_{i,k}$ 时，真实状态与其估计值之间的误差。
$G^i{T}_{bi}+G_i{v}_{bi}$	无人机 i 使用的测量值。无人机 i 在其自己的全局框架 $G_i$ 中的姿态（包括姿态和位置）和线速度。
$b{g}_i,b_{ai},G_{\text{I}}$	无人机 i 的陀螺仪偏差、加速度计偏差和重力向量。
	无人机 i 状态的协方差。
$P_i$	无人机 j 和无人机 i 之间的时间偏移。从无人机 j 的全局参考框架到无人机 i 的全局参考框架的外参变换，包括旋转 ${}^{G_i}{R}_{G_j}$ 和平移 ${}^{G_i}{p}_{G_j}$。
	无人机 i 估计的无人机 j 的状态。
	无人机 j 相对于无人机 i 的相对位置，以后者测量并由后者的机体框架表示，也称为主动相互观测。
N	群体中无人机的总数。

机器人将分割三个标记物，并通过最小化所有三元组标记位置之间的距离来计算全局外参。与[37]相比，我们是在无人机上而不是环境中附加反光带，这支持在起飞区域外进行初始化。与一次性校准[27,28]或依赖于良好初始值的在线估计[20, 21, 45]不同，我们利用我们之前工作中提出的轨迹匹配[36]和因子图优化来校准全局外参参数，并在随后的群体状态估计中不断细化它们。整个过程是完全自主的，不需要初始值。

三. 系统概览

在本节中，我们概述了 Swarm-LIO2 的结构并简要介绍了其模块。为了帮助理解所提出的系统，我们在表 I 中定义了一些重要的符号。我们使用 o 来紧凑地表示点 p∈ R3x1 在姿态 T=(R,t)∈SE(3) 下的刚性变换，即 Top Rp+t。

考虑一个由 N 架无人机组成的空中群体系统，每架无人机都配备了激光雷达和惯性测量单元（IMU）。为了实现去中心化的群体状态估计，每架无人机都需要自动检测系统中的所有队友无人机，并实时估计自身（例如，自我状态估计）以及其他所有队友（例如，相互状态估计）的状态。由于机载计算资源有限和系统维度高，在同一架无人机上同时进行自我状态和相互状态估计具有挑战性。因此，Swarm-LIO2 在每架无人机上估计自我状态并在队友之间广播自我状态。由于自我状态是在每架无人机自己的全局参考框架（即第一个IMU框架）中执行的，所有无人机对的全局框架之间的外参变换也需要校准。有了校准的全局外参，接收到的队友自我状态可以投影到自身的全局参考框架中，从而实现相互状态估计（见图 2）。

总结上述分析，Swarm-LIO2 有两个关键模块。第一个模块是在线初始化，其中每架无人机检测所有队友并与检测到的队友执行时间和空间校准。设 i 表示自身无人机，j 表示检测到的队友候选，由于不同无人机的计算机时钟通常是异步的，因此需要校准任何两个无人机 i,j 之间的时间偏移 iTj，这对于无人机间的数据融合至关重要。然后，自身无人机需要验证队友身份（无人机 ID，这是每个无人机一旦制造完成就分配的唯一数字），如果成功，则校准全局外参变换 ${}^{G_i}{T}_{G_j}=({}^{G_i}{R}_{G_j},{}^{G_i}{p}_{G_j})\in SE(3)$。

第二个模块是状态估计，旨在通过融合自我激光雷达和IMU数据以及相互观测测量来实时估计每架无人机的自我状态（例如，姿态，速度）。当估计的自我状态使用全局外参变换投影到队友的全局框架时，初始化阶段发生的小外参误差如果无人机的行驶距离很长，可能会导致大的相互状态估计误差。为了减轻这种误差，Swarm-LIO2 在线细化全局外参变换以及自我状态。

Swarm-LIO2 的两个模块在群体系统的每架无人机上并行运行，详细情况见图 3。对于初始化模块（第 IV 节），它进一步包含三个并发运行的子模块。第一个子模块监控网络上的新队友无人机并校准其时间偏移 ${}^i{\tau }_j$（第 IV-A 节）。第二个子模块检测在激光雷达点云中观察到的新队友，并校准其全局外参变换 ${}^{G_i}{T}_{G_j}$（第 IV-B 节）。校准后的全局外参随后发送到自身无人机和队友无人机网络上的第三个子模块。然后，第三个子模块接收来自自身无人机或队友无人机网络上的第二个子模块的全局外参，基于此，通过因子图优化校准未在激光雷达点云中观察到的队友的全局外参（第 IV-C 节）。一旦校准了相对于无人机 j 的全局外参 G1TG3，无人机 j 就被视为一个有效的队友，其状态将被添加到状态估计模块中进行估计。同时，外参 GisT 被发送到状态估计模块进行进一步细化。

图 3. 提出的空中群体系统状态估计框架。虚线箭头表示消息只发送一次，而实线箭头表示消息持续以扫描速率发送。

对于状态估计模块（第 V 节），它估计集群状态，包括每个无人机的自我状态和相对于所有队友的全局外部变换。为减少状态维度，Swarm-LIO2 执行一个边缘化步骤（第 V-D 节），通过退化评估来评估当前 LiDAR 测量的退化情况，由 (Z_i) 指示，并执行进一步的边缘化（第 V-E 节）。边缘化状态随后被估计在误差状态迭代卡尔曼滤波器（ESIKF）框架内，通过状态预测（第 V-B 节）和迭代更新（第 V 节）来执行测量建模（第 V-C1 节）。测量包含 LiDAR 点云和相互观测测量（即，由自身 UAV 观测到的队友位置，表示为 (^{b_i}p_{b_j})，以及从队友接收到的由队友观测的自身位置，表示为 (^{b_j}p_{b_i})）。相互观测具有时间不匹配，这些测量在时间上被补偿（第 V-C2 节）。状态估计结果最终传送给其他队友 UAV，供其下一轮状态估计参考（第 V-F 节）。在 Swarm-LIO2 中，所有信息通过基于 IEEE 802.11 架构的完全分布式 Ad-Hoc 网络进行交换（即，IBSS），该架构广泛支持常用 WiFi 模块，可通过编程 WiFi 驱动程序进行配置 [54]。