STEAP: simultaneous trajectory estimation and planning

TL;DR

STEAP introduces a unified probabilistic framework that simultaneously estimates and plans robot trajectories, improving accuracy and efficiency by considering the entire trajectory as a joint estimation and planning problem.

Contribution

The paper presents a novel approach that combines trajectory estimation and planning into a single probabilistic framework, enabling real-time solutions for complex robotic tasks.

Findings

Effective in high-DOF trajectory spaces

Handles uncertainty from limited sensing and model inaccuracies

Operates in real-time on a mobile manipulator

Abstract

We present a unified probabilistic framework for simultaneous trajectory estimation and planning (STEAP). Estimation and planning problems are usually considered separately, however, within our framework we show that solving them simultaneously can be more accurate and efficient. The key idea is to compute the full continuous-time trajectory from start to goal at each time-step. While the robot traverses the trajectory, the history portion of the trajectory signifies the solution to the estimation problem, and the future portion of the trajectory signifies a solution to the planning problem. Building 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. Our approach can contend with high-degree-of-freedom (DOF) trajectory spaces, uncertainty due to limited sensing capabilities, model inaccuracy, the stochastic effect of executing actions, and can find a solution in real-time. We evaluate our framework empirically in both simulation and on a mobile manipulator.