Safe Motion Planning

Follow this topic Ask about this topic

Safe motion planning studies how robots and vehicles can reach goals without collisions while respecting limits on their motion and the environment. Common approaches include control barrier functions, reachability analysis, and model predictive control, alongside graph search, sampling-based planning, and optimization. Learning-based planners increasingly use diffusion models, world models, and risk predictions, often paired with safety filters or formal checks; they are tested in settings such as autonomous driving, UAV flight, manipulation, and multi-robot navigation. A central challenge is balancing reliable safety guarantees under uncertain perception and dynamics with plans that are efficient, responsive, and not overly conservative.

Latest papers

215 papers30 papers in the last four weeks, up 36% on the four weeks before. 0.2% of all new papers.

Mar 25, 2026

Accelerated Spline-Based Time-Optimal Motion Planning with Continuous Safety Guarantees for Non-Differentially Flat Systems

Generating time-optimal, collision-free trajectories for autonomous mobile robots involves a fundamental trade-off between guaranteeing safety and managing computational complexity. State-of-the-art approaches formulate spline-based motion planning as a single Optimal Control Problem (OCP) but often suffer from high computational cost because they include separating hyperplane parameters as decision variables to enforce continuous collision avoidance. This paper presents a novel method that alleviates this bottleneck by decoupling the determination of separating hyperplanes from the OCP. By treating the separation theorem as an independent classification problem solvable via a linear system or quadratic program, the proposed method eliminates hyperplane parameters from the optimisation variables, effectively transforming non-convex constraints into linear ones. Experimental validation demonstrates that this decoupled approach reduces trajectory computation times up to almost 60% compared to fully coupled methods in obstacle-rich environments, while maintaining rigorous continuous safety guarantees.

Dries Dirckx, Jan Swevers, Wilm Decré

Motion Planning Safe Motion Planning Trajectory Optimization Collision Avoidance

Mar 23, 2026

From Singleton Obstacles to Clutter: Translation Invariant Compositional Avoid Sets

This paper studies obstacle avoidance under translation invariant dynamics using an avoid-side travel cost Hamilton Jacobi formulation. For running costs that are zero outside an obstacle and strictly negative inside it, we prove that the value function is non-positive everywhere, equals zero exactly outside the avoid set, and is strictly negative exactly on it. Under translation invariance, this yields a reuse principle: the value of any translated obstacle is obtained by translating a single template value function. We show that the pointwise minimum of translated template values exactly characterizes the union of the translated single-obstacle avoid sets and provides a conservative inner certificate of unavoidable collision in clutter. To reduce conservatism, we introduce a blockwise composition framework in which subsets of obstacles are merged and solved jointly. This yields a hierarchy of conservative certificates from singleton reuse to the exact clutter value, together with monotonicity under block merging and an exactness criterion based on the existence of a common clutter avoiding control. The framework is illustrated on a Dubins car example in a repeated clutter field.

Prashant Solanki, Jasper Van Beers, Coen De Visser

Delft University of Technology, Netherlands

Collision Avoidance Reachability Analysis Safe Motion Planning

Mar 23, 2026

RTD-RAX: Fast, Safe Trajectory Planning for Systems under Unknown Disturbances

Reachability-based Trajectory Design (RTD) is a provably safe, real-time trajectory planning framework that combines offline reachable-set computation with online trajectory optimization. However, standard RTD implementations suffer from two key limitations: conservatism induced by worst-case reachable-set overapproximations, and an inability to account for real-time disturbances during execution. This paper presents RTD-RAX, a runtime-assurance extension of RTD that utilizes a non-conservative RTD formulation to rapidly generate goal-directed candidate trajectories, and utilizes mixed monotone reachability for fast, disturbance-aware online safety certification. When proposed trajectories fail safety certification under real-time uncertainty, a repair procedure finds nearby safe trajectories that preserve progress toward the goal while guaranteeing safety under real-time disturbances.

Evanns Morales-Cuadrado, Long Kiu Chung, Shreyas Kousik +1

S. Coogan is also with the School of Civil and Environmental Engineering.

Safe Motion Planning Motion Planning Trajectory Optimization Runtime Assurance Reachability Analysis

Mar 20, 2026

High-Speed, All-Terrain Autonomy: Ensuring Safety at the Limits of Mobility

A novel local trajectory planner, capable of controlling an autonomous off-road vehicle on rugged terrain at high-speed is presented. Autonomous vehicles are currently unable to safely operate off-road at high-speed, as current approaches either fail to predict and mitigate rollovers induced by rough terrain or are not real-time feasible. To address this challenge, a novel model predictive control (MPC) formulation is developed for local trajectory planning. A new dynamics model for off-road vehicles on rough, non-planar terrain is derived and used for prediction. Extreme mobility, including tire liftoff without rollover, is safely enabled through a new energy-based constraint. The formulation is analytically shown to mitigate rollover types ignored by many state-of-the-art methods, and real-time feasibility is achieved through parallelized GPGPU computation. The planner's ability to provide safe, extreme trajectories is studied through both simulated trials and full-scale physical experiments. The results demonstrate fewer rollovers and more successes compared to a state-of-the-art baseline across several challenging scenarios that push the vehicle to its mobility limits.

James R. Baxter, Bogdan I. Epureanu, Paramsothy Jayakumar +1

Department of Mechanical Engineering, University of Michigan, Ann Arbor, MI · U.S. Army Ground Vehicle Systems Center, Warren, MI

Safe Motion Planning Off-Road Robot Navigation Model Predictive Control Vehicle Dynamics

Mar 20, 2026

Multi-Agent Motion Planning on Industrial Magnetic Levitation Platforms: A Hybrid ADMM-HOCBF approach

This paper presents a novel hybrid motion planning method for holonomic multi-agent systems. The proposed decentralised model predictive control (MPC) framework tackles the intractability of classical centralised MPC for a growing number of agents while providing safety guarantees. This is achieved by combining a decentralised version of the alternating direction method of multipliers (ADMM) with a centralised high-order control barrier function (HOCBF) architecture. Simulation results show significant improvement in scalability over classical centralised MPC. We validate the efficacy and real-time capability of the proposed method by developing a highly efficient C++ implementation and deploying the resulting trajectories on a real industrial magnetic levitation platform.

Bavo Tistaert, Stan Servaes, Alejandro Gonzalez-Garcia +4

Decentralized Multi-Agent Motion Planning Motion Planning Safe Motion Planning Model Predictive Control

Mar 19, 2026

CSSDF-Net: Safe Motion Planning Based on Neural Implicit Representations of Configuration Space Distance Field

High-dimensional manipulator operation in unstructured environments requires a differentiable, scene-agnostic distance query mechanism to guide safe motion generation. Existing geometric collision checkers are typically non-differentiable, while workspace-based implicit distance models are hindered by the highly nonlinear workspace--configuration mapping and often suffer from poor convergence; moreover, self-collision and environment collision are commonly handled as separate constraints. We propose Configuration-Space Signed Distance Field-Net (CSSDF-Net), which learns a continuous signed distance field directly in configuration space to provide joint-space distance and gradient queries under a unified geometric notion of safety. To enable zero-shot generalization without environment-specific retraining, we introduce a spatial-hashing-based data generation pipeline that encodes robot-centric geometric priors and supports efficient retrieval of risk configurations for arbitrary obstacle point sets. The learned distance field is integrated into safety-constrained trajectory optimization and receding-horizon MPC, enabling both offline planning and online reactive avoidance. Experiments on a planar arm and a 7-DoF manipulator demonstrate stable gradients, effective collision avoidance in static and dynamic scenes, and practical inference latency for large-scale point-cloud queries, supporting deployment in previously unseen environments.

Haohua Chen, Yixuan Zhou, Yifan Zhou +1

IRMV Lab, Department of Automation, Shanghai Jiao Tong University, Shanghai 200240, China.

Safe Motion Planning Robot Collision Avoidance Constrained Motion Planning

Mar 17, 2026

Contingency-Aware Planning via Certified Neural Hamilton-Jacobi Reachability

Hamilton-Jacobi (HJ) reachability provides formal safety guarantees for dynamical systems, but solving high-dimensional HJ partial differential equations limits its use in real-time planning. This paper presents a contingency-aware multi-goal navigation framework that integrates learning-based reachability with sampling-based planning in unknown environments. We use Fourier Neural Operator (FNO) to approximate the solution operator of the Hamilton-Jacobi-Isaacs variational inequality under varying obstacle configurations. We first provide a theoretical under-approximation guarantee on the safe backward reach-avoid set, which enables formal safety certification of the learned reachable sets. Then, we integrate the certified reachable sets with an incremental multi-goal planner, which enforces reachable-set constraints and a recovery policy that guarantees finite-time return to a safe region. Overall, we demonstrate that the proposed framework achieves asymptotically optimal navigation with provable contingency behavior, and validate its performance through real-time deployment on KUKA's youBot in Webots simulation.

Kasidit Muenprasitivej, Derya Aksaray

Department of Electrical and Computer Engineering, Northeastern University, Boston, MA, USA, 02115

Safe Motion Planning Robot Navigation Reachability Analysis

Mar 16, 2026

CorrectionPlanner: Self-Correction Planner with Reinforcement Learning in Autonomous Driving

Autonomous driving requires safe planning, but most learning-based planners lack explicit self-correction ability: once an unsafe action is proposed, there is no mechanism to correct it. Thus, we propose CorrectionPlanner, an autoregressive planner with self-correction that models planning as motion-token generation within a propose, evaluate, and correct loop. At each planning step, the policy proposes an action, namely a motion token, and a learned collision critic predicts whether it will induce a collision within a short horizon. If the critic predicts a collision, we retain the sequence of historical unsafe motion tokens as a self-correction trace, generate the next motion token conditioned on it, and repeat this process until a safe motion token is proposed or the safety criterion is met. This self-correction trace, consisting of all unsafe motion tokens, represents the planner's correction process in motion-token space, analogous to a reasoning trace in language models. We train the planner with imitation learning followed by model-based reinforcement learning using rollouts from a pretrained world model that realistically models agents' reactive behaviors. Closed-loop evaluations show that CorrectionPlanner reduces collision rate by over 20% on Waymax and achieves state-of-the-art planning scores on nuPlan.

Yihong Guo, Dongqiangzi Ye, Sijia Chen +2

Department of Computer Science, Johns Hopkins University, Baltimore, USA. · XPENG Motors.

Autonomous Driving Planning Safe Motion Planning RL for Autonomous Driving

Mar 16, 2026

Multi-robot Graph Traversal with Support Coordination under Stochastically Moving Adversaries

Cooperative multi-robot missions require team of robots to traverse environments where adversaries or hazards with stochastic dynamics induce time-varying traversal risk. While support coordination--where robots assist teammates in traversing risky regions--can significantly reduce mission costs, its effectiveness depends on the team's ability to anticipate future risk. We formulate support-based multi-robot graph traversal problem with stochastically moving adversaries, where future risky regions become uncertain as adversaries move through the environment. When adversaries remain stationary, our formulation reduces to the static risky-edge setting. To address the stochastic case, we model individual adversaries as first-order Markov stay-move processes over graph edges and propagate their occupancy distributions over a finite planning horizon to obtain time-indexed edge-risk forecasts. These forecasts inform the support candidate selection and joint robot path planning. Experimental results show that forecast-informed support decisions consistently lower expected team cost relative to evaluated baselines in stochastic motion settings.

Manshi Limbu, Xuan Wang, Gregory J. Stein +2

George Mason University

Multi-Robot Motion Planning Risk-Aware Path Planning Multi-Robot Coordination Graph-Based Planning Safe Motion Planning

Mar 11, 2026

Rethinking Gaussian Trajectory Predictors: Calibrated Uncertainty for Safe Planning

Accurate trajectory prediction is critical for safe autonomous navigation in crowded environments. While many trajectory predictors output Gaussian distributions to represent the multi-modal distribution over future pedestrian positions, the reliability of their confidence levels often remains unaddressed. This limitation can lead to unsafe or overly conservative motion planning when the predictor is integrated with an uncertainty-aware planner. Existing Gaussian trajectory predictors primarily rely on the Negative Log-Likelihood loss, which is prone to predict over- or under-confident distributions, and may compromise downstream planner safety. This paper introduces a novel loss function for calibrating prediction uncertainty which leverages Kernel Density Estimation to estimate the empirical distribution of confidence levels. The proposed formulation enforces consistency with the properties of a Gaussian assumption by explicitly matching the estimated empirical distribution to the Chi-squared distribution. To ensure accurate mean prediction, a Mean Squared Error term is also incorporated in the final loss formulation. Experimental results on real-world trajectory datasets show that our method significantly improves the reliability of confidence levels predicted by different State-Of-The-Art Gaussian trajectory predictors. We also demonstrate the importance of providing planners with reliable probabilistic insights (i.e. calibrated confidence levels) for collision-free navigation in complex scenarios. For this purpose, we integrate Gaussian trajectory predictors trained with our loss function with an uncertainty-aware Model Predictive Control on scenarios extracted from real-world datasets, achieving improved planning performance through calibrated confidence levels.

Fatemeh Cheraghi Pouria, Mahsa Golchoubian, Katherine Driggs-Campbell

Department of Electrical and Computer Engineering, University of Illinois at Urbana-Champaign, Champaign, IL 61820 USA · Department of Mathematical and Computational Sciences, University of Toronto, Toronto, Canada

Pedestrian Trajectory Prediction Safe Motion Planning Trajectory Prediction Uncertainty Calibration

Mar 11, 2026

PC-Diffuser: Path-Consistent Capsule CBF Safety Filtering for Diffusion-Based Trajectory Planner

Autonomous driving in complex traffic requires planners that generalize beyond hand-crafted rules, motivating data-driven approaches that learn behavior from expert demonstrations. Diffusion-based trajectory planners have recently shown strong closed-loop performance by iteratively denoising a full-horizon plan, but they remain difficult to certify and can fail catastrophically in rare or out-of-distribution scenarios. To address this challenge, we present PC-Diffuser, a safety augmentation framework that embeds a certifiable, path-consistent barrier-function structure directly into the denoising loop of diffusion planning. The key idea is to make safety an intrinsic part of trajectory generation rather than a post-hoc fix: we enforce forward invariance along the rollout while preserving the diffusion model's intended path geometry. Specifically, PC-Diffuser (i) evaluates collision risk using a capsule-distance barrier function that better reflects vehicle geometry and reduces unnecessary conservativeness, (ii) converts denoised waypoints into dynamically feasible motion under a kinematic bicycle model, and (iii) applies a path-consistent safety filter that eliminates residual constraint violations without geometric distortion, so the corrected plan remains close to the learned distribution. By injecting these safety-consistent corrections at every denoising step and feeding the refined trajectory back into the diffusion process, PC-Diffuser enables iterative, context-aware safeguarding instead of post-hoc repair...

Eugene Ku, Yiwei Lyu

Department of Computer Science and Engineering, Texas A&M University, College Station, TX, USA

Constrained Motion Planning Trajectory Generation Diffusion-Based Planning Control Barrier Functions Safety Filtering Safe Motion Planning+1

Mar 10, 2026

Provably Safe Trajectory Generation for Manipulators Under Motion and Environmental Uncertainties

Robot manipulators operating in uncertain and non-convex environments present significant challenges for safe and optimal motion planning. Existing methods often struggle to provide efficient and formally certified collision risk guarantees, particularly when dealing with complex geometries and non-Gaussian uncertainties. This article proposes a novel risk-bounded motion planning framework to address this unmet need. Our approach integrates a rigid manipulator deep stochastic Koopman operator (RM-DeSKO) model to robustly predict the robot's state distribution under motion uncertainty. We then introduce an efficient, hierarchical verification method that combines parallelizable physics simulations with sum-of-squares (SOS) programming as a filter for fine-grained, formal certification of collision risk. This method is embedded within a Model Predictive Path Integral (MPPI) controller that uniquely utilizes binary collision information from SOS decomposition to improve its policy. The effectiveness of the proposed framework is validated on two typical robot manipulators through extensive simulations and real-world experiments, including a challenging human-robot collaboration scenario, demonstrating sim-to-real transfer of the learned model and its ability to generate safe and efficient trajectories in complex, uncertain settings.

Fei Meng, Zijiang Yang, Xinyu Mao +2

Hong Kong Center for Construction Robotics, The Hong Kong University of Science and Technology, Hong Kong, China · Department of Mechanical and Automation Engineering, The Chinese University of Hong Kong, Hong Kong, China · Department of Electronic Engineering, The Chinese University of Hong Kong, Hong Kong, China +2

Safe Motion Planning

Mar 7, 2026

Statistical Contraction for Chance-Constrained Trajectory Optimization of Non-Gaussian Stochastic Systems

This paper presents novel method for distribution-free robust trajectory optimization and control of discrete-time, nonlinear, and non-Gaussian stochastic systems, with closed-loop guarantees on chance constraint satisfaction. Our framework employs conformal inference to generate coverage-based confidence sets for the closed-loop dynamics around arbitrary reference trajectories, by constructing a joint nonconformity score to quantify both the validity of contraction (i.e., incremental stability) conditions and the impact of external stochastic disturbance on the closed-loop dynamics, without any distributional assumptions. Via appropriate constraint tightening, chance constraints can be reformulated into tractable, statistically valid deterministic constraints on the reference trajectories. This enables a formal pathway to leverage and validate learning-based motion planners and controllers, such as those with neural contraction metrics, in safety-critical real-world applications. Notably, our statistical guarantees are non-diverging and can be computed with finite samples of the underlying uncertainty, without overly conservative structural priors. We demonstrate our approach in motion planning problems for designing safe, dynamically feasible trajectories in both numerical simulation and hardware experiments.

Rihan Aaron D'Silva, Hiroyasu Tsukamoto

Department of Aerospace Engineering, The Grainger College of Engineering, University of Illinois Urbana-Champaign, Urbana, Illinois 61801

Contraction Theory Trajectory Optimization Chance-Constrained Optimization Chance-Constrained Motion Planning Safe Motion Planning

Mar 6, 2026

Control Barrier Corridors: From Safety Functions to Safe Sets

Safe autonomy is a critical requirement and a key enabler for robots to operate safely in unstructured complex environments. Control barrier functions and safe motion corridors are two widely used but technically distinct safety methods, functional and geometric, respectively, for safe motion planning and control. Control barrier functions are applied to the safety filtering of control inputs to limit the decay rate of system safety, whereas safe motion corridors are geometrically constructed to define a local safe zone around the system state for use in motion optimization and reference-governor design. This paper introduces a new notion of control barrier corridors, which unifies these two approaches by converting control barrier functions into local safe goal regions for reference goal selection in feedback control systems. We show, with examples on fully actuated systems, kinematic unicycles, and linear output regulation systems, that individual state safety can be extended locally over control barrier corridors for convex barrier functions, provided the control convergence rate matches the barrier decay rate, highlighting a trade-off between safety and reactiveness. Such safe control barrier corridors enable safely reachable persistent goal selection over continuously changing barrier corridors during system motion, which we demonstrate for verifiably safe and persistent path following in autonomous exploration of unknown environments.

Ömür Arslan, Nikolay Atanasov

Department of Mechanical Engineering, Eindhoven University of Technology, P.O. Box 513, 5600 MB Eindhoven, The Netherlands. · Department of Electrical and Computer Engineering, University of California San Diego, La Jolla, CA 92093, USA.

Control Barrier Functions Safe Motion Planning Safe Robot Navigation

Mar 6, 2026

Iterative Convex Optimization with Control Barrier Functions for Obstacle Avoidance among Polytopes

Obstacle avoidance of polytopic obstacles by polytopic robots is a challenging problem in optimization-based control and trajectory planning. Many existing methods rely on smooth geometric approximations, such as hyperspheres or ellipsoids, which allow differentiable distance expressions but distort the true geometry and restrict the feasible set. Other approaches integrate exact polytope distances into nonlinear model predictive control (MPC), resulting in nonconvex programs that limit real-time performance. In this paper, we construct linear discrete-time control barrier function (DCBF) constraints by deriving supporting hyperplanes from exact closest-point computations between convex polytopes. We then propose a novel iterative convex MPC-DCBF framework, where local linearization of system dynamics and robot geometry ensures convexity of the finite-horizon optimization at each iteration. The resulting formulation reduces computational complexity and enables fast online implementation for safety-critical control and trajectory planning of general nonlinear dynamics. The framework extends to multi-robot and three-dimensional environments. Numerical experiments demonstrate collision-free navigation in cluttered maze scenarios with millisecond-level solve times.

Shuo Liu, Zhe Huang, Calin A. Belta

Boston University, Brookline, MA, USA · Department of Electrical and Computer Engineering and with the Department of Computer Science, University of Maryland, College Park, MD, USA

Robot Collision Avoidance Safe Motion Planning Control Barrier Functions Model Predictive Control

Feb 28, 2026

Online Generation of Collision-Free Trajectories in Dynamic Environments

In this paper, we present an online method for converting an arbitrary geometric path, represented by a sequence of states, and generated by any planner (e.g., sampling-based planners such as RRT or PRM, search-based planners such as ARA*, etc.), into a kinematically feasible, jerk-limited trajectory. The method generates a sequence of quintic/quartic splines that can be discretized at a user-specified control rate and streamed to a low-level robot controller. Our approach enables real-time adaptation to environmental changes and can be re-invoked at any instant to generate a new trajectory from the robot's current state to a desired target state or sequence of states. Under a bounded-obstacle-velocity assumption, the method provides conditional stopping-safety guarantees over a finite time interval in dynamic environments, while allowing bounded geometric deviation from the original path. Kinematic constraints, including jerk limits, are explicitly considered. We validate the approach in a comparative simulation study against a competing method, demonstrating favorable behavior w.r.t. smoothness, computational time, and real-time performance, particularly with frequent target-state changes (up to 1 [kHz]). Real-robot experiments demonstrate applicability in real-world scenarios, including scenarios with a human as an obstacle.

Nermin Covic, Bakir Lacevic

Faculty of Electrical Engineering, University of Sarajevo, Bosnia and Herzegovina

Constrained Motion Planning Trajectory Optimization Trajectory Generation Robot Motion Generation Safe Motion Planning Dynamic Obstacle Avoidance+1

Feb 27, 2026

Layered Safety: Enhancing Autonomous Collision Avoidance via Multistage CBF Safety Filters

This paper presents a general end-to-end framework for constructing robust and reliable layered safety filters that can be leveraged to perform dynamic collision avoidance over a broad range of applications using only local perception data. Given a robot-centric point cloud, we begin by constructing an occupancy map which is used to synthesize a Poisson safety function (PSF). The resultant PSF is employed as a control barrier function (CBF) within two distinct safety filtering stages. In the first stage, we propose a predictive safety filter to compute optimal safe trajectories based on nominal potentially-unsafe commands. The resultant short-term plans are constrained to satisfy the CBF condition along a finite prediction horizon. In the second stage, instantaneous velocity commands are further refined by a real-time CBF-based safety filter and tracked by the full-order low-level robot controller. Assuming accurate tracking of velocity commands, we obtain formal guarantees of safety for the full-order system. We validate the optimality and robustness of our multistage architecture, in comparison to traditional single-stage safety filters, via a detailed Pareto analysis. We further demonstrate the effectiveness and generality of our collision avoidance methodology on multiple legged robot platforms across a variety of real-world dynamic scenarios.

Erina Yamaguchi, Ryan M. Bena, Gilbert Bahati +1

Department of Mechanical and Civil Engineering and the Department of Aerospace, Caltech, Pasadena, CA 91125, USA

Safety-Critical Control Control Barrier Functions Robot Safety Safety Filtering Safe Motion Planning Dynamic Obstacle Avoidance+1

Feb 26, 2026

Considering Perspectives for Automated Driving Ethics: Collective Risk in Vehicular Motion Planning

Recent automated vehicle (AV) motion planning strategies evolve around minimizing risk in road traffic. However, they exclusively consider risk from the AV's perspective and, as such, do not address the ethicality of its decisions for other road users. We argue that this does not reduce the risk of each road user, as risk may be different from the perspective of each road user. Indeed, minimizing the risk from the AV's perspective may not imply that the risk from the perspective of other road users is also being minimized; in fact, it may even increase. To test this hypothesis, we propose an AV motion planning strategy that supports switching risk minimization strategies between all road user perspectives. We find that the risk from the perspective of other road users can generally be considered different to the risk from the AV's perspective. Taking a collective risk perspective, i.e., balancing the risks of all road users, we observe an AV that minimizes overall traffic risk the best, while putting itself at slightly higher risk for the benefit of others, which is consistent with human driving behavior. In addition, adopting a collective risk minimization strategy can also be beneficial to the AV's travel efficiency by acting assertively when other road users maintain a low risk estimate of the AV. Yet, the AV drives conservatively when its planned actions are less predictable to other road users, i.e., associated with high risk. We argue that such behavior is a form of self-reflection and a natural prerequisite for socially acceptable AV behavior. We conclude that to facilitate ethicality in road traffic that includes AVs, the risk-perspective of each road user must be considered in the decision-making of AVs.

Leon Tolksdorf, Arturo Tejada, Christian Birkner +1

Department of Dynamics and Control, Eindhoven University of Technology, Eindhoven, The Netherlands · CARISSMA Institute of Safety in Future Mobility, Technische Hochschule Ingolstadt, Ingolstadt, Germany · TNO, Integrated Vehicle Safety, Helmond, The Netherlands

Autonomous Driving Autonomous Driving Planning Safe Motion Planning

Feb 24, 2026

Robot Local Planner: A Periodic Sampling-Based Motion Planner with Minimal Waypoints for Home Environments

The objective of this study is to enable fast and safe manipulation tasks in home environments. Specifically, we aim to develop a system that can recognize its surroundings and identify target objects while in motion, enabling it to plan and execute actions accordingly. We propose a periodic sampling-based whole-body trajectory planning method, called the "Robot Local Planner (RLP)." This method leverages unique features of home environments to enhance computational efficiency, motion optimality, and robustness against recognition and control errors, all while ensuring safety. The RLP minimizes computation time by planning with minimal waypoints and generating safe trajectories. Furthermore, overall motion optimality is improved by periodically executing trajectory planning to select more optimal motions. This approach incorporates inverse kinematics that are robust to base position errors, further enhancing robustness. Evaluation experiments demonstrated that the RLP outperformed existing methods in terms of motion planning time, motion duration, and robustness, confirming its effectiveness in home environments. Moreover, application experiments using a tidy-up task achieved high success rates and short operation times, thereby underscoring its practical feasibility.

Keisuke Takeshita, Takahiro Yamazaki, Tomohiro Ono +1

Frontier Research Center, Toyota Motor Corporation, Toyota, Aichi, Japan. · Department of Information Science, Faculty of Information Science, Aichi Institute of Technology, Toyota, Aichi, Japan.

Robotics Sampling-Based Motion Planning Safe Motion Planning Whole-Body Robotic Manipulation

Feb 21, 2026

SafeDrive: Fine-Grained Safety Reasoning for End-to-End Driving in a Sparse World

The end-to-end (E2E) paradigm, which maps sensor inputs directly to driving decisions, has recently attracted significant attention due to its unified modeling capability and scalability. However, ensuring safety in this unified framework remains one of the most critical challenges. In this work, we propose SafeDrive, an E2E planning framework designed to perform explicit and interpretable safety reasoning through a trajectory-conditioned Sparse World Model. SafeDrive comprises two complementary networks: the Sparse World Network (SWNet) and the Fine-grained Reasoning Network (FRNet). SWNet constructs trajectory-conditioned sparse worlds that simulate the future behaviors of critical dynamic agents and road entities, providing interaction-centric representations for downstream reasoning. FRNet then evaluates agent-specific collision risks and temporal adherence to drivable regions, enabling precise identification of safety-critical events across future timesteps. SafeDrive achieves state-of-the-art performance on both open-loop and closed-loop benchmarks. On NAVSIM, it records a PDMS of 91.6 and an EPDMS of 87.5, with only 61 collisions out of 12,146 scenarios (0.5%). On Bench2Drive, SafeDrive attains a 66.8% driving score.

Jungho Kim, Jiyong Oh, Seunghoon Yu +3

Seoul National University, Republic of Korea

Autonomous Driving Autonomous Driving Planning End-to-End Autonomous Driving Safe Motion Planning World Models for Autonomous Driving

Feb 19, 2026

RA-Nav: A Risk-Aware Navigation System Based on Semantic Segmentation for Aerial Robots in Unpredictable Environments

Existing aerial robot navigation systems typically plan paths around static and dynamic obstacles, but fail to adapt when a static obstacle suddenly moves. Integrating environmental semantic awareness enables estimation of potential risks posed by suddenly moving obstacles. In this paper, we propose RA- Nav, a risk-aware navigation framework based on semantic segmentation. A lightweight multi-scale semantic segmentation network identifies obstacle categories in real time. These obstacles are further classified into three types: stationary, temporarily static, and dynamic. For each type, corresponding risk estimation functions are designed to enable real-time risk prediction, based on which a complete local risk map is constructed. Based on this map, the risk-informed path search algorithm is designed to guarantee planning that balances path efficiency and safety. Trajectory optimization is then applied to generate trajectories that are safe, smooth, and dynamically feasible. Comparative simulations demonstrate that RA-Nav achieves higher success rates than baselines in sudden obstacle state transition scenarios. Its effectiveness is further validated in simulations using real- world data.

Ziyi Zong, Xin Dong, Jinwu Xiang +2

Safe Robot Navigation Safe Motion Planning Dynamic Obstacle Avoidance

Feb 1, 2026

SPOT: Spatio-Temporal Obstacle-free Trajectory Planning for UAVs in Unknown Dynamic Environments

We address the problem of reactive motion planning for quadrotors operating in unknown environments with dynamic obstacles. Our approach leverages a 4-dimensional spatio-temporal planner, integrated with vision-based Safe Flight Corridor (SFC) generation and trajectory optimization. Unlike prior methods that rely on map fusion, our framework is mapless, enabling collision avoidance directly from perception while reducing computational overhead. Dynamic obstacles are detected and tracked using a vision-based object segmentation and tracking pipeline, allowing robust classification of static versus dynamic elements in the scene. To further enhance robustness, we introduce a backup planning module that reactively avoids dynamic obstacles when no direct path to the goal is available, mitigating the risk of collisions during deadlock situations. We validate our method extensively in both simulation and real-world hardware experiments, and benchmark it against state-of-the-art approaches, showing significant advantages for reactive UAV navigation in dynamic, unknown environments.

Astik Srivastava, Thomas J Chackenkulam, Bitla Bhanu Teja +2

Robotics Research Center, IIIT Hyderabad, India · IIT-BHU, India

UAV Navigation Safe Motion Planning Motion Planning Dynamic Obstacle Avoidance

Feb 1, 2026

HERMES: A Holistic End-to-End Risk-Aware Multimodal Embodied System with Vision-Language Models for Long-Tail Autonomous Driving

End-to-end autonomous driving models increasingly benefit from large vision-language models for semantic understanding, yet safe and reliable planning under long-tail conditions remains challenging, particularly in mixed-traffic environments involving heterogeneous road users and rare safety-critical interactions. This paper proposes HERMES, a holistic risk-aware end-to-end multimodal driving framework that explicitly incorporates long-tail semantic knowledge into trajectory planning. HERMES employs a foundation-model-assisted annotation pipeline to construct structured Long-Tail Scene Context and Long-Tail Planning Context, capturing hazard-centric scene information, maneuver intent, and risk-aware planning guidance. A Tri-Modal Driving Module then integrates multi-view visual observations, historical ego-motion, and long-tail semantic instructions through intent- and risk-aware conditioning for trajectory generation. Extensive experiments on a large-scale real-world long-tail driving benchmark demonstrate consistent improvements over representative recent baselines in overall planning performance and across diverse safety-critical scenarios. Ablation studies further validate the effectiveness and complementary roles of the major components within HERMES.

Weizhe Tang, Junwei You, Jiaxi Liu +7

Department of Civil and Environmental Engineering, University of Wisconsin–Madison, Madison, WI 53711 USA

Long-Tail Autonomous Driving VLMs for Autonomous Driving End-to-End Autonomous Driving Autonomous Driving Risk-Aware Path Planning Safe Motion Planning+1

Jan 8, 2026

Precomputing Multi-Agent Path Replanning Using Temporal Flexibility

Executing a multi-agent plan can be challenging when an agent is delayed, because this typically creates conflicts with other agents. So, we need to quickly find a new safe plan. Replanning only the delayed agent often does not yield an efficient plan, and sometimes cannot even yield a feasible one. On the other hand, replanning other agents may lead to a cascade of changes and delays, and it is computationally expensive. We show how to efficiently replan a single delayed agent by tracking and using the temporal flexibility of other agents while avoiding cascading delays. This flexibility is the maximum delay that the agent can take without changing the order with agents other than the initially delayed agent, or further delaying other agents. Our algorithm, FlexSIPP, precomputes all possible plans for the delayed agent and returns the changes to the other agents within the given scenario. We demonstrate our method in a real-world case study of replanning trains in the densely-used Dutch railway network and in the MovingAI MAPF benchmark set. Our experiments show that FlexSIPP provides effective solutions relevant to real-world adjustments, and within a reasonable timeframe.

Issa Hanou, Eric Kemmeren, Devin Wild Thomas +1

Delft University of Technology · University of New Hampshire

Safe Interval Path Planning Multi-Agent Path Finding Dynamic Replanning Robust MAPF Safe Motion Planning

Dec 13, 2025

Temporal Cascading of Planning and Control for Quadrotor MPC

Many aerial tasks involving quadrotors demand both instant reactivity and long-horizon planning for obstacle avoidance, energy efficiency, or trajectory tracking. High-fidelity models enable accurate control but are too slow for long horizons. Low-fidelity planners scale but cannot directly control the system, necessitating cascaded architectures. Prevailing hierarchical approaches plan with a simplified model and use a high-fidelity controller for tracking, yet this decomposition is inherently suboptimal. The controller is limited by the coarse plan, and conventional MPC alternatives shorten the horizon to stay real-time feasible. We present UNIQUE, an MPC architecture that replaces this hierarchical stacking with temporal cascading. The planning problem is formulated as the second-tail horizon of a single multi-phase MPC, rather than being solved separately. We align costs across horizons, derive feasibility constraints for the point-mass planning model, and introduce transition constraints that convert high-fidelity states into meaningful low-fidelity states. Parallel point-mass and mixed-integer solvers address nonconvexities while incorporating progressive 3D obstacle smoothing over the planning horizon. In simulations and real flights, under equal computational budgets, UNIQUE improves closed-loop tracking by up to 75% compared with standard MPC and hierarchical baselines. Ablations and Pareto analyses confirm performance gains across variations in horizon, constraint approximations, and smoothing schedules.

Rudolf Reiter, Chao Qin, Leonard Bauersfeld +1

Robotics and Perception Group, Department of Informatics, University of Zurich, Andreasstrasse 15, 8050 Zurich, Switzerland · Institute for Aerospace Studies, University of Toronto, 4925 Dufferin Street, Toronto, ON M3H 5T6, Canada

Robot Trajectory Tracking Aerial Robotics Model Predictive Control UAV Trajectory Optimization Quadrotor Control Safe Motion Planning

Dec 9, 2025

RVC-NMPC: Nonlinear Model Predictive Control with Reciprocal Velocity Constraints for Mutual Collision Avoidance in Agile UAV Flight

This paper presents an approach to mutual collision avoidance based on Nonlinear Model Predictive Control (NMPC) with time-dependent Reciprocal Velocity Constraints (RVCs). Unlike most existing methods, the proposed approach relies solely on observable information about other robots, eliminating the need for excessive communication. The computationally efficient algorithm for computing RVCs, together with the direct integration of these constraints into the NMPC problem formulation at the controller level, allows the whole pipeline to run at 100 Hz. This high processing rate, combined with modeled nonlinear dynamics of the controlled Uncrewed Aerial Vehicles (UAVs), is a key feature that facilitates the use of the proposed approach for agile UAV flight. The proposed approach was evaluated through extensive simulations emulating real-world conditions in scenarios involving up to 10 UAVs and velocities of up to 25 m/s, and in real-world experiments with accelerations up to 30 m/s

^2

. Comparison with the state of the art shows 31% improvement in terms of flight time reduction in challenging scenarios, while maintaining a collision-free navigation in all trials.

Vit Kratky, Robert Penicka, Parakh M. Gupta +2

Department of Cybernetics, Faculty of Electrical Engineering, Czech Technical University in Prague, Czech Republic

Multi-Robot Motion Planning Nonlinear MPC UAV Navigation Model Predictive Control Safe Motion Planning Robot Collision Avoidance

Nov 30, 2025

Constant-Time Planning for Chaining Collision-free Motion to Manipulation Behaviors

Recent progress in contact-rich robotic manipulation has been striking, yet most deployed systems remain confined to simple, scripted routines. One of the barriers is the lack of motion planning algorithms that can provide verifiable guarantees for safety, efficiency and reliability. Constant-Time Motion Planning (CTMP) is a recent step toward such guarantees for collision-free motion in a priori known environments:: a preprocessing phase enables queries to be answered within a fixed, user-specified time budget (e.g., 10 milliseconds). However, CTMP certifies only reachability---a binary predicate---and ignores the manipulation behavior that completes the task, which is increasingly stochastic (e.g., a learned skill) and whose success no single offline rollout can establish, let alone certify. We introduce the Behavioral Constant-Time Motion Planner (B-CTMP), which extends CTMP to two-step manipulation tasks in semi-structured environments: a collision-free motion to a behavior initiation state, followed by execution of a behavior such as grasping or insertion. B-CTMP departs from prior CTMP in two ways: neighborhoods are constructed in object-pose space rather than robot configuration space, and coverage is established by statistical certification rather than a reachability check. A plan is cached only if repeated rollouts lower-bound its success rate above a user-specified threshold, and we prove these bounds hold simultaneously across the entire cache at a prescribed confidence level. For deterministic behaviors a single rollout suffices, recovering the binary check of prior CTMP as a special case. We evaluate B-CTMP on three manipulation tasks---shelf picking, plug insertion, and wheel replacement---in simulation and on real robots. B-CTMP's certified plans succeed consistently where baselines fail during behavior execution, and it rejects infeasible object poses in constant time.

Nayesha Gandotra, Itamar Mishani, Lai Yuan +2

Robotics Institute, School of Computer Science Carnegie Mellon University, United States

Contact-Rich Robotic Manipulation Safe Motion Planning Motion Planning

Nov 5, 2025

Manifold-constrained Hamilton-Jacobi Reachability Learning for Decentralized Multi-Agent Motion Planning

Safe multi-agent motion planning (MAMP) under task-induced constraints is a critical challenge in robotics. Many real-world scenarios require robots to navigate dynamic environments while adhering to manifold constraints imposed by tasks. For example, service robots must carry cups upright while avoiding collisions with humans or other robots. Despite recent advances in decentralized MAMP for high-dimensional systems, incorporating manifold constraints remains difficult. To address this, we propose a manifold-constrained Hamilton-Jacobi reachability (HJR) learning framework for decentralized MAMP. Our method solves HJR problems under manifold constraints to capture task-aware safety conditions, which are then integrated into a decentralized trajectory optimization planner. This enables robots to generate motion plans that are both safe and task-feasible without requiring assumptions about other agents' policies. Our approach generalizes across diverse manifold-constrained tasks and scales effectively to high-dimensional multi-agent manipulation problems. Experiments show that our method outperforms existing constrained motion planners and operates at speeds suitable for real-world applications. Video demonstrations are available at https://youtu.be/RYcEHMnPTH8 .

Qingyi Chen, Ruiqi Ni, Junyoung Kim +1

Department of Computer Science at Purdue University, West Lafayette, IN 47907, USA.

Multi-Robot Motion Planning Constrained Motion Planning Reachability Analysis Decentralized Multi-Agent Motion Planning Safe Motion Planning Motion Planning

Oct 30, 2025

ART-TEB: Adaptive Trajectory Planning for Mobile Robots in Cluttered Environments

Trajectory planning for mobile robots remains a major challenge, particularly in cluttered environments, where existing planning algorithms often fail or produce suboptimal paths. To address this issue, we propose an adaptive trajectory refinement algorithm, ART-TEB, comprising two main stages. First, to ensure safety at the path-segment level, a segment-wise conservative collision test is applied, recursively subdividing risky path segments until the collision risk is eliminated. Second, to guarantee pose-level safety, pose correction based on separation direction and line search is applied, ensuring that each pose in the trajectory is collision-free and maximally clear from obstacles. Simulation results demonstrate that the ART-TEB achieves up to 3.12x higher success rates and up to 23.7x faster average planning times than state-of-the-art approaches. Furthermore, real-world experiments confirm that the robot can safely pass through highly constrained environments while maintaining rapid planning performance.

Hahjin Lee, Young J. Kim

Department of Computer Science and Engineering at Ewha Womans University in Korea

Trajectory Optimization Trajectory Generation Mobile Robotics Safe Motion Planning Robot Motion Planning

Oct 16, 2025

STITCHER: Constrained Trajectory Planning in Complex Environments with Real-Time Motion Primitive Search

Autonomous high-speed navigation through large, complex environments requires real-time generation of agile trajectories that are dynamically feasible, collision-free, and satisfy state or actuator constraints. Modern trajectory planning techniques primarily use numerical optimization, as they enable the systematic computation of high-quality, expressive trajectories that satisfy various constraints. However, stringent requirements on computation time and the risk of numerical instability can limit the use of optimization-based planners in safety-critical scenarios. This work presents an optimization-free planning framework called STITCHER that stitches short trajectory segments together with graph search to compute long-range, expressive, and near-optimal trajectories in real-time. STITCHER outperforms modern optimization-based planners through our innovative planning architecture and several algorithmic developments that make real-time planning possible. Extensive simulation testing is performed to analyze the algorithmic components that make up STITCHER, along with a thorough comparison with three state-of-the-art optimization planners. Simulation tests show that safe trajectories can be created within a few milliseconds for paths that span the entirety of two 50 m x 50 m environments. Hardware tests with a custom quadrotor verify that STITCHER can produce trackable paths in real-time while respecting nonconvex constraints, such as limits on tilt angle and motor forces, with flight speeds up to 63 km/h.

Helene J. Levy, Brett T. Lopez

VECTR Laboratory, University of California, Los Angeles, Los Angeles, CA, USA

Constrained Motion Planning Trajectory Generation UAV Navigation Graph-Based Planning Safe Motion Planning Motion Planning+1