Kino-PAX$^+$: Near-Optimal Massively Parallel Kinodynamic Sampling-based Motion Planner

TL;DR

Kino-PAX$^{+}$ tackles real-time near-optimal kinodynamic motion planning by exploiting massively parallel GPU-like architectures. It constructs a sparse trajectory tree through three parallel subroutines (Propagate, PruneNodes, UpdateTree) within a region-decomposed search, focusing on promising low-cost nodes to iteratively refine solutions. The authors prove probabilistic $\delta$-robust completeness and asymptotic $\delta$-robust near-optimality, showing that the final solution cost converges to $c^*\left(1 + \dfrac{K_c\,\delta}{C_{\Delta}}\right)$ as planning time grows. Empirical results across 6D and 12D systems demonstrate up to three orders of magnitude faster first solutions and lower costs than state-of-the-art serial planners, with robust performance across challenging environments. The work offers a practical, scalable path to real-time, provably near-optimal kinodynamic planning on many-core hardware.

Abstract

Sampling-based motion planners (SBMPs) are widely used for robot motion planning with complex kinodynamic constraints in high-dimensional spaces, yet they struggle to achieve \emph{real-time} performance due to their serial computation design. Recent efforts to parallelize SBMPs have achieved significant speedups in finding feasible solutions; however, they provide no guarantees of optimizing an objective function. We introduce Kino-PAX$^{+}$, a massively parallel kinodynamic SBMP with asymptotic near-optimal guarantees. Kino-PAX$^{+}$ builds a sparse tree of dynamically feasible trajectories by decomposing traditionally serial operations into three massively parallel subroutines. The algorithm focuses computation on the most promising nodes within local neighborhoods for propagation and refinement, enabling rapid improvement of solution cost. We prove that, while maintaining probabilistic $δ$-robust completeness, this focus on promising nodes ensures asymptotic $δ$-robust near-optimality. Our results show that Kino-PAX$^{+}$ finds solutions up to three orders of magnitude faster than existing serial methods and achieves lower solution costs than a state-of-the-art GPU-based planner.

Figures (8)

• Figure 1: Three solutions found by Kino-PAX$^{+}$ using 6D Double Integrator dynamics and minimizing path length cost.
• Figure 2: Illustration of the Kino-PAX$^{+}$ expansion process: (a) Current node sets $V_A$ and $V_{T}$. Numbers within grid cells indicate the lowest-cost trajectories reaching each region $\mathcal{R}_i$. (b) Parallel expansion of nodes in $V_A$ with branching factor $\lambda = 2$. (c) Addition of selected nodes to the unexplored set $V_U$ and corresponding updates to region costs. (d) Pruning of nodes from $V_A$ based on newly acquired trajectory cost information. (e) Updated active set $V_A$, ready for the next iteration of node expansion.
• Figure 3: Three solutions found by Kino-PAX$^{+}$ using 6D Double Integrator dynamics. Subfigure (a) is the first solution found, (b) is an intermediate solution, and (c) is the final solution found.
• Figure 4: An illustration of a $\delta$-robust optimal trajectory $\mathbf{x}^*$ (yellow nodes), with nodes equally spaced in terms of cost increments of $C_\Delta$. Each node lies within a region $\mathcal{R}_i$, and the purple regions collectively form the Optimal Trajectory Regions set $\mathcal{R}^*$.
• Figure 5: Considered Environments.

Theorems & Definitions (15)

• Definition 1: Near-optimal trajectory
• Remark 1
• Remark 2
• Definition 2: Probabilistic $\delta$-Robust Completeness
• Definition 3: Asymptotic $\delta$-robust Near-Optimality
• Definition 4: Covering Balls
• Definition 5: Optimal Trajectory Regions
• Lemma 1
• proof
• Theorem 1