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

Oct 7, 2026cs.AI

MeshSIPP: Efficient Lattice Planning in Dynamic Environment

Autonomous navigation in dynamic environments requires computing spatiotemporal trajectories that satisfy non-holonomic motion constraints. When the trajectories of the moving obstacles are predictable or known, a promising approach is to rely on the combination of state lattices constructed from precomputed feasible motion primitives and Safe Interval Path Planning -- a search-based algorithm with strong theoretical guarantees. While this approach yields feasible paths, the rich primitive sets needed for smooth navigation induce a large branching factor, which becomes costly when coupled with time-dependent obstacle intervals. To this end, we present MeshSIPP, an efficient planner that removes the computational bottleneck by exploiting the fact that many primitives sweep the same regions and can therefore be validated together. MeshSIPP propagates primitives as spatial bundles, screens them with lightweight bounding-interval checks, and defers the expensive exact departure-time search until a primitive reaches its terminal state. A time-aware pruning rule additionally discards redundant space-time branches early in the search. We prove that the resulting search is complete and optimal. Extensive experiments over more than 6,000 benchmark instances and real-time ROS~2 simulations show that MeshSIPP achieves up to a 3

\times

speedup over state-of-the-art spatiotemporal planners.

Marat Agranovskiy, Konstantin Yakovlev

Motion Planning Robot Motion Planning Safe Motion Planning Dynamic Obstacle Avoidance

Oct 7, 2026cs.RO

Adaptive Risk-Certified Event-Triggered Replanning for Dynamic Navigation

Safe navigation in dynamic environments requires robots to plan under obstacle predictions whose errors are uncertain, non-stationary, and can induce rare but safety-critical failures. Existing control-barrier-function safety filters can reject immediately unsafe controls, but they provide little guidance on when the current finite-horizon planning mode itself is becoming unsafe as prediction uncertainty evolves. We propose Conformal Event-Triggered Risk-Certified Replanning (\emph{CERT-Replan}), a framework that uses calibrated barrier risk as an early-warning signal for replanning. CERT-Replan calibrates horizon-indexed obstacle-prediction residuals online and uses the resulting uncertainty radii to evaluate dynamic-obstacle safety margins. A one-step safety filter protects the next applied control, while a horizon-level risk monitor evaluates the upper-tail CVaR of predicted barrier-violation losses along the current MPC rollout. When this risk exceeds an allocated budget, CERT-Replan rejects the current planning mode and selects a lower-risk alternative, such as a different speed profile, corridor, or homotopy class, rather than repeatedly correcting the same nominal plan. In a non-stationary benchmark, CERT-Replan achieves an

83.3\%

collision reduction relative to the safety-filter-only baseline \textcolor{black}{and a

77.8\%

reduction relative to simple replanning triggers}, while reducing average safety-filter intervention by

32.4\%

. \textcolor{black}{With Trajectron++, CERT-Replan achieves

96\%

collision-free operation. Hardware experiments and onboard runtime profiling demonstrate computational feasibility.}

Richie R. Suganda, Bin Hu

Department of Electrical Engineering and Computer University of Houston Houston, TX 77004, USA · Department of Engineering Technology University of Houston Houston, TX 77004, USA

Safe Motion Planning Motion Planning Dynamic Obstacle Avoidance

Oct 5, 2026cs.RO

Distribution-Transfer Safe-Horizon MPC under Mode Uncertainty

Scenario-based MPC is an attractive strategy for chance-constrained motion planning that approximates uncertainty via a finite set of sampled scenarios. As a sampling-based method, scenario-based MPC is sensitive to distribution mismatch. We address this problem in the context of Safe-Horizon Model Predictive Control (SH-MPC) with obstacles governed by switching dynamic modes. From finite mode observations, we construct a confidence set for the unknown categorical mode law and derive a multiplicative domination bound that transfers a Safe-Horizon collision-risk certificate from a selected scenario-sampling distribution to every law in the confidence set. Wasserstein geometry is used to regularize probability reallocation among modes according to the similarity of their induced trajectory predictions, while a collision-risk surrogate biases sampling toward dangerous modes. The resulting certificate explicitly quantifies the additional tightening required under distribution mismatch and exposes the multiplicative conservatism that arises when several obstacle-wise transfer factors are combined

Stephen Crawford, Nora Ayanian

Department of Computer Science, Brown University, Providence, RI, USA.

Model Predictive Control Safe Motion Planning Motion Planning Sampling-Based Motion Planning

Oct 5, 2026cs.RO

Propagating Elevation-Map Uncertainty Through the Contact Maximum in Closed Form

Risk-aware planners score paths on uncertain elevation maps using the path cost's mean and standard deviation. Modeling rigid contact, however, requires computing a maximum over several uncertain cells. First-order propagation loses accuracy here by differentiating at only a single cell, while Monte Carlo sampling requires a full path evaluation per draw. We compute the moments of that contact maximum in closed form using Clark's pairwise recursion. By tracking each contact's covariance against the shared map cells, we propagate the smooth remainder using exact Gaussian quadratic-form identities. A contest-depth calibration, fitted once on two design traverses, closes the aggregate standard-deviation shortfall that remains. On 317 held-out rover path segments, scored against a Monte Carlo reference from the same belief, every pre-registered criterion was met. The corrected Clark fold cuts the median error of the mean from linearization's 2.3% to 0.19% and attains the lowest error in the conditional value at risk (CVaR) at the 90% level of every method tested. A benchmark plan costs just 5.5 microseconds on a GPU. These accuracy gains concentrate at contested contacts. While they seldom change which path is chosen on this terrain, the fold still selects the reference-best path in 98% of decisions against linearization's 93 to 96%. On a second dataset the mean transfers, though the risk number does not.

Aleš Kučera, Karel Zimmermann

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

Uncertainty Quantification Safe Motion Planning Uncertainty Propagation

Oct 5, 2026cs.RO

Dynamics-Aware Adaptive Corridors with Feasibility-Perturbed Trust-Region SQP for Certified Nonholonomic Motion Planning

Optimisation-based parking planners usually impose collision constraints only at the time samples, so a vehicle corner can cut an obstacle between samples, and no executable trajectory exists until the solver converges. We present a planner for car-like vehicles with reverse gear in which every iterate of the optimisation phase satisfies the discretised dynamics exactly and keeps the whole vehicle rectangle clear of obstacles between the samples. Each time interval receives one convex corridor that holds all vehicle corners at both ends and is shrunk by a sweep margin bounding how far the corner paths leave their chords. Separating half-planes give the corridors a direction out of obstacles when the initial guess is in collision; later, heading-aligned boxes are grown from the speed, curvature and step of the current iterate and rebuilt after accepted steps. A feasibility-perturbed trust-region sequential quadratic programming method projects each step onto the dynamics by feedback and verifies it exactly; the cost decreases monotonically, and once the corridors stop changing, limit points are Karush-Kuhn-Tucker points of the corridor-constrained problem or violate a constraint qualification. On 820 benchmark cases the planner succeeds in 818 without penetration (797 from the first initial guess), none of the 7103 evaluated optimisation-phase iterates is unusable, and it succeeds in 96.5% of the cases when 99% of the initial guesses intersect an obstacle. Its maneuvers take 0.7% longer in the median than those of a similarly certified exact-collision baseline. The guarantees hold for the planning model, not for a physical vehicle.

Yang Shi

Jiangsu College of Safety Technology, Jiangsu, China

Constrained Motion Planning Trajectory Generation Autonomous Parking Trust-Region Optimization Safe Motion Planning Motion Planning

Oct 1, 2026eess.SY

Closed-Loop Refinement and Execution for Learned Driving Planners

Learning-based driving planners are usually trained and evaluated in open loop against logged trajectories. In closed loop, a trajectory with small displacement error can still stall the vehicle, steer it into a conflict with surrounding agents, or be executed with abrupt braking. We introduce Closed-Loop Refinement and Execution (CLRE), a hierarchical receding-horizon control framework designed to mitigate these failure modes while leaving the upstream planner frozen and adding no new learned model. The upper layer treats the nominal trajectory as a reference and solves a finite-horizon optimal control problem that trades route progress against interaction with predicted agents. Solving it from several initializations gives a candidate set, and a prediction-conditioned oriented-bounding-box (OBB) feasibility test retains only candidates whose minimum predicted OBB clearance over the horizon meets a threshold. The lower layer executes the lowest-cost survivor, or a route-centerline backup when none remains, through the tracking controller supplied with the planner, augmented by a range-based speed bound and a saturated proportional braking law. In closed-loop simulation on 126 Bench2Drive routes with VAD as the upstream planner, CLRE raises the driving score from 43.41 to 56.42 and route completion from 57.27 to 72.23, and reduces collision events from 70 to 53.

Huaijin Hu, Shanting Wang, Zhongyu Mo +1

Systems Engineering Program, Cornell University, Ithaca, NY 14853 USA

Model Predictive Control Safe Motion Planning Autonomous Driving Planning Dynamic Obstacle Avoidance

Sep 30, 2026cs.RO

FORTE: Forecasting Occupancy for Spatiotemporal Risk-Aware Planning in Dynamic Environments

Safe navigation in dynamic environments requires anticipating future environmental states to account for spatiotemporal risks, specifically when and where collisions may occur. To this end, occupancy grid map (OGM) prediction has been widely adopted as an effective approach. However, existing OGM-based navigation methods often struggle to achieve accurate and efficient forecasting and fail to fully exploit the temporal information in predicted OGMs during planning. To address these challenges, we propose FORTE, a navigation framework that directly exploits the spatiotemporal evolution of predicted occupancy from the perspectives of spatiotemporal occupancy overlap and occupancy directivity. Based on these properties, FORTE evaluates multiple topology-distinct paths and selects the suitable one without explicit object detection or tracking. To support online planning, we formulate a latent diffusion model-based OGM predictor that generates the entire forecast horizon in a non-autoregressive manner while maintaining temporal consistency through temporal shift modules. Extensive evaluations demonstrate that FORTE outperforms state-of-the-art baselines. For prediction, FORTE achieves up to 215.3% higher IoU and 5.24x faster inference; for navigation, it yields up to a 3.5x higher success rate.

Hahjin Lee, Young J. Kim

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

Robot Navigation Safe Motion Planning Spatiotemporal Forecasting

Sep 30, 2026cs.RO

Efficient Multi-Modal Planning with Reward-Guided Preference Optimization for Autonomous Driving

Safe and efficient trajectory planning is essential in autonomous driving. However, existing end-to-end approaches often fall short in both computational efficiency and safety guarantees. Methods based on imitation learning suffer from causal confusion, while rule-based scoring approaches often incur heavy computational overhead and suffer from objective misalignment. Additionally, preference-based methods rely on strict pairwise annotations, limiting data utilization. To overcome these limitations, we propose EMPlan, an efficient multi-modal trajectory planning method powered by reward-guided fine-tuning. We design a hybrid architecture that combines sparse anchors with an offset refinement module for efficient multi-modal trajectory prediction. Sparse anchors provide coarse trajectory candidates with low latency, which are subsequently refined by the offset module for higher prediction accuracy. To enhance safety without incurring additional inference costs, we adopt a two-stage training paradigm consisting of pretraining and reward-guided fine-tuning. During fine-tuning, we leverage rule-based reward signals and unpaired preference supervision to refine the pretrained policy toward safer trajectory selection. We evaluate EMPlan on the non-reactive NAVSIM benchmark, where it strikes a favorable balance between planning accuracy and efficiency, demonstrating superior performance under real-time constraints.

Chenglin Chen, Lujia Wang, Xinhu Zheng +2

Safe Motion Planning Autonomous Driving Planning Multimodal Trajectory Prediction

Sep 29, 2026cs.RO

A Reachability-based Safety Certificate for Dynamical System Motion Policies

Dynamical Systems (DS) are reactive motion policies representing vector fields trained with theoretical guarantees of stability and convergence. To ensure safety during deployment in unknown environments they must be locally reshaped, either through modulation or geometric control barrier function strategies. However, depending on the geometry of the obstacles and the complexity of the DS, these local strategies can lead the system to unavoidable collisions or spurious attractors. In this work, we certify safety with a value function drawn from the notion of backward reachability tube, which measures the worst-case safety along a rollout trajectory of the nominal DS. Usually, such a value function is intractable for a controlled system due to curse of dimensionality. We show that in the DS-based learning-from-demonstration setting, the absence of a control input collapses the reachability problem to a deterministic rollout, and the presence of certain stability conditions truncates the infinite horizon to a finite one, resulting in a well-defined value function. We further show that the value function we devised is the maximal forward-invariant subset of the obstaclefree region for the nominal DS flow. The application of this certificate function is validated across five DS constructions - analytical, Neural ODE, diffeomorphic latent space, LPV-DS, SE(3)and validate it on a Franka manipulator. Modulation and geometric CBFs also suffer from saddle point in cases of headon approach towards an unsafe zone. We show that CBF-on-V avoids this pitfall entirely.

Aditya Vats, Tianyi Xia, Nadia Figueroa

The authors are with the University of Pennsylvania, Philadelphia, PA, USA.

Reachability Analysis Safety-Critical Control Control Barrier Functions Safe Motion Planning

Sep 29, 2026cs.RO

Foundation-Model-Guided Topology-Aware Semantic Risk Fields for Manipulation

Robot motion planning in everyday environments must satisfy hard geometric constraints while accounting for context-dependent semantic risk. We present a foundation-model-guided, topology-aware semantic risk field that extends manipulation safety beyond collision avoidance. For each manipulated-object/scene-object pair, a foundation model provides six directional risk weights and a pair-specific spatial decay scale. The method combines these priors with voxelized 3D scene geometry using topology-aware shielding and geodesic spatial decay. A GPU-parallel backend batches object-level distance and risk computations to construct a dense 3D field that serves as a modular cost for downstream motion planning. We evaluate the field's shielding behavior under full and partial barriers and compare its 3D workspace representation with a pixel-wise semantic-prior baseline. Across three household simulation scenarios, trajectories optimized with the proposed field have lower semantic exposure than collision-only trajectories under the same geometric constraints. We also evaluate the computational practicality and reliability of the supporting pipeline. Together, these results support the proposed field as a practical topology-aware semantic cost representation for manipulation planning beyond collision avoidance.

Giung Lee, Weihang Guo, Lydia E. Kavraki

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

Robot Safety Robotic Manipulation Safe Motion Planning

Sep 29, 2026cs.RO

Trajectory-Level Mode Guidance for Controllable Diffusion-Based Multi-Robot Motion Planning

Motion planning often admits multiple feasible solutions, making multimodal generation valuable, particularly for flexible multi-robot coordination. Diffusion models naturally learn such trajectory distributions, yet incorporating coarse and partial trajectory priors without restricting generation remains challenging. Such priors indicate a desirable region of the solution space rather than a single solution, motivating conditioned generation that preserves multimodality. In this paper, we guide trajectory generation in the clean trajectory space and progressively incorporate trajectory priors with a timestep-dependent guidance strength. At each reverse diffusion step, the reconstructed clean trajectory provides a unified space for integrating planning costs and partial trajectory priors. Planning costs are incorporated through gradient-based refinement, while the partial prior is progressively injected at the corresponding noise levels with decreasing guidance strength. This guides generation toward the prior in early stages while gradually releasing the constraint to preserve the inherent multimodality of the diffusion model. The framework naturally extends to multi-robot planning by incorporating inter-robot collision costs. Experiments on single- and multi-robot planning tasks demonstrate controllable trajectory synthesis, diverse feasible solutions, and safe multi-agent coordination.

Tianyou Yu, Shengze Cai, Chao Xu

Institute of Cyber-System and Control, College of Control Science and Engineering, Zhejiang University, Hangzhou 310027, China

Multi-Robot Motion Planning Trajectory Generation Diffusion-Based Planning Diffusion Guidance Safe Motion Planning Multimodal Diffusion Models

Sep 28, 2026cs.RO

CollisionSplatting: Collision-Aware Motion Planning in 3DGS Scenes with Image-Conditioned Objectives and Adjustable Conservatism

Incorporating dense visual information into motion planning remains challenging, as geometric planners rely on abstracted scene representations that discard visual richness, while learned visual models often lack geometric interpretability and computational efficiency. This paper introduces CollisionSplatting, a simple, modular, GPU-accelerated, probability-inspired distance metric with tunable conservatism that operates directly on standard 3D Gaussian Splatting (3DGS) scenes. When combined with learned image-conditioned reward functions, this metric enables joint geometric and visual planning by unifying collision-aware costs with image-space objectives. We integrate the metric into GPU-accelerated Model Predictive Path Integral (MPPI) and Rapidly-Exploring Random Tree (RRT) planners, and show on-par or better collision-classification performance compared to representative baselines while achieving substantially higher collision-checking throughput and significantly lower VRAM usage. Finally, we demonstrate the effectiveness of our metric in real-world vision-guided navigation and manipulation tasks, highlighting 3DGS as a practical bridge between rich perception and real-time motion planning.

R. Khorrambakht, Joaquim Ortiz-Haro, Stephan Weiss +1

Machines in Motion Lab, Tandon School of Engineering, New York University · Department of Smart Systems Technologies, University of Klagenfurt, Austria. · Artificial and Natural Intelligence Toulouse Institute (ANITI)

3D Gaussian Splatting Model-Based Planning Safe Motion Planning Motion Planning Sampling-Based Motion Planning

Sep 28, 2026cs.RO

Trajectory-Safe Orienteering for Human-Robot Shared Environments

Orienteering problem (OP) has wide real-world applications and also great potential in human-robot collaboration. However, existing approaches struggle to simultaneously ensure safe and feasible trajectories while achieving high-quality task execution in shared workspaces. To this end, this work studies the OP with time windows and variable profits (OPTWVP). A two-stage DEcoupled discrete-Continuous Optimization with Service-time-guided Trajectory (DeCoST) approach is proposed to effectively solve OPTWVP in shared spaces. Meanwhile, the safety-aware time windows of nodes and the discretized workspace are introduced to ensure collision-free trajectories between the end effector and the human. Preliminary results validate the effectiveness of DeCoST in generating collision-free trajectory plans while preserving the quality of orienteering tasks.

Songqun Gao, Elena Basei, Marco Roveri +2

Department of Industrial Engineering, Universit`a di Trento, Trento, Italy. · Interdepartmental Robotics Labs (IDRA), University of Trento, Trento, Italy · Department of Information Engineering and Computer Science, Universit`a di Trento, Trento, Italy.

Trajectory Generation Robot Safety Human-Robot Collaboration Robot Task Planning Safe Motion Planning

Sep 21, 2026cs.RO

Feasibility Distance Fields for Heterogeneous Constraints in Robot Configuration Space

Robot manipulators are monitored by constraint-specific indicators whose units and gradient scales are not comparable, so they do not provide a common measure of the configuration-space motion remaining before violation. We define the feasibility distance field (FDF) as the distance, under a fixed positive-definite joint-space metric, to the union of infeasible configuration sets. Classical distance-to-set theory gives 1-Lipschitz continuity, almost-everywhere differentiability, and unit dual-gradient norm wherever the nearest projection is unique. The robotics contribution is an admissibility analysis showing when practical constraints define non-empty closed sets. We derive admissible formulations for external and self-collision, joint limits, dexterity, Cartesian and task-projected compliance, joint torque under payload, and dynamic manipulability. Since every field uses the same metric, heterogeneous constraints compose by a pointwise minimum, conditioned constraints retain a fixed distance space, and multi-robot constraints produce block-sparse gradients that identify which robots must react. We generate projection-based labels and train neural approximations with a distance loss and an Eikonal penalty. Simulations on a UR5e and a dual-arm cell evaluate seven fields using value, projection, sign, gradient, composition, and moving-obstacle diagnostics. Across 8,000 configurations, the largest feasible-side secant ratio is 0.920, mean learned gradient norms range from 0.994 to 0.998, and projection residuals range from 0.011 to 0.034 rad. Across 24 random obstacle paths, the external and composed collision fields achieve 90.4% and 91.6% success within 3 cm, with sign-error rates below 2%. The results support a common configuration-space margin and identify approximation errors near medial axes and sparsely sampled boundaries.

Xijing Cui, Huayan Pu, Jun Luo +1

First and third authors' affiliation · Second author's affiliation

Constrained Motion Planning Signed Distance Functions Safe Motion Planning

Sep 20, 2026cs.RO

Risk-Aware Motion Planning and Control under Unknown Dynamics with Hybrid Observations

We consider robotic motion planning and control under unknown dynamics with hybrid state observations, where state measurements are available only in parts of the state space. Existing work combines system identification, predicted reachability, graph search and controller synthesis in a hierarchical framework using local affine approximated models over polytopic state space partitioning, but requires state observations for identification and feedback control. Based on this framework, we address blind regions by selecting nominal dynamics and precomputing open-loop control sequences before observation is lost. Since the true dynamics may differ from the selected nominal model, the robot may exit a blind polytope through an unintended facet. We quantify this transition risk and incorporate the possible outcomes into a stochastic transition system. The high-level planning problem is formulated as a stochastic shortest path problem, whose policy guides controller synthesis. A case study demonstrates that the method guides the robot from an initial state to a target while balancing route efficiency and the risks associated with traversing blind regions.

Zhiquan Zhang, Melkior Ornik

University of Illinois Urbana-Champaign, IL, 61801 USA

Robotic Control Safe Motion Planning Robot Motion Planning

Sep 20, 2026cs.RO

FeasibleFlow: One-Step Joint Transport of Configuration Feasibility and Trajectories for End-to-End Driving

End-to-end autonomous driving maps current observations directly to future trajectories, yet those trajectories must remain valid as the scene evolves. Future state modeling aims to address this temporal mismatch, but general representations often contain information unrelated to ego planning and affect trajectory generation only through auxiliary supervision, static conditioning, or proposal evaluation. We propose FeasibleFlow, a one-step end-to-end generative framework that jointly transports a configuration-space feasibility field and multimodal ego trajectories. Our Asymmetric Joint MeanFlow uses the pathwise Jacobian-vector product in the MeanFlow identity to incorporate field evolution into trajectory transport. Because safety feedback is sparser than progress feedback, we further introduce the Anchor-relative ranker (ARR) and Pareto-ReinFlow to balance safety and progress in candidate selection and generation, respectively. Experiments on the NAVSIM benchmark demonstrate the strong performance of FeasibleFlow and validate both the joint transport of feasibility and trajectories and the proposed safety-first mechanisms.

Xiang Li, Bikun Wang, Qing Xu +1

Cross-Domain Computing Solutions, Bosch

Flow Matching Trajectory Generation End-to-End Autonomous Driving Safe Motion Planning Autonomous Driving Planning

Sep 17, 2026cs.RO

Coding Agents with an Obstacle-Aware Harness for Safe Robot Manipulation

Coding agents have emerged as a promising paradigm for robot manipulation: a language model writes the robot controller as a program, and agents built in this way now operate robots without robot-specific training.Whether this paradigm is also safe, however, has not been asked. We evaluate coding agent under a safety constraint, where each task pairs a manipulation goal with an obstacle the robot must not touch. The agent pursues the goal but collides with the obstacle in most cases, treating task completion as its sole objective while neglecting safety. The agent reasons about the obstacle in its traces, and the prompt already forbids touching it, so neither perception nor instruction is at fault; the fault lies in the planning, where the stated constraint never becomes a priority. By decomposing manipulation into a route phase and a contact-rich moment, we locate the source of the failure. Along the route, the model cannot prioritize the safety constraint, having no notion of a clearing route and none of replanning once a chosen route becomes infeasible. At the contact, it is unaware that contact execution is bounded by the same constraint. To close this gap, we present SafeHarness, which equips the model with two obstacle-aware harnesses that enable it to prioritize the safety constraint. Obstacle-aware route planning grounds the objects as bounding boxes and draws candidate routes over them as sequences of waypoints. The agent then plans a route in advance, verifies it, replans when necessary, and only then executes it. Obstacle-aware contact execution instead selects the contact position so that the contact itself avoids the obstacle. SafeHarness attains 71.9% task success and 87.5% collision avoidance, surpassing the previous SOTA by 6.5% and 27.0%, respectively. These results are

2.3\times

and

1.5\times

those of the same agent without harnesses.

Bingxin Xu, Yuzhang Shang, Zhen Dong +1

USC · UCF · UCSB

Robot Safety Robotic Manipulation Safe Motion Planning Robot Collision Avoidance

Sep 16, 2026cs.CV

WZPlanner: Safe End-to-End Path Planning for Autonomous Driving in Work Zones

Work zones alter lane geometry through temporary traffic controls and closures that may be absent from on-board maps, challenging autonomous vehicle (AV) perception and planning. Generalization is also limited by scarce public datasets with structured geometric supervision. We present WorkZonePlan, a dataset comprising 149K+ synthetic and 5K+ real-world multimodal samples with 3D annotations for lane boundaries, work zone boundaries, and driving trajectory options. It also provides 76 closed-loop CARLA scenarios replayed under three weather conditions, yielding 228 Bench2Drive-format evaluation routes. We introduce WAVE (Work-zone-focused AV data generation in Virtual and rEal Environments), a semi-automated pipeline for creating the dataset, and BoundaryFormer (BF), a transformer-based model that jointly predicts lane and work zone boundary polynomials and driving trajectories. BF uses slot attention for boundary prediction. Ablations show that a separate trajectory decoder using boundary slot features substantially improves trajectory prediction over a slot-attention-only approach. Building on this finding, BF++ offers Camera and Camera+LiDAR variants with metric ground-plane encoding, typed boundary/trajectory queries, long-range point anchors, image-space curve refinement, and conservative gated LiDAR fusion. On the 211 routes common to all four models at the evaluation freeze, BF++-Camera and BF++-Camera+LiDAR achieve Driving Scores of 63.0 and 64.4, respectively, compared with 59.3 for SimLingo and 26.1 for TransFuser++ (TF++). BF++ is 40 times smaller than SimLingo and more than 10 times smaller than TF++, while achieving higher Driving Scores. These results support jointly predicting lane boundaries, work zone boundaries, and driving trajectories as a promising direction toward safer AV operation in work zones. Code and dataset: https://github.com/Nishad-Sahu/WZPlanner.

Nishad Sahu, Changzhong Qian, Guangzhou Cai +3

Raj · Department of Electrical and Computer Engineering Carnegie Mellon University, Pittsburgh, PA, USA

Autonomous Driving Datasets End-to-End Autonomous Driving Autonomous Driving Autonomous Driving Perception Safe Motion Planning Autonomous Driving Planning

Sep 16, 2026cs.RO

CaSCo: Cascade-Aware Soft-Collision Motion Planning

Conventional motion planning treats collision as a binary constraint, although contact with different objects can have drastically different consequences. A robot may safely brush against a cardboard box while even minor contact with a glass, laptop, or unstable object may be undesirable. Moreover, a direct robot--object collision can move the contacted object and trigger secondary object--object collisions, making the risk of a motion depend on the physical evolution of the scene rather than only on the robot's geometric path. We present CaSCo, a cascade-aware soft-collision motion planning framework in which a vision-language or language model assigns semantic risk to objects and a physics simulator predicts the consequences of candidate robot motions. CaSCo searches for a path that minimizes the total semantic risk of the unique objects displaced either directly by the robot or indirectly through cascaded collisions. Because collisions change the environment, we augment roadmap states with the predicted object arrangement and the set of objects whose risk has already been incurred. We develop an optimal graph-search algorithm with an admissible and consistent cascade-relaxed heuristic and caching and pruning mechanisms for efficient search. Experiments in cluttered manipulation environments evaluate semantic risk, cascade reasoning, planning efficiency, and real-robot operation.

Shivaram Kumar, Gaoyuan Liu, Yoonchang Sung

Nanyang Technological University

Roadmap-Based Motion Planning Robot Safety Robotic Manipulation Safe Motion Planning Motion Planning

Sep 16, 2026cs.AI

Risk-Aware World Modeling with Flow-Guided Occupancy Evolution for Selective Trajectory Planning in Automated Driving

Safe motion planning in automated driving requires anticipating evolving traffic risks and deciding when to revise the current planned trajectory. We introduce RiskWorld, a risk-aware world modeling framework for shared occupancy forecasting and selective trajectory replacement. Spatial risk fields and temporal actor context are fused with visual bird's-eye-view features. Flow-guided evolution transports occupancy and scene features, while signed residuals correct occupancy after transport. One forecast is generated per planning step and reused across candidates. Each candidate is compared with a current-state persistence reference, yielding a nonnegative collision-score correction. The trajectory selected by current-world evaluation serves as the planning anchor and is replaced only when additional predicted risk triggers intervention and an alternative satisfies component-wise constraints on predicted risk and trajectory error. Candidate geometries remain unchanged. We evaluate RiskWorld for open-loop planning on nuScenes using camera features, annotation-derived current and historical actor states, and dataset-provided map context. RiskWorld achieves the lowest collision rate at a long evaluation horizon of 3 s, and the second-best average L2 error among various state-of-the-art baselines, while running at 11.5 FPS on a single NVIDIA RTX 4090 with 90.81 M parameters. Within-setting ablations show that RiskWorld achieves lower collision rates than the current-state rescoring baseline, while forecast reuse enables additional candidates to be evaluated at low marginal computational cost.

Rongxiang Zeng, Linsen Cai, Jiafu Zhang +7

RWTH Aachen University, Aachen, Germany · Monash University, Melbourne, Australia · Technical University of Munich (TUM), Munich, Germany

World Model-Based Planning 3D Occupancy Prediction World Models for Autonomous Driving Safe Motion Planning Autonomous Driving Planning

Sep 16, 2026cs.RO

OmniRisk: Omnidirectional Trajectory-Risk Learning for Agile Quadrotor Dynamic Avoidance

Agile quadrotor avoidance of fast-moving obstacles requires anticipating collisions and selecting feasible maneuvers within short reaction windows. Reliable predictive avoidance remains challenging because sparse range observations do not directly reveal obstacle motion, while online trajectory optimizers either scale poorly with obstacle count or remain efficient at the expense of reliability in dense, high-speed encounters. We present OmniRisk, an omnidirectional planning framework that learns trajectory-level risk offline for efficient onboard evasion. A fixed-dimensional tensor combines LiDAR range panoramas, dynamic masks, and Cartesian surface velocities to represent geometry and motion jointly. We formulate an asymmetric risk field aligned with obstacle velocity that emphasizes approaching interactions and attenuates receding ones. Accumulating this risk along predicted relative trajectories provides dense supervision and discourages unnecessary hesitation after obstacles pass. A dual-branch circular convolutional network predicts terminal boundary states and dynamic risks for candidate primitives over an omnidirectional anchor lattice in a single forward pass, followed by selection and closed-form reconstruction of the selected candidate primitive. This formulation removes online risk accumulation along trajectories and makes risk-inference cost independent of obstacle count. OmniRisk enables efficient onboard avoidance, with real-world flights demonstrating consecutive evasive maneuvers at relative encounter speeds up to 15 m/s without fine-tuning. Code is available at https://github.com/VANdexj/OmniRisk.

Yifan He, Yang Liu, Wenhao Zhao +8

Differential Robotics, Hangzhou, China · Zhejiang University, Hangzhou, China

Aerial Robotics Safe Motion Planning Dynamic Obstacle Avoidance

Sep 15, 2026cs.RO

CorrRisk-WM: Corridor-Conditioned Risk World Modeling for Safety-Critical Trajectory Planning

Safe local planning requires forecasting surrounding-agent motion and evaluating candidate-specific risks, since identical agent motion can pose different risks to different ego trajectories. We present CorrRisk-WM, a planning-oriented partial world model coupling environment evolution with supervised intrusion and near-miss prediction over bounded candidate-trajectory corridors. A latent environment model recursively predicts agent states and updates agent-agent and agent-map interactions. Each candidate queries the evolving environment through footprint- aware geometry and learned agent-corridor representations. A lightweight recurrent risk module uses temporal context to estimate per-slice hazards; survival aggregation yields first-entry and horizon-level event probabilities. On 29,176 scenarios from 100 Waymo validation shards, CorrRisk-WM achieves intrusion average precision (AP) of 0.8567 and 1-m near-miss first-entry AP of 0.8671. In baseline comparisons, it attains the highest near-miss AP at all three distance thresholds and the lowest observed open-loop collision rate (4.88%), with route progress of 15.35 m. Across three seeds, removing dynamic environment modeling or candidate-conditioned geometric interaction reduces mean intrusion AP from 0.8590 to 0.7624 and 0.7252, respectively. These results support coupling environment evolution with candidate-conditioned geometric reasoning for risk prediction and safety-oriented candidate selection.

Tingyu Guo, Reza Langari

J. Mike Walker ’66 Department of Mechanical Engineering, Texas A&M University, College Station, TX 77843, USA

World Model-Based Planning World Models for Autonomous Driving Safe Motion Planning Autonomous Driving Planning

Sep 14, 2026cs.RO

READ: Learning Risk-Informed Fields for End-to-End Autonomous Driving

Autonomous driving requires more than recognizing what is present in a scene: a planner must determine how road structure, surrounding agents, and their motion states should influence a future maneuver. Existing learning-based planners can capture these influences through latent scene features and trajectory decoders, but the relationship between environmental factors and candidate actions often remains implicit. This limits the ability to inspect, diagnose, or refine how scene context affects the safety of a predicted trajectory. Classical safety fields provide an explicit spatial representation of this relationship, but their risk shapes and relative weights are prescribed in advance and do not adapt to each scene. We introduce READ, a framework that learns an explicit, planning-aligned risk representation from complementary geometric and behavioral constraints. READ instantiates this representation as a continuous spatiotemporal field, enabling differentiable queries along candidate trajectories. The learned field connects scene understanding with action selection by encouraging predicted trajectories to align with low-risk regions, while retaining a differentiable interface for trajectory evaluation and refinement. READ integrates with both end-to-end planners and Vision-Language-Action models. Experiments on NAVSIM show consistent gains across matched end-to-end backbones and strong performance in a VLA setting; READ also achieves competitive results on NAVSIM v2. These results establish learned spatial risk as an explicit, adaptable representation for safe planning.

Zhiyuan Liu, Yuanxin Tian, Zehong Ke +5

School of Vehicle and Mobility, Tsinghua University.

End-to-End Autonomous Driving Safe Motion Planning

Sep 9, 2026cs.RO

Future-Aware Flow Planning for Safe UAV Target Following

UAV target following in cluttered environments requires anticipating target motion. Followers that use only the current target state can lag behind turns or choose blocked corridors. They may also trade safe near-horizon motion for lower tracking error. We propose a future-aware flow planning framework for state-informed UAV target following. Predicted target futures guide clean UAV trajectory generation through residual signals aligned with the planning horizon. Risk-scored repair of the executable prefix is embedded in the sampling loop. On fixed in-distribution (ID) and out-of-distribution (OOD) receding-horizon benchmarks, the planner improves the safety--tracking trade-off. It matches zero measured ID collision rate and achieves the highest ID safe-tracking time fraction. It also gives the lowest OOD macro-average collision rate and final tracking error among the compared methods. It does not dominate every metric: Future-MPC remains smoother and stronger on some threshold-based OOD success metrics under its hand-designed objective. Controlled comparisons show that future conditioning with the adapter improves candidate generation before safety repair. Simulator-facing tests examine interface perturbations, sensing, and controller execution. These results support horizon-aligned future guidance and embedded prefix repair as complementary components for safe UAV target following under the tested simulation conditions.

Boning Feng, Haoran Zhang, Xiaowen Bi +2

Department of Computer and Systems Sciences, Stockholm University, Stockholm, Sweden · School of Urban Planning and Design, Peking University, Shenzhen, 518055, China · Guangdong Provincial Key Laboratory of Risk Perception and Sustainable Governance in Energy Transition, Shenzhen, 518055, Guangdong, China +2

Collision Avoidance UAV Navigation Safe Motion Planning Motion Planning

Sep 8, 2026cs.RO

Online, Reachability-Aware, Sampling-Based Motion Planning

Sampling-Based Model-Predictive Control (MPC) algorithms are a flexible class of controllers used for navigation on a wide range of robotic systems. Historically, such approaches have lacked hard safety guarantees, a shortcoming which we remedy in this work by computing guaranteed reachable-set overapproximations online with a fast, interval-based pipeline. We show that our method achieves similar performance to a state-of-the-art reachability-based planner without the need for the expensive pre-computation step, and can be scaled to systems that are infeasible using existing approaches. Finally, we demonstrate that our technique reduces safety violations by over 99% in a racing simulation and successfully controls a model racecar on real hardware experiments without crashes.

Brendan Gould, Zhiyuan Zhang, Panagiotis Tsiotras +1

Robotic Control Reachability Analysis Robot Safety Model Predictive Control Safe Motion Planning Sampling-Based Motion Planning

Sep 7, 2026cs.RO

CALM: Configuration-Aware Human Intervention Boundaries During Robot Approach

How robot body configuration shapes human intervention during approach remains underexplored. We conducted a within-participants study with 41 participants, measuring final stopping distance, subjective comfort, and exploratory eye-tracking responses across four humanoid arm configurations and two spatial scales. Full forward arm extension increased stopping distance by approximately 31-36 cm relative to arms-down. Spatial scale primarily affected comfort and pupil responses without a detectable stopping-distance shift. We introduce the Configuration-Aware Limit Model (CALM), which translates stopping-distance distributions into configuration-dependent population-coverage boundaries. Estimated boundaries at 80% coverage ranged from 0.88 to 1.47 m. In an illustrative one-dimensional planning analysis, reconfiguration enabled a 1.10 m approach goal that was unreachable with arms remaining fully extended under the same nominal pointwise 20% intervention-probability constraint. These findings support treating body configuration as a planning variable while distinguishing physical safety, behavioral intervention, and subjective cost.

Xinting Gao, Sipu Zhu, Weimin Zhuang

School of Architecture, Tsinghua University, China · Architectural Design & Research Institute of Tsinghua University, China

Robot Safety Safe Motion Planning Human-Robot Interaction Humanoid Robotics

Aug 30, 2026cs.RO

Sampling-based Certified Planning with Graphs of Convex Sets

Planners on graphs of convex sets return trajectories that are collision-free by construction, provided the convex regions are collision-free. The region generator only promises that property probabilistically, and no planner in the family verifies it. We report the first measurement of what the gap costs. On a scaled 14-DOF bimanual library,

3.2\%

of interface samples are in collision, and a search-based GCS planner (\gcsstar) turns that volume error into a

62\%

answer error:

18

of

29

pick-and-place queries return trajectories that drive the arms through the shelves, up to

91

,mm deep, reported as successes. Repairing the library does not work; a ten times stricter acceptance contract, sums-of-squares certified regions, and uniform margins each destroy the connectivity planning needs before they deliver soundness. We instead build a planner that certifies its answers. It samples the overlaps and shared faces of the decomposition, prunes with an admissible informed bound, and verifies the one candidate each search round proposes, continuously, by a chain of clearance certificate balls with no resolution parameter; failures are repaired with local in-region detours, and the convex polish is re-verified. Head-to-head on all

29

task queries it delivers zero invalid answers against

21

for the reference, reaches its first certified answer in

0.11

,s against

1.59

,s for the reference's unverified one, and reproduces the reference optimum exactly on every query whose reference answer is physically valid.

Peng Xie, Amr Alanwar

TUM School of Computation, Information and Technology, Department of Computer Engineering, Technical University of Munich, 74076 Heilbronn, Germany

Constrained Motion Planning Robotic Manipulation Safe Motion Planning Motion Planning Sampling-Based Motion Planning

Aug 30, 2026cs.RO

LARC: Lazy Adaptive Reachability Certification of Robot Manipulator Trajectories

Discrete trajectory checks can miss collisions between sampled robot states. Reachability-based certification bounds motion between states, but uniform time partitions waste computation where clearance is large. We present lazy adaptive reachability certification (LARC), which checks a planned trajectory by bisecting only intervals with an inconclusive clearance test. For piecewise-cubic Hermite joint trajectories, the method bounds link occupancy using midpoint capsules inflated by exact componentwise speed maxima. Certified intervals covering the trajectory provide continuous-time external-obstacle clearance, subject to geometric containment, static obstacles, and a prescribed margin. On 160 AgileX PIPER trajectories from 80 start-goal pairs, LARC matched all decisions of the fixed-fine baseline at depth nine. It used 20328 interval evaluations (24.8% of baseline work), with a median paired speedup of 10.28x. A separate MoveIt/FCL audit checked 158051 states and detected collisions in 21 direct-interpolation controls, none of which LARC certified. The method reduced computation under a shared certificate model, but 27 of 139 sampled-clear trajectories remained uncertified. The sampled audit cannot independently prove continuous-time clearance.

Yu Feng, Hao Wu, Yuzhe Wang +1

Department of Mechanical Engineering, National University of Singapore, Singapore · Singapore Institute of Manufacturing Technology, Agency for Science Technology and Research, Singapore

Reachability Analysis Robot Safety Safe Motion Planning

Aug 12, 2026cs.RO

Learning-Based Behavior Planning for Automated Driving: Real-World Integration and Deployment

Recent research in machine and deep learning has shown the potential of learningbased motion planning approaches to improve the driving behavior of automated vehicles, especially in complex environments. However, their complex nature and lack of transparency can hinder explainability and trustworthiness and complicate safety assurance. Motivated by these challenges, we propose a hybrid planning architecture that combines the advantages of machine learning with the verifiability and the determinism of classical approaches. Specifically, we developed a deep neural network to interpret complex traffic scenes and propose driving behavior, while an optimization-based supervision layer validates this proposal and enforces explicit drivability and safety constraints. We evaluate the learned planner's driving behavior in open-loop studies on real-world urban data, discuss system integration aspects for stable closed-loop operation, and report results from real-world deployment on our research vehicle karl..

Jean-Pierre Busch, Guido Linden, Jan Bergmann +1

Institute for Automotive Engineering (ika), RWTH Aachen University, Germany · ika · Chair of Automotive Technology (FTM), Technical University of Munich +1

Safe Motion Planning Motion Planning Autonomous Driving Planning Explainable Motion Planning

Aug 11, 2026cs.RO

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

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

\sim

97% across trajectories in simulation and hardware experiments.

Sachin Sunil Kelkar, Tanmay Dokania, Yashwanth Kumar Nakka

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

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

Aug 10, 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