Graph-based SLAM-Aware Exploration with Prior Topo-Metric Information

TL;DR

This work tackles autonomous exploration under SLAM by leveraging a prior topo-metric graph to jointly optimize exploration efficiency and pose-graph reliability. It introduces a SLAM-aware path planner that operates on the prior graph via a two-stage strategy: first compute an Open-loop TSP-based path to ensure complete area coverage, then insert distance-efficient and informative loop edges to stabilize the pose graph, using a greedy algorithm with pruning thresholds guided by a $D\text{-}opt(\mathbf{L}_{\gamma})$-based reliability metric. The method is embedded in a hierarchical exploration framework with online prior graph updates and sub-path replanning, enabling online adaptation to new information. Experimental results in simulation and real-world settings demonstrate higher mapping accuracy with comparable exploration efficiency relative to strong baselines, and the approach is open-source for community use and extension.

Abstract

Autonomous exploration requires a robot to explore an unknown environment while constructing an accurate map using Simultaneous Localization and Mapping (SLAM) techniques. Without prior information, the exploration performance is usually conservative due to the limited planning horizon. This paper exploits prior information about the environment, represented as a topo-metric graph, to benefit both the exploration efficiency and the pose graph reliability in SLAM. Based on the relationship between pose graph reliability and graph topology, we formulate a SLAM-aware path planning problem over the prior graph, which finds a fast exploration path enhanced with the globally informative loop-closing actions to stabilize the SLAM pose graph. A greedy algorithm is proposed to solve the problem, where theoretical thresholds are derived to significantly prune non-optimal loop-closing actions, without affecting the potential informative ones. Furthermore, we incorporate the proposed planner into a hierarchical exploration framework, with flexible features including path replanning, and online prior graph update that adds additional information to the prior graph. Simulation and real-world experiments indicate that the proposed method can reliably achieve higher mapping accuracy than compared methods when exploring environments with rich topologies, while maintaining comparable exploration efficiency. Our method has been open-sourced on GitHub.

Figures (7)

• Figure 1: SLAM-Aware exploration with prior topo-metric graph. The right figures show the occupancy grid map built after exploration, the prior topo-metric graph (green), the SLAM pose graph with odometry edges (red) and loop closures (blue). The numbers in blue circles indicate the order in which the robot visits different regions. Compared with other methods, the exploratory trajectory of the proposed method forms three big loops that quickly cover the environment while forming a globally reliable pose graph.
• Figure 2: The diagram of the proposed hierarchical exploration framework, including high-level planning and low-level navigation. The SLAM-aware path planner takes the prior topo-metric graph $\mathcal{G}^{\text{prior}}$ from users and outputs a high-level path $\mathcal{P}^{*}$. The robot follows vertices in $\mathcal{P}^{*}$ to perform either active loop-closing or frontier-based exploration within a region. The prior graph is updated online, based on which the replanning and sub-path optimization modules opportunistically find a better path for exploration.
• Figure 3: The occupancy maps built by the proposed exploration method in various environments. The sizes of environment maps are: (a) $87m\times 69m$, (b) $40m \times 57m$, (c) $138m \times 66m$. The Env. $1$ ($74m\times 74m$) is shown in Fig. \ref{['fig_map_results']}. Each image also shows the robot's pose graph (red), loop closures (blue), the prior topo-metric graph (vertices and edges in green), and the ID of each vertex (numbers in red). The numbers in blue circles indicate the order in which the robot visits different regions, and the loop-closing vertices are marked by orange rings. Fig. \ref{['fig_prior_graphs']}(a) also shows a zoom-in area of Env. $2$. The initial prior graph of Env. $2$ only provides a star-like connectivity between the vertices. After the online connectivity update, the sub-path optimization module finds a better sub-path for exploration.
• Figure 4: (a) An example topo-metric graph derived from a $10m\times 10m$ grid-like structure; (b) the initial TSP walk $\mathcal{P}^{\text{tsp}}$ (blue), the candidate (gray) and the selected (red) loop edges. (c) The ratio of the remaining candidate loop edges after the first iteration of Alg. \ref{['alg_greedy']} until termination. The curves in the upper area of (c) show the corresponding cases without edge pruning.
• Figure 5: Cumulative density distribution of the robot poses uncertainty. Each curve (with the lower and upper bound) is the averaged result over five independent runs. The pose uncertainty is evaluated by the determinant of the covariance matrix associated with each pose (using GTSAM gtsam) in the pose graph, and the distribution of pose uncertainty is discretized into $50$ sub-intervals to compute its cumulative density function.

Theorems & Definitions (5)

• Definition 1: Abstracted pose graph
• Remark 1
• Definition 2: Loop edge
• Proposition 1
• Proof 1