Active SLAM over Continuous Trajectory and Control: A Covariance-Feedback Approach

TL;DR

This paper introduces a novel active SLAM method that optimizes continuous robot trajectories by integrating stochastic control and covariance feedback, improving localization and mapping accuracy.

Contribution

It presents a new covariance-feedback approach combining iterative Covariance Regulation and LQR for continuous trajectory optimization in active SLAM.

Findings

Effective in reducing uncertainty in landmark localization.

Demonstrates improved trajectory planning for better SLAM performance.

Handles stochastic robot dynamics with covariance-aware control.

Abstract

This paper proposes a novel active Simultaneous Localization and Mapping (SLAM) method with continuous trajectory optimization over a stochastic robot dynamics model. The problem is formalized as a stochastic optimal control over the continuous robot kinematic model to minimize a cost function that involves the covariance matrix of the landmark states. We tackle the problem by separately obtaining an open-loop control sequence subject to deterministic dynamics by iterative Covariance Regulation (iCR) and a closed-loop feedback control under stochastic robot and covariance dynamics by Linear Quadratic Regulator (LQR). The proposed optimization method captures the coupling between localization and mapping in predicting uncertainty evolution and synthesizes highly informative sensing trajectories. We demonstrate its performance in active landmark-based SLAM using relative-position measurements with a limited field of view.