Kinodynamic Motion Planning

Follow this topic Ask about this topic

Momentum

2 papers in the last four weeks, down 50% on the four weeks before. 0.0% of all new papers.

Jul 13Week of Sep 28

Latest papers 26

Sep 28, 2026cs.RO

Denoising Multi-Robot Trajectories

Multi-robot trajectory planning is a fundamental problem in multi-robot coordination but remains computationally challenging due to its nonconvex, multimodal, and high-dimensional nature. This work builds upon D4orm, a dynamics-aware diffusion-denoising framework, and develops a family of planning architectures for diverse operational requirements. Unlike conventional numerical optimization methods, D4orm employs sampling-based optimization to generate solution trajectories through massively parallel sampling, leveraging modern computing architectures such as GPUs. Its diffusion-denoising structure iteratively optimizes \textit{deformations} to candidate control trajectories, providing an efficient and versatile paradigm for generating kinodynamically feasible and conflict-free trajectories. Using D4orm as the building block for advanced planners, we present a decoupled planner for improved scalability, an online receding-horizon planner with feedback control, and a distributed planner for resource-constrained settings. Evaluations with differential-drive and holonomic robots in 2D and 3D environments demonstrate that D4orm-based approaches find high-quality solutions faster and more reliably than other sampling-based optimization methods, such as MPPI, as well as a learned diffusion-model-based method. We further demonstrate zero-shot deployment on ten real quadrotors with obstacles, large-scale deconfliction with 100 simulated robots, and fully onboard distributed `lifelong' operation with six ground robots. Overall, these results establish diffusion denoising as a scalable and reliable framework for multi-robot coordination. Code and video: https://github.com/proroklab/d4orm

Yuhao Zhang, Keisuke Okumura, Ajay Shankar +1

Department of Computer Science and Technology, University of Cambridge, U.K. · National Institute of Advanced Industrial Science and Technology (AIST), Japan

Multi-Robot Motion Planning Kinodynamic Motion Planning Trajectory Generation Diffusion-Based Planning Multi-Robot Coordination

Sep 16, 2026cs.RO

DynoFluxBench: Benchmarking Kinodynamic Space-Time Planners in Dynamic Environments

Robots that leave structured, static environments must plan motions that are kinodynamically feasible and safe among moving obstacles. However, there are no dedicated benchmark frameworks that combine both aspects. To overcome this, we present DynoFluxBench, a framework to compare kinodynamic planners in known, dynamic environments with unbounded arrival time. To demonstrate its utility and establish strong baselines, we develop three dedicated planners, named ST-Db-RRT, ST-GBRRT, and KIST, that fuse kinodynamic and space-time methods, covering different kinodynamic search paradigms: ST-Db-RRT expands with randomly selected discontinuity-bounded motion primitives using trajectory optimization, whereas KIST and ST-GBRRT maintain a kinodynamically feasible tree with different heuristic guidance. We analyze the probabilistic completeness guarantees of those new planners in dynamic environments. Finally, we evaluate ST-Db-RRT, ST-GBRRT, and KIST using DynoFluxBench, showing that ST-Db-RRT reaches a first solution up to 32 times faster, while KIST and ST-GBRRT remain valuable where trajectory optimization is fragile. Videos and further analysis can be found at https://dynofluxbench.github.io/dynofluxbench/.

Franz Queißner, Andreas Orthey, Wolfgang Hönig

TU Berlin, Germany

Kinodynamic Motion Planning Motion Planning Dynamic Obstacle Avoidance Sampling-Based Motion Planning

Aug 29, 2026cs.RO

Hydra: A Navigation World Action Model with Discrete Latent Planning and Continuous Flow-Matching Execution

World models let robots imagine possible futures, but exploiting this capability for real-time planning is bottlenecked by a representation misalignment: generative models and planners operate on decoupled manifolds, requiring computationally expensive decoding of every candidate back to the high-dimensional observation space for evaluation. In this paper, we present Hydra, a discrete World Action Model that tackles this by establishing a unified latent manifold over visual states, physical poses, and control actions. By compressing this manifold through modality-specific Vector-Quantized bottlenecks, Hydra yields discrete vocabularies of kinodynamic intents and visual states. This enables Discrete Latent Planning (DLP), where candidates are sampled directly from the shared manifold and ranked by a Kinematic-Perceptual Cost within the discrete latent space. To bridge discrete planning with the continuous commands required for physical actuation, Hydra pairs DLP with conditional Flow Matching to map selected intents to smooth execution trajectories. Evaluated on two physical robotic platforms, Hydra outperforms state-of-the-art navigation world models in goal-directed planning, while matching or exceeding the closed-loop execution capabilities of leading reactive navigation policies.

Mohammad Nazeri, Alexandyr Card, Samira Huber +6

George Mason University · Kiel University · Ludwig Maximilian University Munich +1

Kinodynamic Motion Planning World Models for Robotics Robot Navigation World Model-Based Planning Action-Conditioned World Models Model-Based Planning

Aug 11, 2026cs.RO

Risk-Aware Kinodynamic Motion Planning Under Uncertainty For Safe Navigation on Planetary Environments

For autonomous space exploration, robotic agents need to perform motion planning in which environmental interactions may be unknown. Learning these interactions, such as terrain mechanics for wheeled robots, can introduce uncertainties that lead to risky motion plans and potentially hazardous operations or mission failures. Moreover, uncertainties induced by perception-based systems can exacerbate the problem of safe motion planning. In this letter, we address the problem of performing cost-optimal kinodynamic motion planning with risk awareness. We approach this in two steps. First, a sampling-based planner (AO-RRT) generates a dynamically feasible, risk-aware, and asymptotically cost-optimal trajectory. Second, we formulate motion planning as a nonlinear optimization problem and solve it using sequential convex programming (SCP), using the AO-RRT trajectory as an initial solution. By quantifying risk using conditional value-at-risk (CVaR), we demonstrate a reduction in risk by over

\sim

97% across trajectories in simulation and hardware experiments.

Sachin Sunil Kelkar, Tanmay Dokania, Yashwanth Kumar Nakka

Daniel Guggenheim School of Aerospace Engineering at the Georgia Institute of Technology, Atlanta, GA, 30332 USA.

Trajectory Optimization Kinodynamic Motion Planning Trajectory Generation Space Robotics Robotics Robot Safety+2

Aug 10, 2026cs.RO

Whole-Body Planning for Humanoids Navigating Confined Spaces via Self-Collision Avoidance References

Humanoid locomotion in highly confined environments requires navigating dense environmental obstacles and complex self-collision bounds while maintaining multi-contact dynamic feasibility. Traditional trajectory optimizers frequently struggle in these restricted spaces, as navigating the large collision space with splines on particle abstractions is insufficient and leads to poor local minima. To address this, we propose a three-stage whole-body planning framework that formulates kinematic path planning directly over kinematically reachable rigid-body volumes. By integrating differentiable collision avoidance into a reachability-constrained formulation, our framework synthesizes volume-informed guides that reliably guide a full-order trajectory optimizer over long horizons. We show that these optimized plans serve as high-quality references to train a residual reinforcement learning policy for robust online execution. We validate our approach on the Unitree G1 humanoid across three benchmark testbeds exceeding NIST emergency response standards, achieving restricted confinement ratios (

C_r < 1.5

). Our framework generates feasible trajectories across 12-to-18-second tasks with complex foot and hand contacts where standard baselines fail, while the learned policy successfully tracks these plans under extensive domain randomization in physics simulation.

Carlos Gonzalez, Luis Sentis

Department of Aerospace Engineering and Engineering Mechanics, The University of Texas at Austin, TX 78712, USA.

Constrained Motion Planning Humanoid Robot Locomotion Kinodynamic Motion Planning Robot Motion Planning Robot Collision Avoidance

Aug 10, 2026cs.RO

Entanglement-Free Trajectory Planning for Tethered Mobile Robots with a Slack Tether

In motion planning algorithms for tethered mobile robots, the entanglement state of the tether is a critical aspect to consider during the planning phase. This is particularly important in case of a slack tether, where the shape of the tether is not determined solely by the geometry of the environment and the location of the obstacles, but also by the dynamics of the tether, by the trajectory followed by the robot, and possibly by exogenous forces. In this scenario, preventing entanglement requires planning a robot trajectory that accounts for the entanglement definition and for the dynamics of the robot and of the tether. In this work, we propose a motion planning algorithm for tethered mobile robots with a slack tether that computes dynamically feasible entanglement-free trajectories to navigate through an environment with static obstacles. By considering the entanglement state during all the stages of the planning pipeline, we are able to compute safer trajectories that avoid entanglement during the motion of the robot. We achieve this through a three-step pipeline, which includes (i) the construction of a topological model of the entanglement-free configuration space of the tethered robot, (ii) the generation of a set of candidate paths using this model, and (iii) the computation of a dynamically feasible entanglement-free trajectory by solving a homotopy-constrained trajectory generation problem. The resulting trajectory can then be executed to lead the robot to its target location, while maintaining the tether in an entanglement-free configuration. We demonstrate the benefits of this algorithm in simulations, where we show how the planning algorithm avoids violations of the entanglement constraints, resulting in safer and more reliable trajectories.

Gianpietro Battocletti, Dimitris Boskos, Bart De Schutter

Delft Center for Systems and Control, Delft University of Technology, Delft, The Netherlands

Constrained Motion Planning Kinodynamic Motion Planning Trajectory Generation Safe Motion Planning Robot Motion Planning

Jul 21, 2026cs.RO

Stochastic Multi-Objective Kinodynamic Planning Against Adversaries

This paper addresses multi-objective kinodynamic planning in environments with stochastic hybrid adversaries that probabilistically transition to adversarial modes based on the ego state. The goal is to construct the Pareto-front of paths that trade off execution cost and the probability of safety constraint violation (risk). Existing chance-constrained planners evaluate risk over open-loop trajectories, yielding overly conservative solutions that fail to account for ego-agent reactivity. To address this limitation, we shift the planning space to sequences of closed-loop policies, and integrate sample-based risk evaluation directly into tree construction via Monte-Carlo particle rollouts. We first introduce Stochastic Multi-Objective RRT (SMO-RRT), for which we prove probabilistic completeness, followed by Stochastic Multi-Objective Stable Sparse RRT (SMO-SST), which leverages selective pruning to improve numerical performance at the cost of completeness. For both algorithms, we derive a finite-sample bound on the probability of chance constraint violation for systems with non-Gaussian, state-dependent uncertainty, enabling probabilistically safe planning in a broad class of environments applicable to multi-agent systems, social navigation, and autonomous driving.

Thomas Marshall Vielmetti, Daniel Cherenson, Dimitra Panagou

Constrained Motion Planning Kinodynamic Motion Planning Motion Planning Multi-Objective Motion Planning Sampling-Based Motion Planning

Jul 20, 2026cs.RO

FARO: Feasibility-Aware Robot Motion Optimization

Fast planning of novel behaviors in unseen scenarios remains a fundamental challenge in robotics. The high-dimensional, hybrid, and underactuated nature of humanoid loco-manipulation continues to hinder the realization of this goal. In this paper, we address this challenge by proposing a nested kino-dynamic framework for rapid feasibility checking and dynamically consistent trajectory generation given a candidate contact sequence. By integrating this module with a feasibility-guided tree search and a Large Language Model (LLM)-based contact plan sampling strategy, we demonstrate that the proposed framework can substantially improve the search process. Furthermore, we show that the generated trajectories can be tracked using a reinforcement learning (RL)-based controller and show that the resulting trajectories are of sufficiently high quality for execution in real-world loco-manipulation scenarios. A supplementary video is available at: https://youtu.be/R6qCHoCormQ.

Michal Ciebielski, Shafeef Omar, Aaron Johnson +1

Munich Institute of Robotics and Machine Intelligence (MIRMI), Technical University of Munich (TUM), Germany. · Institute for Advanced Study, Technical University of Munich, Garching, Germany. · Department of Mechanical Engineering, Carnegie Mellon University, Pittsburgh, PA, USA.

Humanoid Loco-Manipulation Kinodynamic Motion Planning Trajectory Generation Robot Motion Generation

Jul 17, 2026cs.RO

Minimum Time Dubins Airplane Paths with Asymmetric Climb Rates

Dubins airplane paths approximate the limited maneuverability of fixed-wing vehicles with minimum curvature and climb rate constraints. However, the symmetric climb rate constraints result in sub-optimal paths and conservative vehicle performance. In this work, we propose asymmetric Dubins airplane paths, which consider asymmetric climb rates for climbing and descending. We revisit the time optimality conditions and show that the asymmetric flight path angle constraints preserve optimality. We show that by considering asymmetric climb rates, we can take advantage of full performance of the vehicle, reducing the minimum time by 71% for connecting randomly generated states. We also demonstrate that the added climb rate results in 2.8 times faster to find the median solution time when integrated into a sampling-based planning task on rugged terrain, due to the added feasibility. We further demonstrate the practicality of the approach with a real-world flight.

Jaeyoung Lim, Giuseppe Loianno

Department of Electrical Engineering and Computer Sciences, University of California, Berkeley, CA 94720, USA

Aerial Robotics Constrained Motion Planning Kinodynamic Motion Planning Trajectory Generation UAV Navigation UAV Trajectory Optimization+1

Jul 4, 2026cs.RO

Fast Asymptotically Optimal Kinodynamic Planning via Vectorization

Sampling-based motion planners have been shown to be effective for systems with complex kinodynamic constraints and high dimensionality. However, these algorithms struggle to achieve real-time performance, leading to recent efforts to parallelize planning. While GPU-accelerated planners have achieved significant speedups, existing approaches require specialized CUDA programming that limits accessibility and portability. We present Parallel Asymptotically Optimal Kinodynamic RRT (PAKR), a massively parallel kinodynamic planner leveraging JAX and the XLA compiler to achieve GPU acceleration through standard Python tooling. By combining our parallel planner with the AO-x meta-algorithm, we achieve asymptotic optimality through fast iterative replanning. We provide a theoretical analysis of probabilistic completeness, analyze the effects of batch size and branching factor on convergence, and demonstrate scalability to complex dynamics using the MuJoCo-XLA simulator. Experiments show competitive runtimes with state-of-the-art GPU planners and superior solution quality.

Yitian Gao, Andrew Lu, Zachary Kingston

Department of Computer Science, Purdue University

Constrained Motion Planning Kinodynamic Motion Planning GPU Acceleration Motion Planning Sampling-Based Motion Planning

Jun 25, 2026cs.RO

BOWConnect: Parallel Bayesian Optimization over Windows with Learned Local Cost Maps for Sample-Efficient Kinodynamic Motion Planning

This paper presents BOWConnect, a bidirectional parallel kinodynamic motion planner that addresses three fundamental limitations of existing sampling-based methods: sample inefficiency in high-dimensional state spaces, unreliable cost heuristics under dynamic constraints, and poor performance in narrow passage environments. Unlike classical planners that rely on random control sampling and geometric distance heuristics, BOWConnect integrates Bayesian Optimization over Windows (BOW) as a learning-based steering function within a parallel tree-based exploration framework, enabling each worker to learn local cost maps and constraints to guide sampling toward dynamically feasible and collision-free controls. A bidirectional architecture simultaneously grows forward and backward trees from the start and goal regions in parallel threads, with a spatial hashing mechanism enabling fast connection queries and a boundary value problem solver generating kinodynamically consistent bridge trajectories. Extensive evaluations across ten benchmark environments demonstrate that BOWConnect achieves 100% success while delivering the fastest or near-fastest planning time in complex scenarios, including narrow passages and non-convex spaces where state-of-the-art planners fail or degrade substantially. Real-world deployment on a ground vehicle and a quadrotor confirms real-time planning with no collisions. Videos of real-world and simulated experiments, high-resolution versions of the figures, and the open-source code are available at https://bow-connect.github.io/.

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

Department of Computer Science, Louisiana State University New Orleans, New Orleans, LA 70148, USA · Knight Foundation School of Computing and Information Sciences, Florida International University, Miami, FL 33199, USA

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

Jun 17, 2026cs.RO

Coupled Routing and Configuration Optimization for Multi-Viewpoint Robotic Inspection

We present a unified framework that turns a set of 6-DoF inspection viewpoints into a time-optimal, collision-free route for a 9-DoF robotic system. Unlike modular pipelines that fix a single inverse-kinematics (IK) configuration per viewpoint, build an all-pairs travel-time map, and then route, our method jointly optimizes the visiting order and the per-viewpoint configuration in a single global search. The three-dimensional self-motion manifold of each viewpoint is parameterized in closed form so that the pose constraint holds by construction, the rest-to-rest travel time is approximated by a closed-form admissible double-integrator surrogate, and the tour is encoded by random keys. A derivative-free optimizer (CMA-ES) minimizes a cheap penalized objective over order and configuration, after which direct-collocation trajectory optimization is applied only to the edges of the selected route to certify dynamic feasibility and torque limits, and to return exact timings. This reduces the trajectory solves from quadratic to linear in the number of viewpoints and removes the decoupling that prevents modular pipelines from being globally time-optimal. Simulations and real-robot experiments on a KUKA LBR iiwa with a 2-DoF linear stage validate feasibility, smooth execution, and reduced end-to-end inspection time relative to modular and naive distance-based baselines.

Minh Nhat Vu, Khang Nguyen, Vu Trung Tran +1

Center for Artificial Intelligence Research (CAIR), VinUniversity, Hanoi, Vietnam · VinRobotics JSC, Hanoi, Vietnam · Division of Computer and Mathematical Sciences, Mohamed bin Zayed University of Artificial Intelligence (MBZUAI), Abu Dhabi, UAE.

Constrained Motion Planning Trajectory Optimization Kinodynamic Motion Planning Robotic Inspection Trajectory Generation Robot Task Planning+1

Jun 10, 2026cs.RO

KinematicRL: A Sim-to-Real Reinforcement Learning Framework For Social Navigation With Kinodynamic Feasibility

Deep Reinforcement Learning (DRL) has shown promise for social navigation, yet its real-world deployment remains hindered by a persistent sim-to-real gap arising from simplified first-order dynamics and context-specific human state estimation pipelines. This work presents a unified framework that addresses these limitations to produce dynamically feasible navigation policies suitable for real-world deployment. First, theoretical analysis reveals that tracking error between simulated and actual robot position decays exponentially with increased control order, motivating the use of higher-order control inputs as DRL action space. A second-order control formulation tailored to differential drive robots is developed, complemented by a stochastic iterative Linear Quadratic Regulator (iLQR) that pretrains the policy via a divergence minimization objective. Second, to avoid the added system complexity of camera-LiDAR fusion, a cluster-based human tracking pipeline using only 2D LiDAR is introduced. Human detections are associated according to both spatial proximity and velocity similarity, enabling reliable differentiation of nearby pedestrians and yielding stable velocity estimates through temporal aggregation. Third, we introduce an unbiased residual gating block to balance reaction- and memory-based behaviors while handling time-varying crowd sizes, both critical for social navigation. The resulting policy, KinematicRL, consistently improves kinematic performance and adapts to varying number of detected humans. Experiments in real-world environments demonstrate that, when combined with the proposed tracking pipeline, KinematicRL can be deployed on a real differential drive robot with minimal modifications.

Zhiming Xu, Haodong Yang, Chengju Liu +2

School of Computer Science and Technology, Tongji University, Shanghai 201804, China · Department of Electronics and Information Engineering, Tongji University, Shanghai 201804, China · Shanghai Institute of Intelligent Science and Technology, Tongji University, Shanghai 201210, China

Kinodynamic Motion Planning Sim-to-Real Transfer Robotic RL Social Robot Navigation

May 28, 2026cs.RO

Bidirectional Incremental Generalized Hybrid A*

We focus on the problem of efficient anytime kinodynamic planning for systems with complex dynamics in unstructured environments that make using precomputed motion primitives infeasible. Directly applying A* here is computationally infeasible due to the curse of dimensionality. Methods such as Hybrid A* (HA*) address this by pruning the search tree by discretizing the state space, but the coupling between pruning and discretization resolution can eliminate a vertex on the solution path. The Incremental Generalized Hybrid A* (IGHA*) breaks this coupling by organizing anytime search over a hierarchy of resolutions and by freezing vertices for later expansion rather than pruning. However, IGHA* can still freeze a vertex on the solution path, forcing the search to spend expansions elsewhere before reaching it. Our key insight is that bidirectional-IGHA* (Bi-IGHA*) not only gains the expected reduction in effective search depth from classical bidirectionality, but also mitigates this frozen-vertex barrier: when one search is blocked, the opposing search can reach it at a low resolution. We formalize this structural separation between Bi-IGHA* and IGHA* and empirically show a reduction in effective branching factor beyond the theoretical expectation from classic bidirectionality alone. Through open- loop experiments in R3, R4, and R6, we show that Bi- IGHA* substantially reduces expansions over IGHA*. Closed- loop experiments further demonstrate improved performance and feasibility in simulation and on a real-world robotic system. Link to Website: https://personalrobotics.github.io/IGHAStar/biighastar.html

Sidharth Talia, Oren Salzman, Siddhartha Srinivasa

Kinodynamic Motion Planning Tree Search Graph-Based Planning Motion Planning Robot Motion Planning

May 26, 2026cs.RO

AURA: Asymptotically Optimal Uncertainty-Robust Replanning Algorithm for Kinodynamic Systems

Sampling-based motion planners offer a practical and scalable approach to kinodynamic motion planning, notably for high-dimensional, underactuated, or non-holonomic systems. However, these planners are typically used offline, requiring execution to begin only after the trajectory has been computed. In addition, the planned trajectory may not be accurately tracked in the presence of motion uncertainty, leading to deviations from the nominal solution. In this work, these limitations were addressed within a unified framework, AURA, an asymptotically-optimal meta-planner framework that improves both path quality and tracking performance during execution. In addition to the main execution thread, this framework comprises a replanning method that continuously explores the state space and refines the trajectory during execution, and an optimization process that refines future control inputs to reduce tracking error. Together, these components enable AURA to leverage asymptotically optimal planning online while improving execution accuracy under motion uncertainty. The proposed approach is evaluated in both simulation and real-world environments across multiple systems, demonstrating consistent improvements in trajectory quality, tracking accuracy, and overall performance compared with baselines.

Seyedali Golestaneh, Zhuoyun Zhong, Donghyung Lee +1

Department of Robotics Engineering, Worcester Polytechnic Institute (WPI), Worcester, MA 01609, USA

Robot Trajectory Tracking Kinodynamic Motion Planning Motion Planning Explainable Motion Planning Sampling-Based Motion Planning

May 20, 2026cs.RO

A Terrain-Adaptive epsilon-Constraint MPC for Uneven Terrain Kinodynamic Planning

Kinodynamic planning for car-like vehicles on uneven terrain requires simultaneously optimizing competing objectives such as path efficiency and pose stability. This work presents an adaptive epsilon-constraint method integrated into a Model Predictive Control (MPC) framework, where the epsilon bounds are dynamically adjusted based on terrain descriptors to explore the Pareto front in real time. To capture vehicle-terrain dynamics, we develop a semi-parametric model combining analytical vehicle dynamics with a Sparse Gaussian Process (SGP) trained on the same terrain descriptors. The proposed epsilon-MPC is evaluated against MPPI and GAKD baselines, achieving a 94% navigation success rate while reducing maximum orientation deviation by 24% and improving multi-objective trade-off quality by 23%.

Otobong Jerome, Geesara Kalathunga, Tiago Nascimento

Laborat´orio de Engenharia de Sistemas e Rob´otica, Universidade Federal da Para´ıba (UFPB), Jo˜ao Pessoa, Para´ıba, 58055-000, Brazil · School of Computer Science, University of Lincoln, Lincoln, United Kingdom

Constrained Motion Planning Kinodynamic Motion Planning Robot Navigation Model Predictive Control Vehicle Dynamics Motion Planning+1

May 20, 2026cs.RO

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

May 19, 2026cs.RO

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, 2026cs.RO

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, 2026cs.RO

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, 2026cs.RO

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 Mobile Robotics Motion Planning+1

Mar 24, 2026cs.MA

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, 2026cs.RO

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 Sampling-Based Motion Planning

Feb 2, 2026cs.RO

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, 2025cs.RO

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, 2025cs.RO

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+2