Information-Based Active SLAM Via Topological Feature Information-based Active SLAM via Topological

  • View
    0

  • Download
    0

Embed Size (px)

Text of Information-Based Active SLAM Via Topological Feature Information-based Active SLAM via Topological

  • Information-based Active SLAM via Topological Feature Graphs

    Beipeng Mu1 Matthew Giamou1 Liam Paull2 Ali-akbar Agha-mohammadi3

    John Leonard2 Jonathan How1

    Abstract— Exploring an unknown space and building maps is a fundamental capability for mobile robots. For fully au- tonomous systems, the robot would further need to actively plan its paths during exploration. The problem of designing robot trajectories to actively explore an unknown environment and minimize the map error is referred to as active simultaneous localization and mapping (active SLAM). Existing work has focused on planning paths with occupancy grid maps, which do not scale well and suffer from long term drift. This work proposes a Topological Feature Graph (TFG) representation that scales well and develops an active SLAM algorithm with it. The TFG uses graphical models, which utilize independences between variables, and enables a unified quantification of exploration and exploitation gains with a single entropy metric. Hence, it facilitates a natural and principled balance between map exploration and refinement. A probabilistic roadmap path- planner is used to generate robot paths in real time. Experi- mental results demonstrate that the proposed approach achieves better accuracy than a standard grid-map based approach while requiring orders of magnitude less computation and memory resources.

    I. INTRODUCTION

    The exploration of an unknown space is a fundamental capability for a mobile robot, with diverse applications such as disaster relief, planetary exploration, and surveillance. In the absence of a global position reference (e.g., GPS) the robot must simultaneously map the space and localize itself within that map, referred to as SLAM. If a mobile robot is able to successfully recognize parts of the map when it returns to them, referred to as loop closure, then it can significantly reduce its mapping and localization error. The problem of active SLAM focuses on designing robot trajectories to actively explore an environment and minimize the map error.

    Previous work has been done on designing trajectories to reduce robot pose uncertainty when the map is known or there exists a global position reference [1]–[5]. There also exists work that maximizes myopic information gain on the next action with partially known maps [6], [7]. However, in this work, the goal is to build a map of an unknown environment thus the robot needs to plan its path and perform SLAM at the same time (active SLAM) with a focus on global map quality. Active SLAM is non-trivial because the robot must trade-off the benefits of exploring new areas and exploiting visited areas to close loops [8].

    1Laboratory for Information and Decision Systems, MIT, 77 Mass Ave, Cambridge, MA, USA {mubp, mgiamou, jhow}@mit.edu

    2Computer Science and Artificial Intelligence Laboratory, MIT, 77 Mass Ave, Cambridge, MA, USA, {lpaull, jleonard}@mit.edu

    3Qualcomm Research, 5775 Morehouse Drive, San Diego, CA, USA, aliagha@qualcomm.com

    Fig. 1: Simultaneous planning, localization and mapping problem – purple polygons represent obstacles, green circles represent fea- tures with their size denoting uncertainties in pose estimates. The problem is to find milestones (gray circles) of robot poses, and plan a trajectory (red line) that minimizes feature uncertainties.

    Selecting the right metric to quantify the benefits of exploration and exploitation is fundamental to active SLAM but challenging. For example, the seminal work of Bourgault et al. [9] formulates the problem as a trade-off between information gain about the map and entropy reduction over the robot pose:

    u∗ = max u

    w1ISLAM (x, u) + w2IOG(x, u) (1)

    where IOG is the information gained over the occupancy grid (OG) map (grid of independent binary random variables de- noting occupancy) and ISLAM is the information gained over of the robot poses (dependent Gaussian random variables). Similarly, Stachniss et al. [10] use a Rao-Blackwellized particle filter (RBPF) to represent the robot poses and the map, and then consider the informativeness of actions based on the expected resultant information gain. Other information metrics within a similar framework, such as the Cauchy- Schwarz quadratic mutual information [11], the D-optimality criterion [12], and the Kullback-Leibler divergence [13] have also been proposed recently. These two information gains are computed separately and maintaining the balance between them often requires careful parameter tuning on the weights w1, and w2.

    Recently, graph-based optimization approaches to the SLAM problem have become very popular due to their ability to exploit the naturally sparse connectivity between robot poses and features in the map. These approaches have proven to have better scalability than the RBPF approaches, which ultimately suffer from particle depletion as the size of the environment grows. Within the graph-based approaches there are two main flavors: pose graphs and feature-based graphs. In the pose-graph approaches, sensor data is used

    2016 IEEE 55th Conference on Decision and Control (CDC) ARIA Resort & Casino December 12-14, 2016, Las Vegas, USA

    978-1-5090-1837-6/16/$31.00 ©2016 IEEE 5583

  • to generate relative transformation constraints between robot poses directly, and an underlying OG map is often required to represent the environment. For example, [14], [15] optimizes the robot trajectory by iteratively computing transformations between laser scans, but still maintains an underlying OG map and plans paths using sample-based approaches such as the probabilistic roadmap or the RRT* algorithm. [16] optimizes robot trajectory with features in a structured en- vironment for also maintains an OG for collision check. Information quantification over the OG map representation carries over known shortcomings of bad scalability and robustness [17]. The grid map is also an approximation because the conditional dependencies between the grid cells are discarded. For example, if there is significant drift in the robot’s pose estimate, this uncertainty is not reflected explicitly in the OG map. As a result, a straight corridor will appear curved, but their relative map entropies will be equivalent. In addition, OG maps also have large memory footprints.

    In a feature-based representation, features are explicitly maintained in the graph and give a layout of the map. However, active-SLAM on feature-based graphs is hard because features do not offer obstacle information, which is crucial for the robot to check path feasibility. This paper proposes the first, to our knowledge active SLAM approach that plans robot paths to directly optimize a global feature- based representation without any underlying OG representa- tion. Rather than formulating the problem as area coverage over an OG map [18], we set it up as entropy reduction over the map features subject to a budget constraint. Since the feature estimates and pose trajectory are necessarily correlated, we can remove the pose uncertainty from the traditional objective function (1) and directly optimize over the map quality.

    Figure 1 shows an example scenario. The locations of fea- tures are marked by green circles and the size of each circle represents its uncertainty. Gray circles represent samples of robot poses, and purple polygons represent obstacles. The planning problem is then to quantify information gains on the samples and find a trajectory connecting the samples that can minimize feature uncertainties.

    In summary, there are three primary contributions. First, a feature-based topology graph is proposed to represent the map of features as well as obstacles in an efficient way. Second, a feature-focused information metric is developed to quantify uncertainties in the map, in both visited and unvisited places. Finally, a path planning algorithm is pre- sented that uses the feature-based topological graph to enable the robot to actively explore with the objective of directly reducing the uncertainty of the map. The approach is tested in a Gazebo simulated environment, as well as in a real world environment with a turtlebot.

    II. PROBLEM STATEMENT

    Assume that there exists a library of static features that can be uniquely identified as landmarks to localize the robot in the environment, denoted as L = {L1, L2, · · ·LM}. Notice

    that the number of features present in the environment could be less than M , and is not known a priori. The exact locations of the present features are not known a priori either and need to be established by the robot. When moving in the environment, the robot’s trajectory is a sequence of poses XT = {X0, X1, · · · , XT }, where X0 gives the initial distribution of the robot pose, typically set as the origin with low uncertainty. The robot can obtain two kinds of observations. The odometry ot is the change between two consecutive poses with probability model p(ot|Xt, Xt−1). A feature measurement zt is a measurement between the current pose and features zt = {zt,1, · · · , zt,M}. When a feature is not observable, zt,i is defined to be null. The corresponding probability model of zt is p(zt|Xt,L).

    A factor graph is a sparse representation of the variables. Each node represents either a feature Li or a robot pose Xt. Let p(L) =

    ∏M i=1 p(Li) denote the prior for features.

    Each factor is a feature prior p(Li), an odometry ot or a feature measurement zt,i. The joint posterior of X and L is then the product of priors and likelihood of the observations o = {o1, · · · , oT } and z = {z1, · · · , zT }:

    p(X,L|o, z) ∝ p(L) T∏ t=1

    p(ot|Xt, Xt−1)p(zt|Xt,L). (2)

    The SLAM problem of jointly inferring the most likely posterior (MAP) feature position