A Multifrontal QR Factorization Approach to Distributed Inference Applied to Multirobot Localization and Mapping

Frank Dellaert, Alexander Kipp, Peter KrauthausenAAAI 2005

Abstract

This paper introduces a mathematically rigorous framework for distributed Simultaneous Localization and Mapping (SLAM) using Multifrontal QR (MFQR) factorization. By representing the robot network as a physical manifestation of a clique tree, the proposed method allows a team of robots to solve for a global map and trajectories using localized computation. The approach avoids the communication bottlenecks of centralized systems while maintaining the convergence speed of direct solvers. Experimental results from simulations and a deployment of ten Sony Aibo robots demonstrate the algorithm's effectiveness in handling noisy real-world data and loop closures.

Introduction

Simultaneous Localization and Mapping (SLAM) is a foundational challenge in autonomous robotics. In multi-robot scenarios (D-SLAM), the complexity increases significantly. Traditional centralized approaches, where all sensor data is sent to a single server, create significant communication bottlenecks and single points of failure. Conversely, distributed gradient descent methods, while robust, often suffer from slow convergence.

This research proposes a middle ground: a distributed inference method that is as mathematically rigorous and fast as centralized solvers but operates across a network of intelligent sensor nodes. The core innovation lies in adapting Multifrontal QR (MFQR) factorization—a high-performance sparse matrix technique—to the SLAM problem.

Methodology: From Factor Graphs to Clique Trees

The authors model the SLAM problem as a Gaussian Markov Random Field (GMRF), where nodes represent robot poses and landmarks, and edges represent measurements. This factor graph is then transformed into a "clique tree" (or junction tree), which serves as the computational backbone for the distributed inference process.

The clique tree structure used to organize distributed computation.

In this structure, the SLAM problem is decomposed into many small, dense "frontal matrices." The inference proceeds in two main phases:

  1. Factorization (Bottom-Up): Robots process local data at the leaves of the tree and pass condensed "update matrices" up toward a coordinator node. These matrices represent the distilled essence of the measurement history.
  2. Back-substitution (Top-Down): Once the root node (coordinator) solves the global constraints, it propagates the solution back down the tree, allowing each robot to finalize its local trajectory and map.

Communication Efficiency and Distributed Inference

A key advantage of this approach is its communication efficiency. Instead of sharing raw, high-bandwidth sensor data, robots only exchange "update matrices." These partial square-root factorizations are significantly smaller than the raw data but contain all necessary information for a global solution.

Sequence diagram illustrating the communication protocol between nodes (N) and the coordinator (C).

This protocol allows robots to maintain a "local view" of their environment for immediate navigation while waiting for the final global alignment from the coordinator. This "snap-to-grid" behavior ensures that local maps are eventually consistent with the global coordinate system.

Experimental Validation

The MFQR approach was validated through three distinct setups:

  • Small-scale Simulation: Demonstrated basic shared landmark perception between two robots.
  • Large-scale Simulation: Involved three robots creating a complex graph with multiple loops, testing the algorithm's ability to handle loop closures.
  • Real-world Experiment: A team of ten Sony Aibo robots was deployed in a 5x5 meter hallway with 32 artificial landmarks. Despite the Aibos' notoriously noisy odometry and sensors, the distributed system successfully closed loops and produced a map matching the performance of a centralized implementation.

Conclusion and Future Work

The distributed MFQR approach provides a mathematically exact Maximum A Posteriori (MAP) estimate with significantly reduced communication overhead compared to centralized methods. While successful, the current implementation focuses on linearized systems, requiring multiple iterations for non-linear SLAM problems. Future research will explore optimizing clique tree structures to minimize computational "fill-in" and addressing the challenges of unknown data association in unstructured environments.

Summary updated

BibTeX
@inproceedings{dellaert2005multifrontal,
  author = {Frank Dellaert and Alexander Kipp and Peter Krauthausen},
  title = {{A Multifrontal QR Factorization Approach to Distributed Inference Applied to Multirobot Localization and Mapping}},
  booktitle = {AAAI Conference on Artificial Intelligence},
  year = {2005},
}
Figure