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

TL;DR

PiP-X presents an online funnel-based replanning framework that blends sampling-based motion planning with invariant-set guarantees to achieve kinodynamically feasible trajectories in dynamic environments. It introduces an augmented directed graph that encodes traversability and compossibility of volumetric funnels, enabling rapid incremental replanning via a shortest-path subtree rooted at the goal. Offline, a funnel library and backward-reachability analysis establish verified invariant regions around nominal trajectories; online, configurations are sampled, funnels are connected, and the graph is rewired as obstacles change. Validation on a simulated 6DOF quadrotor in maze and forest-like environments demonstrates robust set-invariance, fast online replanning, and favorable trajectory-length metrics. This work bridges discrete graph-search methods with invariant-set theory to address the two-point boundary value problem implicitly in dynamic, nonlinear systems.

Abstract

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 a novel online single-query sampling-based motion re-planning algorithm - PiP-X, using finite-time invariant sets - funnels. We combine concepts from sampling-based methods, nonlinear systems analysis and control theory to create a single framework that enables feedback motion re-planning for any general nonlinear dynamical system in dynamic workspaces. A volumetric funnel-graph is constructed using sampling-based methods, and an optimal funnel-path from robot configuration to a desired goal region is then determined by computing the shortest-path subtree in it. Analysing and formally quantifying the stability of trajectories using Lyapunov level-set theory ensures kinodynamic feasibility and guaranteed set-invariance of the solution-paths. The use of incremental search techniques and a pre-computed library of motion-primitives ensure that our method can be used for quick online rewiring of controllable motion plans in densely cluttered and dynamic environments. We represent traversability and sequencibility of trajectories together in the form of an augmented directed-graph, helping us leverage discrete graph-based replanning algorithms to efficiently recompute feasible and controllable motion plans that are volumetric in nature. We validate our approach on a simulated 6DOF quadrotor platform in a variety of scenarios within a maze and random forest environment. From repeated experiments, we analyse the performance in terms of algorithm-success and length of traversed-trajectory.

Figures (13)

• Figure 1: Online funnel-based re-planning algorithm, PiP-X $-$ (a) Motion plan, a sequence of finite-time invariant sets $-$funnels, each "dropping" into the subsequent funnel, finally into the goal region. (b) Rewiring of the funnel-path when changes in the environment are sensed (c) Underlying search-graph with funnel-edges (d) Trajectory of the robot lies completely inside the traversed funnel-path (e) Analysing sequencibility of funnels (f) Encoding the information of traversability (solid motion-edges) and compossibility (dashed continuity-edges) in an augmented graph to enable quick and efficient graph-based re-planning. The solution path is $\mathcal{B}_2$$-$$\mathcal{A}_2$$-$$\mathcal{A}_1$$-$$\mathcal{G}_1$
• Figure 2: A sample funnel $-$ finite-time backward-reachable invariant set to a compact region of desired final states, $\mathcal{X}_f$
• Figure 3: Compossibility of funnels from the library illustrating shifts, $\Psi_c(.)$ along cyclic coordinates (invariant dynamics). ($\mathcal{F}_1$, $\mathcal{F}_3$) is motion-plan compossible, whereas ($\mathcal{F}_1$, $\mathcal{F}_2$) is not $-$ i.e. outlet of $\mathcal{F}_1$ is completely contained within the inlet of $\mathcal{F}_3$ after an appropriate shift operation, $\Psi_{c_3}$
• Figure 4: Overview of PiP-X algorithm: consists of an offline stage of dynamical system analysis, and an online phase of sampling-based graph construction and incremental re-planning in dynamic environments
• Figure 5: (a) Search funnels existing in the $\mathbb{R}^+ \times \mathbb{R}^n$ (b) Augmented search graph representing planning in $\mathcal{C}$-space with compossibility information. Solid edges are finite-cost motion-edges representing traversability, dashed-edges are zero-cost continuity-edges encoding compossibility information $-$ whether trajectories "flow" into the subsequent funnel (c) Conventional search graph used by typical tree/graph-based motion-planners

Theorems & Definitions (6)

• Definition 1
• Definition 2
• Definition 3
• Definition 4
• Definition 5
• Definition 6