多机器人协同感知1——问题建模

1. 多机系统解决的本质问题是什么？

看完论文回头补充~

2. Landmark

下面这张图表示多个机器人的位姿图和landmark：

pose graph & landmark 示意图

这里衍生出几个问题：什么是landmark？landmark在单机和多机系统中的作用分别是什么，对应的formulate的问题又是什么？对于包含全局地图（Mesh或点云）来说，是否需要优化landmark，以及如何保证不同机器人看到的landmark的一致性以及构建的全局地图的一致性。

2.1 什么是 Landmark？

在 SLAM 系统中，理解 Landmark（路标/特征点）需要将其在物理前端与数学后端剥离开来看。

物理视角（前端锚点）： Landmark 是现实环境中的静止、显著且能被重复观测的物理特征。不考虑线特征或语义信息的时候，landmark就是前端提取的特征点对应的3D空间点。在视觉 SLAM 中，它们是被提取出的角点、边缘（如 ORB、FAST 特征点），甚至可以是高阶的线、面或语义物体。前端的核心任务是通过描述符匹配和几何校验，完成数据关联（Data Association），即认出“不同视角下看到的是同一个物理锚点”。
数学视角（后端变量）： 一旦进入优化器，物理特征就变成了纯粹的数学变量 $l_j \in \mathbb{R}^3$。它们在因子图中作为独立的节点，与机器人的位姿节点 $x_i$ 相连，构成了约束网络的基础。

2.2 Landmark 在单机与多机系统中的作用及问题建模 (Formulation)

Landmark 在单机和多机系统中的处理方式不同，这直接反映在优化问题的数学建模上：

2.2.1 单机系统：联合优化 (Joint Optimization)

在以 ORB-SLAM 或 VIO（如 VINS-Fusion 的局部滑动窗口）为代表的高精度单机模块中，位姿和 Landmark 通常会被联合优化（Bundle Adjustment, BA）。

ORB-SLAM2 的做法：无论是局部建图线程（Local BA），还是全局回环线程（Global BA），都在同时优化相机位姿（Pose）和 3D 地图点（Landmark）。这就导致它精度极高，但也使得它的计算复杂度非常庞大。
VINS-Fusion 的做法：在它的前端和 VIO 滑动窗口阶段（Estimator）：VINS-Fusion 是一个紧耦合框架。它在滑动窗口内，不仅优化位姿、速度、IMU零偏，而且显式地优化了Landmark。只不过它为了数值稳定性，没有直接优化 Landmark 的 (X,Y,Z)，而是优化了 Landmark 在相机坐标系下的逆深度。在它的全局回环阶段（Pose Graph Thread）为了保证实时的计算效率，VINS-Fusion 在检测到回环后，丢弃了所有的 Landmark，仅仅构建了一个只包含位姿的4自由度的位姿图进行优化。
作用： 显式地参与地图构建，通过重投影误差不断修正自身的 3D 坐标，同时拉扯相机的位姿。
数学建模： 这是一个标准的无约束非线性最小二乘问题（假设不考虑零空间固定）：
\[\min_{\mathcal{X}, \mathcal{L}} \sum_{(i,j)} \| z_{ij} - h(x_i, l_j) \|_{\Sigma}^2\]
其中，$\mathcal{X}$ 是所有位姿节点，$ \mathcal{L}$ 是所有 Landmark 节点，$z_{ij}$ 是观测值（如像素坐标），$h(\cdot)$ 是投影函数。这种模型精度极高，但矩阵维度随 Landmark 数量呈爆炸式增长。

2.2.2 多机系统：边缘化与纯位姿图优化 (DPGO)

在多机协同系统（如分布式后端）中，为了满足严苛的通信带宽限制和分布式计算要求，系统通常采取降维策略。

作用转变： Landmark 不再是后端优化的变量。两个机器人在前端匹配到共享的 Landmark 集合后，会在本地利用这些点计算出一个相对位姿变换（Relative Pose, $T_{AB}$）。一旦 $T_{AB}$ 计算完成，这些 Landmark 的历史使命就结束了，直接被丢弃（或边缘化），不进入全局分布式优化器。
数学建模： 多机后端被降维成了一个纯粹的分布式位姿图优化（Distributed Pose Graph Optimization, DPGO），仅包含位姿节点：
\[\min_{X_\alpha} \sum_{\alpha \in \mathcal{R}} \sum_{i} \| r_{\alpha_i}(X_{\alpha_i}, X_{\alpha_{i+1}}) \|^2 + \sum_{(\alpha_i, \beta_j) \in \mathcal{L}} \rho\Big( \| r_{\beta_j}^{\alpha_i}(X_{\alpha_i}, X_{\beta_j}) \|^2 \Big)\]
公式中只剩下单机局部里程计残差 $r_{\alpha_i}$ 和多机相对位姿残差 $r_{\beta_j}^{\alpha_i}$，并引入鲁棒核函数 $\rho$ 抵抗错误闭环。Landmark 在此模型中被完全剥离。

2.3 全局地图的一致性：是否需要优化 Landmark？

结论：不需要在全局后端直接优化 Landmark。 全局地图（Mesh 或稠密点云）由数以百万计的顶点组成。它们与稀疏的 Landmark 同源，但它们是“被动”的血肉，依附于“主动”的位姿骨架。保证地图一致性的核心逻辑是：只要位姿轨迹在全局达成了一致，附着其上的地图就必然会重合。 这在不同的地图表征下有不同的数学实现：

2.3.1 基于 Mesh 的地图融合 (形变图优化)

Mesh 具有表面拓扑结构，直接改变部分顶点会造成网格撕裂。系统可以通过构建形变图（Deformation Graph）来实现一致性。

当 DPGO 算出了全局一致的位姿 $\bar{X}_i$ 后，机器人在本地执行局部 Mesh 优化（Local Mesh Optimization, LMO）：

\[\min_{X^x_i, M_k} \underbrace{\sum_{i} \|X_i \ominus \bar{X}_i\|_{\Sigma_x}^2}_{\text{位姿（骨架）锚定}} + \underbrace{\sum_{k} \sum_{l} \|R_k^M(g_l - g_k) + t_k^M - t_l^M\|_{\Sigma}^2}_{\text{Mesh表面刚性}} + \underbrace{\sum_{i} \sum_{l} \|R_i^x g_{il} + t_i^x - t_l^M\|_{\Sigma}^2}_{\text{骨肉刚性}}\]

这个公式强迫 Mesh 顶点像贴了硅胶皮肤一样，平滑地跟随优化后的骨架 $\bar{X}_i$ 移动，同时保持局部几何形状不被破坏。由于各机器人的骨架在回环处已经被拉齐，它们形变后的局部 Mesh 放在同一个全局坐标系下时，边缘自然会完美拼接。

2.3.2 基于点云的地图融合 (子图刚体变换)

点云是无结构的，不需要维持表面刚性。其一致性过程退化为极其简单的子图刚体变换。

全局点云 $\mathcal{P}_{global}$ 是所有局部子图 $\mathcal{P}_i$ 通过最新骨架位姿 $\bar{X}_i$ 映射到全局坐标系的并集：

\[\mathcal{P}_{global} = \bigcup_{i=0}^N \left\{ \bar{X}_i \cdot p_k^{local} \mid p_k^{local} \in \mathcal{P}_i \right\}\]

为解决因微小位姿误差导致的“两面墙重叠（Ghosting）”问题，工程上通常会在重投影后，引入体素滤波下采样（Voxel Grid Filtering）或更新3D 概率占据网格（OctoMap），将微小的物理偏差在概率上抹平。

2.3.3 Landmark 丢弃之后，系统究竟被什么“约束”着？

当 Landmark 在多机后端被丢弃后，替代它成为约束条件的是机器人之间的“相对位姿（Relative Pose）”。在 SLAM 工程和数学优化领域，这种约束可以分为“软约束”和“硬约束”两种视角来理解：

1. SLAM 工程师视角的“软约束” (Soft Constraints)：相对位姿残差 在集中式位姿图优化中，“约束”本质上是观测残差。前端利用共同看到的 Landmark，计算出一个相对位姿的测量值：

\[\tilde{X}_{\beta_j}^{\alpha_i} = (\tilde{R}_{\beta_j}^{\alpha_i}, \tilde{t}_{\beta_j}^{\alpha_i})\]

在目标函数中，这个相对位姿化身为残差函数（如弦距离）：

\[r_{\beta_j}^{\alpha_i}(X_{\alpha_i}, X_{\beta_j}) \triangleq \left( w_R \| R_{\beta_j} - R_{\alpha_i} \tilde{R}_{\beta_j}^{\alpha_i} \|_F^2 + w_t \| t_{\beta_j} - t_{\alpha_i} - R_{\alpha_i} \tilde{t}_{\beta_j}^{\alpha_i} \|_2^2 \right)^{1/2}\]

此时，目标函数 $\min \sum r^2$ 中的自变量已百分之百变成了机器人的位姿状态 $X$，不再有 Landmark 的影子。

2. 数学家视角的“硬约束” (Hard Constraints)：分布式状态共识 在多机分布式计算（如 ADMM 算法）中，真正的硬约束（subject to）是为了切断变量、实现算力解耦而人为引入的。机器人 $\alpha$ 和 $\beta$ 各自维护自己认为的全局轨迹 $X^{(\alpha)}$ 和 $X^{(\beta)}$。为了强迫它们在公共相遇点达成一致，优化问题被表述为带约束的形式：

$\min_{X^{(\alpha)}, X^{(\beta)}} \quad f_{\alpha}(X^{(\alpha)}) + f_{\beta}(X^{(\beta)})$ $\text{s.t.} \quad X_{\text{shared}}^{(\alpha)} = X_{\text{shared}}^{(\beta)}$

或者引入虚拟边缘变量 $z$：

\[\text{s.t.} \quad X_{\text{shared}}^{(\alpha)} = z, \quad X_{\text{shared}}^{(\beta)} = z\]

总结： Landmark 就像是“纽带”，前端把纽带打成了一个结（相对位姿变换）。到了后端，系统不再关心这条纽带是由多少根纤维（3D 空间点）编成的，而是只关心这个“结”本身，并把它作为两台机器人位姿之间的拉力（软约束），或者强制要求两台机器人对这个结的位置达成共识（硬约束）。

3. 基于滤波or基于平滑？

首先感叹一下《视觉SLAM 14讲》这本书真的常看常新，上次阅读这本书可能已经是2年之前了，但回过头来发现那时我并未真正理解SLAM后端，也不理解滤波方法和优化方法的区别到底是什么。在阅读多机协同SLAM的论文时我终于有点感受到两种方法之间的差异，我觉得从简单来说，滤波就算一步，在多机通信受限的时候能够提供快速的状态估计，Swarm-LIO2在多机交互的时候使用了这种思想。滤波的方法遵循马尔可夫性，认为当前状态只与前一/几个时刻的状态相关，而与其他时刻的状态与观测都无关。而优化的方法则综合利用了所有时刻的信息，即使状态变化较大也更加可靠，虽然计算开销变大了，但优化方法依然更适用于大型场景。

以下是今天翻阅14讲时读到的一个小节，以前在读的时候由于知识浅薄不能明白这一节的意义，现在看来当时错过了很多这种细节，惭愧惭愧。

4.一致性

机器人各自的位姿是如何保证在全局坐标系下是一致的呢？只要有两个机器人之间回环的相对位姿变换就可以保证吗？到底如何理解位姿在全局上达成了一致？Gemini给出了一个非常有意思的弹簧类比，这个类比很直观的表现了误差的平均化，整理如下：

核心问题：机器人位姿在全局上是如何达成一致的？

仅仅“有”两个机器人之间的相对位姿变换是不够的。 相对位姿只是一个测量值（带有噪声），真正保证它们在全局上达成一致的，是背后的“全局图优化（Global Graph Optimization）”机制。

为了透彻理解“全局一致”到底是什么意思，我们可以用弹簧质点模型来建立物理直觉，并结合初始化和优化的过程来拆解。单次相对位姿只是提供了一条“弹簧约束”，它能把独立的图连起来，但不能保证全局准确。

4.1 全局坐标系的初始化

在机器人刚下水或刚启动时，Robot A 和 Robot B 都认为自己的起点是宇宙的中心 $(0,0,0)$。此时，它们生活在两个平行的局部坐标系中。

当它们第一次相遇（第一次产生共视），前端算出了一个相对位姿变换 $T_{AB}$。这个 $T_{AB}$ 的作用是“破冰”。系统会利用这第一个 $T_{AB}$，执行一次“鲁棒分布式初始化”：

系统指定 Robot A 的原点作为全局坐标系的绝对原点。
Robot B 把自己所有的历史轨迹，直接乘以 $T_{AB}^{-1}$，强行搬到 Robot A 的坐标系下。

此时，它们在同一个坐标系下了，但这叫“一致”吗？不叫。 因为这只是基于单次测量的粗略对齐，伴随着里程计的漂移，如果它们继续走下去，轨迹依然是错位的。

4.2 为什么需要优化？

假设 Robot A 和 Robot B 继续各自探索，由于各自的里程计（视觉或惯性）都在累积误差，过了十分钟，它们第二次相遇了。

前端通过第二次共视，又算出了一个新的相对位姿 $T’_{AB}$。
但是，如果此时你拿 Robot A 当前在全局坐标系下的绝对位姿 $X_A$，乘以这个新的 $T’_{AB}$，算出来的 Robot B 的位姿，绝对不可能等于 Robot B 自己在全局坐标系下的当前位姿 $X_B$。
数学表达就是：$X_A \cdot T’_{AB} \neq X_B$。

这就叫“全局不一致”（Inconsistency）。 测量数据（$T’_{AB}$）和系统当前的状态（$X_A, X_B$）产生了严重的自相矛盾。

4.3 弹簧质点模型的物理直觉

那么，如何消除这种矛盾，达成真正的“全局一致”？我们引入位姿图优化（Pose Graph Optimization）。

请想象这样一个物理系统：

质点（Node）： 机器人的每一个位姿 $X_i$，都是一个漂浮在 3D 空间中的铁球。
里程计弹簧（Odometry Edge）： 机器人自己的前后两个位姿之间，连着一根蓝色的弹簧。这根弹簧的“自然长度”是里程计测出来的相对运动。
回环弹簧（Loop Closure Edge）： 当两个机器人相遇时，Robot A 的铁球和 Robot B 的铁球之间，被硬拉上了一根红色的弹簧。这根弹簧的“自然长度和扭转角度”就是前端算出来的相对位姿 $T_{AB}$。

一开始，因为累积误差的存在，Robot A 和 Robot B 的铁球位置不对，这根红色的回环弹簧会被严重拉扯或挤压（产生了残差）。

什么是全局优化？ 就是你松开手，让这个由成千上万个铁球和弹簧组成的巨大系统，在三维空间中自由释放内部的弹力。有的铁球被往前拉一点，有的被往后拽一点。系统的总势能（所有弹簧被拉伸/压缩的能量总和）在这个过程中不断下降。

数学上的等价表达，就是最小化所有残差的平方和：

\[\min_{X} \sum_{\text{里程计}} \| X_i^{-1} X_{i+1} - \Delta X_{odom} \|^2 + \sum_{\text{回环}} \| X_A^{-1} X_B - T_{AB} \|^2\]

什么是全局达成了一致？ 当整个弹簧系统彻底静止，达到了势能最低点（Minimum Energy State）时，我们就说所有的位姿在全局上达成了一致（Consensus）。全局一致性是建立在过约束（Over-determined）系统之上的。只有当大量的里程计弹簧和多条回环弹簧相互交织，通过非线性优化（比如 ADMM 或 D-GNC 算法）将所有矛盾的张力释放到最低点时，这些漂浮的坐标轴才算真正“焊死”在了一个逻辑严密的全局地图中。

在这个静止状态下：

没有任何一个测量（相对位姿）是被完美满足的（每根弹簧都还有一点点微小的形变）。
但是，所有测量之间的矛盾被平均、最优地化解了。Robot A 和 Robot B 的绝对坐标 $(X_A, X_B)$ 妥协到了一个让全网误差最小的黄金位置。