DPGO:异步和并行分布式位姿图优化 2020 RA-L best paper

文章目录

前言
一、介绍
二、使用步骤
- [A. 分布式和并行位姿图优化（PGO）](#A. 分布式和并行位姿图优化（PGO）)
- [B. 异步并行优化](#B. 异步并行优化)
[三. 问题表述](#三. 问题表述)
[四. 提出的算法](#四. 提出的算法)
- [A. 通信线程](#A. 通信线程)
- [B. 优化线程](#B. 优化线程)
- [C. 实施细节](#C. 实施细节)
[五. 收敛性分析](#五. 收敛性分析)
- [A. 算法的全局视角](#A. 算法的全局视角)
- [B. 收敛的充分条件](#B. 收敛的充分条件)
[六. 实验结果](#六. 实验结果)
- [A. 模拟评估](#A. 模拟评估)
- [B. 在基准PGO数据集上的评估](#B. 在基准PGO数据集上的评估)
[七. 结论](#七. 结论)

前言

代码：github

原文：原文

摘要---我们提出了异步随机并行位姿图优化（ASAPP），这是首个用于多机器人同时定位与建图（SLAM）中分布式位姿图优化（PGO）的异步算法 。通过使机器人能够在无需同步的情况下优化其局部轨迹估计 ，ASAPP提供了对通信延迟的韧性 ，并减轻了在网络中等待落后的机器人的需求。此外，ASAPP 可应用于 PGO 的秩约束松弛，这是一个重要的非凸黎曼优化问题类别，支持最近在全局最优 PGO 方面的突破。在有限延迟的情况下，我们通过选择足够小的步长，证明了 ASAPP 的全局一阶收敛性。推导出的步长依赖于最坏情况下的延迟和问题的内在稀疏性，且在无延迟的情况下与同步算法的已知结果一致。基于模拟数据集和真实数据集的数值评估表明，与最先进的同步算法相比，ASAPP 展现了较好的性能，并证明了其在实践中对各种延迟的韧性。

一、介绍

多机器人同时定位与建图（SLAM）是许多现实世界机器人应用的基础能力。位姿图优化（PGO）是多机器人SLAM的核心技术，能够将各个机器人的轨迹进行融合，并赋予参与的机器人对环境的共同空间理解。许多多机器人PGO方法需要在基站进行观测数据的集中处理，这不仅通信开销大，而且容易受到单点故障的影响。相比之下，分布式方法更具优势，因为它们有效缓解了集中化带来的通信、隐私和脆弱性问题。

最近关于分布式PGO的研究取得了重要进展；参见如[1]、[2]及其中的参考文献。然而，尽管如此，现有的分布式算法本质上是同步的 ，这意味着机器人需要在网络上传递信息或在预定点等待 ，以确保在分布式优化过程中信息的及时共享。这样做可能会产生巨大的通信开销 ，并增加实现的复杂性。另一方面，简单地去掉同步算法中的同步步骤，可能会减缓收敛速度，甚至导致发散，这在理论和实践中都是如此。

在本工作中，我们提出了ASAPP（异步随机并行位姿图优化），这是首个异步且可证明收敛的分布式PGO算法。我们从现有的并行和异步算法[3]--[5]、[7]、[10]中汲取灵感，并将这些思想应用于解决PGO背后的非凸黎曼优化问题 。在ASAPP中，每个机器人以高频率执行本地优化循环 ，而无需等待来自其他机器人的网络更新。这使得ASAPP在实践中更易实现，并且对通信延迟更具灵活性。此外，最近在集中式PGO领域的突破性进展（如SE-Sync[12]）表明，通过求解一系列秩约束松弛问题，可以获得PGO的全局最优解。在本工作中，我们借鉴了SE-Sync及后续工作的宝贵见解[2]、[13]，并展示了ASAPP能够解决PGO及其秩约束松弛问题。

本工作重点解决分布式SLAM后端（即位姿图优化）。我们的求解器可以与独立的前端模块结合，形成完整的SLAM解决方案，类似于当前最先进的多机器人SLAM系统，例如[14]、[15]。

贡献

由于异步算法允许通信延迟具有较大的不确定性和不可预测性，因此通常不清楚在什么条件下它们能够在实践中收敛。在本工作中，我们对这一问题提供了严格的答案，并首次给出了异步算法在非凸PGO问题上的已知收敛结果 。特别地，我们展示了只要最坏情况的延迟不至于过大，ASAPP在使用足够小的步长时，解决PGO及其秩约束松弛问题时总是能够收敛到一阶临界点，并且具有全局亚线性收敛速度。推导出的步长依赖于最大延迟和问题的内在稀疏性，且当没有延迟时，步长简化为同步算法中已知的常数 1 / L 1/L 1/L（其中 L L L是 L i p s c h i t z Lipschitz Lipschitz常数）。通过对模拟数据集和真实世界数据集的数值评估，结果表明，ASAPP与当前最先进的同步方法相比表现良好，且在各种通信延迟条件下均展现了较强的韧性。这些结果展示了所提算法在现实分布式环境中的实际价值。

在SLAM中，经常遇到的矩阵流形的例子包括正交群 O ( d ) O(d) O(d)、特殊正交群 S O ( d ) SO(d) SO(d) 和特殊欧几里得群 S E ( d ) SE(d) SE(d)。在这项工作中，我们使用 M ⊆ E \mathcal{M}\subseteq\mathcal{E} M⊆E 来表示一个一般的矩阵子流形，其中 E \mathcal{E} E 是所谓的环境空间（在这项工作中， E \mathcal{E} E 总是欧几里得空间）。流形上的每个点 x ∈ M x\in\mathcal{M} x∈M 都有一个相关的切空间 T x M T_{x}\mathcal{M} TxM。非正式地说， T x M T_{x}\mathcal{M} TxM 包含了在 x x x 处保持在 M \mathcal{M} M 上的所有可能的变化方向。由于 T x M T_{x}\mathcal{M} TxM 是一个向量空间，我们还赋予它标准的Frobenius内积，即对于两个切向量 η 1 , η 2 ∈ T x M \eta_{1},\eta_{2}\in T_{x}\mathcal{M} η1,η2∈TxM， ⟨ η 1 , η 2 ⟩ ≜ tr ⁡ ( η 1 ⊤ η 2 ) \left\langle\eta_{1},\eta_{2}\right\rangle\triangleq\operatorname{tr}\left(\eta_{1}^{\top}\eta_{2}\right) ⟨η1,η2⟩≜tr(η1⊤η2)。内积诱导了一个范数 ∥ η ∥ ≜ ⟨ η , η ⟩ \|\eta\|\triangleq\sqrt{\langle\eta,\eta\rangle} ∥η∥≜⟨η,η⟩ 。最后，一个切向量可以通过一个重投影 Retr ⁡ x : T x M → M \operatorname{Retr}{x}: T{x}\mathcal{M}\rightarrow\mathcal{M} Retrx:TxM→M 映射回流形，这是一个保持流形一阶结构的光滑映射[16]。

黎曼优化考虑在流形上最小化一个函数 f : M → R f:\mathcal{M}\rightarrow R f:M→R。一阶黎曼优化算法，包括在这项工作中提出的算法，经常使用黎曼梯度 grad ⁡ f ( x ) ∈ T x M \operatorname{grad} f(x)\in T_{x}\mathcal{M} gradf(x)∈TxM，它对应于切空间中的最大上升方向。对于矩阵子流形，黎曼梯度是通过将通常的欧几里得梯度 ∇ f ( x ) \nabla f(x) ∇f(x) 正交投影到切空间上得到的，即 grad ⁡ f ( x ) = Proj ⁡ T x M ∇ f ( x ) [ 16 ] \operatorname{grad} f(x)=\operatorname{Proj}{T{x}\mathcal{M}}\nabla f(x)[16] gradf(x)=ProjTxM∇f(x)[16]。如果 grad ⁡ f ( x ⋆ ) = 0 \operatorname{grad} f\left(x^{\star}\right)=0 gradf(x⋆)=0，我们称 x ⋆ ∈ M x^{\star}\in\mathcal{M} x⋆∈M 为一阶临界点。

图1：（a）示例位姿图G，其中包含4个机器人，每个机器人有3个位姿。每条边表示一个相对位姿测量。私有位姿用灰色表示。（b）对应的机器人级图GR。如果两个机器人共享任何相对测量（机器人间的环路闭合），它们之间将会连接。请注意，在分布式优化的任何时刻，机器人无需与其他机器人共享它们的私有位姿。

二、使用步骤

A. 分布式和并行位姿图优化（PGO）

在追求去中心化异步算法的过程中，我们注意到同步的去中心化PGO已被广泛研究。Tron等人[17]、[18]提出了基于地理测地距离的一组重塑代价函数上的分布式黎曼梯度下降方法，在该方法下，算法能够证明收敛。类似的基于梯度的带线搜索方法也有提出[19]。Cunningham等人[20]--[22]提出了DDF-SAM，一种用于基于特征的SLAM的去中心化系统，其中机器人交换共同观测变量上的高斯边际。在[20]--[22]中使用本地缓存进一步支持异步通信，尽管在这种情况下没有讨论收敛性。Choudhary等人[23]提出了交替方向乘子法（ADMM）作为解决PGO的去中心化方法。然而，由于优化问题的非凸性质，ADMM的收敛性尚未得到证明。更近期的研究中，Choudhary等人[1]提出了一种两阶段方法，其中每个阶段使用分布式Gauss-Seidel[3]来解决一个松弛或线性化的PGO问题。这一两阶段方法[1]还与[15]中的异常值剔除方案结合使用。在我们近期的工作[2]中，我们通过直接优化PGO及其秩约束松弛[13]来避免显式线性化。所提出的求解器在黎曼流形的乘积上执行分布式块坐标下降，并且能够证明收敛到一阶临界点，具有全局亚线性收敛速率。在另一条研究方向上，Fan和Murphey[24]提出了一种经验加速方法。PGO求解器适用于基于广义近端方法的分布式优化。

B. 异步并行优化

上述工作具有前景，但关键在于它们依赖于同步来保证收敛，这限制了它们在网络化自主系统中的实际应用价值。然而，在更广泛的优化文献中，关于并行和异步优化的研究层出不穷，部分受到大规模机器学习和深度学习应用的推动。异步梯度算法的研究始于Bertsekas和Tsitsilis的开创性工作[3]，并引发了异步随机块坐标和随机梯度算法的最新发展，参见[4]--[7]、[9]--[11]及其中的参考文献。我们尤其关注用于非凸优化的异步并行方案，这些方案在[10]、[11]中已有研究。在本工作中，我们将这些方法推广到可行集为非凸矩阵流形乘积的情境，这一情境受PGO的启发。

三. 问题表述

在本节中，我们正式定义多机器人SLAM背景下的姿态图优化（ P G O PGO PGO）。给定相对姿态测量（可能涉及不同的机器人），我们的目标是在全球参考框架中联合估计所有机器人的轨迹。设 R = { 1 , 2 , ... , n } \mathcal{R} = \{1, 2, \ldots, n\} R={1,2,...,n} 为与 n 个机器人相关联的索引集合。用 T i τ = ( R i τ , t i τ ) ∈ SE ⁡ ( d ) T_{i_{\tau}} = \left(R_{i_{\tau}}, t_{i_{\tau}}\right) \in \operatorname{SE}(d) Tiτ=(Riτ,tiτ)∈SE(d) 表示机器人 i ∈ R i \in \mathcal{R} i∈R 在时间步 τ \tau τ 的姿态，其中 d ∈ { 2 , 3 } d \in \{2, 3\} d∈{2,3} 是估计问题的维度。这里 R i τ ∈ S O ( d ) R_{i_{\tau}} \in SO(d) Riτ∈SO(d) 是一个旋转矩阵， t i τ ∈ R d t_{i_{\tau}} \in \mathbb{R}^{d} tiτ∈Rd 是一个平移向量。从 T i τ T_{i_{\tau}} Tiτ 到 T j s T_{j_{s}} Tjs 的相对姿态测量表示为 T ~ j s i τ = ( R ~ j s i τ , t ~ j s i τ ) ∈ SE ⁡ ( d ) \widetilde{T}{j{s}}^{i_{\tau}} = \left(\widetilde{R}{j{s}}^{i_{\tau}}, \widetilde{t}{j{s}}^{i_{\tau}}\right) \in \operatorname{SE}(d) T jsiτ=(R jsiτ,t jsiτ)∈SE(d)。我们假设以下标准噪声模型，该模型被 S E − S y n c SE-Sync SE−Sync[12]及其后续工作[2]、[13]使用，

R ~ j s i τ = R ‾ j s i τ R i τ , j s ϵ , R i τ , j s ϵ ∼ Langevin ( I d , w R ) ( 1 ) \begin{align*} \widetilde{R}{j_s}^{i\tau} &= \underline{R}{j_s}^{i\tau} R_{i_\tau, j_s}^{\epsilon}, \quad R_{i_\tau, j_s}^{\epsilon} \sim \text{Langevin}(I_d, w_R) \qquad(1) \end{align*} R jsiτ=RjsiτRiτ,jsϵ,Riτ,jsϵ∼Langevin(Id,wR)(1)

t ~ j s i τ = t ‾ j s i τ + t i τ , j s ϵ , t i τ , j s ϵ ∼ N ( 0 , w t − 1 I d ) . ( 2 ) \begin{align*} \widetilde{t}{j_s}^{i\tau} &= \underline{t}{j_s}^{i\tau} + t_{i_\tau, j_s}^\epsilon, \quad t_{i_\tau, j_s}^\epsilon \sim \mathcal{N}(0, w_t^{-1} I_d). \end{align*} \qquad(2) t jsiτ=tjsiτ+tiτ,jsϵ,tiτ,jsϵ∼N(0,wt−1Id).(2)

上述中， T ‾ j s i τ = ( R ‾ j s i τ , t ‾ j s i τ ) ∈ SE ⁡ ( d ) \underline{T}{j{s}}^{i_{\tau}}=\left(\underline{R}{j{s}}^{i_{\tau}},\underline{t}{j{s}}^{i_{\tau}}\right)\in\operatorname{SE}(d) Tjsiτ=(Rjsiτ,tjsiτ)∈SE(d) 表示真实（即无噪声）的相对变换。旋转上的各向同性 L a n g e v i n Langevin Langevin噪声[12]在平移上的高斯噪声扮演类似的角色。给定形式为(1)-(2)的噪声测量，我们寻求在 R \mathcal{R} R 中所有机器人轨迹的最大似然估计（ M L E MLE MLE）。这样做相当于解决以下非凸规划问题[12]。

问题1（最大似然估计）。

min ⁡ ∑ ( i τ , j s ) ∈ E w R ∥ R j s − R i τ R ~ j s i τ ∥ F 2 + w t ∥ t j s − t i τ − R i τ t ~ j s i τ ∥ 2 2 s.t. R i τ ∈ S O ( d ) , t i τ ∈ R d , ∀ i ∈ R , ∀ τ . ( p 1 ) \begin{array}{l} \min\sum_{\left(i_{\tau}, j_{s}\right)\in E} w_{R}\left\|R_{j_{s}}-R_{i_{\tau}}\widetilde{R}{j{s}}^{i_{\tau}}\right\|{F}^{2}+w{t}\left\|t_{j_{s}}-t_{i_{\tau}}-R_{i_{\tau}}\widetilde{t}{j{s}}^{i_{\tau}}\right\|{2}^{2}\\ \text{ s.t.}\quad R{i_{\tau}}\in SO(d), t_{i_{\tau}}\in \mathbb{R}^{d},\forall i\in\mathcal{R},\forall\tau. \qquad(p1) \end{array} min∑(iτ,js)∈EwR Rjs−RiτR jsiτ F2+wt tjs−tiτ−Riτt jsiτ 22 s.t.Riτ∈SO(d),tiτ∈Rd,∀i∈R,∀τ.(p1)

问题 ( P 1 ) \left(P_{1}\right) (P1) 可以用姿态图 G ≜ ( V , E ) G\triangleq(V, E) G≜(V,E) 紧凑表示，其中每个顶点在 V V V 中对应于一个机器人拥有的单一姿态。注意，目标函数中的求和是在所有 E 中的边之上进行的，如果从 T i τ T_{i_{\tau}} Tiτ 到 T j s T_{j_{s}} Tjs 有相对测量，则形成从 i τ i_{\tau} iτ 到 j s j_{s} js 的边。图1a显示了一个姿态图的例子。

在本文中，我们进一步考虑了 ( P 1 ) \left(P_{1}\right) (P1) 的秩限制松弛，这是由SE-Sync[12]首次提出的。将 S t i e f e l Stiefel Stiefel流形表示为 St ⁡ ( d , r ) ≜ { Y ∈ R r × d : Y ⊤ Y = I d } \operatorname{St}(d, r)\triangleq\left\{Y\in \mathbb{R}^{r\times d}: Y^{\top} Y=I_{d}\right\} St(d,r)≜{Y∈Rr×d:Y⊤Y=Id}，其中 r ≥ d r\geq d r≥d。 ( P 1 ) \left(P_{1}\right) (P1) 的秩-r松弛定义为以下非凸黎曼优化问题。

问题2（秩-r松弛）。

min ⁡ ( i τ , j s ) ∈ E w R ∥ Y j s − Y i τ R ~ j s i τ ∥ F 2 + w t ∥ p j s − p i τ − Y i τ t ~ j s i τ ∥ 2 2 s.t. Y i τ ∈ St ⁡ ( d , r ) , p i τ ∈ R r , ∀ i ∈ R , ∀ τ . ( p 2 ) \begin{align*} &\min_{\left(i_{\tau}, j_{s}\right)\in E} w_{R}\left\|Y_{j_{s}}-Y_{i_{\tau}}\widetilde{R}{j{s}}^{i_{\tau}}\right\|{F}^{2}+w{t}\left\|p_{j_{s}}-p_{i_{\tau}}-Y_{i_{\tau}}\widetilde{t}{j{s}}^{i_{\tau}}\right\|{2}^{2}\\ &\text{ s.t. } Y{i_{\tau}}\in\operatorname{St}(d, r), p_{i_{\tau}}\in \mathbb{R}^{r},\forall i\in\mathcal{R},\forall\tau. \qquad(p2) \end{align*} (iτ,js)∈EminwR Yjs−YiτR jsiτ F2+wt pjs−piτ−Yiτt jsiτ 22 s.t. Yiτ∈St(d,r),piτ∈Rr,∀i∈R,∀τ.(p2)

请注意，对于 r = d r=d r=d，Stiefel流形与正交群 St ⁡ ( d , d ) = O ( d ) \operatorname{St}(d, d)=O(d) St(d,d)=O(d) 是相同的。在这种情况下， ( P 2 ) \left(P_{2}\right) (P2) 被称为 ( P 1 ) \left(P_{1}\right) (P1) 的正交松弛，通过去掉 S O ( d ) SO(d) SO(d) 上的行列式约束获得。随着 r 超过 d，我们得到了一系列秩限制问题，每个问题都具有 ( P 2 ) \left(P_2\right) (P2) 的形式，但搜索空间略有"提升"，由 r 决定。秩松弛的思想最初由 SE-Sync[12] 提出，其中 Rosen 等人使用一个非常相似的秩松弛层次结构作为代理来解决 PGO 的半定松弛问题，从而恢复原始 MLE ( P 1 ) \left(P_{1}\right) (P1) 的全局最小化器。这个优雅的结果激励我们除了原始的 MLE 公式 ( P 1 ) \left(P_{1}\right) (P1) 之外，还考虑 ( P 2 ) \left(P_{2}\right) (P2)。虽然 SE-Sync 假设了一个集中式（单机器人）场景，我们专注于分布式 PGO 并开发了一个异步算法来解决通信延迟情况下的 ( P 1 ) \left(P_{1}\right) (P1) 和 ( P 2 ) \left(P_{2}\right) (P2)。

为了设计去中心化算法（第 IV 节），将 ( P 1 ) \left(P_1\right) (P1) 和 ( P 2 ) \left(P_2\right) (P2) 重写为机器人层面的更抽象形式更为方便，可以这样做。

问题3（机器人级别的优化问题）。
min ⁡ ( f i i ( C i , W i ) + ∑ ( i , j ) ∈ E R h i ( C i ) ) ( P ) \min \left( f_{ii}(C_i, W_i) + \sum_{(i,j) \in E_R} h_i(C_i) \right) \quad (P) min fii(Ci,Wi)+(i,j)∈ER∑hi(Ci) (P)

约束条件： C i ∈ M i , ∀ i ∈ R C_i \in M_i, \forall i \in \mathcal{R} Ci∈Mi,∀i∈R。

在问题 P P P中，每个变量 i i i 串联了所有由机器人 i ∈ R i \in \mathcal{R} i∈R 拥有的变量。例如，在问题 (P2) 中， i i i 包含了机器人 i i i 的所有"提升"的旋转和平移变量。设 n i n_i ni 为机器人 i i i 的姿态数量。那么，

C i = [ Y i 1 P i 1 ... Y i n i P i n i ] , ( 5 ) C_i = \begin{bmatrix} Y_{i1} & P_{i1} & \ldots & Y_{in_i} & P_{in_i} \end{bmatrix}, \qquad (5) Ci=[Yi1Pi1...YiniPini],(5)

M i = ( St ⁡ ( d , r ) × R r ) n i . ( 6 ) \mathcal{M}_i = (\operatorname{St}(d, r) \times \mathbb{R}^r)^{n_i}. \qquad (6) Mi=(St(d,r)×Rr)ni.(6)

问题 P P P 中的成本函数包括一组机器人对之间共享的成本 f i j : M i × M j → R f_{ij}: \mathcal{M}_i \times \mathcal{M}_j \rightarrow \mathbb{R} fij:Mi×Mj→R，以及一组个别机器人的私有成本 h i : M i → R h_i: \mathcal{M}i \rightarrow \mathbb{R} hi:Mi→R。直观上， f i j f{ij} fij 是由机器人 i i i 的任意姿态与机器人 j j j 的姿态之间的相对测量形成的。相比之下， h i h_i hi 是由机器人 i i i 自身轨迹内的相对测量形成的。

类似于姿态图的定义方式，我们可以使用机器人级图 G R ≜ ( R , E R ) G_{\mathcal{R}} \triangleq (\mathcal{R}, E_{\mathcal{R}}) GR≜(R,ER) 来编码问题 § 的结构；参见图 1b。 G R G_{\mathcal{R}} GR 可以被视为姿态图的"简化"图，其中每个顶点对应于单个机器人 i ∈ R i \in \mathcal{R} i∈R 的整个轨迹。如果机器人 i i i 和 j j j 共享任何相对测量 ( i τ , j s ) ∈ E \left(i_{\tau}, j_{s}\right) \in E (iτ,js)∈E，则它们在 G R G_{\mathcal{R}} GR 中相连。在这种情况下，我们称 j j j 为 i i i 的邻近机器人， j s j_{s} js 为机器人 i i i 的邻近姿态。如果一个姿态变量不是任何其他机器人的邻近姿态，我们称这个姿态为私有姿态[2]。我们注意到，对于机器人 i i i 来说，为了评估共享成本 f i j f_{ij} fij，它只需要知道机器人 j j j 轨迹中的邻近姿态（参见图 1）。这个属性在保护任何队友的隐私方面至关重要。

四. 提出的算法

问题§中的成本函数包括一组机器人对之间共享的成本 f i j : M i × M j → R f_{ij}:\mathcal{M}{i}\times\mathcal{M}{j}\rightarrow \mathbb{R} fij:Mi×Mj→R，以及一组个别机器人的私有成本 h i : M i → R h_{i}:\mathcal{M}{i}\rightarrow \mathbb{R} hi:Mi→R。直观上， f i j f{ij} fij 是由机器人 i 的任意姿态与机器人 j 的姿态之间的相对测量形成的。相比之下， h i h_{i} hi 是由机器人 i 自身轨迹内的相对测量形成的。

类似于姿态图的定义方式，我们可以使用机器人级别的图 G R ≜ ( R , E R ) G_{\mathcal{R}}\triangleq(\mathcal{R}, E_{\mathcal{R}}) GR≜(R,ER) 来编码问题§的结构；参见图1b。 G R G_{\mathcal{R}} GR 可以被视为姿态图的"简化"图，其中每个顶点对应于单个机器人 i ∈ R i\in\mathcal{R} i∈R 的整个轨迹。如果机器人 i 和 j 共享任何相对测量 ( i τ , j s ) ∈ E \left(i_{\tau}, j_{s}\right)\in E (iτ,js)∈E，则在 G R G_{\mathcal{R}} GR 中它们是相连的。在这种情况下，我们称 j 为 i 的邻近机器人， j s j_{s} js 为机器人 i 的邻近姿态。如果一个姿态变量不是任何其他机器人的邻近姿态，我们称这个姿态为私有姿态[2]。我们注意到，对于机器人 i 来说，为了评估共享成本 f i j f_{ij} fij，它只需要知道机器人 j 轨迹中的邻近姿态（参见图1）。这个属性在保护参与机器人的隐私[1]、[2]方面至关重要，即在任何时候，机器人不需要与其队友分享其私有姿态。

A. 通信线程

作为通信模块的一部分，每个机器人 i ∈ R i \in \mathcal{R} i∈R 实现了一个本地数据结构，称为缓存，其中包含机器人自己的变量 x i x_i xi，以及从机器人的邻居那里接收到的最新邻近姿态副本。Cunningham等人[20]-[22]提出了一个非常相似的设计，允许异步通信，尽管作者没有讨论异步设置中的收敛性。

由于只有 i i i 可以修改 x i x_i xi，因此机器人 i i i 的缓存中的 x i x_i xi 的值在任何时候都保证是最新的。相比之下，由于通信延迟，来自其他机器人的邻近姿态副本可能会过时。例如，当机器人 i i i 接收并使用机器人 j j j 的姿态副本时， j j j 可能已经由于其本地优化过程更新了其姿态。在第五节中，我们将展示ASAP（异步并行算法）能够抵御这种网络延迟。尽管如此，为了ASAP能够收敛，我们仍然假设通信过程引起的总延迟保持有界（第五节）。通信线程在缓存上执行以下两个操作。

● 接收：在通过网络从邻近机器人 j j j 接收到邻近姿态，例如 ( R j s , t j s ) (R_{j_s}, t_{j_s}) (Rjs,tjs) 后，通信线程更新缓存中的相应条目以存储新值。

● 发送：定期（当通信可用时），机器人 i i i 还将其最新的公共姿态变量（即与其他机器人有机器人间测量的姿态）传输给其邻居。回想第三节，机器人 i i i 不需要发送其私有姿态，因为这些姿态不是其他机器人优化其估计所需的。

B. 优化线程

与通信线程并行，优化线程由一个本地时钟调用，该时钟根据速率为 λ > 0 的泊松过程进行滴答。

定义1（泊松过程[25]）。考虑一系列正的、独立的随机变量 X1, X2, ..., J，它们代表连续事件（在这种情况下，是时钟滴答）之间经过的时间。设 N(t) 是直到时间 t ≥ 0 的事件数量。计数过程 N(t), t ≥ 0 是一个速率为 X > 0 的泊松过程，如果到达时间间隔 {Xi, X2, ...} 具有共同的指数分布函数，
P ( X k ≤ α ) = 1 − e − λ α , α ≥ 0. ( 7 ) P(X_k \leq \alpha) = 1 - e^{-\lambda \alpha}, \alpha \geq 0. \qquad (7) P(Xk≤α)=1−e−λα,α≥0.(7)

使用泊松时钟源于 Boyd 等人[26]设计的随机 g o s s i p gossip gossip 算法，并且是分析分布式随机算法全局行为的常用工具。速率参数 λ 在机器人之间是相同的。在实践中，我们可以根据网络延迟的程度和机器人的计算能力调整 λ。使用这个本地时钟，优化线程循环执行以下操作。

· 读取：对于每个邻近机器人 j ∈ N(i)，读取在本地缓存中存储的 j 的值。将读取的值表示为 x j x_j xj。

回想一下，εj 可能是过时的，例如，如果机器人 i 没有从机器人 j 那里收到最新的信息。此外，读取值 x i x_i xi，表示为 x i x_i xi。回想第四节A部分， x i x_i xi 保证是最新的。

在实践中， x j x_j xj 只包含机器人 j 的邻近姿态集合，因为 f i j f_{ij} fij 与 j 的其他姿态无关（图1）。然而，为了简化符号和分析，我们处理 j 时，就好像它包含了 j 的所有姿态集合。

计算：为机器人 i 形成局部成本函数，表示为 g i ( x i ) : M i → R g_{i}(x_{i}): \mathcal{M}{i} \rightarrow \mathbb{R} gi(xi):Mi→R，通过聚合问题 § 中涉及 x i x{i} xi 的相关成本，

g i ( x i ) = h i ( x i ) + ∑ j ∈ N ( i ) f i j ( x i , x ^ j ) . ( 8 ) g_i(x_i) = h_i(x_i) + \sum_{j \in N(i)} f_{ij}(x_i, \hat{x}_j). \qquad (8) gi(xi)=hi(xi)+j∈N(i)∑fij(xi,x^j).(8)

计算机器人 i 当前估计 x ^ i \hat{x}_{i} x^i 处的黎曼梯度，

η i = grad ⁡ g i ( x ^ i ) ∈ T x ^ i M i . ( 9 ) \eta_i = \operatorname{grad} g_i(\hat{x}i) \in T{\hat{x}_i} \mathcal{M}_i. \qquad (9) ηi=gradgi(x^i)∈Tx^iMi.(9)

更新：在下一个本地时钟滴答时，按照负梯度方向更新 x i x_{i} xi，

x i ← Retr ⁡ x ^ i ( − γ η i ) , ( 10 ) x_{i} \leftarrow \operatorname{Retr}{\hat{x}{i}}(-\gamma \eta_{i}), \qquad (10) xi←Retrx^i(−γηi),(10)

其中 γ > 0 \gamma > 0 γ>0 是一个恒定的步长。方程 (10) 提供了机器人可以遵循的最简单的更新规则。在第五节中，我们进一步证明了这个更新规则的收敛性。

为了在实践中加速收敛，SE-Sync [12] 和 Cartan-Sync [13] 使用了一种称为预处理的启发式方法，这也适用于 ASAPP。使用预处理时，使用以下替代的更新方向，
x i ← Retr ⁡ x ^ i ( − γ Precon ⁡ g i ( x ^ i ) [ η i ] ) . ( 11 ) x_{i} \leftarrow \operatorname{Retr}{\hat{x}{i}}(-\gamma \operatorname{Precon} g_{i}(\hat{x}{i})[\eta{i}]). \qquad (11) xi←Retrx^i(−γPrecongi(x^i)[ηi]).(11)

在 (11) 中，Precon g i ( x ^ i ) : T x ^ i M i → T x ^ i M i g_{i}(\hat{x}{i}): T{\hat{x}{i}}\mathcal{M}{i} \rightarrow T_{\hat{x}{i}}\mathcal{M}{i} gi(x^i):Tx^iMi→Tx^iMi 是切空间上的一个线性、对称且正定的映射，它近似于黎曼Hessian的逆。直观上，预处理帮助一阶方法从使用成本函数的（近似）二阶几何结构中受益，这通常可以在条件较差的问题上显著加快速度。

C. 实施细节

为了使本地时钟模型有效，我们要求 Read-Compute-Update 序列的总执行时间小于泊松时钟的到达时间间隔，以便当前序列可以在下一个序列开始之前完成。这个要求在实践中相当宽松，因为所有三个步骤只涉及最小的计算和访问本地内存。在最坏的情况下，由于到达时间间隔平均由 1 / λ 1/\lambda 1/λ 决定[25]，人们也可以降低时钟速率 λ \lambda λ 来为每次更新创造更多时间。

此外，我们注意到，尽管优化和通信线程并发运行，但为了确保所谓的个体姿态的原子读写，需要最少的线程级同步。具体来说，如果另一个线程正在积极修改其值，那么线程不能读取缓存中的姿态（否则读取的值将无效）。这种同步可以很容易地使用软件锁来执行。

五. 收敛性分析

A. 算法的全局视角

在第四节中，我们从每个机器人的本地视角描述了ASAP算法。然而，为了建立收敛性，我们需要从全局视角分析这个算法的系统行为[6]、[7]、[9]、[26]。为此，设 k = 0 , 1 , ... k=0,1,\ldots k=0,1,... 是一个虚拟计数器，记录所有机器人应用的总更新操作次数。此外，让随机变量 i k ∈ R i_k \in \mathcal{R} ik∈R 代表在全局迭代 k 时进行更新的机器人。我们强调，k 和 i k i_k ik 仅用于理论分析，并且在实践中对任何机器人都是未知的。

回想第四节B部分，所有更新步骤都是由 n = ∣ R ∣ n=|\mathcal{R}| n=∣R∣ 个独立的泊松过程生成的，每个过程的速率为 λ \lambda λ。从全局视角来看，合并这些本地过程相当于创建一个单一的、全局的泊松时钟，速率为 λ n \lambda n λn。此外，在任何时候，所有机器人都有相等的概率生成下一个更新步骤，即对于所有 k ∈ N k \in \mathbb{N} k∈N， i k i_k ik 是独立同分布的，并且在集合 R \mathcal{R} R 上均匀分布。参见[25]以获取这些结果的证明。

利用这个结果，我们可以从全局视角写出ASAP的迭代过程；参见算法1。我们使用 x k ≜ [ x 1 k x 2 k ... x n k ] x^k \triangleq \begin{bmatrix} x_1^k & x_2^k & \ldots & x_n^k \end{bmatrix} xk≜[x1kx2k...xnk] 来表示所有机器人姿态在 k 次全局迭代后（即，在 k 次总更新步骤后）的值。注意 x 位于乘积流形 M ≜ M 1 × M 2 × ... M n \mathcal{M} \triangleq \mathcal{M}1 \times \mathcal{M}2 \times \ldots \mathcal{M}n M≜M1×M2×...Mn 上。在全局迭代 k 时，从 R \mathcal{R} R 中随机均匀选择一个机器人 i k i_k ik（第2行）。然后机器人 i k i_k ik 遵循第四节B部分的步骤来更新自己的变量（第3-6行）。我们已经使用了 x ^ i k \hat{x}{i_k} x^ik 始终是最新的事实（第3行），而 x ^ j k \hat{x}{j_k} x^jk 对于 B ( j k ) B(j_k) B(jk) 总更新步骤是过时的（第4行）。除了机器人 i k i_k ik 之外，所有其他机器人都不更新（第7行）。作为后续分析有用的附加符号，我们注意到第7行可以等价地写成 x j k + 1 = Retr ⁡ x j k ( − γ η j k ) with η j k = 0 x_j^{k+1} = \operatorname{Retr}{x_j^k}(-\gamma \eta_j^k) \text{ with } \eta_j^k = 0 xjk+1=Retrxjk(−γηjk) with ηjk=0。

B. 收敛的充分条件

我们为ASAPP的收敛到一阶临界点建立了充分条件。由于篇幅限制，所有的证明均推迟到附录[27]中。在此，我们采用了常用的部分异步模型[3]，该模型假设由于异步性引起的延迟不会是任意大的。实际上，延迟的大小受多种因素的影响，如通信速率（见第IV-A节）。

本地优化的速率（第四节B部分）和固有的网络延迟。为了分析的目的，我们假设所有这些因素可以总结为一个单一的常数 B，它限制了算法1中的最大延迟，即所有机器人应用的全局迭代次数（即，更新步骤）。

假设1（有界延迟）。在算法1中，存在一个常数 B ≥ 0 B \geq 0 B≥0，使得对于所有 j k j_k jk， B ( j k ) ≤ B B(j_k) \leq B B(jk)≤B。

假设1对延迟施加了最坏情况下的上限，并允许实际延迟在这个上限内波动。此外，对于MLE问题 ( P 1 ) \left(P_{1}\right) (P1) 及其秩限制松弛 ( P 2 ) \left(P_{2}\right) (P2)，梯度满足Lipschitz型条件，这在我们的先前工作[2]中得到证明，并将在后面的分析中广泛使用。

引理1（拉回成本的Lipschitz型梯度[2]、[28]） 。将 ( P 1 ) \left(P_{1}\right) (P1) 和 ( P 2 ) \left(P_{2}\right) (P2) 的成本函数表示为 f : M → R f: \mathcal{M} \rightarrow \mathbb{R} f:M→R。定义拉回成本为 f ^ x ≜ f ∘ Retr ⁡ x : T x M → R \hat{f}_x \triangleq f \circ \operatorname{Retr}_x: T_x\mathcal{M} \rightarrow \mathbb{R} f^x≜f∘Retrx:TxM→R。存在一个常数 L ≥ 0 L \geq 0 L≥0，使得对于任何 x ∈ M x \in \mathcal{M} x∈M 和 η ∈ T x M \eta \in T_x\mathcal{M} η∈TxM，

∣ f ^ x ( η ) − [ f ( x ) + ⟨ η , grad ⁡ x f ⟩ ] ∣ ≤ L 2 ∥ η ∥ 2 . ( 12 ) \left|\hat{f}_x(\eta) - \left[f(x) + \left\langle \eta, \operatorname{grad}_x f \right\rangle \right]\right| \leq \frac{L}{2} \left\|\eta\right\|^2. \qquad (12) f^x(η)−[f(x)+⟨η,gradxf⟩] ≤2L∥η∥2.(12)

条件(12)最早由[28]提出，作为将Lipschitz连续梯度适应到黎曼优化的改进。利用有界延迟假设和(12)中的Lipschitz型条件，我们可以继续分析算法1（从全局视角）单次迭代后成本函数的变化。我们在以下引理中正式陈述结果。

引理2（算法1的下降性质）。在假设1下，算法1的每次迭代满足，
f ( x k + 1 ) − f ( x k ) ≤ − γ 2 ∥ grad ⁡ i k f ( x k ) ∥ 2 − γ − L γ 2 2 ∥ η i k k ∥ 2 + Δ B L 2 α 2 γ 3 2 ∑ j k ∈ N ( i k ) ∑ k ′ = k − B k − 1 ∥ η j k k ′ ∥ 2 , ( 13 ) \begin{align*} f\left(x^{k+1}\right) - f\left(x^{k}\right) \leq &-\frac{\gamma}{2}\left\|\operatorname{grad}{i{k}}f\left(x^{k}\right)\right\|^{2} - \frac{\gamma - L\gamma^{2}}{2}\left\|\eta_{i_{k}}^{k}\right\|^{2} \\ &+ \frac{\Delta B L^{2} \alpha^{2} \gamma^{3}}{2} \sum_{j_{k} \in N(i_{k})} \sum_{k^{\prime}=k-B}^{k-1} \left\|\eta_{j_{k}}^{k^{\prime}}\right\|^{2}, \quad (13) \end{align*} f(xk+1)−f(xk)≤−2γ gradikf(xk) 2−2γ−Lγ2 ηikk 2+2ΔBL2α2γ3jk∈N(ik)∑k′=k−B∑k−1 ηjkk′ 2,(13)

其中 η i k \eta_{i}^{k} ηik 表示机器人 i 在第 k 次迭代中采取的更新， α > 0 \alpha > 0 α>0 是与重投影相关的常数， Δ > 0 \Delta > 0 Δ>0 是机器人级图 G R G_{\mathcal{R}} GR 的最大度数。

在(13)中，右侧的最后一项是对一组 { η j k k ′ } \left\{\eta_{j_{k}}^{k^{\prime}}\right\} {ηjkk′} 的平方范数的求和，其中每个 η j k k ′ \eta_{j_{k}}^{k^{\prime}} ηjkk′ 对应于邻居 j k j_{k} jk 在早期迭代 k ′ k^{\prime} k′ 中采取的更新。这一项是系统延迟的直接结果，也是证明异步设置中收敛性的主要障碍。实际上，如果没有这一项，很容易验证任何满足 0 < γ < 1 / L 0 < \gamma < 1/L 0<γ<1/L 的步长都保证 f ( x k + 1 ) ≤ f ( x k ) f(x^{k+1}) \leq f(x^{k}) f(xk+1)≤f(xk)，从而导致收敛行为。然而，有了(13)中的最后一项，每次迭代后的整体成本可能会增加。尽管依赖延迟的误差项带来了额外的挑战，我们的下一个定理表明，通过足够小的步长，这个误差项是无关紧要的，并且ASAP算法可以证明会收敛到一阶临界点。

定理1（ASAP算法的全局收敛性）。设 f ⋆ f^{\star} f⋆ 是问题§的最优值的任何全局下界。定义 ρ ≜ Δ / n \rho \triangleq \Delta / n ρ≜Δ/n。设 γ ˉ > 0 \bar{\gamma} > 0 γˉ>0 是一个满足以下条件的步长上限，

2 ρ α 2 B 2 L 2 γ ˉ 2 + L γ ˉ − 1 ≤ 0. ( 14 ) 2\rho\alpha^{2} B^{2} L^{2}\bar{\gamma}^{2} + L\bar{\gamma} - 1 \leq 0. \qquad (14) 2ρα2B2L2γˉ2+Lγˉ−1≤0.(14)

特别地，以下 γ ˉ \bar{\gamma} γˉ 的选择满足 (14)：

γ ˉ = { 1 + 8 ρ α 2 B 2 − 1 4 ρ α 2 B 2 L , B > 0 , 1 / L , B = 0. ( 15 ) \bar{\gamma} = \left\{ \begin{array}{ll} \frac{\sqrt{1 + 8\rho\alpha^2 B^2} - 1}{4\rho\alpha^2 B^2 L}, & B > 0, \\ 1/L, & B = 0. \end{array} \right. \qquad (15) γˉ={4ρα2B2L1+8ρα2B2 −1,1/L,B>0,B=0.(15)

在假设1下，如果 0 < γ ≤ γ ˉ 0 < \gamma \leq \bar{\gamma} 0<γ≤γˉ，ASAP算法收敛到一个一阶临界点，具有全局次线性速率。具体来说，在 K 次总更新步骤后，

min ⁡ k ∈ { 0 , ... , K − 1 } E i 0 : K − 1 [ ∥ grad ⁡ f ( x k ) ∥ 2 ] ≤ 2 n ( f ( x 0 ) − f ⋆ ) γ K . ( 16 ) \min_{k \in \{0, \ldots, K-1\}} E_{i_{0: K-1}}\left[\left\|\operatorname{grad} f\left(x^{k}\right)\right\|^{2}\right] \leq \frac{2 n\left(f\left(x^{0}\right) - f^{\star}\right)}{\gamma K}. \qquad (16) k∈{0,...,K−1}minEi0:K−1[ gradf(xk) 2]≤γK2n(f(x0)−f⋆).(16)

注1. 据我们所知，定理1为异步算法在解决矩阵流形乘积上的非凸优化问题时首次建立了收敛性结果。收敛步长 γ ˉ \bar{\gamma} γˉ 的存在具有理论重要性，我们进一步注意到，其表达式(15)针对各种问题特定参数提供了正确的定性见解，我们将在下文讨论。

与最大延迟(B)的关系：随着最大延迟B的减少， γ ˉ \bar{\gamma} γˉ 增加。直观上，随着通信越来越可用，每个机器人可以在不引起发散的情况下采取更大的步长。 γ ˉ \bar{\gamma} γˉ 与 B 之间的反向关系在异步优化文献中是众所周知的，最初由 Bertsekas 和 Tsitsilis[3]在欧几里得设置中建立。

与问题稀疏性 ( p ) (p) (p)的关系：随着 ρ \rho ρ 的减少， γ ˉ \bar{\gamma} γˉ 增加。回想一下 ρ ≜ Δ / n \rho \triangleq \Delta / n ρ≜Δ/n 被定义为一个机器人拥有的最大邻居数与总机器人数之间的比率。因此， ρ \rho ρ 是机器人级图 G 稀疏性的度量。直观上，随着 G 变得更加稀疏，机器人可以使用更大的步长，因为他们的问题变得越来越解耦。这种 γ ˉ \bar{\gamma} γˉ 与问题稀疏性之间的正相关性是最新异步算法中的关键特征；参见例如[5]。

与问题平滑性 ( L ) (L) (L)的关系：从(15)中可以看出， γ ˉ \bar{\gamma} γˉ 随着 O ( 1 / L ) \mathcal{O}(1/L) O(1/L) 渐近增加。此外，当没有延迟(B=0)时，我们的步长与众所周知的常数 1/L 相匹配，同步梯度下降就是用这个常数收敛到一阶临界点的；参见例如[28]。

六. 实验结果

我们在C++中实现了ASAP算法，并在模拟和真实世界的PGO数据集上评估其性能。我们使用ROPTLIB[29]进行与流形相关的计算，以及使用机器人操作系统(ROS)[30]进行机器人间的通信。泊松时钟是通过在每次迭代后让优化线程暂停一段随机时间来实现的，这个随机时间呈指数分布。我们用C++实现了ASAP算法，并在模拟和真实世界的PGO数据集上评估了其性能。我们使用ROPTLIB[29]进行与流形相关的计算，以及使用机器人操作系统(ROS)[30]进行机器人间的通信。泊松时钟是通过在每次迭代后让优化线程暂停一段随机时间来实现的，这个随机时间呈指数分布，其速率为 λ \lambda λ（默认为1000 Hz）。由于每次迭代所花费的时间微不足道，我们预计这种实现与第四节B部分中的理论模型之间的实际差异可以忽略不计。所有机器人都被模拟为在配备有Intel i7四核CPU和16GB内存的台式计算机上运行的独立ROS节点。

对于每个PGO问题，我们使用ASAP算法解决其秩限制松弛问题(P2)，其中 r=5。正如先前的工作[4]-[7],[9]-[11]中通常所做的那样，在我们的实验中，我们经验性地选择步长。在优化过程中，我们记录了黎曼梯度范数 ∥ grad ⁡ f ( x k ) ∥ \left\|\operatorname{grad} f\left(x^{k}\right)\right\| gradf(xk) 的演变，这衡量了收敛到一阶临界点的程度。此外，我们还记录了最优性差距 f ( x k ) − f ( x ⋆ ) f\left(x^{k}\right) - f\left(x^{\star}\right) f(xk)−f(x⋆)，其中 x ⋆ x^{\star} x⋆ 是通过Cartan-Sync[13]计算的PGO问题(P1)的全局最小化解。在第六节B部分，我们还使用[2]中的方法将解决方案四舍五入到SE(d)，然后计算与全局最小化解相关的平移均方根误差(RMSE)和平旋转RMSE（以弦距离计）。

图2：在5个机器人模拟上的性能评估。通信延迟固定为0.5秒。我们将ASAP算法（步长 γ = 5 × 1 0 − 4 \gamma = 5 \times 10^{-4} γ=5×10−4）与一个基线算法进行比较，基线算法中每个机器人使用黎曼信任域方法来优化其局部变量。为了进行全面评估，我们以不同的优化速率运行基线算法，记录其在同步和异步环境下的性能。(a) ASAPP估计的示例轨迹，5个机器人的轨迹以不同颜色显示。机器人间的测量（闭环）显示为黑色虚线。(b) 相对于集中式全局最小化解的最优性差距 $$f(x^{k}) - f(x^{\star})。 © 黎曼梯度范数 ∥ grad ⁡ f ( x k ) ∥ \left\| \operatorname{grad} f(x^{k}) \right\| gradf(xk) 。

A. 模拟评估

我们在模拟的多机器人SLAM场景中评估ASAP算法，其中5个机器人在3D网格中相邻移动，轨迹呈割草机轨迹（图2a）。每个机器人有100个姿态。以0.3的概率，在彼此相距1米以内的姿态之间生成轨迹内外的闭环。所有测量都受到2°标准差的Langevin旋转噪声和0.05米标准差的高斯平移噪声的影响。为了在初始化期间最小化通信，我们通过在全球姿态图的生成树上传播相对测量来初始化解决方案。模拟中使用的步长是 γ = 5 × 1 0 − 4 \gamma = 5 \times 10^{-4} γ=5×10−4。在第一个实验中，我们通过让每个机器人每0.2秒通信一次来模拟通信延迟。我们将ASAP算法（无预处理）的性能与一个基线算法进行比较，基线算法中每个机器人使用二阶黎曼优化信任域（RTR）方法来优化其局部变量，类似于[2]中的方法。从SE-Sync[12]开始，RTR已成为集中式或同步设置中的默认求解器，因为它具有全局收敛保证，并且能够利用成本函数的二阶几何特性。为了进行全面评估，我们记录了这个基线算法在不同优化速率（即机器人更新其局部轨迹的频率）下的性能。

图2b显示了所评估算法实现的最优性差距，以实际时间为函数。相应的黎曼梯度范数的减少在图2c中显示。ASAP算法优于所有变体的基线算法（虚线曲线）。我们注意到，基线算法的行为是预期的。在低速率下，例如， λ = 1 H z \lambda=1~Hz λ=1 Hz（深蓝色虚线曲线），基线算法本质上是同步的，因为每个机器人都可以从其他机器人那里获得最新的姿势信息。然而，经验收敛速度仍然很慢，因为每个机器人需要在每次迭代后等待最新信息的到来。在高频率下，例如， λ = 1000 H z \lambda=1000~Hz λ=1000 Hz（深黄色虚线曲线），机器人基本上以异步方式行为。然而，由于RTR在每次迭代中不调节步长，机器人经常在错误的方向上显著改变它们的解决方案（由于使用过时的信息），这导致收敛速度慢甚至不收敛。相比之下，ASAP算法可以证明是收敛的，而且能够有效地利用异步性来加速。

此外，我们还评估了ASAP算法在广泛的通信延迟下的性能。由于空间限制，我们只在图3中显示了梯度范数方面的性能。我们注意到ASAP算法在所有情况下都收敛，证明了它在实践中对各种延迟的韧性。此外，随着延迟的减少，收敛速度变得更快，因为机器人能够从彼此那里获得更最新的信息。

图3：ASAP算法（步长 γ = 5 × 1 0 − 4 \gamma = 5 \times 10^{-4} γ=5×10−4）在不同通信延迟下的收敛速度。随着延迟的减少，收敛速度变快，因为机器人能够从彼此那里获得更最新的信息。

B. 在基准PGO数据集上的评估

我们在基准PGO数据集上评估ASAP算法，并将其性能与分布式高斯-塞德尔（DGS）[1]进行比较，DGS是最近多机器人SLAM系统[14]、[15]中用于分布式PGO的最新同步方法。每个数据集被分成5个部分，模拟5个机器人的协作SLAM任务。

遵循 Choudhary 等人[1]的方法，我们通过分布式弦初始化来初始化旋转估计。为了初始化平移，我们固定旋转估计，并使用分布式高斯-塞德尔方法解决平移上的简化线性系统。为了测试在没有准确初始化的情况下的场景，我们将旋转和平移初始化的高斯-塞德尔迭代次数限制在50次。

从初始估计开始，我们运行带有预处理的ASAP算法60秒，假设固定延迟为0.1秒。相应地，我们在问题（在线性化初始估计）上运行DGS[1] 600次同步迭代，即60/0.1 = 600。这种设置本质上有利于DGS，因为每次DGS迭代都需要机器人多次通信并按照特定顺序更新，这在现实中可能会增加执行时间。

表I比较了两种方法实现的最终成本值，对于ASAP算法，我们首先将解决方案四舍五入到SE(d)。对于ASAP算法，我们还报告了使用的步长、最终梯度范数以及与通过Cartan-Sync[13]计算的全局最小化解的估计误差。正如我们的结果所示，ASAP算法通常与DGS相比表现良好，特别是在初始化质量较差时。这是一个重要的优点，因为好的初始化方案（如在[1]中开发的分布式弦初始化）通常是迭代的，因此在通信方面成本较高。此外，ASAP解决方案接近全局最小化解，除了在Manhattan数据集上，旋转和平移误差相对较高。

我们通过观察得出本节结论，即在某些大型数据集上，ASAP算法的收敛速度在迭代接近临界点时较慢。这是次线性收敛速率的结果，而在我们的情况下，收敛速度进一步受到通信延迟的影响。为了解决这个问题，未来的工作可以考虑加速方法以实现更高的精度。为此，最近将Nesterov加速推广到PGO的论文[24]提供了一个有希望的方向。

表I：在基准PGO数据集上的评估。每个数据集被分成5个机器人的轨迹。我们在固定的通信延迟0.1秒下运行ASAP算法60秒。作为参考，我们还运行了600次同步迭代的DGS[1]。我们比较了两种方法的最终成本值，并用粗体突出显示更好的解决方案。对于ASAP算法，我们还报告了使用的步长、实现的梯度范数以及相对于全局最小化解的旋转和平移均方根误差（RMSE），这些是由Cartan-Sync[13]计算的。

七. 结论

我们提出了ASAP算法，这是第一个异步且可证明容忍延迟的算法，用于解决分布式姿态图优化及其秩限制的半定松弛问题。ASAP算法使每个机器人能够以高频率运行其本地优化过程，而无需等待网络上同行的更新。在假设通信延迟的最坏情况下有界的前提下，我们建立了ASAP算法的全局一阶收敛性，并展示了存在一个收敛步长，其值取决于最坏情况下的延迟和问题的固有稀疏性。当没有延迟时，我们进一步展示了这个步长与同步算法中相应的常数完全匹配。在模拟和真实世界数据集上的数值评估证实了ASAP算法在减少总体执行时间方面的优势，并展示了其对广泛通信延迟的韧性。

我们在第五节的理论研究中假设了最坏情况下有界的延迟。未来的工作可以考虑不那么保守的策略。例如，最近的论文[31]在无界延迟下建立了异步坐标下降的收敛性，其中只假设延迟的尾部分布衰减得足够快。另一个开放问题是，在什么条件下，对于一阶方法可能持有更强的性能保证，例如二阶最优性。最近的工作已经在这方面展示了有希望的结果[32]、[33]。