Simultaneous Trajectory Estimation and Planning via Probabilistic Inference

Authors: Mustafa Mukadam, Jing Dong, Frank Dellaert, Byron Boots

In this paper, we provide a unified approach to trajectory estimation and planning. The key idea is to view these two problems, usually considered separately, as a single problem. At each time-step the robot is tasked with finding the complete continuous-time trajectory from start to goal. This can be quite difficult; the robot must contend with a potentially high-degree-of-freedom (DOF) trajectory space, uncertainty due to limited sensing capabilities, model inaccuracy, and the stochastic effect of executing actions, and the robot must find the solution in (faster than) real time. To overcome these challenges, we build on recent probabilistic inference approaches to continuous-time localization and mapping and continuous-time motion planning. We solve the joint problem by iteratively recomputing the maximum a posteriori trajectory conditioned on all available sensor data and cost information. We show how to implement our approach on a real system, and we evaluate our framework empirically in both simulation and on a mobile manipulator.