Kinodynamic Motion Planning

Follow this topic Ask about this topic

Kinodynamic motion planning searches for motions that are not only collision-free but also physically executable under a robot’s dynamics and control limits. Common approaches include sampling-based tree planners such as RRT variants, heuristic search and motion lattices, trajectory optimization and model predictive control, and learned models or policies that guide planning. Methods are tested on systems ranging from ground robots and manipulators to drones and multi-robot teams, including cluttered scenes, moving obstacles, and uncertain dynamics. A central challenge is balancing fast, robust planning in high-dimensional settings with guarantees such as safety, completeness, and near-optimality.

Latest papers

46 papers5 papers in the last four weeks, down 37% on the four weeks before. 0.0% of all new papers.

May 20, 2026

CMC-Opt: Constraint Manifold with Corners for Inequality-Constrained Optimization

We introduce a manifold-based framework for addressing optimization problems with equality and inequality constraints found in robotics. Our approach transforms the original problem into an unconstrained optimization problem directly on the constrained state space. To achieve this, we introduce ``constraint manifolds with corners" to represent the state space satisfying mixed nonlinear equality and inequality constraints. We further extend manifold optimization algorithms to operate on this new topological structure. We demonstrate the power and robustness of our framework in the context of a large-scale kinodynamic planning problem, successfully generating dynamically feasible trajectories where standard methods fail.

Yetong Zhang, Frank Dellaert

College of Computing Georgia Institute of Technology Atlanta, USA

Kinodynamic Motion Planning Riemannian Optimization Constrained Optimization Motion Planning Robot Motion Planning Robotic Optimization

May 19, 2026

KIO-planner: Attention-Guided Single-Stage Motion Planning with Dual Mapping for UAV Navigation

Autonomous UAV flight in confined, wall-dense environments requires low-latency and reliable motion planning under strict safety constraints. Traditional optimization-based planners suffer from mapping latency and easily fall into local minima when navigating through dense structural obstacles. Meanwhile, existing end-to-end learning methods struggle to extract fine-grained geometric features from raw depth images and lack hard kinodynamic constraints, leading to unpredictable collisions near walls. To address these issues, we propose KIO-planner, an attention-guided single-stage trajectory planning framework. First, we integrate a Convolutional Block Attention Module (CBAM) into the perception backbone to adaptively focus on critical structural edges and traversable space. Second, we introduce a novel Dual Mapping mechanism--comprising physical bounds activation and a deterministic Geometric Safety Shield in the depth-pixel space--to enforce kinodynamic feasibility and collision-free flight without global map fusion. Extensive high-fidelity simulated experiments demonstrate that KIO-planner enables highly agile navigation at speeds up to 3.0 m/s. Compared to the state-of-the-art baseline, KIO-planner achieves lower inference latency (approximately 24 ms) and generates significantly smoother trajectories, reducing control cost by 28.4%. Most notably, our Dual Mapping substantially increases the worst-case safety margin, measured by minimum distance to obstacles, from 0.48 m to 0.76 m, ensuring fast, smooth, and safer navigation in highly constrained environments.

Dexing Yao, Haochen Li, Junhao Wei +11

Faculty of Applied Sciences Macao Polytechnic University Macao, China · College of Animal Science and Technology Zhongkai University of Agriculture and Engineering Guangzhou, China · School of Economics and Management South China Normal University Macao, China +1

Constrained Motion Planning Kinodynamic Motion Planning UAV Navigation Safe Motion Planning Motion Planning Robot Collision Avoidance

May 10, 2026

Efficient Multi-Robot Motion Planning with Precomputed Translation-Invariant Edge Bundles

Solving multi-robot motion planning (MRMP) requires generating collision-free kinodynamically feasible trajectories for multiple interacting robots. We introduce Kinodynamic Translation-Invariant Edge Bundles or KiTE-Extend, a planner-agnostic action selection mechanism for sampling-based kinodynamic motion planning. KiTE-Extend uses a library of trajectory segments computed offline to guide action selection during online planning, improving the ability of existing planners to identify feasible motion segments without altering state propagation, collision checking, or cost evaluation, and without changing their theoretical guarantees. While KiTE-Extend can modestly improve single-agent planners, its benefits are most clear in the multi-agent setting, where it is able to explore more effectively and significantly improve planning through the dense spatiotemporal constraints introduced by robot-robot interaction. Through experiments on multiple kinodynamic systems and environments, we show that KiTE-Extend reduces planning time and improves scalability across the three most common MRMP paradigms: centralized, prioritized, and conflict-based.

Himanshu Gupta, Paul Motter, Aritra Chakrabarty +5

Smead Aerospace Engineering Sciences, University of Colorado Boulder · Department of Computer Science, University of Colorado Boulder · Cherry Creek High School, Greenwood Village, CO, USA

Multi-Robot Motion Planning Kinodynamic Motion Planning Motion Planning Sampling-Based Motion Planning

May 9, 2026

Terminal Matters: Kinodynamic Planning with a Terminal Cost and Learned Uncertainty in Belief State-Cost Space

In many real-world robotic tasks, robots must generate dynamically feasible motions that reliably reach desired goals even under uncertainty. Yet existing sampling-based kinodynamic planners typically optimize accumulated trajectory costs and treat goal reaching as a feasibility check, rather than explicitly optimizing terminal-state quality, such as goal preference or goal-reaching reliability. In this work, we introduce a terminal-cost formulation for kinodynamic planning that allows terminal-state quality to be optimized alongside accumulated trajectory cost. We prove that AO-RRT, an asymptotically optimal kinodynamic planner, preserves its asymptotic optimality under this augmented objective. We further extend the formulation to belief space and prove that minimizing the Wasserstein distance between the terminal belief and the goal improves a lower bound on the probability of reaching the goal region. The resulting planner, KiTe, uses this terminal-cost objective to encode goal preferences and improve reliability under uncertainty. To support systems without analytical uncertainty models, we learn dynamics and process uncertainty directly from data and integrate the learned belief dynamics into planning. Experiments on Flappy Bird, Car Parking, and Planar Pushing show that KiTe consistently improves goal-reaching success under uncertainty. Real-world Planar Pushing experiments further demonstrate that KiTe can plan effectively with learned dynamics and uncertainty. Source code is available at https://github.com/elpis-lab/KiTe.

Zhuoyun Zhong, Seyedali Golestaneh, Constantinos Chamzas

All authors are affiliated with the Department of Robotics Engineering, Worcester Polytechnic Institute (WPI), Worcester, MA 01609, USA

Belief-Space Planning Trajectory Optimization Kinodynamic Motion Planning Trajectory Generation Motion Planning Sampling-Based Motion Planning

May 3, 2026

Optimized and kinematically feasible multi-agent motion planning

Multi-agent motion planning (MAMP) is an important problem for autonomous systems with multiple agents. In this work we propose a two-step method for finding optimized and kinematically feasible solutions to MAMP problems. The first step finds an initial feasible solution using state-of-the-art methods such as conflict-based search (CBS) or priority-based search (PBS), and the second step is an improvement step which improves the solution by solving a multi-phase optimal control problem (OCP) where the initial solution is used to warm-start the solver. We also propose a method for generating motion primitives in an optimized way under the constraint that the primitive durations are all multiples of the same sample time. We evaluate our proposed framework on a MAMP problem for tractor-trailer systems. We extend the safe interval path planning with interval projections (SIPP-IP) algorithm so it can handle more general cost functions and larger agents, but our results show that for the tractor-trailer system a simple lattice-based planner performs better due to less conservative collision checks. Our experiments also indicate that CBS performs better than PBS for this system as it achieves a higher success rate in environments with obstacles and had a lower average runtime, although both planners achieve solutions of similar quality after the improvement step.

Anja Hellander, Kristoffer Bergman, Daniel Axehill

Division of Automatic Control, Linköping University, Sweden · Saab AB, Linköping, Sweden

Constrained Motion Planning Trajectory Optimization Multi-Agent System Optimization Kinodynamic Motion Planning Decentralized Multi-Agent Motion Planning Mobile Robotics+2

Apr 14, 2026

From Kinematics to Dynamics: Learning to Refine Hybrid Plans for Physically Feasible Execution

In many robotic tasks, agents must traverse a sequence of spatial regions to complete a mission. Such problems are inherently mixed discrete-continuous: a high-level action sequence and a physically feasible continuous trajectory. The resulting trajectory and action sequence must also satisfy problem constraints such as deadlines, time windows, and velocity or acceleration limits. While hybrid temporal planners attempt to address this challenge, they typically model motion using linear (first-order) dynamics, which cannot guarantee that the resulting plan respects the robot's true physical constraints. Consequently, even when the high-level action sequence is fixed, producing a dynamically feasible trajectory becomes a bi-level optimization problem. We address this problem via reinforcement learning in continuous space. We define a Markov Decision Process that explicitly incorporates analytical second-order constraints and use it to refine first-order plans generated by a hybrid planner. Our results show that this approach can reliably recover physical feasibility and effectively bridge the gap between a planner's initial first-order trajectory and the dynamics required for real execution.

Lidor Erez, Shahaf S. Shperberg, Ayal Taitler

Department of Industrial Engineering and Management, Ben-Gurion University of the Negev, Be’er Sheva, Israel · Stein Faculty of Computer and Information Science, Ben-Gurion University of the Negev, Be’er Sheva, Israel

Robot Task Planning Constrained Motion Planning Kinodynamic Motion Planning Robotic RL

Apr 13, 2026

Optimal Kinodynamic Motion Planning Through Anytime Bidirectional Heuristic Search with Tight Termination Condition

This paper introduces Bidirectional Tight Informed Trees (BTIT*), an asymptotically optimal kinodynamic sampling-based motion planning algorithm that integrates an anytime bidirectional heuristic search (Bi-HS) and ensures the meet-in-the-middle property (MMP) and optimality (MM-optimality). BTIT* is the first anytime MEET-style algorithm to utilize termination conditions that are efficient to evaluate and enable early termination on-the-fly in batch-wise sampling-based motion planning. Experiments show that BTIT* achieves strongly faster time-to-first-solution and improved convergence than representative non-lazy informed batch planners on two kinodynamic benchmarks: a 4D double-integrator model and a 10D linearized Quadrotor. The source code is available here.

Yi Wang, Bingxian Mu, Shahab Shokouhi +1

Department of Mechanical Engineering, University of New Hampshire, Durham, NH, 03824, USA. · Faculty of Sustainable Design Engineering, University of Prince Edward Island, Charlottetown, PE C1A 4P3, Canada

Motion Planning Kinodynamic Motion Planning Sampling-Based Motion Planning Tree Search

Apr 5, 2026

Primitive-based Truncated Diffusion for Efficient Trajectory Generation of Differential Drive Mobile Manipulators

We present a learning-enhanced motion planner for differential drive mobile manipulators to improve efficiency, success rate, and optimality. For task representation encoder, we propose a keypoint sequence extraction module that maps boundary states to 3D space via differentiable forward kinematics. Point clouds and keypoints are encoded separately and fused with attention, enabling effective integration of environment and boundary states information. We also propose a primitive-based truncated diffusion model that samples from a biased distribution. Compared with vanilla diffusion model, this framework improves the efficiency and diversity of the solution. Denoised paths are refined by trajectory optimization to ensure dynamic feasibility and task-specific optimality. In cluttered 3D simulations, our method achieves higher success rate, improved trajectory diversity, and competitive runtime compared to vanilla diffusion and classical baselines. The source code is released at https://github.com/nmoma/nmoma .

Long Xu, Choilam Wong, Yuhang Zhong +3

Diffusion-Based Planning Diffusion Models for Motion Generation Mobile Manipulation Kinodynamic Motion Planning

Apr 1, 2026

Sampling-based Task and Kinodynamic Motion Planning under Semantic Uncertainty

This paper tackles the problem of integrated task and kinodynamic motion planning in uncertain environments. We consider a robot with nonlinear dynamics tasked with a Linear Temporal Logic over finite traces (

\ltlf

) specification operating in a partially observable environment. Specifically, the uncertainty is in the semantic labels of the environment. We show how the problem can be modeled as a Partially Observable Stochastic Hybrid System that captures the robot dynamics,

\ltlf

task, and uncertainty in the environment state variables. We propose an anytime algorithm that takes advantage of the structure of the hybrid system, and combines the effectiveness of decision-making techniques and sampling-based motion planning. We prove the soundness and asymptotic optimality of the algorithm. Results show the efficacy of our algorithm in uncertain environments, and that it consistently outperforms baseline methods.

Qi Heng Ho, Zachary N. Sunberg, Morteza Lahijanian

Q. H. Ho is with the Kevin T. Crofton Department of Aerospace and Ocean Engineering, Virginia Tech, Blacksburg, VA, USA · Z. N. Sunberg and M. Lahijanian are with the Smead Department of Aerospace Engineering Sciences, University of Colorado Boulder, CO, USA

Semantic Uncertainty Estimation Robot Task Planning Kinodynamic Motion Planning Sampling-Based Motion Planning

Mar 24, 2026

Planning over MAPF Agent Dependencies via Multi-Dependency PIBT

Modern Multi-Agent Path Finding (MAPF) algorithms must plan for hundreds to thousands of agents in congested environments within a second, requiring highly efficient algorithms. Priority Inheritance with Backtracking (PIBT) is a popular algorithm capable of effectively planning in such situations. However, PIBT, and its variants like Enhanced PIBT (EPIBT), is constrained by its rule-based planning procedure and lacks generality because it restricts its search to paths that collide with at most one other agent. In this paper, we describe a new perspective on solving MAPF by planning over agent dependencies. Taking inspiration from PIBT's priority inheritance logic, we define the concept of agent dependencies and propose Multi-Dependency PIBT (MD-PIBT) that searches over agent dependencies. MD-PIBT is a general framework where specific parameterizations can reproduce PIBT and EPIBT. At the same time, alternative configurations generalize PIBT and EPIBT to multi-step planning capable of reasoning paths that collide with more than one other agent. Our experiments demonstrate that MD-PIBT effectively plans for as many as 10,000 homogeneous agents under various kinodynamic constraints, including pebble motion, rotation motion, and differential drive robots with speed and acceleration limits. We perform thorough evaluations on different variants of MAPF and find that MD-PIBT is particularly effective in MAPF with large agents. Our code is available at https://github.com/lunjohnzhang/MD-PIBT.

Zixiang Jiang, Yulun Zhang, Rishi Veerapaneni +1

University Of Melbourne · Robotics Institute, Carnegie Mellon University

Kinodynamic Motion Planning Motion Planning Multi-Agent Planning Multi-Agent Path Finding

Mar 17, 2026

Ultrafast Sampling-based Kinodynamic Planning via Differential Flatness

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, both of which cause 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 FLASK, a fast parallelized sampling-based kinodynamic motion planning framework 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 problem 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 framework is fast, exact, and compatible with any sampling-based motion planner, while offering theoretical guarantees on probabilistic exhaustivity and asymptotic optimality based on the closed-form BVP solutions. 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.

Thai Duong, Clayton W. Ramsey, Zachary Kingston +2

Department of Computer Science, Rice University, Houston, TX, 77005, USA

Constrained Motion Planning Kinodynamic Motion Planning Motion Planning Differential Flatness Sampling-Based Motion Planning

Mar 6, 2026

Multi-Robot Trajectory Planning via Constrained Bayesian Optimization and Local Cost Map Learning with STL-Based Conflict Resolution

We address multi-robot motion planning under Signal Temporal Logic (STL) specifications with kinodynamic constraints. Exact approaches face scalability bottlenecks and limited adaptability, while conventional sampling-based methods require excessive samples to construct optimal trajectories. We propose a two-stage framework integrating sampling-based online learning with formal STL reasoning. At the single-robot level, our constrained Bayesian Optimization-based Tree search (cBOT) planner uses a Gaussian process as a surrogate model to learn local cost maps and feasibility constraints, generating shorter collision-free trajectories with fewer samples. At the multi-robot level, our STL-enhanced Kinodynamic Conflict-Based Search (STL-KCBS) algorithm incorporates STL monitoring into conflict detection and resolution, ensuring specification satisfaction while maintaining scalability and probabilistic completeness. Benchmarking demonstrates improved trajectory efficiency and safety over existing methods. Real-world experiments with autonomous surface vehicles validate robustness and practical applicability in uncertain environments. The STLcBOT Planner will be released as an open-source package, and videos of real-world and simulated experiments are available at https://stlbot.github.io/.

Sourav Raxit, Abdullah Al Redwan Newaz, Jose Fuentes +3

Department of Computer Science, University of New Orleans, New Orleans, LA 70148, USA · Knight Foundation School of Computing and Information Sciences, Florida International University, Miami, FL 33199, USA · Providence College, Department of Mathematics & Computer Science, Providence, RI 02918, USA

STL Motion Planning Multi-Robot Motion Planning Constrained Motion Planning Kinodynamic Motion Planning Conflict-Based Search Bayesian Optimization

Mar 5, 2026

cuRoboV2: Dynamics-Aware Motion Generation with Depth-Fused Distance Fields for High-DoF Robots

Effective robot autonomy requires motion generation that is safe, feasible, and reactive. Current methods are fragmented: fast planners output physically unexecutable trajectories, reactive controllers struggle with high-fidelity perception, and existing solvers fail on high-DoF systems. We present cuRoboV2, a unified framework with three key innovations: (1) B-spline trajectory optimization that enforces smoothness and torque limits; (2) a GPU-native TSDF/ESDF perception pipeline that generates dense signed distance fields covering the full workspace, unlike existing methods that only provide distances within sparsely allocated blocks, up to 10x faster and in 8x less memory than the state-of-the-art at manipulation scale, with up to 99% collision recall; and (3) scalable GPU-native whole-body computation, namely topology-aware kinematics, differentiable inverse dynamics, and map-reduce self-collision, that achieves up to 61x speedup while also extending to high-DoF humanoids (where previous GPU implementations fail). On benchmarks, cuRoboV2 achieves 99.7% success under 3kg payload (where baselines achieve only 72--77%), 99.6% collision-free IK on a 48-DoF humanoid (where prior methods fail entirely), and 89.5% retargeting constraint satisfaction (vs. 61% for PyRoki); these collision-free motions yield locomotion policies with 21% lower tracking error than PyRoki and 12x lower cross-seed variance than GMR. A ground-up codebase redesign for discoverability enabled LLM coding assistants to author up to 73% of new modules, including hand-optimized CUDA kernels, demonstrating that well-structured robotics code can unlock productive human-LLM collaboration. Together, these advances provide a unified, dynamics-aware motion generation stack that scales from single-arm manipulators to full humanoids. Code is available at https://github.com/NVlabs/curobo.

Balakumar Sundaralingam, Adithyavairavan Murali, Stan Birchfield

Robot Motion Generation Kinodynamic Motion Planning Robot Collision Avoidance Trajectory Optimization Signed Distance Functions

Feb 2, 2026

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 their serial computation design results in planning speeds that scale poorly with problem complexity. 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

Nicolas Perrault, Qi Heng Ho, Morteza Lahijanian

Department of Aerospace Engineering Sciences, University of Colorado Boulder, CO, USA · Department of Aerospace and Ocean Engineering, Virginia Tech, VA, USA

Constrained Motion Planning Kinodynamic Motion Planning Motion Planning Sampling-Based Motion Planning

Mar 22, 2025

From Kinematic Motion Planners to Dynamic Autonomous Navigation with Obstacle Avoidance (Extended version)

Kinematic motion planners are among the most widely used control approaches in robotic applications. By modeling the robot as a first-order system, they generate a feedback-based desired velocity field that guides the robot toward a target while avoiding obstacles. However, extending such feedback planners to systems with higher-order dynamics, while preserving safety and stability properties, is not a straightforward task. In the present work, we propose an approach that adapts existing feedback-based kinematic motion planners to second-order autonomous systems while retaining their safety and almost global asymptotic stability guarantees. We consider two general classes of kinematic motion planners: those derived from navigation functions and those defined directly through desired velocity fields without relying on an underlying navigation function. To validate the proposed methodology, two feedback-based kinematic motion planners are adapted to second-order systems and evaluated both in simulations and experimentally.

Mayur Sawant, Abdelhamid Tayebi

Department of Electrical and Computer Engineering, Lakehead University, Thunder Bay, ON P7B 5E1, Canada

Robotic Control Kinodynamic Motion Planning Safe Motion Planning Motion Planning Robot Motion Planning

Mar 15, 2025

D4orm: Multi-Robot Trajectories with Dynamics-aware Diffusion Denoised Deformations

This work presents an optimization method for generating kinodynamically feasible and collision-free multi-robot trajectories that exploits an incremental denoising scheme in diffusion models. Our key insight is that high-quality trajectories can be discovered merely by denoising noisy trajectories sampled from a distribution. This approach has no learning component, relying instead on only two ingredients: a dynamical model of the robots to obtain feasible trajectories via rollout, and a fitness function to guide denoising with Monte Carlo gradient approximation. The proposed framework iteratively optimizes a deformation for the previous trajectory with the current denoising process, allows anytime refinement as time permits, supports different dynamics, and benefits from GPU acceleration. Our evaluations for differential-drive and holonomic teams with up to 16 robots in 2D and 3D worlds show its ability to discover high-quality solutions faster than other black-box optimization methods such as MPPI. In a 2D holonomic case with 16 robots, it is almost twice as fast. As evidence for feasibility, we demonstrate zero-shot deployment of the planned trajectories on eight multirotors. Code and video: https://github.com/proroklab/d4orm

Yuhao Zhang, Keisuke Okumura, Heedo Woo +2

University of Cambridge · UK · National Institute of Advanced Industrial Science and Technology (AIST) +1

Multi-Robot Motion Planning Constrained Motion Planning Trajectory Optimization Kinodynamic Motion Planning Trajectory Generation Diffusion-Based Planning+3