Asynchronous Distributed Smoothing and Mapping via On-Manifold Consensus ADMM

2024年ICRA论文，Michael Kaess组的一篇工作，将分布式ADMM算法应用于多机SLAM后端。解决的核心问题是机器人的位姿状态大多是属于\(SO(3)\)或\(SE(3)\)的，因此不具备欧式空间的直接加减性质，因此无法直接套用ADMM。另外标准的ADMM算法要求对齐完整的向量，但多机框架下的位置约束并非时刻重叠，而是离散的回环点。并且多机之间的通信链路并不能保证持续建立，需要在偶发通信的条件下执行ADMM算法。

论文原文：Asynchronous Distributed Smoothing and Mapping via On-Manifold Consensus ADMM

1.交替方向乘子法 (Alternating Direction Method of Multipliers, ADMM)

读懂这篇论文需要先理解标准的ADMM算法：

标准多智能体一致性优化的原始目标可以表示为：

\[\arg\min_{\overline{x} \in \mathbb{R}^{|\mathcal{R}|n}} \sum_{i \in \mathcal{R}} f_i(x_i)\] \[\text{s.t. } x_i = x_j \quad \forall (i,j) \in \mathcal{E}\]

由于直接约束 $x_i = x_j$ 会导致系统强耦合，C-ADMM 引入了 边变量 (Edge Variables) $z_{(i,j)}$ 进行数学解耦：

\[\arg\min_{\overline{x}, \overline{z}} \sum_{i \in \mathcal{R}} f_i(x_i)\] \[\text{s.t. } x_i = z_{(i,j)}, \quad x_j = z_{(j,i)} \quad \forall (i,j) \in \mathcal{E}\]

基于此，构造出增广拉格朗日函数 (Augmented Lagrangian)：

\[\mathcal{L} = \sum_{i \in \mathcal{R}} f_i(x_i) + \sum_{j \in \mathcal{N}_i} \langle \lambda_{(i,j)}, x_i - z_{(i,j)} \rangle + \frac{\beta}{2} ||x_i - z_{(i,j)}||^2\]

其中，$\lambda$ 是对偶变量（即“拉格朗日乘子”或“裁判的报价”），$\beta$ 是惩罚力度。系统通过交替更新 $x$、$z$ 和 $\lambda$ 逼近最优解。

2.存在问题

1. 收敛速度慢：基于分布式梯度下降的方法（如 ASAPP）需要海量的通信轮次。
2. 网络要求苛刻：许多算法要求全网机器人保持同步通信，无法应对真实世界中的网络延迟和中断。
3. 流形约束处理不足：机器人的位姿（如 $SE(3)$）存在于李群流形空间，无法直接在欧氏空间中进行加减运算。

为此，本文提出了一种名为 MESA (Manifold, Edge-based, Separable ADMM) 的ADMM算法以解决上述问题。

3.MESA算法

3.1 可分离性 (Separable) 与共享变量

多机 SLAM 中，机器人无需共享完整轨迹，只需对齐发生多机闭环的“共享变量”。对于未发生共视的局部节点，机器人只需在本地独立优化。定义 $S_{(i,j)} \triangleq \Theta_i \cap \Theta_j$ 为机器人 $i$ 和 $j$ 的共享变量集。

3.2 流形约束 (On-Manifold Constraints)

由于机器人位姿属于流形空间（如 $SE(3)$），不能直接使用欧氏空间的减法 $(x_i - x_j)$。论文引入了通用的流形约束函数 $q_s(\cdot, \cdot)$。C-SLAM 问题被重新表述为：

\[\arg\min_{\Theta, Z} \sum_{i \in \mathcal{R}} \sum_{m \in M_i} ||h_m(\Theta_i) - m||_{\Sigma_m}^2\] \[\text{s.t. } q_s(\theta_{s_i}, z_{(i,j)_s}) = 0, \quad q_s(\theta_{s_j}, z_{(j,i)_s}) = 0 \quad \forall \theta_s \in S_{(i,j)}\]

3.3 MESA 的交替迭代过程

结合上述改造，MESA 的核心迭代公式如下：

1. 机器人本地优化 (Local Update)：

   \[\Theta_i^{k+1} = \arg\min_{\Theta_i} \left( \sum ||h_m - m||^2 + \sum \langle \lambda^k, q_s \rangle + \sum \frac{\beta^k}{2} ||q_s||^2 \right)\]

2. 更新虚拟相遇点 (Edge Variable Update)： 提取出解析解或近似解。对于 $SE(N)$ 的 Split 约束，论文通过分离平移（线性插值）和旋转（SLERP 球面线性插值）来求解 $z$。

3. 裁判更新对偶变量 (Dual Variable Update) 与 增大惩罚 (Penalty Update)。

3.4 工程技巧

配方法与“偏置先验” (Biased Prior)

传统的增广拉格朗日函数包含内积项 $\langle \lambda, q_s \rangle$，这无法直接被现有的开源 SLAM 库（如 GTSAM）识别。作者通过代数配方法，将目标函数中的惩罚项和内积项合并为一个完全平方公式：

\[\frac{\beta_{(i,j)}}{2} \left|\left| q_s(\theta_{s_i}, z_{(i,j)_s}) + \frac{\lambda_{(i,j)_s}}{\beta_{(i,j)}} \right|\right|^2\]

解释：这个合并后的公式在数学上等价于因子图中的一个 带有动态偏移量的单节点先验观测 (Biased Prior)。只需在因子图中添加此 Prior Factor，即可无缝调用标准非线性最小二乘求解器执行 ADMM 的本地优化。

直接误差更新

在标准的 C-ADMM 中，$\lambda$ 的更新基于当前状态与中间点 $z$ 的误差：$\lambda^{k+1} = \lambda^k + \beta^k(q_s(\theta_i, z))$。但作者发现：

“This error underestimates the magnitude of the constraint satisfaction gap (i.e. the error between $\theta_{s_i}$ and $\theta_{s_j}$).” （这种误差低估了约束满足的差距幅度，即 $\theta_i$ 和 $\theta_j$ 之间的真实误差）。

为此，论文提出了极其有效的修改方案（公式 18）：

\[\lambda_{(i,j)_s}^{k+1} = \lambda_{(i,j)_s}^k + \beta_{(i,j)}^k (q_s(\theta_{s_i}^{k+1}, \theta_{s_j}^{k+1}))\]

使用两个机器人的直接流形差异来更新对偶变量。这一修改在通信完成后执行，并未破坏本地优化的数学解耦，同时极大地加速了全网的收敛速度。

4. 异步边缘通信 (Asynchronous Edge-based Communication)

MESA 算法不需要全网时钟同步。它在后台默默运行，遵循以下机制：

• 如果网络断开，机器人在本地利用历史的 $z$ 和 $\lambda$ 继续迭代优化其位姿图。
• 当任意两个机器人相遇并建立通信时，它们仅交换共享变量 $\theta_s$，随后立刻更新 $z$、$\lambda$ 和 $\beta$。
• 这种将通信中断视为“未通信的迭代轮次”的策略，赋予了算法极强的网络鲁棒性。

5. 实验与评估核心结论

1. 评价指标创新：提出了平均残差 (Mean Residual, $r_{mean}^2$)。该指标穷举了不同机器人的状态估计组合，既考察优化精度，又严厉惩罚了多机之间的“不一致性”。
2. 流形约束对比：实验证明，Geodesic (测地线) 和 Split 约束的精度远超 Chordal 近似。
3. SOTA 对比：在各类基准数据集中，MESA 在收敛速度（所需的通信次数）和最终绝对精度上，均显著超越了传统的分布式算法（如 ASAPP, DGS, MB-ADMM），其结果极其逼近中央集权式的计算极限。