Riemannian Optimization for Active Mapping with Robot Teams

TL;DR

ROAM tackles distributed active mapping and planning for robot teams by casting the problem as consensus-constrained optimization on Riemannian manifolds. It introduces a distributed gradient-based algorithm that alternates consensus updates with local gradient steps, relying only on one-hop communication and preserving manifold structure via the Exponential map. The framework is instantiated for two non-Euclidean tasks: distributed semantic octree mapping (log-odds on a PMF over semantic classes) and distributed SE(3) trajectory planning, each with consensus guarantees and, under reasonable assumptions, near-optimality relative to a centralized solution. Empirical evaluations in simulation and real-world indoor experiments demonstrate scalable consensus, effective map fusion, and practical feasibility with modest communication overhead, all implemented in open-source ROS-based code.

Abstract

Autonomous exploration of unknown environments using a team of mobile robots demands distributed perception and planning strategies to enable efficient and scalable performance. Ideally, each robot should update its map and plan its motion not only relying on its own observations, but also considering the observations of its peers. Centralized solutions to multi-robot coordination are susceptible to central node failure and require a sophisticated communication infrastructure for reliable operation. Current decentralized active mapping methods consider simplistic robot models with linear-Gaussian observations and Euclidean robot states. In this work, we present a distributed multi-robot mapping and planning method, called Riemannian Optimization for Active Mapping (ROAM). We formulate an optimization problem over a graph with node variables belonging to a Riemannian manifold and a consensus constraint requiring feasible solutions to agree on the node variables. We develop a distributed Riemannian optimization algorithm that relies only on one-hop communication to solve the problem with consensus and optimality guarantees. We show that multi-robot active mapping can be achieved via two applications of our distributed Riemannian optimization over different manifolds: distributed estimation of a 3-D semantic map and distributed planning of SE(3) trajectories that minimize map uncertainty. We demonstrate the performance of ROAM in simulation and real-world experiments using a team of robots with RGB-D cameras.

Figures (17)

• Figure 1: Overview of our distributed multi-robot active mapping approach. (a) A team of robots, denoted by vertex set ${\cal V}$, collaboratively explores an unknown environment. Each robot builds a local map using onboard sensor measurements and computes a local plan for the team, with the goal of maximizing the collective information gathered by the team. The local maps and plans are communicated over a peer-to-peer network whose connectivity is represented by the edge set ${\cal E}$. (b) As the robot team continues communication, the local maps stored by different robots become globally consistent in that they store similar information about the environment.
• Figure 2: Application of Alg. \ref{['alg:dist_opt']} to the leading eigenvector problem. (a) Data distribution $Z = [Z_1^\top Z_2^\top]^\top$, where $Z_1$ and $Z_2$ are separately known to agent $1$ and agent $2$. (b) State initialization, limited to the unit circle $\mathbb{S}^1$ since we are only interested in eigenvector directions. (c) Consensus and local objective function updates, shown in green and teal arcs, respectively. The exponential mapping of $\mathbb{S}^1$ maintains the manifold structure of the states throughout the update steps. (d) Level-set of the covariance matrix of the global data $Z$ is shown, alongside its leading eigenvector $x^*$.
• Figure 3: Semantically annotated point cloud (top) obtained from an RGBD sensor, where each object category is shown with a unique color and an octree map (bottom) obtained from the semantic point cloud.
• Figure 4: Collision and informativeness score for robot pose $\mathbf{X}$. Each sampled viewpoint $\mathbf{V}_l \in {\cal X}(\mathbf{X})$ is colored differently. For each viewpoint, the field of view and the distance from the nearest obstacle determine the Shannon mutual information $I(\mathbf{m}; \mathbf{z} \mid \mathbf{V}_l, p_t(\mathbf{m}))$ and the log-distance $\log{D(\mathbf{V}_l, p_t(\mathbf{m}))}$, respectively. The weight $\lambda_{\mathbf{V}_l}(\mathbf{X})$ dictates the contribution of $\mathbf{V}_l$ to the total score function $\mathfrak{f}(\mathbf{X}, p_t(\mathbf{m}))$, colored white.
• Figure 5: Software stack for multi-robot distributed active mapping. The blue blocks are local to each robot, whereas the red blocks require communication with neighboring robots. The communication links between pairs of robots are represented by violet lines.

Theorems & Definitions (6)

• Example
• Definition 1
• Theorem 1
• proof
• Lemma 1
• proof