PiP-X: Online feedback motion planning/replanning in dynamic environments using invariant funnels

Computing kinodynamically feasible motion plans and repairing them on-the-fly as the environment changes is a challenging, yet relevant problem in robot navigation. We propose an online single-query sampling-based motion re-planning algorithm using finite-time invariant sets, commonly referred to as "funnels". We combine concepts from nonlinear systems analysis, sampling-based techniques, and graph-search methods to create a single framework that enables feedback motion re-planning for any general nonlinear dynamical system in dynamic workspaces. A volumetric network of funnels is constructed in the configuration space using sampling-based methods and invariant set theory; and an optimal sequencing of funnels from robot configuration to a desired goal region is then determined by computing the shortest-path subgraph (tree) in the network. Analyzing and formally quantifying the stability of trajectories using Lyapunov level-sets ensures kinodynamic feasibility and guaranteed set-invariance of the solution paths. Though not required, our method is capable of using a pre-computed library of motion primitives to speedup online computation of controllable motion plans that are volumetric in nature. We introduce a novel directed-graph data structure to represent the funnel-network and its inter-sequencibility; helping us leverage discrete graph-based incremental search to quickly rewire feasible and controllable motion plans on-the-fly in response to changes in the environment. We validate our approach on a simulated cart-pole, car-like robot, and 6DOF quadrotor platform in a variety of scenarios within a maze and a random forest environment. Using Monte Carlo methods, we evaluate the performance in terms of algorithm success, length of traversed trajectory, and runtime.