Abstract

We introduce a novel formulation of motion planning, for continuous-time trajectories, as probabilistic inference. 1. We first show how smooth continuous-time trajectories can be represented by a small number of states using sparse Gaussian process (GP) models. 2. We next develop an efficient gradient-based optimization algorithm that exploits this sparsity and GP interpolation. We call this algorithm the Gaussian Process Motion Planner (GPMP). 3. 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. 4. Finally, we extend GPMP2 to an incremental algorithm, iGPMP2, that can efficiently replan when conditions change. We benchmark our algorithms against several sampling-based 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. # Related Work ## Sampling-based algorithms These can effectively find feasible trajectories for high-dimensional systems, but the trajectories often exhibit jerky and redundant motion and therefore require post-processing to address optimality.

Examples

• Probability Roadmaps (PRMs) (Kavraki et al., 1996)
  • Construct a dense graph from random samples in obstacle-free areas of the robot’s configuration space. PRMs can be used for multiple queries by finding the shortest path between a start and goal configuration in the graph.
• Rapidly exploring random trees (RRTs) (Kuffner and LaValle, 2000)
  • Find trajectories by incrementally building space-filling trees through directed sampling. RRTs are very good at finding feasible solutions in highly constrained problems and high-dimensional search spaces. Both PRMs and RRTs offer probabilistic completeness, ensuring that, given enough time, a feasible trajectory can be found, if one exists. Despite guarantees, sampling-based algorithms may be difficult to use in real-time applications owing to computational challenges. Often computation is wasted exploring regions that may not lead to a solution. Recent work in informed techniques (Gammell et al., 2015) combatted this problem by biasing the sampling approach to make search more tractable.

Trajectory Optimization algorithms

These minimize an objective function that encourages trajectories to be both feasible and optimal. A drawback of these is that in practice, a fine quantization of the trajectory is necessary to integrate cost information when reasoning about thin obstacles and tight constraints.

In addition, these algorithms are locally optimal and hence require different starting and ending points (perturbations to initial conditions) on repeated runs to find a feasible solution, which can incur high computational cost.

The latter problem can be circumvented using a feasible solution found by a Sampling-based algorithm as an initial trajectory.

Examples

In contrast to sampling-based planners, trajectory optimization starts with an initial, possibly infeasible, trajectory and then optimizes the trajectory by minimizing a cost function. - Covariant Hamiltonian optimization for motion planning (CHOMP) and related methods (Byravan et al., 2014; He et al., 2013; Marinho et al., 2016; Ratliff et al., 2009; Zucker et al., 2013) optimize a cost functional using covariant gradient descent - Stochastic trajectory optimization for motion planning (STOMP) (Kalakrishnan et al., 2011) optimizes non-differentiable costs by stochastic sampling of noisy trajectories. - TrajOpt (Schulman et al., 2014, 2013) solves a sequential quadratic program and performs convex continuous-time collision checking. In contrast to sampling-based planners, trajectory optimization methods are very fast, but only find locally optimal solution. The computational bottleneck results from evaluating costs on a fine discretization of the trajectory or, in difficult problems, repeatedly changing the initial conditions until a feasible trajectory is discovered.

Motion planning as trajectory optimization

Our goal is to find trajectories $$\boldsymbol\theta(t):t\to \mathbb{R}^{D}$$ where $D$ denotes the dimension of the state, that 1. satisfy our constraints (are Feasible) and; 2. minimize the cost (Optimal) We defined motion-planning in Motion-Planning Algorithm.

GPs for continuous-time trajectories

The GP Prior

We take continuous-time trajectories as samples from a vector valued GP$$\boldsymbol\theta(t)\sim \mathcal{GP}(\pmb\mu(t),\mathbfcal{K}(t,t'))$$where - $\boldsymbol\mu(t)$ is a VV mean function and; - $\mathbfcal{K}(t,t')$ is a matrix-valued covariance function. A VV GP is a collections of Random Variables any finite number of which have a joint Gaussian pdf.

We can say that for any collection of times $\mathbf{t}=\{ t_{0},\dots,t_{N} \}$ $\boldsymbol\theta$ has a joint Gaussian distribution: $$\boldsymbol\theta=[\boldsymbol\theta_{0}\ \dots \ \boldsymbol\theta_{N}]^{T}\sim \mathcal{N}(\boldsymbol\mu,\mathbfcal{K})$$with mean vector $\boldsymbol\mu$ and covariance kernel $\mathbfcal{K}$ defined as $$\boldsymbol\mu=[\boldsymbol\mu_{0} \ \dots \ \boldsymbol\mu_{N}]^{T},\quad\mathbfcal{K}=[\mathbfcal{K}(t_{i},t_{j})]_{ij,0\le i,j\le N}$$We use the bold $\boldsymbol\theta$ to denote the matrix formed by vectors $\boldsymbol\theta_{i}\in\mathbb{R}^{D}$ which are support states that parameterize the continuous-time trajectory $\boldsymbol\theta(t)$.

A Gauss-Markov model

We use a structured kernel generated by a LTV SDE $$\dot{\boldsymbol\theta}(t)=\mathbf{A}(t)\theta(t)+\mathbf{u}(t)+\mathbf{F}(t)\mathbf{w}(t)$$where $\mathbf{u}(t)$ is our control and $\mathbf{w}(t)$ is a white noise process i.e. $$\mathbf{w}(t)\sim \mathcal{GP}(\mathbf{0},\mathbf{Q}_{C}\delta(t-t'))$$Here $\mathbf{Q}_{C}$ is the power-spectral density matrix and $\delta(t-t')$ is the Dirac delta function. The solution to the initial value problem of this LTV-SDE is $$\boldsymbol\theta(t)=\boldsymbol\Phi(t,t_{0})\boldsymbol\theta_{0}+\int\limits _{t_{0}}^{t}\boldsymbol\Phi(t,\tau)(\mathbf{u}(\tau)+\mathbf{F}(\tau)\mathbf{w}(\tau)) \, d\tau$$where $\boldsymbol\Phi$ is the transition matrix. The mean and covariance functions of the GP defined by this LTV-SDE are calculated by taking first and second moments $$\begin{align*} \tag{1}\tilde{\mu}(t)&=\boldsymbol\Phi(t,t_{0})\boldsymbol\mu_{0}+\int\limits _{t_{0}}^{t}\boldsymbol\Phi(t,s)\mathbf{u}(s) \, ds\\ \tag{2}\tilde{\mathbfcal{K}}(t,t')&=\boldsymbol\Phi(t,t_{0})\mathbfcal{K}_{0}\boldsymbol\Phi(t',t_{0})^{T}+\int\limits _{t_{0}}^{\min(t,t')}\boldsymbol\Phi(t,s)\mathbf{F}(s)\mathbf{Q}_{C}\mathbf{F}(s)^{T}\boldsymbol\Phi(t',s)^{T} \, ds \end{align*}$$where $\boldsymbol\mu_{0}$ and $\mathbfcal{K}_{0}$ are the initial mean and covariance of the start state.

$\mathfrak{S}$