Safe Motion Planning

Follow this topic Ask about this topic

Momentum

21 papers in the last four weeks, up 110% on the four weeks before. 0.2% of all new papers.

Jul 13Week of Sep 28

Latest papers 152

Aug 10, 2026physics.flu-dyn

Wind-Informed Rapid Flight-Planning in Complex Urban Topologies via Machine Learning and Experimental Validation

Advanced air mobility operations hold the potential to enhance and expand regional transportation of both people and goods in populated areas. However, hazardous flight conditions arising from interactions between wind and the built environment remain a significant challenge for aerial vehicles in urban settings. This work proposes a novel framework towards safe flight planning of aerial vehicles in windy urban environments. A learning-based surrogate model is trained to rapidly predict flow fields from readily available information such as building geometry and incident wind. This surrogate prediction is used to calculate a volumetric flight challenge scalar field based on critical flow parameters and proximity to structures. A safe, flow-informed flight trajectory is then identified through a cost-minimizing pathfinder. The complete system is demonstrated experimentally through flight tests of a micro aerial vehicle through a model urban geometry placed in a large fan-array wind tunnel. Comparing this approach to trajectories generated without knowledge of the wind field, we find the flow-informed approach reduces undesired vehicle displacement and improves flight stability. This work is among the first practical demonstrations of safe, wind-aware methodologies for advanced air mobility in urban environments.

Peter I. Renn, Alejandro A. Stefan-Zavala, Julian Humml +8

1: California Institute of Technology, Pasadena, CA, USA · 2: Technology Innovation Institute, Abu Dhabi, UAE

UAV Navigation Safe Motion Planning Motion Planning

Aug 10, 2026cs.RO

Navigating the Proximity-Safety Balance: Constraint Decomposition for Human Following in Pedestrian Crowds

Following a target human in crowded environments involves an inherent conflict between staying close to the target and navigating safely among surrounding pedestrians and obstacles. This conflict becomes more severe in dense scenarios, where aggressive following risks collisions and conservative margins lead to target loss, especially when pedestrian behaviors are unfamiliar or unpredictable. Existing reinforcement learning (RL) methods typically encode these competing objectives into a single dense reward, but the resulting proximity-safety balance is implicit and difficult to adjust across conditions. To address this, we decompose the human-following task into a sparse task reward and independent cost constraints within a multi-constraint RL formulation, where each constraint is managed through cost thresholds with direct behavioral meaning rather than implicit reward weight ratios, allowing explicit and tunable control over the trade-off. We further quantify the prediction uncertainty of human motions and integrate these estimates into the RL costs to enhance safety under unpredictable conditions. Extensive experiments across both in-distribution and out-of-distribution settings demonstrate that our method achieves an effective proximity-safety balance compared to baselines. Real-robot deployment further validates the feasibility of our method in real-world scenarios. More details are available on our project page: https://nav-ps-balance.github.io/.

Shiting Gong, Jianpeng Yao, Jinfeng Wang +2

University of Pennsylvania, PA, USA · University of California, Riverside, CA, USA · 3Stanford University, CA, USA +2

Constrained RL Robotic RL Social Robot Navigation Safe Motion Planning

Aug 10, 2026cs.RO

Energy-Structured Latent World Models with Neural Time Fields for Physically Constistent Open-World Motion Planning

Physically consistent motion planning remains a fundamental challenge in embodied AI, as generated trajectories must strictly conform to real-world execution dynamics. While latent world models offer a promising approach by predicting these dynamics, existing methods learn unconstrained future representations where absorbed physics remains implicit. Therefore, they fail to form reusable physical knowledge, which compromises reliability in unpredictable open-world navigation. To address this, we propose a novel Energy-Structured Latent World Model (ELWM). Our key idea is to structure the ELWM latent state to explicitly carry energy and momentum, ensuring strictly causal transitions via dissipation and control ports. Trained on multimodal RGB-D and inertial interaction histories, our model guarantees physically consistent predictions. We further implement this for motion planning by constructing Physics-Conditioned Neural Time Fields (PC-NTF), a key technical cornerstone that integrates ELWM into an arrival time field via the Eikonal equation to yield a physically-informed navigation policy. Across held-out scenes, our evaluation reveals significant improvements. Compared to generic latent models, PC-NTF reduces 0.8-s motion-prediction NRMSE from 0.36 to 0.29. Against Active Neural Time Fields, it improves navigation success from 81.3% to 89.7% and SPL from 0.64 to 0.73, while cutting the physical collision rate from 12.1% to 5.8% and the Eikonal residual from 0.083 to 0.031. Beyond these targeted gains, our results demonstrate that embedding explicit physical structures into latent spaces intrinsically bridges the gap between predictive world models and safe, dynamically feasible motion planning.

Yapeng Liu, Yuanzhao Zhai, Bo Ding +2

PDL Lab, College of Computer Science and Technology, National University of Defense Technology

World Model Learning World Models World Model-Based Planning Safe Motion Planning Latent World Models Robot Motion Planning

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

Aug 10, 2026cs.RO

Graph-Guided Safe Diffuser: Topological Graph Guidance for Safe Diffusion Planning

Many diffusion-based planners enforce safety through inference-time guidance, but such interleaved trajectory deformations often degrade kinematic feasibility due to manifold rupture. We propose Graph-Guided Safe Diffuser (G2SD), a hierarchical framework that leverages a high-level topological graph planner to guide a low-level diffusion model. G2SD enforces safety at a structural level by abstracting the data manifold into a learned latent graph, on which high-level planning is performed. Continuous trajectories are generated by diffusion planners, which are conditioned on the graph node representations selected by the high-level planner. Theoretical analyses demonstrate conditions under which manifold rupture occurs in diffusion planners, and show that G2SD improves safety by reducing the constraint violation probability as the number of segments increases. Experiments demonstrate that G2SD substantially outperforms baselines, increasing goal-reaching rate without any collision from 40-50% to 98% in Maze2D navigation and also achieving superior task scores in locomotion.

Nakgyu Yang, KwangBin Lee, SooJean Han

School of Electrical Engineering, Korea Advanced Institute of Science and Technology (KAIST) Daejeon, Republic of Korea

Constrained Motion Planning Trajectory Generation Diffusion-Based Planning Graph-Based Planning Safe Motion Planning

Aug 10, 2026cs.RO

Particle-Based Conformal Prediction for Contact-Aware Uncertainty Calibration in Stratified Configuration Spaces

Reliable uncertainty representation is essential for deploying autonomous systems that interact with their environment, as robots must reason about how uncertainty arising from both stochasticity and model mismatch is impacted by contacts with obstacles (e.g., when navigating through a cluttered environment or inserting a part into an assembly). We propose Calibrated Particle-sets for Trans-dimensional Uncertainty Representation (CaPTURe), a geometry-aware, conformal prediction-based algorithm that generates probabilistically valid prediction regions of the unknown future system configuration using particle-based models of arbitrary fidelity. While calibrated uncertainty predictions are essential for safe and efficient planning, analytical or learned motion models are often inaccurate - due to limited data, simplifying assumptions, unmodeled effects, etc. - which can lead to unsafe executions or task failure. Additionally, when a robot contacts an obstacle, the distribution of its future configurations can become multimodal or disjoint, or lie along manifolds of lower intrinsic dimension than the space of possible robot configurations. Our method uses a calibration dataset of system transitions to locally calibrate motion uncertainty estimates, constructing regions guaranteed to contain the future robot configuration at a user-set probability. Our calibration procedure captures how motion uncertainty varies between contact-rich and contactless motions, leading to sufficient coverage in both cases. We evaluate our method on two simulated planning tasks: controlling a marble around a labyrinth and performing tight-tolerance peg-in-hole insertion with a manipulator. Compared to relevant baselines, CaPTURe achieves the user-specified coverage requirement both in and out of contact and achieves up to a 30% absolute improvement in task success rate over the best baseline.

Luís Marques, Kristian Popov, Dmitry Berenson

Department of Robotics, University of Michigan, Ann Arbor, MI, 48109, USA · Department of Aerospace Engineering, University of Michigan, Ann Arbor, MI, 48109, USA

Contact-Rich Robotic Manipulation Conformal Prediction Safe Motion Planning Robot Motion Planning Uncertainty Calibration

Aug 9, 2026cs.RO

Hierarchical Topology-Aware Planning and Control of Underwater Vehicle-Manipulator Systems in Confined Environments

This paper addresses autonomous intervention with an underwater vehicle--manipulator system (UVMS) in confined, cluttered, and partially known environments, where poor maneuverability, narrow passages, and uncertain execution may cause the robot to enter unrecoverable regions. We propose MANTA, a three-layer hierarchical planning-and-control framework that couples passage accessibility, manipulation feasibility, and closed-loop execution. The first layer performs global connectivity reasoning in a conservative reduced base space to extract traversable corridor candidates toward the task region. The second layer refines each candidate corridor by jointly optimizing the continuous base motion and arm trajectory, producing a collision-free base--arm trajectory. The third layer learns a reach-and-hold base policy using Gaussian-process model-based reinforcement learning (MBRL) through MC-PILCO, enabling trajectory tracking and station keeping at the planned manipulation state. During execution, the framework monitors map updates and can trigger recovery and route repair when the active passage becomes infeasible. MANTA is evaluated in confined UVMS planning and closed-loop tracking experiments. Across 120 matched planning queries, it achieves higher task success than full-state sampling-based baselines while producing larger clearance margins and lower arm motion. The learned MC-PILCO policy further reduces position and yaw tracking errors on both training and unseen tube-like references. These results show MANTA as a structured and data-efficient framework for safe autonomous underwater intervention in caves, tubes, and cluttered subsea structures.

Mohamed Abdelwahab, Ruggero Carli, Damiano Varagnolo +1

Department of Information Engineering, University of Padova, Via Gradenigo 6/B, 35131 Padova, Italy · Department of Electrical and Information Engineering, Polytechnic University of Bari, Via Orabona 4, 70125 Bari, Italy · Department of Engineering Cybernetics, Norwegian University of Science and Technology, O. S. Bragstads plass 2D, 7034 Trondheim, Norway

Underwater Robotics Safe Motion Planning Motion Planning Autonomous Underwater Navigation Robot Motion Planning

Aug 8, 2026cs.RO

MPPI Planning with Gaussian-Based Human Cost Function for Social Navigation

Safe robot navigation in crowded spaces requires planning that accounts for where people will be, not only where they are now. Model Predictive Path Integral (MPPI) control is an effective sampling-based planner, but many implementations encode humans as static point obstacles at their current positions, underestimating risk in dynamic scenes. We propose Predictive Gaussian Interaction Fields (PGIF), a spatiotemporal cost formulation that propagates pedestrian predictions forward over the full planning horizon and encodes them as anisotropic Gaussian repulsive fields aligned with each pedestrian's direction of motion. The forward spread of each field grows with the pedestrian's speed, creating a motion cone danger zone that penalises robot trajectories entering the pedestrian's path of travel more strongly than those approaching from behind. The formulation is closed-form and fully parallelisable across rollouts, adding no measurable computational overhead. Evaluated over 300 randomised crowd scenarios at three density levels, PGIF-MPPI achieves a 0% collision rate at every density level, compared with up to 82% for vanilla MPPI, while maintaining real-time planning performance.

Chinmay Mundane

VJTI, India

Social Robot Navigation Safe Motion Planning MPPI Control Sampling-Based Motion Planning

Aug 7, 2026cs.RO

CoCoNav: Conformal Control for Safe Robot Navigation in Crowds

Safe and efficient robot navigation in crowds requires anticipating pedestrian motion despite uncertain and potentially shifting prediction errors. Existing reactive methods can produce oscillatory behavior, while predictive planners often treat forecasts as exact or rely on restrictive error models. Incorporating conservative uncertainty sets as hard constraints can also render model predictive control (MPC) infeasible. We propose \textit{CoCoNav}, a crowd-navigation framework that combines online conformal calibration with runtime-certified planning. A horizon-specific conformal proportional--integral controller adapts trajectory-error bounds to regulate long-run empirical coverage, enabling the framework to respond to changing prediction errors. A \textit{relax-then-verify} planner preserves solver feasibility by generating nominal trajectories with soft-constrained MPC and separately certifying them, together with contingency maneuvers, against the calibrated bounds before execution. Simulations and quadruped experiments show that CoCoNav achieves a favorable balance among collision avoidance, task success, and navigation efficiency relative to the evaluated baselines.

Cheng Guo, Mingzhe Ni, Zheng Liang +5

Department of Computer Science, The University of Manchester, Manchester, UK. · Human-Robot Interfaces and Interaction Laboratory, Italian Institute of Technology, Genoa, Italy. · Genisom AI, Shanghai, China. +3

Constrained Motion Planning Conformal Prediction Safe Robot Navigation Social Robot Navigation Robot Safety Model Predictive Control+1

Aug 5, 2026cs.RO

Failing Gracefully: Mitigating Impact of Inevitable Robot Failures

Service robots operate in household environments shared with humans, pets, and everyday objects, where they are highly susceptible to failures such as software crashes, hardware degradation, or unpredictable interactions. While roboticists strive to minimize failures, some remain inevitable, making it critical to mitigate their potential consequences for safe and reliable deployment. This paper introduces a novel safety formulation that evaluates both the probability of impactful interactions between robots and surrounding entities during failures, and the severity of their outcomes. By quantifying the impact of failures on different entities, our approach enables robots to make informed planning decisions that balance safety with task efficiency. To support systematic evaluation, we also present FailBench, a MuJoCo-based simulation framework for studying robot-environment interactions under diverse failure modes, including sensing issues and actuator malfunctions. Together, our safety formulation and FailBench provide a foundation for developing safer and more robust motion plans and learned policies in real-world household environments.

Duc M. Nguyen, Saad A. Ghani, Andrew Marshall +3

Department of Computer Science, George Mason University · RobotiXX Lab, George Mason University

Robotics Simulation Robotics Robot Task Planning Safe Motion Planning

Aug 5, 2026cs.RO

GASP: GPU-Accelerated Safe Planner for Real-Time Collision-Aware Motion Generation with Latent Trajectory Sampling

We present GASP, a GPU-Accelerated Safe Planner for real-time, collision-aware joint-space motion generation in known environments. GASP combines a clamped B-spline trajectory parameterization with a convolutional residual neural network that predicts the free interior control points, while analytically inserted boundary control points enforce initial and final derivative constraints for collision-aware planning under non-stationary conditions. A conditional variational autoencoder samples multiple trajectory candidates, which are decoded and validated in parallel on the GPU, yielding a batched planner for collision-aware coupled joint-space motion with near-millisecond inference. We validate GASP as an online motion-generation module, where it achieves analytical-level success rates with high collision-aware feasibility and substantially reduces inference time relative to GPU-based trajectory optimization. We further deploy GASP as a reinforcement-learning reset planner in competitive robotic table tennis, matching the baseline return rate while roughly halving training-time collisions.

Colin Merk, Stefanos Charalambous, Peter Dürr +1

Sony, Zurich, Switzerland.

Constrained Motion Planning Trajectory Generation Robot Motion Generation Safe Motion Planning Robot Motion Planning Robot Collision Avoidance

Aug 4, 2026cs.RO

Flying over The Uncertain Nature (FORTUNE): Intelligent and Humanistic 3D Path Planning for Low-Altitude Collaboration

The proliferation of low-altitude intelligent agents is increasing the demand for timely and socially responsible collaborative sensing in dynamic urban environments. However, jointly addressing heterogeneous spatiotemporal demands, environmental uncertainty, and human-centered operational constraints remains challenging. This paper studies 3D multi-UAV path planning and task assignment under uncertain ground PoI demands. Unlike existing work assuming static and fully known PoIs, we model persistent, temporally predictable, and emergent demands within a unified framework. We further incorporate altitude-dependent societal and environmental costs, including noise exposure and public safety risks, to balance sensing performance with socially compliant operations. To solve the resulting large-scale mixed-integer nonlinear problem, we propose FORTUNE, a hierarchical offline-online framework. Offline, a Transformer predicts Type-II PoI activation windows, while an enhanced sparrow search algorithm generates coordinated flight plans through priority-aware decoding and danger-aware evolution. Online, a lightweight refinement module accommodates emerging Type-III PoIs while preserving global mission coherence. Experiments on real-world traffic data and synthetic scenarios show that FORTUNE consistently outperforms state-of-the-art methods in effectiveness, scalability, and practical applicability.

Minghui Liwang, Wenhan Jia, Xinlei Yi +3

Multi-Robot Motion Planning Safe Motion Planning Multi-Robot Task Allocation Motion Planning

Aug 4, 2026cs.RO

Accelerating Human-Aware Robot Trajectory Generation via Diffusion and Consistency Distillation

This research proposes a constrained motion planning framework for robot manipulators in human-robot interaction (HRI). For a non-redundant manipulator with a fully specified end-effector pose, additional requirements such as collision avoidance and self-collision avoidance are difficult to handle as simple null-space secondary tasks. This limitation makes it challenging to generate feasible joint-space trajectories in HRI environments where safety and kinematic constraints must be considered simultaneously. To address this limitation, collision- and self-collision-aware trajectories are generated using Rapidly-exploring Random Tree (RRT) and RRT* algorithms, and the resulting dataset is used to train a diffusion model that generates constraint-satisfying trajectories through guided sampling. To reduce the inference time required for iterative diffusion sampling, consistency distillation is applied, and a joint-weighted jerk regularization term is incorporated into the loss function to promote smoother trajectories by penalizing abrupt changes in joint acceleration. Simulation results show that the consistency model generates 150 trajectory candidates in less than 100 ms, maintains a high episode success rate, and substantially reduces joint and end-effector jerk when jerk regularization is applied.

Byeong-Il Ham, Hyun-Bin Kim, Kyung-Soo Kim

Constrained Motion Planning Trajectory Generation Diffusion-Based Planning Robot Motion Generation Safe Motion Planning Robot Collision Avoidance

Aug 1, 2026cs.RO

StochSIPP: Safe Interval Path Planning in Stochastic Dynamic Environments

Safe navigation under uncertain time-dependent blockage requires anticipating observations before committing to motion. We present StochSIPP, an exact contingent planner for temporal roadmaps with uncertain edge and vertex statuses revealed locally during execution. StochSIPP uses SIPP to generate certified-safe macro-actions that terminate at the next observation or the goal, and bounded AND/OR search over a cached action--observation graph to select actions for every reachable observation outcome. Optimistic and robust SIPP relaxations provide admissible lower and upper bounds for bounded AND/OR search. When every interval declared deterministically safe is truly safe, sensing is exact, and execution follows the planned timing, the resulting policy is provably collision-free. With correct independent probabilities and complete action and outcome generation, it minimizes expected arrival time within the roadmap and horizon. Experiments on controlled roadmap instances show that StochSIPP preserves the observed success of safe fixed-path baselines while reducing arrival time, and solves gated scenarios in which conservative fixed-path planners return no plan. A scalability study further reveals rapid growth as the number of simultaneously observed uncertain statuses increases.

Ajith Kemisetti, Shahaf S. Shperberg, Yoonchang Sung

The University of Texas at Austin · Ben Gurion University of the Negev · Nanyang Technological University

Roadmap-Based Motion Planning Safe Motion Planning Motion Planning

Aug 1, 2026cs.RO

Learning-Based Motion Planning for Dynamic Environments: From Foundational Algorithms to Emerging Paradigms

Motion planning in dynamic environments is a fundamental problem in robotics, aiming to generate safe and efficient paths, trajectories, or control actions in the presence of moving obstacles, uncertain predictions, and multi-agent interactions. It has broad applications in autonomous driving, service robotics, warehouse logistics, human-robot collaboration, crowd navigation, and multi-robot systems. This survey reviews representative works published primarily between 2015 and 2025, with a particular focus on how recent learning-based advances extend, complement, or interact with classical planning foundations. We first revisit classical planning methods as algorithmic foundations and reference frameworks for learning-based extensions. We then propose a role-of-learning taxonomy that categorizes existing methods according to how learning participates in the planning pipeline, including direct policy learning, learning-augmented classical planning, hybrid planning, and training enhancement methods. For each category, we summarize the main problem settings, representative algorithms, key ideas, integration mechanisms, strengths, and limitations. We further analyze how observation representations, prediction uncertainty, interaction modeling, planner integration, safety constraints, and training strategies shape learning-based motion planning in dynamic environments. Finally, we discuss open challenges and future directions, including sim-to-real gap, safe and certifiable planning, dense crowd navigation, perception-planning coupling, and embodied AI.

Zongyuan Shen, Shalabh Gupta, Shancheng Zhao +7

College of Information Science and Technology, Jinan University, Guangzhou 510632, China. · Department of Electrical and Computer Engineering, University of Connecticut, Storrs, CT 06269, USA. · Guangzhou Maritime University, Guangzhou 510725, China. +4

Robot Policy Learning Safe Motion Planning Motion Planning Dynamic Obstacle Avoidance

Jul 31, 2026cs.RO

Safe Vision Language Action Models via Barrier Enhanced Flow Matching

This article presents a modular inference framework that integrates Flow Matching generative models with formal Control Barrier Function (CBF) safety guarantees. Unlike existing methods that apply external safety filters to a model's final output, our approach modifies the Flow Matching denoising process within the model to inherently generate safe trajectories. By employing a smooth Log-Sum-Exponential aggregate barrier, we enforce safety over entire action chunks. This aggregate barrier ensures a minimal increase in computational overhead and does not alter the semantic intent of the model. We show that, within the proposed framework, the 2-Wasserstein distance between the generated distribution and the target distribution remains bounded. Our method eliminates the need for safety-specific datasets or costly model retraining, providing a versatile solution for safe inference. We validate the approach on two robotic manipulation platforms and a 2D navigation benchmark, verifying that our framework achieves reliable safety without degrading the success rate of the model.

Kasra Sinaei, Hung-Chieh Wu, Donald Ebeigbe

The authors are with the Department of Electrical Engineering, The Pennsylvania State University

Flow Matching Control Barrier Functions Vision-Language-Action Models Safe Motion Planning

Jul 31, 2026cs.RO

Homotopy-Aware Corridor Generation without Predefined Reference Paths

Generating safe corridors is essential for collision-free robotic motion planning, yet most existing methods rely on predefined reference paths, which bias corridor geometry and implicitly limit the homotopy classes that can be explored. We propose a reference-path-free corridor generation framework on graphs of convex sets (GCS) that constructs corridors directly as sequences of convex sets, allowing corridor structure to emerge from the free-space representation rather than from a guiding path. To reason about similarity among corridors, we extend visibility-based deformation from paths to convex-set sequences, enabling the fusion of topologically redundant corridors while preserving distinct alternatives. To overcome the limited adaptability of existing GCS methods based on static global decompositions, we further develop an adaptive multi-scale GCS, in which a sampling-based fine-scale graph supports localized updates and a visibility-based coarse-scale graph enables compact global exploration. The two levels maintain topological consistency, allowing incremental updates without full graph reconstruction under environmental uncertainty. Numerical experiments characterize GCS construction, corridor generation, homotopy-aware exploration, and local updates, showing efficient graph construction, stable trajectory-level performance, and shorter-duration homotopy-aware trajectories than existing baselines. Hardware experiments on ground and aerial robots, including deployment with onboard localization, further validate the framework under translated and previously unknown obstacles.

Haoze Dong, Minghan Li, Meng Guo +1

School of Advanced Manufacturing and Robotics, Peking University, Beijing 100871, China

Roadmap-Based Motion Planning Safe Motion Planning Motion Planning Robot Motion Planning

Jul 29, 2026cs.RO

Risk-Aware Motion Planning with Learned Trajectory Primitives and Probabilistic Safety Assessment

This paper presents a radial basis function network (RBFN)-informed motion planning framework for safe and efficient urban autonomous driving. The proposed approach combines RBFN-based candidate trajectory generation with an analytic collision probability assessment and optimization-based trajectory refinement. The network learns jerk-minimal trajectories, enabling the MPC to operate within a reduced and dynamically consistent search space. Candidate motion primitives are selected based on an accurate probabilistic risk measure. This design decreases solver complexity while preserving safety and constraint satisfaction. The framework is evaluated in numerous urban driving scenarios. Results demonstrate improved risk awareness and fewer vehicle-limit violations compared to benchmark methods. The proposed approach integrates learning-based trajectories into optimization-based motion planning, thereby ensuring safety and interpretability.

Marc Kaufeld, Dian Zhuang, Johannes Betz

Professorship of Autonomous Vehicle Systems, Technical University of Munich, 85748 Garching, Germany; Munich Institute of Robotics and Machine Intelligence (MIRMI) · School of Engineering and Design, Technical University of Munich, 85748 Garching, Germany

Collision Avoidance Constrained Motion Planning Trajectory Optimization Trajectory Generation Safe Motion Planning Motion Planning+2

Jul 28, 2026cs.RO

SGTP: Sampling-based Game-Theoretic Planning for Real-Time Multi-Vehicle Autonomous Racing

Autonomous multi-vehicle racing requires real-time planning of diverse competitive behaviors in intense interactions. Existing planners often struggle to balance strategic diversity and computational efficiency. To address this challenge, we propose Sampling-based Game-Theoretic Planning (SGTP), a real-time framework that combines game-theoretic reasoning with GPU-accelerated sampling of control sequences and dynamics rollouts. Sampled trajectories are ranked using a game-aware cost to capture competitive interactions and generate diverse racing behaviors. Our planner then performs feasibility selection by explicitly enforcing track-boundary and dynamic collision-avoidance constraints, ensuring safe and reliable transitions between racing strategies. Extensive simulations on challenging tracks show that SGTP achieves a 95.24% win rate and a 99.35% task-completion ratio in highly interactive races, with a mean computational time of 0.095 s over multiple iterative solving steps. We also demonstrate the successful application of SGTP in large-scale scenarios with up to 10 agents. We release our code and provide an open-source benchmark of multi-agent autonomous racing algorithms to facilitate future research. Project page: https://sgtp-racing.github.io/.

Zhouheng Li, Fangguo Zhao, Mattia Piccinini +6

Zhejiang University, Hangzhou, China. · Technical University of Munich, Garching, Germany. · Nanyang Technological University, Singapore.

Autonomous Racing Safe Motion Planning Motion Planning Multi-Agent Planning Sampling-Based Motion Planning

Jul 24, 2026cs.RO

Conformal Constraint Tightening for Chance-Constrained Motion Planning with Unknown Dynamics

Motion planning algorithms compute control sequences that drive autonomous robots to goal regions while avoiding unsafe states. Existing methods, from sampling-based planning to deep reinforcement learning, typically provide task-completion guarantees only with respect to a nominal model or simulator, which may be invalidated when the true dynamics are unknown or difficult to model accurately. This letter addresses this limitation for systems with unknown dynamics and an available approximate nominal model, contributing a planner-agnostic constraint-tightening procedure that equips existing planners with a probabilistic task-completion guarantee on the true system. We leverage conformal prediction to provide a probabilistic bound on the nominal-to-true trajectory deviation over a distribution of planning problems. We tighten the planning constraints using that bound, and show that solving the tightened problem under the nominal model is a sufficient condition for solving the original problem on the true system with a prescribed probability. We validate the theoretical guarantees empirically and demonstrate substantially improved task completion relative to nominal-model planning.

Shubham Natraj, Bruno Sinopoli, Yiannis Kantaros

Department of Electrical and Systems Engineering, Washington University in St. Louis, 1 Brookings Dr, St. Louis, MO 63130, USA · School of Electrical, Computer and Energy Engineering, Arizona State University, 650 E. Tyler Mall, Tempe, AZ 85281, USA

Constrained Motion Planning Safe Motion Planning Motion Planning

Jul 24, 2026cs.RO

Safe Learning Predictive Control for Ego-World Robotic Systems

Safe autonomous navigation in shared environments requires the ability to anticipate and react to the latent behaviors of surrounding robots. In this paper, we propose SOWL-MPC, a safe learning-based predictive control strategy for a novel scenario, which we name ego-world robotic framework. In this setting, the control policy of the world robot is unknown and the ego exploits data to learn it and perform safe maneuvers. The proposed architecture combines an online learning mechanism based on Sparse Variational Gaussian Processes (SVGPs) with a receding-horizon control scheme. Relying solely on noisy state measurements, our approach infers a posterior distribution over the latent world policy, which is updated on streaming data via Online Variational Conditioning (OVC). The learned policy is propagated through the nonlinear world dynamics using an approximate moment propagation scheme, and fed to an uncertainty-aware Model Predictive Control (MPC), thus enabling safe maneuvering of the ego robot. The real-time feasibility and safety guarantees of SOWL-MPC are demonstrated through extensive Monte Carlo virtual experiments in ROS 2, and validated on real-world robotic hardware in an indoor arena.

Davide Valenti, Giuseppe Notarstefano

Department of Electrical, Electronic and Information Engineering, University of Bologna, Bologna, Italy

Multi-Agent Control Safe Robot Navigation Robot Safety Model Predictive Control Safe Motion Planning

Jul 23, 2026cs.RO

A Real-Time Generalized Nash Equilibrium Framework for Interaction-Aware Autonomous Driving in Mixed Traffic

Safe and efficient navigation in mixed-traffic environments remains a critical challenge for Autonomous Vehicles (AVs), primarily due to the complex interdependence between the AV's decisions and the unpredictable reactions of human drivers. This paper introduces a comprehensive decision-making framework that formulates the driving interaction as a Generalized Nash Equilibrium Problem (GNEP). Unlike decoupled optimization approaches, this framework explicitly models shared safety and geometric constraints, ensuring that the feasibility of the AV's strategy is dynamically linked to the opponent's actions. To solve this non-convex problem in real-time, we propose a dedicated solver based on Particle Swarm Optimization (PSO). The complete architecture was validated on a test track using a real autonomous Renault Zoé interacting with a human driver. Experimental results demonstrate the system's ability to handle critical scenarios by generating comfortable, human-like trajectories. Benchmarks confirm the solver's operational feasibility, achieving convergence in under 50 ms.

Nouhed Naidja, Mohamed-Cherif Rahal, Steve Pechberti +3

L2S · Institut VEDECOM, Versailles, France · Laboratoire des Signaux et Syst`emes (L2S) +1

Game Theory Autonomous Driving Safe Motion Planning Autonomous Driving Planning

Jul 21, 2026cs.RO

From Distances to Trajectories: Real-Time Signed Distance Function Mapping and Distance-Accelerated Motion Planning for UAVs

Autonomous flight in cluttered environments requires a robot to build a geometric map of its surroundings and plan safe, dynamically feasible trajectories, all onboard and in real time. Conventional approaches treat mapping and planning as separate stages and often rely on binary occupancy for collision checking. We argue that these two stages should be co-designed around a single representation: a signed distance function (SDF). By encoding distance to the nearest obstacle, an SDF provides richer information for planning and trajectory optimization than occupancy alone. We develop an Octree REsidual Network (OREN) that pairs an explicit octree prior with an implicit neural residual to reconstruct SDFs online from point cloud observations with the efficiency of volumetric methods and the accuracy and differentiability of neural methods. In tandem, we develop Bubble

^\star

, a search-based planner that exploits the distance information to grow maximal collision-free balls, which we call bubbles, with formal guarantees of termination, completeness, and failure detection. Planning over a graph of bubbles significantly reduces collision checks compared to a grid-based A

^\star

search and returns a bubble sequence that forms a safe corridor for trajectory optimization. We demonstrate the integrated OREN-Bubble

^\star

approach onboard a quadrotor, navigating unseen indoor environments in real time under tight compute constraints. OREN improves SDF estimation by

22

% compared to baselines, while Bubble

^\star

finds trajectories spanning

\approx 90

m through a cluttered environment in

1

-

3

sec., whereas baselines take up to

10

sec. in the same environment.

Jason Stanley, Zhirui Dai, Qihao Qian +6

UC San Diego, La Jolla, USA · Electrical and Computer Engineering Department, UC San Diego, 9500 Gilman Dr, La Jolla, 92093, CA, USA. · Shield AI, San Diego, USA +1

Trajectory Generation Signed Distance Functions UAV Navigation UAV Trajectory Optimization Safe Motion Planning Motion Planning+1

Jul 21, 2026cs.RO

Pose-Parameterized Motion Planning and CBF-QP Self-Collision Filtering for a Long-Reach Drilling Boom

Long-reach drilling booms must reach successive poses without self-collision. Moving from operator-supervised control toward autonomy requires collision-aware motion planning and execution. For the Sandvik SB60, this study adapts established methods by integrating pose-parameterized planning with a capsule-based control barrier function quadratic program (CBF-QP) in measured-state inverse kinematics (IK). A fixed task-specific parameter set within each task generates waypoints, detours, timed references, and chained motion without target-specific retuning. The offline detour planner screens candidate waypoints using 23 selected rod-segment-to-body-region distances, whereas the online CBF-QP filters joint velocities using 14 configured capsule-pair constraints from a nine-primitive whole-body capsule model. Evaluation considers two drilling tasks in a manufacturer-developed SB60 Simscape Multibody model: a five-target restricted-orientation tour and a three-target full-pose tour. Across several hundred thousand samples, the method produced zero IK failures, generated several detour waypoints, achieved millimetre-level mean final-position error, and recorded no sampled CBF margins below the reported thresholds.

Mehdi Heydari Shahna, Tuomo Kivelä, Jouni Mattila

Faculty of Engineering and Natural Sciences, Tampere University, 33720 Tampere, Finland. · Sandvik Mining and Construction Oy, Tampere, Finland.

Constrained Motion Planning Control Barrier Functions Robotic Tool Use Safe Motion Planning Robot Motion Planning Robot Collision Avoidance

Jul 20, 2026cs.RO

Beyond Fixed Goal Delivery: Online POMDP Planning for Target Interception in Crowds

Target interception in crowded environments requires reaching a moving objective while navigating among multiple uncertain human agents. Since human navigation intent is not directly observable, the robot must reason over multiple possible future interaction outcomes. We formulate interception in crowds as a partially observable Markov decision process and solve it online using tree search under a fixed computational budget. In this setting, the action-space structure directly shapes the search tree and how computational effort is allocated. We perform a controlled comparison between a sequential path-speed planner, which first plans a spatial path and then modulates speed along it, and a unified planner that jointly branches over steering and speed within tree search. Across simulations with up to 200 humans, both approaches perform similarly at low crowd density but diverge sharply as density increases. At the highest crowd density, the sequential planner has a safe-interception rate 31 percentage points lower and requires 44% more time than the unified steering-speed planner, revealing a structural limitation of spatial restriction. Project webpage: https://tic-planning.github.io/

Himanshu Gupta, Kelvin Aladum, Nisar Ahmed +2

Dept. of Aerospace Engineering Sciences, University of Colorado Boulder, USA · Dept. of Computer Science, University of Colorado Boulder, USA

Robot Navigation Safe Motion Planning Partially Observable Markov Decision Processes

Jul 18, 2026cs.RO

Token-Wise Latent Streaming from Slow Reasoners to Fast Planners for Dynamic Vision Language Navigation

Vision-Language Navigation in dynamic, human-centric environments exposes a fundamental tension: linguistic reasoning is slow and deliberative, whereas safe, socially compliant planning should be instant and reactive. The resulting observation staleness is safety-critical: a maneuver chosen during inference can already be unsafe by the time it executes. We observe that, long before a VLM finishes its inference, its intermediate hidden states already encode action-relevant intent. We propose SPARK-VLN, a dual-system framework for dynamic social VLN that streams the slow VLM reasoner's knowledge to a fast flow-matching expert planner throughout token generation, providing fresh and evolving guidance during inference. This design is realized by three modules: a Token-Wise Hidden Streamer that extracts intermediate hidden states along the token generation process, a Sequence-to-Slot Latent Bridge that projects them into fixed-size latent slots, and an Evolving Latent Conditioner that infuses them into the expert planner. We also introduce a human-centric benchmark suite for dynamic social vision-language navigation that keeps pedestrians and the robot active throughout inference and reports navigation success, social compliance, human collisions, and explicit staleness statistics. Across these settings, SPARK-VLN mproves navigation success and social compliance while sustaining inference efficiency. Webpage: https://hutslib.github.io/SPARK-VLN/.

Tianshuai Hu, Yangyi Zhong, Zeying Gong +7

The Hong Kong University of Science and Technology. · The Hong Kong University of Science and Technology (Guangzhou). · National University of Singapore. +1

Vision-Language Navigation Social Navigation Safe Motion Planning VLMs for Robotics

Jul 17, 2026cs.RO

RAVEN: Reinforcement-Adaptive Visibility-Graph Planning for Robust Humanoid Navigation with Collision-Free MPC

Humanoid navigation in dynamic environments requires long-horizon planning while respecting short-horizon dynamic and safety constraints. Classical visibility-graph planners combined with model predictive control (MPC) can efficiently generate collision-free trajectories, but their performance depends on manually tuned parameters and accurate system modeling. In real robotic systems, control delays, state-estimation noise, and locomotion uncertainties can cause overshoot and constraint violations even when the nominal path is geometrically optimal. We propose RAVEN, a hierarchical reinforcement learning (RL)-MPC framework for robust humanoid navigation. Unlike prior approaches that use learning to tune cost weights or replace planning entirely, RAVEN employs RL to adapt the geometric construction of a visibility-graph planner by modifying obstacle inflation and related graph parameters. By directly reshaping the free-space geometry, the learned planner alters the topology of the global path to compensate for delay and tracking imperfections. A collision-free MPC layer then tracks the planned trajectory while explicitly enforcing velocity bounds and obstacle-avoidance constraints. By training under realistic delays and observation noise, RAVEN learns planning adaptations that improve robustness while retaining explicit long-horizon geometric planning and constrained optimization, in contrast to end-to-end learning approaches. We evaluate RAVEN against a manually tuned visibility-graph MPC baseline and a pure RL navigation policy. Results demonstrate reduced overshoot near obstacles, improved robustness in narrow passages, and more reliable navigation under delay and noise. These findings indicate that reinforcement-adaptive graph construction combined with constrained MPC provides an effective and interpretable alternative to end-to-end learning for robust humanoid navigation.

Ruochen Hou, Shiqi Wang, Beom Jun Kim +3

Robotics and Mechanisms Laboratory (RoMeLa), Department of Mechanical and Aerospace Engineering, University of California, Los Angeles, CA 90095, USA.

Reinforcement Learning Robot Navigation Graph-Based Planning Safe Motion Planning

Jul 16, 2026cs.RO

Interventional Causal Circuits for Safe Robot Action Testing and Failure Recovery

Safe physical AI for robot actions are required not only likely to succeed but tested to be safe before execution. In practice, however, formal testing of motion parameters is computationally expensive, and the cost scales poorly with the dimensionality of the action space. When a proposed action is rejected by a tester, the naive response is to resample blindly until a passing candidate is found. This is wasteful, uninformative, and offers no convergence. We argue that rejection should instead trigger causal diagnosis: a principled identification of which action parameter caused the failure and what corrective value maximises the probability of passing testing under the interventional probability distribution. We propose a closed-loop framework that couples a Joint Probability Tree (JPT) with a Causal Circuit derived from a Marginal-Deterministic Variable Tree, enabling exact polytime computation without retraining, or additional data collection. The framework validates tractability of all interventional queries before the robot begins operating, and out-of-support candidates are detected and excluded from correction automatically. We perform experiments in a ROS2 simulation environment, and the framework demonstrates complementary roles across quality of distribution: under a high-quality JPT, the Causal Circuit reduces failed attempts by 10.3% and under a degraded JPT, it reduces total failed attempts by 37%. Every rejected plan produces a structured, interpretable causal report naming the primary cause variable, its observed value, and the recommended corrective region, supporting operator oversight and autonomous recovery without a separately trained failure model.

Naren Vasantakumaar, Tom Schierenbeck, Michael Beetz

Institute for Artificial Intelligence, University of Bremen

Robot Failure Detection Robot Failure Recovery Robot Safety Safe Motion Planning Causal Interpretability Causal Intervention+1

Jul 16, 2026cs.RO

Motion Planning with Model-Based Diffusion via Constraint Optimization and Adaptive Scheduling

Single-Robot Motion Planning (SRMP) in highly non-convex constrained environments, where robots must satisfy collision-free guarantees, dynamic feasibility, and task-related constraints, is challenging under complex constraints and computational limits. Recent Model-Based Diffusion (MBD) approaches recast the SRMP as trajectory optimization that samples from a posterior over trajectories, using known dynamics, and analytically estimates the score function from rollout samples to guide diffusion denoising toward a low-cost, clean trajectory without demonstration learning. While existing works further adapt MBD to constrained environments and showcase promising performance, they are still limited by (1) enforcing safety either via soft feasibility diffusion priors or hard projection operators, but lack a unified framework to integrate both, and (2) fixing safety enforcement to neglect the changing of diffusion scheduling. Therefore, we introduce Model-Based Diffusion via Constraint Optimization and Adaptive Scheduling (MD-COAS) for SRMP that unifies the inexact Augmented Lagrangian Method (iALM) soft diffusion prior with a Convex Feasible Set (CFS)-based hard projection operator, and adaptively schedules and co-optimizes safety enforcement, along with diffusion scheduling. Experiments demonstrate that our method achieves higher safety & success rates, faster convergence, and lower final costs than baseline planners on randomly generated highly non-convex 2D benchmarks and a 7-DoF robot arm avoidance task.

Zhilin He, Bowei Li, Jianlin Dou +2

Carnegie Mellon University · University of Pennsylvania

Constrained Motion Planning Trajectory Optimization Trajectory Generation Diffusion-Based Planning Model-Based Planning Safe Motion Planning+2

Jul 14, 2026cs.RO

Model-Based Diffusion Optimal Control for Multi-Robot Motion Planning

Multi-Robot Motion Planning in continuous environments, where robots must generate dynamically feasible, collision-free trajectories, is challenging due to the combinatorial growth of the joint trajectory space and the difficulty of enforcing dynamic feasibility and hard safety constraints. Recent approaches recast trajectory planning as probabilistic inference, sampling from a posterior over trajectories using diffusion models whose score functions are learned from demonstration data. While showing promising performance, these approaches are limited: they often rely on sizable demonstration datasets and struggle to rigorously enforce dynamics and hard safety constraints during sampling. To this end, we introduce Model-Based Diffusion Optimal Control (MDOC), a model-based diffusion planner that efficiently produces dynamically feasible trajectories without relying on data. Crucially, we show that MDOC's safety mechanism -- combining known dynamics models with Control Barrier Function-constrained projections -- naturally scales to multi-robot planning settings through Conflict-Based Search. Across simulation experiments, this integrated method consistently outperforms representative baseline planners in sample efficiency, geometric smoothness, and success rate, while reducing computation time and producing collision-free trajectories.

Zhilin He, Yorai Shaoul, Jiaoyang Li

Carnegie Mellon University, Pittsburgh, PA, USA

Multi-Robot Motion Planning Constrained Motion Planning Trajectory Generation Diffusion-Based Planning Conflict-Based Search Model-Based Planning+2