Continuous-time Gaussian process motion planning via probabilistic inference

Abstract

We introduce a novel formulation of motion planning, for continuous-time trajectories, as probabilistic inference. We first show how smooth continuous-time trajectories can be represented by a small number of states using sparse Gaussian process (GP) models. We next develop an efficient gradient-based optimization algorithm that exploits this sparsity and Gaussian process interpolation. We call this algorithm the Gaussian Process Motion Planner (GPMP). We then detail how motion planning problems can be formulated as probabilistic inference on a factor graph. This forms the basis for GPMP2, a very efficient algorithm that combines GP representations of trajectories with fast, structure-exploiting inference via numerical optimization. Finally, we extend GPMP2 to an incremental algorithm, iGPMP2, that can efficiently replan when conditions change. We benchmark our algorithms against several samplingbased and trajectory optimization-based motion planning algorithms on planning problems in multiple environments. Our evaluation reveals that GPMP2 is several times faster than previous algorithms while retaining robustness. We also benchmark iGPMP2 on replanning problems, and show that it can find successful solutions in a fraction of the time required by GPMP2 to replan from scratch.

Online Motion Planning Over Multiple Homotopy Classes with Gaussian Process Inference

“…where f gp i is a binary factor connecting consecutive states, f obs i is a unary collision likelihood factor, and f intp i,τ is a collision likelihood factor for a state at τ between consecutive support states. This factor graph has a sparse structure which can be exploited for rapidly solving the optimization problem [7].…”

Fast Manipulability Maximization Using Continuous-Time Trajectory optimization

“…where f p 0 and f p N define the prior distributions on the start and end states, and f gp i is the GP prior factor as defined in [7]. Furthermore, this property allows for interpolation of the trajectory in O(1) time [6].…”

Stochastic Optimization for Trajectory Planning with Heteroscedastic Gaussian Processes

“…where Q c (t) is an isotropic time-varying power-spectral density matrix, Q c (t) = Q c (t)I. A similar dynamical system has been utilized in estimation [15], [16], calibration [17] and planning [12], [18]. However, the crucial difference in our approach is that the covariance Q c (t) is time-varying and consequently generates a heteroscedastic GP [19].…”

“…Given that, we need to condition this GP with a fictitious observation on the goal state with mean µ N and covariance K N . This can be accomplished while still preserving the sparsity of the kernel matrix [12]…”

“…The Gaussian process (GP) motion planning family of algorithms [9]- [12] employs continous-time trajectory representation in order to overcome the computational cost incurred by using large number of states. The GPMP algorithm [9] parametrizes the trajectory with a few support states and then uses GP interpolation to query the trajectory at any time of interest.…”