Occupancy-SLAM: Simultaneously Optimizing Robot Poses and Continuous Occupancy Map

TL;DR

The paper tackles the problem of simultaneously estimating a robot trajectory and a continuous occupancy map from 2D LiDAR scans in an offline batch setting. It introduces Occupancy-SLAM, a nonlinear least-squares framework that jointly optimizes the pose sequence and occupancy values M on a grid through a differentiable bilinear interpolation, combining observation, odometry, and smoothing terms and solved with a Gauss-Newton–style method. Analytical Jacobians are derived for the observation w.r.t. poses and map, the odometry term, and the smoothing operator, enabling uncertainty quantification via the information matrix. Experimental results on simulations and real datasets show systematic improvements in both pose accuracy and map quality compared with Cartographer and other baselines, validating the benefits of joint optimization with a continuous map representation in an offline setting.

Abstract

In this paper, we propose an optimization based SLAM approach to simultaneously optimize the robot trajectory and the occupancy map using 2D laser scans (and odometry) information. The key novelty is that the robot poses and the occupancy map are optimized together, which is significantly different from existing occupancy mapping strategies where the robot poses need to be obtained first before the map can be estimated. In our formulation, the map is represented as a continuous occupancy map where each 2D point in the environment has a corresponding evidence value. The Occupancy-SLAM problem is formulated as an optimization problem where the variables include all the robot poses and the occupancy values at the selected discrete grid cell nodes. We propose a variation of Gauss-Newton method to solve this new formulated problem, obtaining the optimized occupancy map and robot trajectory together with their uncertainties. Our algorithm is an offline approach since it is based on batch optimization and the number of variables involved is large. Evaluations using simulations and publicly available practical 2D laser datasets demonstrate that the proposed approach can estimate the maps and robot trajectories more accurately than the state-of-the-art techniques, when a relatively accurate initial guess is provided to our algorithm. The video shows the convergence process of the proposed Occupancy-SLAM and comparison of results to Cartographer can be found at \url{https://youtu.be/4oLyVEUC4iY}.

Figures (9)

• Figure 1: Sampling strategy for generating observations from a laser scan. (a) Observation points are sampled on a beam in the scan using equidistant sampling strategy. The red point indicates occupied state and the blue points indicate free states. The distance between two consecutive points is $s$ (the resolution). (b) All sampled observation points at a given time step.
• Figure 2: The bilinear interpolation method for a continuous coordinate $P_m$, whose occupancy value can be computed by those of the four adjacent grid cell nodes $m_{wh}, m_{({w+1})h}, m_{w({h+1})}, m_{({w+1})({h+1})}$.
• Figure 3: Simulation environments, robot trajectory results and uncertainty maps. (a) and (b) show the simulation environments (the black lines indicate the obstacles in the scene) and the trajectories of ground truth, odometry inputs, Cartographer and our approach for one dataset in each of the two simulation experiments. (c) and (d) show the uncertainty maps of our approach for the same datasets.
• Figure 4: Comparison of Translation Error and Rotation Error.
• Figure 5: The OGMs and point cloud maps generated by ground truth poses, poses from Cartographer and poses from our approach in one dataset for each simulation. The point cloud maps are generated by projection of the scan endpoints using the poses. The first two rows are the OGMs and point cloud maps of the dataset in Simulation 1, and the third to fourth rows are the OGMs and point cloud maps of the dataset in Simulation 2. (b) and (c) show the results of original sampling rate. (d) and (e) show the results of 0.5 times sampling rate.