Sampling-based Task and Kinodynamic Motion Planning under Semantic Uncertainty

Qi Heng Ho; Zachary N. Sunberg; Morteza Lahijanian

cs.ROApr 1, 2026

Sampling-based Task and Kinodynamic Motion Planning under Semantic Uncertainty

Authors: Qi Heng Ho, Zachary N. Sunberg, Morteza Lahijanian

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

Abstract

This paper tackles the problem of integrated task and kinodynamic motion planning in uncertain environments. We consider a robot with nonlinear dynamics tasked with a Linear Temporal Logic over finite traces ( $\ltlf$ ) specification operating in a partially observable environment. Specifically, the uncertainty is in the semantic labels of the environment. We show how the problem can be modeled as a Partially Observable Stochastic Hybrid System that captures the robot dynamics, $\ltlf$ task, and uncertainty in the environment state variables. We propose an anytime algorithm that takes advantage of the structure of the hybrid system, and combines the effectiveness of decision-making techniques and sampling-based motion planning. We prove the soundness and asymptotic optimality of the algorithm. Results show the efficacy of our algorithm in uncertain environments, and that it consistently outperforms baseline methods.

Figures & tables

Fig. 1 : This example shows two possible motion trajectories in Example 1 , where the observation quality is better near the fire than above the trees. In both (blue and orange) trajectories, the drone evolves in continuous time until it reaches $r_{1,2}^{o}$ or $r_{1,1}^{o}$ , which triggers a guard $G_{T}$ . Let both observations be $o=fire$ Then, it updates its memory state $m$ , and updates its belief $b(e)$ of the environment state. The drone state evolves again until it reaches exit A, in which $G_{R}$ is triggered. This causes a belief update to the task DFA belief $b(q)$ . In this example, the orange trajectory has higher probability of success than blue.

Panel of Figure 2 — Fig. 2 : Benchmark Results of each algorithm for the four environments with a $60$ second time limit over $100$ trials.

Panel of Figure 3 — Fig. 3 : C-Rock Sample with two different initial beliefs.

Fig. 4 : Drone Fire Detection Example: Exit A (green) and exit B (blue), fire sites (orange), obstacles (red). The yellow and gray hemispheres are observation regions.

Semantic Uncertainty Estimation

See all 14 papers

Provably Safe Motion Planning Under Unknown Disturbances
May 26, 2026Ibon Gracia, Qi Heng Ho, Luca Laurenti +1Sampling-Based Motion Planning Chance-Constrained Motion Planning
Department of Aerospace Engineering Sciences at the University of Colorado Boulder, CO, USA · Department of Aerospace and Ocean Engineering, Virginia Tech, VA, USA · Delft University of Technology, the Netherlands, and the Italian Institute of Artificial Intelligence, Italy
Terminal Matters: Kinodynamic Planning with a Terminal Cost and Learned Uncertainty in Belief State-Cost Space
May 9, 2026Zhuoyun Zhong, Seyedali Golestaneh, Constantinos ChamzasBelief-Space Planning Trajectory Optimization
All authors are affiliated with the Department of Robotics Engineering, Worcester Polytechnic Institute (WPI), Worcester, MA 01609, USA
Continuous-Time Gaussian Belief Trees for Motion Planning
Jul 3, 2026Rayan Mazouz, Qi Heng Ho, Zachary N. Sunberg +1Belief-Space Planning Constrained Motion Planning
Department of Aerospace Engineering Sciences at the University of Colorado Boulder, CO, USA · Department of Aerospace and Ocean Engineering at Virginia Tech, Blacksburg, VA, USA

May 26, 2026J/K move · Enter open · S save

Provably Safe Motion Planning Under Unknown Disturbances

Ibon Gracia, Qi Heng Ho, Luca Laurenti, Morteza Lahijanian

Department of Aerospace Engineering Sciences at the University of Colorado Boulder, CO, USA · Department of Aerospace and Ocean Engineering, Virginia Tech, VA, USA · Delft University of Technology, the Netherlands, and the Italian Institute of Artificial Intelligence, Italy

Sampling-Based Motion Planning Chance-Constrained Motion Planning Constrained Motion Planning Robot Safety Wasserstein Distributionally Robust Optimization Safe Motion Planning+2

Open paper AskPDF arXiv

We present a provably safe sampling-based motion planning algorithm for robotic systems affected by random disturbances of unknown distribution. We consider systems with linear or linearizable dynamics evolving in workspace with arbitrary-shaped obstacles subject to state and control constraints. Safety requirements are formulated as chance-constraints. Our approach leverages data from trajectories of the system to learn a Wasserstein ambiguity tube, i.e., a sequence of ambiguity sets, which contains the trajectory of the system's state distribution with high confidence. This ambiguity tube is then used in a probabilistically complete algorithm to grow a sampling-based motion planning tree that respects the constraints of the problem. We show that learning several lower-dimensional ambiguity tubes instead of a single high-dimensional one effectively reduces the conservatism and boosts scalability. Additionally, we design an efficient bandit-based validity checker that remarkably increases the empirical performance of our approach without sacrificing probabilistic completeness. Case studies show our algorithm finds valid plans in cluttered environments under strict safety thresholds, outperforming state-of-the-art methods.

Abstract

Semantic Uncertainty Estimation

Explore similar work

Provably Safe Motion Planning Under Unknown Disturbances

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

Continuous-Time Gaussian Belief Trees for Motion Planning