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

文章目录

四. 集群初始化
五. 去中心化群体状态估计

四. 集群初始化

在本节中，我们介绍了集群系统的每个 UAV 上运行的集群初始化过程的三个子模块。第一个子模块（第 IV-A 节）监控网络上的新队友无人机并校准相对于它们的时间偏移。第二个子模块（第 IV-B 节）通过 LiDAR 测量监控新的队友无人机，验证它们的身份，并通过轨迹匹配校准相对于集群的全局外部变换。第三个子模块（第 IV-C 节）监控从第二模块或网络接收到的外部更新，并通过新颖的因子图优化方法校准相对于其他队友的全局外部变换。

A. 网络与时间校准上的新队友监控

第一个子模块检测网络上的队友无人机，保持与队友的连接，并执行相对于它们的时间偏移校准。为实现这一目的，每个 UAV 会在 Ad-Hoc 网络中以固定的 1 Hz 频率持续广播其身份信息，包括其 UAV ID 和 IP 地址。这个身份信息通常称为“心跳”包，用于连接状态监控和通信状态维护。必要时，“心跳”包也可以加密以防止无人机信息泄露或网络攻击。在接收到来自队友的身份信息后，自身 UAV 将队友添加到队友列表中并保持连接状态。对于列表中的每个队友，自身 UAV 分配两种状态之一：“已连接”或“已断开”。当某个队友被添加到列表中时，其对应状态被初始化为“已连接”。如果自身 UAV 连续两秒未接收到队友的信息，则队友的状态会变为“已断开”。当从断开队友再次接收到身份信息时，状态会切换回“已连接”。

在网络上发现新队友后，校准相对于该队友的关键时间偏移。对于每个队友 UAV，采用基于精确时间协议（PTP）[55] 中对等延迟机制的分布式时间校准方法来获取对应的时间偏移。自身 UAV 将向队友 UAV 发送请求消息，并接收来自队友的响应消息。通过利用这些消息的时间戳，自身 UAV 可以计算相对于队友的时间偏移 (\tau_{ij})。为抑制随机误差或波动，该过程重复 30 次，并取 (\tau_{ij}) 的平均值。由于典型 UAV 飞行时间内的时钟漂移可忽略不计（例如，小于一小时），对任何 UAV 而言，一次时间偏移估计就足以满足大多数集群任务需求。也就是说，对于 UAV (i)，一旦其相对于 UAV (j) 的时间偏移 (\tau_{ij}) 被获得，对应的时间偏移校准就被视为完成，在没有响应消息的情况下关闭该过程。
算法 1：新队友检测、身份验证及全局外部校准
输入： 排除已知队友无人机后的 LiDAR 原始点云扫描数据，UAV 的里程计 $O^i_{T_i}$ 、时间变换数据 $T^i_{L_i}$ 、UAV $i$ 的姿态轨迹 $G^i_TJ$ ，轨迹匹配的阈值 $t h r$
输出： 队友 UAV 数量 $M$ 、UAV $i$ 的全局外部变换 $G^i_TJ$ 、UAV $i$ 在其全局参考坐标系下的 LiDAR 点云位置 $P^i_bj$

$b^i_p = \text{反射强度过滤}(b^i_p)$
$M^i_{b_p} = \text{快速欧几里得聚类}(b^i_p)$
for $m = 1 : M$ do
$G_{1_{pm}} = \text{临时轨迹跟踪}(\Delta t, G^i_{T_bi}, b^i_p, M^i_{b_p,m})$ ;
$T_{mj}.\text{推入}(G_{1_{pm}})$ ;
if $\text{轨迹退出}(G_{1_{pm}})$ then
for $j = 1 : N$ ; $\neq i$ do
$res_{T} = \text{轨迹匹配}(G^i_Tj, G_{1_{pm}})$ ;
if $res_{T} < thr$ then
$G_i TG_j = T_i$ ;
$KaTeX parse error: Double subscript at position 4: G_i_̲p_j = G_{pm}$ ;
break
end if
end for
end if
end for

当时钟漂移较大时，可以通过固定频率（例如 1 Hz）估计时间偏移。此机制适用于系统内的 UAV，且系统稳健到足以在某个队友失败时切换到其他队友的时间源。每个 UAV 会定期在网络上与每个队友的时间偏移进行自校准。校准的时间偏移 $t_j$ 被存储在哈希表中，其中键是 UAV 的 ID，值是时间偏移。当 UAV 接收到来自队友的数据时，该数据带有队友的时间戳。对于自身 UAV，接收到的数据会根据时间偏移进行修正，以保证时间一致性。

B. 通过 LiDAR 观测和外部校准检测新队友

对于队友列表中的任何 UAV，除了时间偏移校准，每个 UAV 还需要校准其空间位姿偏移，即 UAVs 之间的外部变换。此节描述了如何校准那些直接通过 LiDAR 观测到的队友的全局外部校准。

我们提出了一种新的反射过滤和聚类提取方法，以便更容易地检测到队友 UAV 的 LiDAR 点云测量。在检测到的物体轨迹聚集后，我们通过轨迹匹配机制实现快速的集群初始化以及全局外部校准。

为实现上述方法，对每个 UAV 的机身附着反射性标记点，以便被队友通过 LiDAR 识别和追踪。基于 LiDAR 观测的反射点的相对位置，可以进行外部校准。

假设点云集 $G^i T_{rm} = \{G^i p_{m,k} \mid k = 1, \ldots, K\}$ 表示队友 UAV $j$ 的 m-th 物体轨迹，其中每个 $G^i T_{j}$ 表示队友 UAV $j$ 观测到的轨迹点集。若以下轨迹匹配问题有唯一解，则 m-th 物体被识别为队友 UAV $j$ ：

$\arg \min_{G_i TG_j} \frac{1}{K} \sum_{k=1}^K \left\| G^i_{T_{pm,k}} - G_i TG_j G^j_{p b j,k} \right\|_2^2 < thr.$

考虑到可能的短程通信中断，部分数据可能会丢失。因此，我们仅选择同时戳匹配的点 $G^i_{p m, k}$ 参与轨迹匹配。为避免数据过多引起的计算延迟，使用一个滑动窗口来存储最新 $X$ 个匹配位置进行匹配。通过在函数轨迹匹配中调用所需的匹配点，来实现上述的轨迹匹配过程（如算法第 8 行所示）。

由于无唯一解时，物体的运动轨迹可能呈现直线趋势，我们通过下式评估目标对象形状：

$\mathcal{H} = \sum_{k=1}^K \left( G^i_{p_{m,k}} - G^{\mathcal{C}}_{p_{m,k}} \right) \cdot \left( G^i_{p_{m,k}} - G^{\mathcal{C}}_{p_{m,k}} \right)^T.$

若第二大奇异值超过给定阈值，则认为匹配成功。此过程确保了当目标为直线时能有唯一解，从而实现可靠的校准。
在这里插入图片描述
图 4. 新检测物体的初始化示意图，点云按反射率着色。此处，自身 UAV 需要在其视野内检测并识别其他队友 UAV。球体表示预测区域，包围盒中心表示跟踪器的更新位置。(a) 反射率过滤。(b) 通过剔除尺寸过大的物体来去除异常点。© 跟踪真实的潜在队友并累积轨迹。(d) 经过轨迹匹配后，物体被识别为具有正确 UAV ID 的队友 UAV。

在这里插入图片描述
图5. 基于去中心化因子图优化的全局外参校准的说明。为了处理因子图的规范自由度，我们插入了一个先验因子作为 $G_{i}=I$ 。注意，因子 ${}^{G_{i}}\check{T}_{G_{j}}$ 是从队友无人机接收到的全局外参变换或通过自身无人机直接轨迹匹配获得的。

TrajMatching( ${}^{G_{i}}\mathcal{T}_{j}$ , ${}^{G_{i}}\mathcal{T}_{m}$ )[57]。临时跟踪器轨迹 ${}^{G_{i}}\mathcal{T}_{m}$ 的匹配是使用所有接收到的队友无人机的轨迹 $G_{j}\mathcal{T}_{j},j=1,\cdots,N$ 进行的，直到匹配误差小于给定阈值，表明对象 m 本质上是具有无人机 ID j 的队友的观测，而 1 的解给出了全局外参 ${}^{G_{i}}\breve{T}_{G_{j}}$ 的初始估计。同时，第 m 个临时跟踪器将从临时跟踪器列表中移除，并成为一个将在状态估计模块（第 V 节）中使用的队友跟踪器。基于轨迹匹配的初始化流程示例如图4所示。

最终，通过轨迹匹配获得的外参 ${}^{G_{i}}{ T}_{G_{j}}$ 被发送到自身无人机的第三个子模块以及队友列表中的所有队友无人机。注意， ${}^{G_{i}}\check{T}_{G_{j}}$ 的发送只发生一次，因为在临时跟踪器被移除后，下一个周期不会执行轨迹匹配来产生外参 ${}^{G_{i}}\check{T}_{G_{j}}$ 。

C. 未直接观测队友的因子图优化全局外参校准

除了上述详细说明的基于轨迹匹配的识别方法外，还提出了一种新颖的去中心化因子图优化方法，用于校准未直接观测队友的全局外参变换，这加快了识别和群体初始化的过程。在Swarm-LIO2中，每架无人机将与队友列表中的所有队友无人机共享通过轨迹匹配获得的全局外参变换。然后，每架无人机构建并维护一个因子图（见图5），其中变量 $G_{i},G_{j},\cdots$ 是所有无人机（包括有或没有直接观测的队友）的全局参考框架，因子 ${}^{G_{i}}\check{T}_{G_{j}}$ 是任意两架无人机之间的全局外参变换，这些可以通过自身无人机使用轨迹匹配校准或从队友无人机接收。通过固定自身无人机的全局框架 $G_{i}$ ，它可以使用全局外参变换 ${}^{G_{i}}\check{T}_{G_{j}}$ 作为约束来求解所有其他与自身无人机在因子图中相连的无人机的全局框架。随后，每架无人机（无论是否直接观测）与自我无人机之间的全局外参可以推断为 $G_{i}\hat{T}_{G_{j}}=G_{i}G_{j}^{-1}$ 。

第三个子模块在第二个子模块之后运行，因此也以扫描速率循环运行。具体来说，它从第二个子模块接收外参 ${}^{G_{i}}\breve{T}_{G_{j}}$ 和从网络接收的外参 GkT $G_{l}$ 。如果接收到的外参（无论是来自第二个子模块还是网络）对应于因子图中之前不存在的新边，则将在因子图中创建对应于此外参的边。否则，接收到的外参将被丢弃以避免信息重复使用。另一方面，如果同一边上有多个全局外参变换，例如通过自身无人机 i 的轨迹匹配获得的 ${}^{G_{i}}\breve{T}_{G_{j}}$ 和通过队友无人机（通过网络接收）的轨迹匹配获得的 ${}^{G_{j}}\breve{T}_{G_{i}}$ ，这些全局外参变换的平均值被计算并用作因子，这可以有效节省因子图中的因子数量。如果因子图更新，则使用 iSAM2[58]执行优化过程，如果之前未发送，优化后的全局外参将作为初始估计 ${}^{G_{i}}\hat{T}_{G_{j}}$ 发送到状态估计模块，用于在线全局外参细化。

备注 1：如所示，第一个子模块与第二和第三个子模块以不同的频率并行运行。第一个子模块以 1 Hz 的频率运行，而第二和第三个子模块则以扫描频率运行。这三个子模块的递归特性使得集群能够在任务进行中发现、识别并校准相对于新加入的无人机的全局外部参数。

备注 2：对于这三个子模块，每个模块分为两个部分。三个子模块的第一部分分别监控网络中的新队友、通过 LiDAR 测量观察到的新队友或因子图中的新边。第二部分则分别进行时间校准、轨迹匹配和因子图优化。虽然第一部分以各自的频率运行，但第二部分仅在第一部分检测到新队友或新边时才运行。

五. 去中心化群体状态估计

在本节中，我们将介绍完全去中心化的群体状态估计模块，包括相互状态估计、基于ESIKF的自我状态估计和全局外参细化。

A. 相互状态估计

群体系统中每个无人机的一个关键任务是估计其他任何无人机 j 的状态 $G_{i} x_{b_{j}}$ （包括姿态 ${}^{G_{i}} T_{b_{j}}$ 和速度 ${}^{G_{i}} v_{b_{j}}$ ），称为相互状态估计。这对于各种群体应用非常重要，例如相互避碰、编队飞行等。然而，估计所有队友无人机的完整状态 ${}^{G_{i}} x_{b_{j}}$ 是高维任务，计算上要求很高。为了降低系统复杂性，我们提议仅在每个无人机上估计自我状态，表示为 ${}^{G_{i}}\overline{x}_{b_{i}}$ 。估计的自我状态通过网络交换。然后，无人机 i 可以通过直接将接收到的无人机 j 的自我状态投影到无人机 i 的全局框架中，使用全局外参变换 ${}^{G_{i}} T_{G_{j}}=\left({}^{G_{i}} R_{G_{j}},{}^{G_{i}} p_{G_{j}}\right)$ 来估计队友 j 的状态，表示为 ${}^{G_{i}}\overline{x}_{b_{j}}$ ：

$\begin{align*} & G_i\check{T}_{b_j}={}^{G_i} T_{G_j}{}^{G_j}\check{T}_{b_j},\\ & G_i\check{v}_{b_j}={}^{G_i} R_{G_j}{}^{G_j}\check{v}_{b_j}. \end{align*}\qquad(3)$

其中 ${}^{G_{j}}\breve{T}_{b_{j}}$ 是接收到的无人机 j 的自我状态。注意 $G_{j}\breve{T}_{b_{j}}$ 在无人机 j 上表示为 ${}^{G_{j}}\breve{T}_{b_{j}}$ （因为它是在无人机 j 上的估计），但在无人机 i 上有重音符号 $(\check{\cdot})$ （因为它作为无人机 i 的测量值）。

上述过程中的一个问题是它需要知道全局外参变换 ${}^{G_{i}} T_{G_{j}}=\left({}^{G_{i}} R_{G_{j}},{}^{G_{i}} p_{G_{j}}\right)$ 。尽管它们可以通过第 IV 节描述的初始化过程进行校准，但可能仍存在误差。我们提议在状态估计模块中与自我状态估计一起不断细化这些外参变换。表示细化后的外参变换为 ${}^{G_{i}}\widetilde{T}_{G_{j}}=\left({}^{G_{i}}\widetilde{R}_{G_{j}},{}^{G_{i}}\widetilde{p}_{G_{j}}\right)$ ，队友 j 的相互状态可以通过(3)计算，其中 ${}^{G_{i}} T_{G_{j}}=\left({}^{G_{i}} R_{G_{j}},{}^{G_{i}} p_{G_{j}}\right)$ 被替换为 ${}^{G_{i}}\widetilde{T}_{G_{j}}=\left({}^{G_{i}}\widetilde{R}_{G_{j}},{}^{G_{i}}\widetilde{p}_{G_{j}}\right)$ 。这种机制即使在由于遮挡或队友进出视野(FoV)导致频繁的相互观测丢失的情况下，也能实现平滑且稳定的相互状态估计。

为了在状态估计中细化外参变换，对于在初始化模块(第 IV 节)中校准过的每个新队友 j，我们将 ${}^{G_{i}} T_{G_{j}}=\left({}^{G_{i}} R_{G_{j}},{}^{G_{i}} p_{G_{j}}\right)$ 添加到无人机 i 的现有状态向量中，因此其值可以与其他状态一起在统一的 ESIKF 框架中估计。此外，从初始化模块校准的外参立即作为附加状态组件 ${}^{G_{i}} T_{G_{j}}$ 的初始估计 ${}^{G_{i}}\widehat{T}_{G_{j}}$ 。

B. 状态和协方差预测

我们首先介绍状态和协方差的预测方案。为了说明，我们选择无人机 i 作为自我无人机，并假设在初始化模块中已经找到并校准了 N-1 个队友无人机。设 $\tau$ 表示第 k 个激光雷达帧期间的 IMU 测量索引，离散状态转移模型如下所示：

$x_{i,\tau+1}=x_{i,\tau}\boxplus\left(\Delta t_{\tau}f_{i}\left(x_{i,\tau},u_{i,\tau},w_{i,\tau}\right)\right),\qquad(4)$

其中 $\Delta t_{\tau}$ 是两个连续 IMU 测量之间的时间间隔， $x_{i,\tau}$ 表示第 i 架无人机在第 $\tau$ 个 IMU 测量时的真实状态，其时间戳是 $t_{i,\tau}$ 。此外，我们使用符号 $\boxplus/\boxminus$ 定义在[59]中，以紧凑地表示状态流形上的“加法”。具体来说，对于状态流形 $\times R^n$ ， $\boxplus$ 操作及其逆操作 $\boxminus$ 定义如下：

$\begin{bmatrix}R\\ a\end{bmatrix}\boxplus\begin{bmatrix}r\\ b\end{bmatrix}=\begin{bmatrix}RExp(r)\\ a+b\end{bmatrix};\begin{bmatrix}R_{1}\\ a\end{bmatrix}\boxminus\begin{bmatrix}R_{2}\\ b\end{bmatrix}=\begin{bmatrix}\log(R_{2}^{T}R_{1})\\ a-b\end{bmatrix}$

其中 $R_{1}, R_{2} \in SO(3), r \in \mathbb{R}^3, a, b \in \mathbb{R}^n, \operatorname{Exp}(\cdot): \mathbb{R}^3 \to SO(3)$ 是 $SO (3)$ 上的指数映射， $\log(\cdot): SO(3) \to \mathbb{R}^3$ 是其逆对数映射。

状态向量 $x_i$ 、过程噪声向量 $w_i$ 和输入 $u_i$ （省略时间索引）定义如下：
$\begin{align*} & x_i\triangleq\left[\begin{array}{cccccc}{}^{G_i} R_{b_i}^T&{}^{G_i} p_{b_i}^T&{}^{G_i} v_{b_i}^T& b_{g_i}^T& b_{a_i}^T&{}^{G_i} g^T\\ \cdots&{}^{G_i} R_{G_j}^T&{}^{G_i} p_{G_j}^T&\cdots\end{array}\right]^T\in\mathcal{M},\\ & w_i\triangleq\left[\begin{array}{cccccc} n_{g_i}^T& n_{a_i}^T& n_{b_{g_i}}^T& n_{b_{a_i}}^T\end{array}\right]^T,\\ & u_i\triangleq\left[\begin{array}{cccc}\omega_{m_i}^T& a_{m_i}^T\end{array}\right]^T, \end{align*}\qquad(5)$

其中 $j=1,2,\cdots, i-1, i+1,\cdots, N$ 。
离散状态转移函数 $f_{i}$ 定义如下：

$f_{i} \triangleq \left[\begin{matrix}\omega_{m_{i}} - b_{g_{i}} - n_{g_{i}}\\ \frac{1}{2} \binom{G_{i}}{R_{b_{i}}}(a_{m_{i}} - b_{a_{i}} - n_{a_{i}}) + {}^{G_{i}}g) \Delta t_{\tau}\\ \frac{1}{G_{i}}R_{b_{i}}(a_{m_{i}} - b_{a_{i}} - n_{a_{i}}) + {}^{G_{i}}g\\ n_{bgi}\\ n_{bai}\\ 0_{3\times 1}\\ \vdots\\ 0_{3\times 1}\\ 0_{3\times 1}\\ \vdots\\ 0_{3\times 1}\\ \vdots\end{matrix}\right] \qquad(6)$

其中 $\omega_{m_{i}}, a_{m_{i}}$ 表示无人机 $i$ 的 IMU（陀螺仪和加速度计）测量值， $n_{g_{i}}$ 和 $n_{a_{i}}$ 是 IMU 测量的白噪声， $b_{g_{i}}$ 和 $b_{a_{i}}$ 是 IMU 的偏差。
偏差被建模为具有高斯噪声 $n_{b_{gi}}$ 和 $n_{b_{ai}}$ 的随机游走过程。状态向量 $x_{i}$ 中每个元素的含义在表 I 中介绍，状态流形 $\mathcal{M}$ 在 7 中定义，其维数为 $18 + 6 (N - 1)$ 。

$\mathcal{M}\triangleq\underbrace{SO(3)\times \mathbb{R}^{15}}_{\text{dim}=18}\times\underbrace{\cdots\times SO(3)\times \mathbb{R}^{3}\times\cdots}_{\text{dim}=6(N-1)}\qquad(7)$

根据 (4) 中的状态模型，在接收到新的 IMU 测量后，在 ESIKF 框架下执行状态和协方差的预测。更具体地说，通过将过程噪声 $w_{i,\tau}$ 设置为零来预测状态和协方差。预测的详细演示可以参考 [11,12]。

C. 误差状态迭代状态更新

在新的激光雷达扫描结束时间 $t_{i, k}$ 时，迭代执行更新步骤，融合点云测量和相互观测测量（如果有）。在以下各节中，我们将介绍点云测量的测量模型，以及在 [36] 中未出现的新型相互观测测量。

测量建模：在一般的 ESIKF 框架中，对于任何第 k 轮的测量 $y_{k}$ ，我们可以将测量模型写为

$y_{k} = h(x_{k}, v_{k})\qquad(8)$

其中 $h(x_{k}, v_{k})$ 是依赖于真实状态 $x_{k}$ 和测量噪声 $v_{k}$ 的测量模型，假设 $v_{k}$ 是零均值多元高斯噪声。为方便和简化描述，我们在以下公式中省略了下标 k。

一旦接收到新的激光雷达扫描，将执行运动补偿以获得未失真的点云。然后计算点到平面的距离以生成点云约束。运动补偿的细节可以参考 [11]。当前扫描的第 n 个未失真点投影到机体框架中，表示为 ${}^{b_{i}} p_{n}$ ，设 $u_{n}$ 表示全局框架 $G_{i}$ 中相应平面的法向量，该平面上有一个点 ${}^{G_{i}} q_{n}$ 。考虑到第 n 个点的激光雷达测量噪声 $n_{p,n}$ ，我们得到第 n 个点测量的测量模型为 [11,12]

$\underbrace{u_{n}^{T}(^{G_{i}}T_{b_{i}}\circ(^{b_{i}}p_{n}+n_{p,n})-^{G_{i}}q_{n})}_{h_{p,n}(x_{i},n_{p,n})}\qquad(9)$

这定义了一个关于包含自我姿态 ${}^{G_{i}} T_{b_{i}}$ 的状态向量 $x_{i}$ 的隐式测量方程。法向量 $u_{n}$ 和点 ${}^{G_{i}} q_{n}$ 是已知向量， $n_{p, n}$ 是点测量噪声，两者都可以参考文献 [11,12]。

除了点云测量外，相互观测测量也用于状态更新，这可以通过在初始化模块中从临时跟踪器演变而来的队友跟踪器获得。具体来说，使用在第 $V-B_{i}$ 节获得的预测姿态 ${}^{G_{i}}\widetilde{T}_{b_{i}}$ ，我们可以获取每个队友无人机 j 在无人机 i 的机体框架中描述的预测位置 ${}^{b_{i}}\widehat{p}_{b_{j}}=\left({}^{G_{i}}\widehat{T}_{b_{i}}^{-1G_{i}}\bar{T}_{G_{j}}\right)\circ^{G_{j}}\breve{p}_{b_{j}}$ 。

围绕预测位置 ${}^{b_{i}}\widehat{p}_{b_{j}}$ 的激光雷达点将从原始激光雷达点中移除。剩余的点 $b_{i}\mathcal{P}$ 将用于初始化模块中的 ReflectivityFiltering $\left({}^{b_{i}}\mathcal{P}\right)$ （第 IV-B 节算法 1，用于新队友检测）。此外，围绕预测队友位置的点将用于欧几里得聚类，以获得相互观测测量。如果聚类得到一个有效对象，该对象的质心位置将被视为无人机 j 被无人机 i 观测到的实际位置，称为无人机 i 相对于无人机 j 的“主动观测测量”，表示为 $b_{i}\breve{p}_{b_{j}}$ 。每个无人机将共享此主动观测测量并与所有队友通过 Ad-Hoc 网络接收队友的测量。从无人机 j 接收到的观测测量称为“被动观测测量”，表示为 $b_{j}\breve{p}_{b_{i}}$ ，代表无人机 i 被队友 j 观测到的自我位置。

主动观测测量 ${}^{b_{j}}\breve{p}_{b_{j}}$ 的明确测量模型可以通过使用真实的全局外参 ${}^{G_{i}} T_{G_{j}}$ 和无人机 i 的真实自我姿态 ${}^{G_{i}} T_{b_{i}}=\left({}^{G_{i}} R_{b_{1}},{}^{G_{i}} p_{b_{1}}\right)$ 将无人机 j 的真实位置 ${}^{G_{j}} p_{b_{j}}$ 投影到无人机 i 的机体框架中获得。进一步考虑到主动观测 $n_{a o, i j}\sim\mathcal{N}\left(0,\Sigma_{a o, i j}\right)$ 由于队友无人机 j 上的点测量不完整，主动观测测量的模型为：

${}^{b_{i}}\breve{p}_{b_{j}}=\left({}^{G_{i}}T_{b_{i}}^{-1}{}^{G_{i}}T_{G_{j}}\right)\circ{}^{G_{j}}p_{b_{j}}+{n}_{ao,ij}.\qquad(10)$

这个测量方程不幸地涉及到了无人机 j 的真实位置 ${}^{G_{j}} p_{b_{j}}$ ，这不是如 (5) 中定义的状态向量 $x_{i}$ 的一部分。为了解决这个问题，我们利用了从无人机 j 接收到的估计自我位置 ${}^{G_{j}}\breve{p}_{b_{j}}$ 及其协方差 $\Sigma_{p_{j}}$ 。然后，无人机 j 的真实位置被建模为 ${}^{G_{j}} p_{b_{j}}={}^{G_{j}}\breve{p}_{b_{j}}+n_{p_{j}}$ ，其中噪声 $n_{p_{j}}\sim\mathcal{N}\left(0,\breve{\Sigma}_{p_{j}}\right)$ 。因此，主动观测测量的测量模型可以推导为：

${}^{b_{i}}\breve{p}_{b_{j}}=\underbrace{\left({}^{G_{i}}T_{b_{i}}^{-1}{}^{G_{i}}T_{G_{j}}\right)\circ\left({}^{G_{j}}\breve{p}_{b_{j}}+n_{p_{j}}\right)+n_{ao,ij}}_{h_{ao,ij}\left(x_{i},n_{p_{j}},n_{ao,ij}\right)}\qquad(11)$

这定义了一个关于包含自我姿态 ${}^{G_{i}} T_{b_{i}}$ 和全局外参 ${}^{G_{i}} T_{G_{j}}$ 的状态向量 $x_{i}$ 的有效测量方程。接收到的队友位置 ${}^{G_{j}}\breve{p}_{b_{j}}$ 是已知的， $n_{p_{j}}$ 和 $n_{a o, i j}$ 是测量噪声。

同样地，无人机 i 的被动观测测量 ${}^{b_{j}}\breve{p}_{b_{i}}$ 的明确测量模型可以通过使用真实的全局外参 ${}^{G_{i}} T_{G_{j}}$ 和无人机 j 的真实自我姿态 ${}^{G_{j}} T_{b_{j}}=\left({}^{G_{j}} R_{b_{j}},{}^{G_{j}} p_{b_{j}}\right)$ 将无人机 i 的真实位置 ${}^{G_{i}} p_{b_{i}}$ 投影到无人机 j 的机体框架中获得。然后考虑到被动观测测量的测量噪声 $n_{p o, i j} \sim \mathcal{N}\left(0,\Sigma_{p o, i j}\right)$ ，测量模型为：

${}^{b_{j}}\breve{p}_{b_{i}}=({}^{G_{j}}T_{b_{j}}^{-1}{}^{G_{i}}T_{G_{j}}^{-1})\circ{}^{G_{i}}p_{b_{i}}+n_{po,ij}.\qquad(12)$

为了解决这个问题，我们利用从无人机 j 接收到的估计自我位置 ${}^{G_{j}}\breve{p}_{b_{j}}$ 及其协方差 $\Sigma_{T_{j}}$ ，将无人机 j 的真实姿态建模为 ${}^{G_{j}} T_{b_{j}}={}^{G_{j}}\check{T}_{b_{j}}\boxplus n_{T_{j}}$ ，其中噪声 $n_{T_{j}}\sim\mathcal{N}\left(0,\check{\Sigma}_{T_{j}}\right)$ 。因此，被动观测测量模型为：

$\begin{align*} & b_j\breve{p}_{b_i}=\underbrace{\left(\left({}^{G_j}\breve{T}_{b_j}\boxplus n_{T_j}\right)^{-1 G_i} T_{G_j}^{-1}\right)\circ{}^{G_i} p_{b_i}+n_{p o, i j}}_{h_{p o, i j}\left(x_i, n_{T_j}, n_{p o, i j}\right)}\\ &\end{align*}$

这定义了一个关于包含自我位置 ${}^{G_{i}} p_{b_{i}}$ 和全局外参 $G_{i} T_{G_{j}}$ 的状态向量 $x_{i}$ 的有效测量方程。接收到的队友姿态 ${}^{G_{j}} T_{b_{j}}$ 是已知的， $n_{T_{j}}$ 和 $n_{p o, i j}$ 是测量噪声。

总结来说，整个测量向量 y、观测函数 h 和观测噪声 v（为简化省略了下标 k）为：

$\begin{align*} & y=\left[\underbrace{\cdots, 0,\cdots}_{\text{点测量}},\underbrace{\cdots,{}^{b_i}\breve{p}_{b_j}^T,\cdots}_{\text{主动观测测量}},\underbrace{\cdots,{}^{b_j}\breve{p}_{b_i}^T,\cdots}_{\text{被动观测测量}}\right]^T,\\ & h=\left[\cdots, h_{p, n}^T,\cdots, h_{a o, i j}^T,\cdots, h_{p o, i j}^T,\cdots\right]^T,\\ & v=\left[\cdots, n_{p, n}^T,\cdots, n_{p_j}^T, n_{a o, i j}^T,\cdots, n_{T_j}^T, n_{p o, i j}^T,\cdots\right]^T. \end{align*}$
2) 相互观测测量的时间补偿：为了使测量模型 (11) 和 (13) 有效，涉及的状态和测量应该在同一时间。然而，由于不同无人机之间状态估计的异步性质以及传输延迟的存在，不同无人机的状态和测量通常是异步的。因此，有必要对接收的测量或状态与测量模型中的自我状态之间的时间不匹配进行补偿。虽然之前的工作 [36] 忽略了这种时间不匹配，但本文基于恒定速度模型仔细解决了这个问题。
对于主动观测测量模型 (11)，测量 ${}^{b_{i}}\breve{p}_{b_{j}}$ 是一组未失真的点，投影到扫描结束时间 $t_{i, k}$ （见第 IV-B 节）。然而，接收到的无人机 j 的位置 ${}^{G_{j}}\breve{p}_{b_{j}}$ 是在无人机 j 的系统时间 $t_{j, k}$ 估计的。为了在时间 $t_{i, k}$ 建立有效的测量模型，无人机 j 的位置 ${}^{G_{j}}\breve{p}_{b_{j}}$ 应该根据其估计速度 ${}^{G_{j}}\breve{v}_{b_{j}}$ 从估计时间（即 $t_{j, k}$ ）补偿到建立测量模型的时间（即 $t_{i, k}$ ），根据恒定速度模型：

${}^{G_j}\breve{p}_{b_j}^{\text{comp}}={}^{G_j}\breve{p}_{b_j}+{}^{G_j}\breve{v}_{b_j}\left(t_{i, k}-t_{j, k}+{}^i\tau_j\right),\qquad(15)$

应将其代入 (11) 以提供原始测量 ${}^{b_{i}}\breve{p}_{b_{j}}$ 。因此，具有时间补偿的结果测量模型为：

$\begin{align*} {}^{b_{i}}\breve{p}_{b_{j}}=&\left({}^{G_{i}} T_{b_{i}}^{-1}{}^{G_{i}} T_{G_{j}}\right)\circ\left({}^{G_{j}}\breve{p}_{b_{j}}\right.\\ &\left.+{}^{G_{j}}\breve{v}_{b_{j}}\left(t_{i, k}-t_{j, k}+{}^{i}\tau_{j}\right)+n_{p_{j}}\right)+n_{a o, i j},\end{align*}\qquad(16)$

这是一个关于包含自我姿态 ${}^{G_{i}} T_{b_{i}}$ 和全局外参 ${}^{G_{i}} T_{G_{j}}$ 的状态 $x_{i}$ 的测量方程。
对于被动观测测量模型 (13)，被动观测测量 $b_{j}\breve{p}_{b_{i}}$ 是从无人机 j 传输并在无人机 j 的系统时间 $t_{j, k}$ 估计的。为了在由 $t_{j, k}$ 指示的时间建立有效的测量模型，(13) 中的所有状态和其他测量也应在 $t_{j, k}$ 。接收到的无人机 j 的状态 ${}^{G_j} T_{b_j}$ 已经标记了 $t_{j, k}$ ，而自我位置 ${}^{G_{i}} p_{b_{i}}$ ，即在 $t_{i, k}$ 的状态，可以使用恒定速度模型进行补偿，如下：
$G_i p_{b_i}^{\text{comp}}={}^{G_i} p_{b_i}+{}^{G_i} v_{b_i}\left(t_{j, k}-t_{i, k}-{}^i\tau_j\right),\quad(17)$

应将其代入 (13) 以提供原始状态 ${}^{b_{j}} p_{b_{i}}$ 。具有时间补偿的测量模型因此为：

$\begin{align*} {}^{b_j}\breve{p}_{b_i}=&\left({}^{G_j}\breve{T}_{b_j}\boxplus n_{T_j}\right)^{-1}{}^{G_i} T_{G_j}^{-1}\circ\left({}^{G_i} p_{b_i}\right.\\ &\left.+{}^{G_i} v_{b_i}\left(t_{j, k}-t_{i, k}-{}^{i}\tau_j\right)\right)+n_{p o, i j},\end{align*} \quad(18)$

这是一个关于包含自我位置 ${}^{G_{i}} p_{b_{i}}$ 、速度 ${}^{G_{i}} v_{b_{i}}$ 和全局外参 $G_{i} T_{G_{j}}$ 的状态 $x_{i}$ 的测量方程。
3) 状态和协方差更新：基于激光雷达点测量模型 (9)、具有时间补偿的相互观测模型 (11) 和 (13)，我们利用迭代卡尔曼滤波器 (ESIKF) [53] 重复更新状态。这个过程将重复直到收敛，然后获得最优的状态估计和协方差。卡尔曼增益和更新步骤的详细计算可以参考 [11,12]。更新后，将根据第 V-D 节执行协方差重新初始化，以进行下一轮的状态估计。

D. 边缘化

在 5 中定义的状态维度随着群体规模线性增长，导致 ESIKF 的计算复杂度几乎呈立方增长。为了解决先前工作中[36]状态维度爆炸和计算复杂度的问题，我们提出了一种新颖的边缘化方法。在无人机系统的飞行中，由于激光雷达传感器的检测范围和视场（FoV）限制，无人机通常无法始终观察到所有队友无人机。观测到的队友（无论是主动还是被动）的全局外参变换持续激发，如 (11) 和 (13) 所示，而不是。因此，我们只需要更新能够观察到自身无人机的队友无人机的全局外参（贡献被动观测测量）或者是被自身无人机观测到的队友无人机的全局外参（贡献主动观测测量）。这是通过以下边缘化操作实现的。

为了简化，我们省略了代表无人机 i 的第 k 次估计的下标 k 和 i。在接收到第 k 次激光雷达扫描后，我们如第 V-Cl 节详细描述的那样识别相互观测。设 A 表示在当前扫描中观测到的队友无人机的集合，B 表示未观测到的队友无人机的集合。设 $x_{1}$ 表示由自身状态和相对于集合 A 中队友的全局外参组成的子状态，而 $x_{2}$ 表示由相对于集合 B 中队友的全局外参组成的补充状态。我们得到 $\operatorname{dim}(x_{1})=18+6K$ 和 $\operatorname{dim}(x_{2})=6(N-1-K)$ ，其中 K 代表具有相互观测的队友的数量。

观测（即， $\operatorname{dim}(\mathcal{A})=6K$ ）。此外，在当前轮的状态估计中，假设 $(\widehat{x},\widehat{P})$ 为经过正常 ESIKF 预测步骤（即第 V-B 节）后传播的状态和协方差。然后，它们可以被划分为：

$x\sim\mathcal{N}(\widehat{x},\widehat{P})=\mathcal{N}(\begin{bmatrix}\widehat{x}_{1}\\ \widehat{x}_{2}\end{bmatrix},\begin{bmatrix}\widehat{\Sigma}_{11}&\widehat{\Sigma}_{12}\\ \widehat{\Sigma}_{21}&\widehat{\Sigma}_{22}\end{bmatrix}).\qquad(19)$

由于 $x_{2}$ 由于缺乏持续激励将不会被更新，我们将其从 $x$ 中边缘化，导致两个子状态的先验分布：

$x_{1}\sim\mathcal{N}(\widehat{x}_{1},\widehat{\Sigma}_{11}),\quad x_{2}\sim\mathcal{N}(\widehat{x}_{2},\widehat{\Sigma}_{22}).\qquad(20)$

为了更新子状态 $x_{1}$ ，我们注意到测量模型

$y=h(x,v)=h(x_{1},v_{1}),\qquad(21)$

其中 y 包括点测量和相互观测测量（主动和被动），这些都只依赖于 $x_{1}$ 。然后， $x_{1}$ 可以通过将先验分布 $x_{1}\sim\mathcal{N}(\widehat{x}_{1},\widehat{\Sigma}_{11})$ 与测量 y 融合，遵循正常的 ESIKF 更新步骤（即第 V-C3 节）进行更新。假设更新后的状态估计和协方差分别为 $\overline{x}_{1}$ 和 $\overline{\Sigma}_{11}$ 。然后，我们有 $x_{1}\sim\mathcal{N}\left(\widehat{x}_{1},\widehat{\Sigma}_{11}\right)$ 并且子状态 $x_{2}$ 仍然保持在 $x_{2}\sim\mathcal{N}(\widehat{x}_{2},\widehat{\Sigma}_{22})$ 。现在 $x_{1}$ 和 $x_{2}$ 是两个独立的分布，它们应该在随后的 ESIKF 步骤中分别发展。具体来说，对于 $x_{2}$ ，它受其状态转移函数的约束：

$x_{2,\tau+1}=x_{2,\tau}\qquad(22)$

而对于 $x_{1}$ ，它受

$x_{1,\tau+1}=x_{1,\tau}\boxplus(\Delta t_{\tau}f_{1}(x_{1,\tau},u_{\tau},w_{\tau})),\qquad(23)$

其中 $f_{1}\left(x_{1,\tau},u_{\tau},w_{\tau}\right)$ 取 (6) 中的 $f\left(x_{\tau}, u_{\tau}, w_{\tau}\right)$ 的前 $18 + 6 K$ 个元素。

在下一轮的 ESIKF 中，两个子状态将从它们各自的初始分布 $x_{1}\sim\mathcal{N}(\bar{x}_{1},\bar{\Sigma}_{11})$ 和 $x_{2}\sim\mathcal{N}(\widehat{x}_{2},\bar{\Sigma}_{22})$ 开始传播，并遵循它们各自的状态转移函数 (23) 和 (22)。这个过程可以通过从初始分布 $x\sim\mathcal{N}(\bar{x},\bar{P})$ 传播完整系统紧凑地表达，如下所定义

$\bar{x}=\begin{bmatrix}\bar{x}_{1}\\ \widehat{x}_{2}\end{bmatrix},\bar{P}=\begin{bmatrix}\bar{\Sigma}_{11}&0\\ 0&\widehat{\Sigma}_{22}\end{bmatrix}.\qquad(24)$

将后验分布 $x_{1}\sim\mathcal{N}\left(\bar{x}_{1},\bar{\Sigma}_{11}\right)$ 和先验分布 $x_{2}\sim\mathcal{N}\left(\widehat{x}_{2},\widehat{\Sigma}_{22}\right)$ 打包到联合分布 $x\sim\mathcal{N}(\bar{x},\bar{P})$ 中被称为“协方差重新初始化”。有了协方差重新初始化，下一步的传播可以简单地遵循完整系统的标凈 ESIKF 预测步骤 (4)，详见第 V-B 节。

E. 退化评估

先前提出的 ESIKF 将更新队友无人机的全局外参以及自我状态。然而，只有在激光雷达扫描包含足够的几何特征时，更新才有效。在一些极端环境中，激光雷达传感器可能会遇到退化，其中点云未能提供足够的约束来确定其自我姿态，使得无法从相互观测测量中区分全局外参和自我运动，这是我们先前工作[36]所面临的问题。为了解决这个问题，我们提出自动检测激光雷达退化。如果发生退化，先前估计的全局外参将与相互观测测量一起使用，以提供确定自我姿态的约束。两种情况之间的切换（即，与自我状态一起更新全局外参，以及使用当前估计的全局外参更新自我状态）可以通过如下边缘化操作自动实现。

当激光雷达退化发生时，我们通过将 A 设置为 null 并将 B 设置为所有队友无人机的完整集合，将所有全局外参从状态向量中边缘化出去。因此，子状态 $x_{1}$ 只包括自我状态， $\operatorname{dim}\left(x_{1}\right)=18$ ，而 $x_{2}$ 包含相对于所有队友的全局外参变换， $\operatorname{dim}\left(x_{2}\right)=6(N-1)$ 。对于测量模型 (21)，我们将其重写为

$y=h(x,v)=h(x_{1},x_{2},v)=h(x_{1},\underbrace{[x_{2},v]}_{v_{\text{ext}}})=h(x_{1},v_{\text{ext}}),\qquad(25)$

其中边缘化的子状态 $x_{2}$ 是一个外源随机信号（即，它与状态 $x_{1}$ 独立），就像测量噪声 $v$ 一样，因此它与 $v$ 组合形成扩展的测量噪声 $V_{\text{ext}}$ 。通过遵循 (22) 传播其子系统获得“测量噪声” $x_{2}$ 的分布。包括 $x_{1}$ 的更新和随后的预测步骤在内的其余步骤，将与第 V-D 节中的步骤相同。除了上述的边缘化外，无人机 i 的相互观测噪声 $v=\left[n_{a o, i j}, n_{p o, i j}\right]$ （见第 V-CI 节）将被调整到较小的值，以提供足够的约束用于自我姿态确定。

为了实现上述操作，需要一个退化评估模块。受 [60,61] 的启发，我们通过对 $h_{p,n}(x,0)$ 的雅可比矩阵 $J_{T}$ 实施奇异值分解（SVD）来评估无人机 i 的退化情况，关于自我姿态 ${}^{G_{i}}T_{b_{i}}$ ：

$J_{T}=\begin{bmatrix}-u_{n}^{TG_{i}}R_{b_{i}}\lfloor^{b_{i}}p_{n}\rfloor_{\wedge}&u_{n}^{T}\end{bmatrix},\qquad(26)$

其中符号 $\lfloor a\rfloor_{\wedge}$ 表示向量 $a\in \mathbb{R}^{3\times 1}$ 的斜对称矩阵，该矩阵映射叉积操作。通过计算 $J_{T}$ 的奇异值，找到最小的 $\lambda$ ，并将其与预定义的退化阈值 $\epsilon_{d}$ 进行比较，我们可以获得评估结果。如果 $\lambda<\epsilon_{d}$ ，则认为无人机 i 遇到了激光雷达退化，上述相应的响应将被激活用于本轮更新。
值得一提的是，所提出的方法可以在两种模式之间自动切换：当检测到退化时，使用全局外参变换和相互观测测量来准确确定自我状态；当没有退化发生时，使用激光雷达的点云测量来细化全局外参状态。

F. 状态估计结果的广播

状态估计完成后，包括更新后的自我姿态 ${}^{G_{i}}\bar{T}_{b_{i}}$ 、速度 ${}^{G_{i}}\bar{v}_{b_{i}}$ 、姿态协方差 $\bar{P}_{i}$ 和细化的全局外参变换 ${}^{G_{i}}\bar{T}_{G_{j}}$ 的结果将通过去中心化的 Ad-Hoc 网络与所有队友无人机共享。发送给队友的自我姿态和速度用于他们后续的相互状态估计，参考第 3 节。发送给队友的自我姿态、姿态协方差和细化的外参变换用于构建他们相互观测测量（第 V-C1 节）以进行下一步估计。
备注 3： 估计结果的广播还将导致细化的全局外参变换与在任务中途加入群体的新无人机共享。共享的外参将触发新无人机的因子图更新，通过插入从网络接收到的细化外参变换。优化因子图将获得新无人机与现有群体之间的外参。另一方面，共享的外参变换不会触发群体中现有无人机的任何因子图更新，因为这条边已经在因子图中存在。