A State Lattice Approach for Motion Planning under Control and Sensor Uncertainty

Abstract. Reliable motion planners have to take into account not only the kinematic constraints of the robot but, also, the uncertainty of both the motion and sensor models. In this way, it is possible to evaluate a motion plan based not just on the maximum likelihood path, but also in deviations from that path that have a non-negligible probability. As a result, motion plans are more robust and require a lower number cor-rections during the online implementation of the plan. In this paper we address the problem of motion planning under uncertainty in both mo-tion and sensor models using a state lattice. The approach manages a very efficient representation of the state space, calculates on-demand the a-priori probability distributions of the most promising states with an Extended Kalman Filter, and executes an informed forward exploration of the state space with Anytime Dynamic A*. We provide results with a differential drive robot under different scenarios, showing the ability of the planner to calculate optimal solutions that minimize the probability of collision and the time to reach the goal state.