[论文解读] GeoD: Consensus-based Geodesic Distributed Pose Graph Optimization

Eric Cristofalo, Eduardo Montijano+1 more|arXiv (Cornell University)|Oct 1, 2020

Robotics and Sensor-Based Localization参考文献 47被引用 8

一句话总结

GeoD 是一种基于共识的分布式位姿图优化算法，通过连续时间梯度下降最小化 SE(3) 流形上的测地线误差。它在大型图上实现收敛速度比集中式和分布式方法快达 100 倍，并且在平均情况下相比 SE-Sync 的误差降低 3.4 倍。

ABSTRACT

We present a consensus-based distributed pose graph optimization algorithm for obtaining an estimate of the 3D translation and rotation of each pose in a pose graph, given noisy relative measurements between poses. The algorithm, called GeoD, implements a continuous time distributed consensus protocol to minimize the geodesic pose graph error. GeoD is distributed over the pose graph itself, with a separate computation thread for each node in the graph, and messages are passed only between neighboring nodes in the graph. We leverage tools from Lyapunov theory and multi-agent consensus to prove the convergence of the algorithm. We identify two new consistency conditions sufficient for convergence: pairwise consistency of relative rotation measurements, and minimal consistency of relative translation measurements. GeoD incorporates a simple one step distributed initialization to satisfy both conditions. We demonstrate GeoD on simulated and real world SLAM datasets. We compare to a centralized pose graph optimizer with an optimality certificate (SE-Sync) and a Distributed Gauss-Seidel (DGS) method. On average, GeoD converges 20 times more quickly than DGS to a value with 3.4 times less error when compared to the global minimum provided by SE-Sync. GeoD scales more favorably with graph size than DGS, converging over 100 times faster on graphs larger than 1000 poses. Lastly, we test GeoD on a multi-UAV vision-based SLAM scenario, where the UAVs estimate their pose trajectories in a distributed manner using the relative poses extracted from their on board camera images. We show qualitative performance that is better than either the centralized SE-Sync or the distributed DGS methods.

研究动机与目标

• 解决在集中式计算不可行的大规模多机器人系统中对可扩展、分布式 SLAM 的需求。
• 开发一种直接基于 SO(3) 上测地线距离的位姿图优化方法，避免使用凸性弦松弛。
• 利用李雅普诺夫理论和多智能体共识工具，确保收敛具有可证明的保证。
• 通过确保稳定且无振荡的行为，实现实时、任意时间操作，适用于安全关键系统。
• 设计一种轻量级、分布式的架构，使每个机器人独立维护其自身的优化线程，并仅与邻居通信。

提出的方法

• 将位姿图优化表述为基于位姿图拓扑的连续时间共识协议，每个节点独立维护其自身的优化线程。
• 使用 SO(3) 上的测地线距离计算旋转误差，避免先前工作中采用的弦松弛方法。
• 采用一步分布式初始化，以强制实施成对旋转一致性与最小平移一致性——两个新的收敛条件。
• 利用反对称矩阵表示和矩阵对数进行旋转更新，对测地线误差执行梯度下降。
• 通过变分法和李群理论推导更新规则，确保更新始终位于 SE(3) 流形上。
• 采用李雅普诺夫稳定性分析，证明算法收敛至测地线位姿图误差的局部最小值。

实验结果

研究问题

• RQ1 能否在不使用弦松弛的情况下，通过测地线误差实现分布式位姿图优化算法的可证明收敛？
• RQ2 在基于共识的分布式框架中，相对测量的哪些一致性条件足以保证收敛？
• RQ3 在收敛速度和解的精度方面，GeoD 与集中式（SE-Sync）和分布式（DGS）方法相比表现如何？
• RQ4 GeoD 是否能在大规模位姿图（>1000 个位姿）上高效扩展，同时保持低误差和快速收敛？
• RQ5 在基于视觉的相对位姿估计的多架无人机真实场景 SLAM 中，GeoD 表现如何？

主要发现

• 在所有数据集上，GeoD 的收敛速度比 DGS 快 20 倍，且与 SE-Sync 提供的全局最小值相比，平均误差降低 3.4 倍。
• 在位姿数超过 1000 个的图上，GeoD 的收敛速度比 DGS 快超过 100 倍，展现出卓越的可扩展性。
• 尽管基于梯度下降，GeoD 的收敛速度仍比 SE-Sync 快 700 倍，且弦值误差仅比全局最小值高 32.5%。
• 一步分布式初始化成功实现了两个新一致性条件：成对旋转一致性和最小平移一致性。
• 在一次真实的多架无人机视觉 SLAM 实验中，GeoD 生成的轨迹估计在定性上优于 SE-Sync 和 DGS。
• 通过李雅普诺夫分析，GeoD 保持了稳定性和收敛性保证，确保在安全关键应用中不会发散或振荡。