Dual-Stage Path Planning for Active Pose-Graph SLAM by Graph Topology

In this work, we present a dual-stage active pose-graph simultaneous localization and mapping (SLAM) framework for a robot in the three-dimensional(3D) environment. This framework aims to find the best path for rapid unknown environment exploration and efficient loop-closing reducing the uncertainty of pose-graph SLAM estimation. For online evaluating the uncertainty of pose-graph, we draw connections between the Fisher information matrix of pose-graph SLAM on mathfrak{so} (3)times mathbb{R}{3} and the weighted Laplacian matrix. Based on this relationship, we propose the active SLAM method by extending a two-stage exploration algorithm. The active graph-based SLAM framework incorporates two planning stages - local stage and global stage. In the local stage, the Rapidly-exploring Random Tree (RRT) is used to generate the optimal path for joint objective function. The global stage is for explicitly transiting the robot to different sub-areas in the environment. Simulation and experiment results show that this framework has a good performance on exploration and improving the quality of SLAM simultaneously.