PaperlayerView exact revision on arXiv

Source-linked AI summary

Continuous-Time Gaussian Process Motion Planning via Probabilistic Inference

Mustafa Mukadam, Jing Dong, Xinyan Yan, Frank Dellaert, Byron Boots

arXiv:1707.07383v3cs.RO

TL;DR

The paper addresses efficient motion planning for continuous-time trajectories while avoiding the computational burden of dense representations and slow replanning from scratch. It uses sparse Gaussian-process trajectory representations with factor-graph inference, yielding GPMP2 and incremental iGPMP2. GPMP2 is consistently several times faster than nearby competitors, while iGPMP2 solves replanning an order of magnitude faster than GPMP2 from scratch.

  • Problem

    Existing motion planners face computational or trajectory-quality limitations when handling continuous-time constraints, fine discretization, and changing conditions.

  • Method

    The paper represents continuous-time trajectories with sparse Gaussian processes and formulates planning as factor-graph probabilistic inference solved by structure-exploiting optimization.

  • Results

    GPMP2 is consistently several times faster than nearby trajectory-optimization and sampling-based competitors, while iGPMP2 replans an order of magnitude faster than GPMP2 from scratch.

  • Takeaways & Limitations

    Sparse GP interpolation and incremental factor-graph inference support efficient motion planning and fast replanning for real-time applications.

  • Takeaways & Limitations

    The approach is limited in handling motion constraints such as nonlinear inequalities, for which integrating sequential quadratic programming remains future work.

Abstract

from arXiv · show

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 GP 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 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.

1. Introduction

Motion planning seeks trajectories that satisfy task constraints while optimizing qualities such as smoothness. The paper addresses limitations of existing planners with continuous-time GP representations and factor-graph inference, extending the approach to efficient replanning.

  • Motivation: Motion planners seek trajectories that are both feasible and optimal, balancing task constraints with qualities such as smoothness.Feasibility commonly includes obstacle avoidance and goal achievement, while optimality can penalize velocity or acceleration.
  • Existing approaches: Sampling-based methods can find feasible high-dimensional trajectories, but often produce jerky, redundant motion requiring optimality-oriented post-processing.Optimal sampling-based planners can also be computationally inefficient on high-dimensional problems with challenging constraints.
  • Existing approaches: Trajectory optimization jointly encourages feasibility and optimality but commonly requires fine discretization for thin obstacles and tight constraints.Such methods may also be locally optimal and need repeated initialization attempts to find feasible solutions.
  • Proposed approach: GPMP represents smooth continuous-time trajectories with sparse support states, uses GP interpolation for queries, and optimizes support states using propagated cost information.The representation supports trajectory queries at any time of interest with efficient GP regression.
  • Proposed approach: GPMP2 formulates motion planning as probabilistic inference on factor graphs, enabling structure-exploiting nonlinear least-squares optimization.The formulation represents optimality through a trajectory prior and feasibility through a likelihood function.
  • Contributions: iSAM-style incremental factor-graph inference extends GPMP2 to efficiently solve replanning problems.The paper benchmarks GPMP and GPMP2 against leading planners and reports faster GPMP2 performance and order-of-magnitude faster incremental replanning with iGPMP2.

2. Related work

Prior motion-planning work includes sampling-based and trajectory-optimization methods, continuous-time representations, probabilistic inference, and incremental replanning. GPMP2 combines sparse GP interpolation with factor-graph optimization, while iGPMP2 updates only necessary trajectory regions.

  • Sampling-based planning: Sampling-based planners construct roadmaps or trees through random sampling to find paths in configuration space.PRMs support multiple queries through graph search, while RRTs incrementally build space-filling trees.
  • Trajectory optimization: Trajectory optimization starts from an initial trajectory and minimizes a cost function, with methods such as CHOMP using covariant gradient descent.These methods differ from sampling-based planners by optimizing an existing trajectory rather than constructing paths from samples.
  • Continuous-time representations: Continuous-time representations can reduce the computational cost of fine discretization, while Gaussian processes additionally provide uncertainty and sparse parameterization.Structured GPs enable fast interpolation for fine-resolution obstacle evaluation while retaining a small number of support states.
  • Probabilistic inference: Probabilistic inference methods have been applied to planning and control, and this work uses factor-graph nonlinear least squares to bring SLAM-style efficiency to motion planning.The same factor-graph structure also supports incremental inference for replanning.
  • Replanning: Earlier replanning methods face discretization or feasibility limitations, whereas the proposed incremental approach updates the trajectory only where necessary.This reduces redundant calculations and supports fast replanning.

3. Motion planning as trajectory optimization

Trajectory optimization formulates motion planning as finding a continuous-time state trajectory that satisfies constraints while minimizing a trajectory-quality objective. The formulation encodes smoothness, collision avoidance, joint limits, and task-specific endpoint requirements.

  • Problem formulation: Trajectory optimization seeks a continuous-time trajectory θ(t) mapping time to robot states while satisfying constraints and minimizing cost.The state dimension is D, and states may include configurations and higher-order derivatives.
  • Constraints: Inequality functionals G_i[θ(t)] encode constraints such as joint-angle limits.The formulation permits zero or more inequality constraints depending on the planning problem.
  • Constraints: Equality functionals H_i[θ(t)] encode task-dependent requirements such as start and end configurations, velocities, or end-effector orientation.The number of equality constraints may also be zero for some problems.
  • Objective: The objective functional F[θ(t)] evaluates trajectory quality through terms such as smoothness and collision costs.Smoothness can penalize higher-order derivatives including velocity or acceleration, while collision costs enforce collision-free motion.

4. Gaussian processes for continuous-time trajectories

The paper represents smooth continuous-time robot trajectories as samples from vector-valued Gaussian processes, using structured priors that support sparse parameterization and efficient interpolation.

  • Continuous-time trajectories are modeled as samples from a vector-valued GP with mean function µ(t) and covariance function K(t, t′).
  • 4.3 Gaussian process interpolation: Only a sparse set of support states parameterizes the trajectory, while GP regression queries states at arbitrary times using adjacent support states in O(1) operations.
  • The GP prior encourages trajectory smoothness and penalizes deviation from its prior mean through the negative log prior cost.
  • 4.2 A Gauss-Markov model: A Gauss-Markov construction gives the GP an exactly sparse tridiagonal precision matrix, enabling fast interpolation and efficient optimization.
  • The linear system model covers the robot manipulators studied, while the framework can be extended to nonlinear models.
  • 4.3 Gaussian process interpolation: Dense interpolation permits high-resolution collision-cost evaluation while propagating gradient information back to update only the sparse support states.

5. Gaussian process motion planning

GPMP combines the GP trajectory representation with gradient-based optimization, evaluating obstacle costs on densely interpolated trajectories while updating a sparse set of support states.

  • GPMP combines a Gaussian process trajectory representation with gradient descent-based optimization for motion planning.
  • 5.1 Cost functionals: The objective balances a GP prior cost penalizing deviation from the prior mean against an obstacle cost through a trade-off parameter λ.
  • 5.2 Optimization: The augmented trajectory includes velocities and accelerations, which preserve a Markovian prior and provide direct quantities for obstacle-gradient computation.
  • 5.2 Optimization: The GPMP update generalizes CHOMP through an augmented trajectory and generalized prior.
  • 5.3 Compact trajectory representations and faster updates via GP interpolation: GP interpolation upsamples sparse trajectories, evaluates collision costs and gradients at high resolution, and projects the information back onto support states.
  • GPMP’s gradient-based scheme can converge slowly and incur costly gradient calculations, motivating the subsequent GPMP2 method.

6. Motion planning as probabilistic inference

The paper formulates motion planning as probabilistic inference, encoding smoothness in a GP prior and collision avoidance in a likelihood. GPMP2 exploits factor-graph sparsity and nonlinear least-squares optimization for faster inference.

  • 6.1 Probabilistic inference for motion planning: Motion planning is formulated as inference over trajectories conditioned on desired events, primarily collision-free events.
  • 6.1 Probabilistic inference for motion planning: The prior encourages smooth trajectories, the likelihood represents collision freedom, and the optimal trajectory is obtained by maximum a posteriori estimation.
  • 6.2 Factor graph formulation: The posterior factors into a graph containing start and goal priors, GP prior factors, regular obstacle factors, and interpolated obstacle factors.
  • 6.2 Factor graph formulation: Interpolated obstacle factors evaluate costs between support states and use adjacent states to update the sparse support-state representation.
  • 6.3 Solving the inference problem: GPMP2 solves sparse linear systems using block-tridiagonal structure, sparse Cholesky decomposition, and forward-backward passes.
  • 6.3 Solving the inference problem: Combining structure-exploiting iterations with Gauss-Newton or Levenberg-Marquardt’s quadratic convergence makes GPMP2 faster than GPMP.

7. Incremental inference for fast replanning

The paper reformulates replanning as incremental inference on factor graphs, using Bayes trees to update only affected parts of an existing solution. This supports efficient online updates when goals, state estimates, or environmental information change.

  • 7. Incremental inference for fast replanning: Incremental factor-graph inference updates an existing motion-planning solution instead of resolving the entire problem from scratch.Replanning addresses partially changed conditions, while full re-optimization can be too slow and duplicate work when most of the problem is unchanged.
  • 7.1 Bayes tree as incremental inference: Variable elimination converts the planning factor graph into a sparse Bayes net, with start-to-goal ordering yielding a chain-like structure.For this ordering, the separator is S_j = {θ_j+1}, supporting the sparse factorization used by the Bayes tree.
  • 7.1 Bayes tree as incremental inference: A Bayes tree is a directed tree-structured graphical model whose nodes are cliques and whose conditional densities represent factored probabilistic relationships.In the planning chain, the cliques are C_k = {θ_k, θ_k+1}.
  • 7.1 Bayes tree as incremental inference: Bayes trees enable minimal recomputation by refactorizing only cliques containing affected variables and the cliques between them and the root.Unaffected cliques remain unchanged; changes closer to the root require less computation in planning cases.
  • 7.2 Replanning using Bayes tree: Replanning examples update either the root for a changed goal or trajectory-related tree parts for a new observation at θ2.The changed Bayes-tree regions correspond to the parts of the trajectory that get updated.
  • 7.2 Replanning using Bayes tree: iGPMP2 combines GPMP2 with iSAM2 to add or replace factors and return an updated optimal trajectory through Bayes-tree updates.The workflow solves the original batch problem, incorporates new information, and updates the Bayes tree inside iSAM2.

8. Implementation details

The implementation combines GP-based trajectory priors, sphere-based collision costs, soft motion constraints, and nonlinear least-squares optimization. GPMP2 uses a constant-velocity prior, while GPMP uses a constant-acceleration prior.

  • 8. Implementation details: GPMP is implemented on CHOMP, while GPMP2 and iGPMP2 use GTSAM; the implementations are released through the gpmp2 C++ library and a ROS interface.The implementation uses CHOMP’s framework for GPMP and GTSAM for GPMP2 and iGPMP2.
  • 8.1 GPMP: GPMP uses a constant-acceleration, jerk-minimizing prior over position, velocity, and acceleration to encourage smooth trajectories.The prior is centered on a zero-jerk trajectory and penalizes jerk during optimization.
  • 8.2.1 Obstacle cost: The robot is approximated by multiple spheres, and collision costs use forward kinematics, signed distances, a voxel-grid SDF, and hinge losses.The sphere representation makes surface-to-obstacle distance tractable; the hinge-loss vector aggregates costs across the robot model.
  • 8. Implementation details: GPMP2 solves nonlinear least squares with Levenberg-Marquardt, stopping after 100 iterations or when relative error decrease falls below 10^-4.The initial damping parameter is 0.01; iGPMP2 uses iSAM2 with default settings.
  • 8.2 GPMP2: GPMP2 uses a constant-velocity prior over position and velocity, centered on a zero-acceleration trajectory.Acceleration is omitted from the Markovian state because it is not needed for the gradients considered.
  • 8.2.1 Obstacle cost: The safety distance ϵ creates nonzero obstacle cost near surfaces, encouraging a minimum clearance beyond collision avoidance.The obstacle weight σ_obs controls the associated cost scale.
  • 8.2.3 Motion constraints: Equality and inequality motion constraints are incorporated as soft factors rather than directly enforced during unconstrained least-squares inference.Joint- and velocity-limit factors use hinge losses with safety margins, and final violations are detected and clamped.

9. Evaluation

The evaluation compares GPMP variants with trajectory-optimization and sampling-based planners across WAM and PR2 datasets, including incremental replanning. GPMP2 performs consistently well, while iGPMP2 trades success rate for substantially faster replanning.

  • Setup: The benchmarks use 24 WAM problems and 198 PR2 problems across four environments, with successful trajectories validated on a real 7-DOF WAM arm.The compared methods include GPMP, GPMP2 variants, TrajOpt, CHOMP, RRT-Connect, and LBKPIECE.
  • Batch benchmark: GPMP2-intp achieves a 30–50% speedup in average and maximum runtime over GPMP2-no-intp.Interpolation reduces runtime while enabling dense collision checking and smooth execution trajectories.
  • Batch benchmark: On WAM, TrajOpt-11 solves 20% of problems, whereas GPMP2-intp has the second-lowest runtime with a much higher success rate.GPMP2-no-intp achieves the highest WAM success rate.
  • Batch benchmark: On PR2, GPMP2-intp has the lowest runtime, is twice as fast as TrajOpt-11, and is 30 times faster than RRT-Connect with a slightly higher success rate than TrajOpt-11.RRT-Connect has a slightly higher success rate than GPMP2-intp, but its timing would increase with post-processing or smoothing.
  • Analysis: GPMP2 failures occur at infeasible local minima, although random restarts or GPMP-GRAPH can help address this issue.GPMP2 converges well before the ten-second maximum runtime.
  • Incremental replanning: iGPMP2 provides an order-of-magnitude replanning speedup but has a lower success rate than GPMP2.The replanning benchmarks contain 72 WAM and 54 PR2 problems.
  • Incremental replanning: For PR2 replanning, iGPMP2 succeeds on 81.5% of problems with goal distance below 2.0, versus 51.9% when the distance is at least 2.0.The L2 distance measures the difference between original and new goal configurations.

10. Discussion

The discussion positions GPMP2 as a sparse, continuous-time probabilistic-inference planner that is faster than related trajectory optimizers and supports rapid incremental replanning. Its scope is limited by local optimization, incomplete probabilistic guarantees, and difficulty handling nonlinear inequality constraints.

  • Comparisons with related work: GPMP2 represents trajectories with GPs and solves probabilistic inference as nonlinear least squares, exploiting sparse structure with quadratic-convergence solvers.Its updates require linearization and Cholesky decomposition of the sparse linear system.
  • Comparisons with related work: GPMP2 is faster than TrajOpt even when TrajOpt uses a small number of trajectory states.TrajOpt's hard obstacle constraints make its constrained optimization problem more difficult and slower to solve.
  • Comparisons with related work: Unlike sampling-based methods, GPMP2 does not guarantee probabilistic completeness and may struggle on harder problems such as mazes.Random initializations, GPMP-GRAPH, or refinement of sampling-based paths are described as possible remedies.
  • Incremental planning: Incremental factor-graph inference enables very fast replanning, which the discussion identifies as useful for real-time, real-world applications.This avoids the large initial exploration required by multi-query planners for simpler changing-goal problems.
  • Limitations and future work: Iterative nonlinear least-squares methods provide no global optimality guarantees, although global optimality is not strictly necessary for smooth, collision-free objectives.The paper notes that prior motion-planning approaches face similar local-minimum issues.
  • Limitations and future work: The proposed approach is limited in handling nonlinear inequality motion constraints; integrating sequential quadratic programming remains future work.SQP has previously been used for motion planning with such constraints.

11. Conclusion

GPMP2 combines continuous-time GP trajectory representations with structure-exploiting inference, while iGPMP2 extends this framework for efficient incremental replanning. In benchmarks, GPMP2 is consistently faster than competing planners, and iGPMP2 replans substantially faster than solving from scratch.

  • GPMP2 uses GP interpolation to represent continuous-time trajectories with few support states while propagating optimization costs back to those states.The trajectory is queried at arbitrary times, but only support states are updated during optimization.
  • GPMP2 is consistently faster, often several times faster, than state-of-the-art trajectory-optimization and sampling-based algorithms on 7-DOF arm-planning benchmarks.The evaluation covers two datasets in multiple environments.
  • iGPMP2 solves replanning problems incrementally in an order of magnitude less time than resolving them from scratch with GPMP2.The authors identify this incremental property as useful for real-time, real-world planning applications.

Appendix A: The trajectory prior

The trajectory prior is conditioned on observations, including a goal state, to obtain a Gaussian distribution over the state trajectory. The resulting mean and covariance are expressed through GP covariance blocks and the goal-state uncertainty.

  • The appendix begins by conditioning a state distribution on observations through a linear observation equation.
  • Conditioning produces a Gaussian state distribution with mean µ and covariance K.
  • Goal-state conditioning selects the final state through C = [0 . . . 0 I] and uses the goal covariance KN.
  • The conditioned mean adjusts the prior mean using cross-covariance with the final state and the inverse sum of final-state and goal covariances.
  • The conditioned covariance subtracts a cross-covariance correction formed with the inverse sum of final-state and goal covariances.
  • The covariance representation is rewritten using the Woodbury matrix identity, while the trajectory covariance is assembled from blocks linking the final state to all states.

Appendix B: Sparsity of the likelihood in GPMP2

The GPMP2 likelihood Jacobian is sparse because obstacle and interpolated obstacle factors depend only on one support state or two adjacent support states. This locality makes the resulting information matrix block-tridiagonal.

  • The isotropic observation-noise definition is used in establishing the sparse likelihood structure.
  • The Jacobian H has dimensions M × (N + 1 + N × nip) by (N + 1) × D, so H⊤H has dimensions (N + 1) × D by (N + 1) × D.
  • H and H⊤H are partitioned into blocks indexed by system degrees of freedom, observation dimensionality, and support-state positions.
  • Regular obstacle factors depend on their connected support state, whereas interpolated obstacle factors depend on the preceding and following support states.
  • Each Jacobian block H(i, j) is zero unless state index j equals the factor's support index si or its successor si + 1.
  • Nonzero products H(k, i)⊤H(k, j) require both indices to belong to the same adjacent support-state pair, namely sk or sk + 1.
  • Because distant state pairs never satisfy this condition, H⊤H and H⊤Σ−1obsH are block-tridiagonal.
Loading 1707.07383v3…