Model-based Dynamic Pose Graph SLAM in Unstructured Dynamic Environments

Abstract

Navigation in dynamic environments is a challenge for autonomous vehicles operating without prior maps or global position references. This poses high risk to vehicles that perform scientific studies and monitoring missions in marine Arctic environments characterized by slowly moving sea ice with few truly static landmarks. Whereas mature simultaneous localization and mapping (SLAM) approaches assume a static environment, this work extends pose graph SLAM to spatiotemporally evolving environments. A novel model-based dynamic factor is proposed to capture a landmark's state transition model - whether the state be kinematic, appearance or otherwise. The structure of the state transition model is assumed to be known a priori, while the parameters are estimated on-line. Expectation maximization is used to avoid adding variables to the graph. Proof-of-concept results are shown in small- and medium-scale simulation, and small-scale laboratory environments for a small quadrotor. Preliminary laboratory validation results shows the effect of mechanical limitations of the quadrotor platform and increased uncertainties associated with the model-based dynamic factors on the SLAM estimate. Simulation results are encouraging for the application of model-based dynamic factors to dynamic landmarks with a constant-velocity kinematic model.