Multi-Step Look-Ahead Trajectory Planning in SLAM: Possibility and Necessity

In this paper, the possibility and necessity of multi-step trajectory planning in Extended Kalman Filter (EKF) based SLAM is investigated. The objective of the trajectory planning here is to minimize the estimation error of the robot and landmark locations subject to a given time horizon. We show that the problem can be regarded as an optimization problem for a gradually identified model. A numerical method is proposed for trajectory planning using a variant of the nonlinear Model Predictive Control (MPC). The proposed method is optimal in the sense that the control action is computed using all the information available at the time of decision making. Simulation results are included to compare the results from the one-step look-ahead trajectory planning and the proposed multi-step look-ahead technique.