Ultrafast Sampling-based Kinodynamic Planning via Differential Flatness

Abstract

Motion planning under dynamics constraints, i.e., kinodynamic planning, enables safe robot operation by generating dynamically feasible trajectories that the robot can accurately track. For high-\dof robots such as manipulators, sampling-based motion planners are commonly used, especially for complex tasks in cluttered environments. However, enforcing constraints on robot dynamics in such planners requires solving either challenging two-point boundary value problems (BVPs) or propagating robot dynamics over time, both of which are computational bottlenecks that drastically increase planning times. Meanwhile, recent efforts have shown that sampling-based motion planners can generate plans in microseconds using parallelization, but are limited to geometric paths. This paper develops AkinoPDF, a fast parallelized sampling-based kinodynamic motion planning technique for a broad class of differentially flat robot systems, including manipulators, ground and aerial vehicles, and more. Differential flatness allows us to transform the motion planning problem from the original state space to a flat output space, where an analytical time-parameterized solution of the BVP and dynamics integration can be obtained. A trajectory in the flat output space is then converted back to a closed-form dynamically feasible trajectory in the original state space, enabling fast validation via ``single instruction, multiple data" parallelism. Our method is fast, exact, and compatible with any sampling-based motion planner. We extensively verify the effectiveness of our approach in both simulated benchmarks and real experiments with cluttered and dynamic environments, requiring mere microseconds to milliseconds of planning time.

Figures (9)

• Figure 1: Motion planning for a "pick and place" task in a cluttered environment with narrow passage: a dynamically feasible trajectory (a) generated from our kinodynamic planner (AkinoPDF) can be accurately tracked by a UR5 robot. Meanwhile, tracking a geometric path (b) leads to collisions (shown in red) that topple the nearby boxes. Multiple intermediate states are overlaid to illustrate the robot's motion. Our planner is real-time and generates trajectories in $\sim 90\mu s$ by leveraging differential flatness and "single instruction, multiple data" (simd) parallelism.
• Figure 2: Configuration samples $\mathbf{a}, \mathbf{b}, \mathbf{c}$ and $\mathbf{d}$, discretized from a linear path (a), as in VAMP thomason2024vamp, and from our closed-form time-parameterized motions (b), can be efficiently checked for collision using simd parallelism.
• Figure 3: Formulating the kinodynamic planning problem in the flat state space with linear dynamics.
• Figure 4: Visualization of our trajectories with DynoBench ortizharo2025iDbAstar benchmarking problems: the trajectories from our kinodynamic RRT* and our version of SST* are plotted as blue and magenta solid curves, respectively, while the iDb-A* and SST* baselines' are shown as orange dashed and green dotted curves, respectively.
• Figure 5: Visualization of trajectories generated by our kinodynamic RRT-Connect (bottom) and by the baseline planner, RRT-Connect + TOPP-RA (top), for a Franka Emika Panda robot in: (a) cage, (b) box, (c) bookshelf thin, and (d) table pick environments. For each environment, the top image shows a trajectory from the baseline with collided configurations in red, while the bottom image demonstrates that our trajectory is collision-free.

Theorems & Definitions (5)

• Definition 1: Differential Flatness
• Example 1
• Example 2
• Example 3
• Example 4: Fixed-time and minimum-time local paths