Topology-Guided Path Integral Approach for Stochastic Optimal Control in Cluttered Environment

TL;DR

This paper introduces a topology-guided path integral control method for stochastic robot motion planning in cluttered environments, effectively avoiding local minima and ensuring collision-free, feasible trajectories.

Contribution

It develops a homology-embedded sampling planner integrated with path integral control to improve global optimality in complex environments.

Findings

Successfully avoids local minima in cluttered environments

Produces collision-free, dynamically feasible trajectories

Validated on synthetic and quadrotor control scenarios

Abstract

This paper addresses planning and control of robot motion under uncertainty that is formulated as a continuous-time, continuous-space stochastic optimal control problem, by developing a topology-guided path integral control method. The path integral control framework, which forms the backbone of the proposed method, re-writes the Hamilton-Jacobi-Bellman equation as a statistical inference problem; the resulting inference problem is solved by a sampling procedure that computes the distribution of controlled trajectories around the trajectory by the passive dynamics. For motion control of robots in a highly cluttered environment, however, this sampling can easily be trapped in a local minimum unless the sample size is very large, since the global optimality of local minima depends on the degree of uncertainty. Thus, a homology-embedded sampling-based planner that identifies many (potentially) local-minimum trajectories in different homology classes is developed to aid the sampling process. In combination with a receding-horizon fashion of the optimal control the proposed method produces a dynamically feasible and collision-free motion plans without being trapped in a local minimum. Numerical examples on a synthetic toy problem and on quadrotor control in a complex obstacle field demonstrate the validity of the proposed method.