[Paper Review] Approximately Optimal Continuous-Time Motion Planning and Control via Probabilistic Inference

Mustafa Mukadam, Ching-An Cheng+2 more|arXiv (Cornell University)|Feb 23, 2017

Robotic Path Planning Algorithms25 references4 citations

TL;DR

This paper proposes PIPC, a probabilistic inference-based algorithm for approximately optimal continuous-time motion planning and control in stochastic systems with arbitrary higher-order nonlinear performance indices. By formulating the problem as posterior inference on a Gaussian process-based probabilistic graphical model, PIPC achieves linear complexity in the number of nonlinear factors, enabling efficient online receding-horizon planning with closed-loop performance.

ABSTRACT

The problem of optimal motion planing and control is fundamental in robotics. However, this problem is intractable for continuous-time stochastic systems in general and the solution is difficult to approximate if non-instantaneous nonlinear performance indices are present. In this work, we provide an efficient algorithm, PIPC (Probabilistic Inference for Planning and Control), that yields approximately optimal policies with arbitrary higher-order nonlinear performance indices. Using probabilistic inference and a Gaussian process representation of trajectories, PIPC exploits the underlying sparsity of the problem such that its complexity scales linearly in the number of nonlinear factors. We demonstrate the capabilities of our algorithm in a receding horizon setting with multiple systems in simulation.

Motivation & Objective

• Address the challenge of optimal motion planning and control in continuous-time stochastic systems with complex, higher-order nonlinear performance indices.
• Overcome the intractability of exact solutions in general continuous-time optimal control problems, especially when nonlinear factors couple states and actions over time.
• Develop an algorithm that scales efficiently with problem complexity by exploiting sparsity in the underlying structure of the performance index.
• Enable real-time, closed-loop planning and control by transforming the optimal control problem into a dual inference problem on a probabilistic graphical model.
• Extend existing inference-based control frameworks to handle arbitrary nonlinear factors beyond instantaneous costs, enabling richer performance specifications.

Proposed method

• Formulate the optimal control problem as a dual probabilistic inference problem using a generalized Linear-Exponential-Quadratic-Gaussian (gLEQG) framework.
• Represent trajectories as Gaussian processes to enable nonparametric, smooth policy representations with uncertainty quantification.
• Use Laplace approximation to derive a Gaussian posterior approximation over trajectories, enabling efficient inference.
• Exploit sparsity in the factor graph representation of the performance index to ensure computational complexity scales linearly with the number of nonlinear factors.
• Implement a receding horizon control strategy where the policy is recursively updated using Bayesian inference as new observations are acquired.
• Integrate the algorithm into both Markov Decision Processes (MDPs) and Partially-Observable MDPs (POMDPs) for robustness in uncertain environments.

Experimental results

Research questions

• RQ1 Can probabilistic inference be used to solve continuous-time optimal control problems with arbitrary higher-order nonlinear performance indices?
• RQ2 Does the duality between optimal control and inference extend to problems with non-instantaneous, nonlinear cost factors?
• RQ3 Can the computational complexity of solving such problems be made linear in the number of nonlinear factors through sparsity exploitation?
• RQ4 How does the performance of the proposed inference-based method compare to traditional open-loop and closed-loop planning algorithms in dynamic, uncertain environments?
• RQ5 Can the algorithm be efficiently applied in real-time, receding-horizon settings with partial observability and non-Gaussian noise?

Key findings

• PIPC achieves significantly higher success rates than open-loop methods in both MDP and POMDP settings, especially under high system noise ($Q_x$) or complex obstacle environments.
• In the 2D holonomic robot benchmark, closed-loop PIPC achieved 100% success rate in MDP cases with $Q_x = 0.01$, while open-loop methods dropped to 85% under increased noise ($Q_x = 0.03$).
• For the WAM and PR2 robot arms, PIPC maintained 100% success rate in MDP cases with $Q_x = 0.01$, and even outperformed open-loop POMDP methods under high noise ($Q_x = 0.03$, success rate 90% vs. 75%).
• Aggregate metrics (time, path length, path cost) increased with noise and obstacle count, but successful runs under PIPC showed consistent improvements in trajectory quality and robustness.
• The algorithm demonstrated scalability to high-dimensional systems (7-DOF arms), confirming its applicability beyond low-dimensional problems.
• The closed-loop PIPC consistently outperformed open-loop baselines, with success rate differences increasing under higher stochasticity, indicating strong robustness to uncertainty.