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

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.

💡 Research Summary

The paper introduces Kino‑PAX+, a massively parallel kinodynamic sampling‑based motion planner that runs on modern many‑core GPUs and provides asymptotic near‑optimality guarantees. Traditional sampling‑based motion planners (SBMPs) such as RRT, EST, and SST are designed for serial execution; while they can handle complex dynamics, they are far too slow for real‑time applications. Recent GPU‑based planners (e.g., Kino‑PAX) achieve millisecond‑scale feasibility but lack any cost‑based refinement, so they cannot guarantee high‑quality trajectories.

Kino‑PAX+ addresses this gap by decomposing the three classic SBMP operations—node selection, node propagation, and node pruning—into three fully parallel sub‑routines. The state space is partitioned into hyper‑cubic regions of diagonal length δ. Each region stores the current lowest‑cost trajectory reaching it (cost(Ri)), updated atomically by parallel threads. During each iteration, all active nodes (VA) are expanded in parallel with a branching factor λ: each thread samples a random control input and integration time, propagates the dynamics, checks collision and state constraints, and, if the resulting trajectory segment is valid, maps the new state to a region Ri. If the cumulative cost from the start to the new state improves the stored cost(Ri), the region’s cost is atomically updated and the new node is placed into the unexplored set (VU). Only nodes that are the cheapest for their region are kept for future expansion; all others are moved to an inactive set (VI) or permanently pruned (VT) after a configurable inactivity threshold.

The authors prove two key theoretical properties. First, under the standard assumptions of Lipschitz‑continuous dynamics, compact state/control spaces, and Chow’s small‑time local accessibility, the algorithm is probabilistically δ‑robustly complete: with probability one it will eventually find a feasible trajectory if one exists. Second, by restricting expansion to the cheapest nodes in each local neighborhood, the algorithm’s cost sequence converges to within a factor (1 + β) of the optimal cost c* as planning time goes to infinity, i.e., it is asymptotically δ‑robustly near‑optimal. The proofs extend the analysis of SST to the parallel setting, showing that the atomic region‑cost updates preserve the necessary monotonicity and that the pruning step does not discard any node that could belong to an optimal solution.

Experimental evaluation covers three benchmark families: (1) a 6‑D double‑integrator (position‑velocity) with path‑length minimization, (2) a 12‑D nonlinear vehicle model with energy‑effort cost, and (3) complex 3‑D environments with many obstacles. Kino‑PAX+ is compared against CPU‑based SST and RRT*, as well as the original GPU feasibility planner Kino‑PAX. Results demonstrate up to three orders of magnitude speed‑up in finding the first feasible solution (≈10 ms for the 6‑D case, ≈120 ms for the 12‑D case) and consistently lower solution costs—30 %–50 % better than SST and 20 %–35 % better than the feasibility‑only GPU planner for the same planning horizon. Over longer planning times (several seconds), the cost converges rapidly toward the optimal value, staying within 5 % of c* in most trials. Memory usage is also reduced because only the cheapest node per region is retained, keeping the total node count well within GPU memory limits.

The paper discusses practical considerations such as the choice of δ and λ, which affect the trade‑off between exploration breadth and exploitation depth, and notes that very complex nonlinear constraints can increase contention on atomic updates. Future work is suggested on adaptive region partitioning (e.g., k‑d tree based) and dynamic adjustment of the branching factor to further improve scalability.

In conclusion, Kino‑PAX+ is the first GPU‑accelerated kinodynamic planner that simultaneously delivers real‑time feasibility, systematic cost refinement, and provable asymptotic near‑optimality. Its ability to produce high‑quality trajectories within milliseconds makes it a strong candidate for latency‑critical robotics applications such as autonomous driving, aerial swarm navigation, and high‑speed manipulation.