Organizations: Delft University of Technology Delft, Netherlands · ETH Zurich Zurich, Switzerland · Virginia Tech Virginia, USA · UCLouvain Louvain-la-Neuve, Belgium · University of Colorado Boulder Boulder, USA
Motion planning for stochastic systems aims to find a sequence of control inputs that drives a system to a goal set while avoiding unsafe states with high probability. Existing approaches, however, are either restricted to linear dynamics with perfect knowledge of the underlying distributions or lack formal correctness guarantees. To address these limitations, we propose Learn-To-Propagate Distributionally-Robust Forest (L2P-DR-Forest), a distributionally-robust sampling-based planning algorithm for nonlinear stochastic systems with unknown initial state and process noise distributions. Our approach leverages recent advances in optimal transport to learn ambiguity sets that contain the true system distributions with high confidence. We develop an efficient method for propagating these sets through the system dynamics and for verifying their safety. We then integrate these techniques into a novel sampling-based planning algorithm to grow a forest of search trees in the system's state space while accounting for and mitigating the growth of ambiguity-sets radii. We prove that the algorithm is correct, i.e., every returned plan satisfies the specifications with the prescribed confidence, and, under some feasibility and distributional assumptions, is probabilistically complete. Evaluations on several robotic benchmarks demonstrate that our algorithm significantly outperforms existing methods while providing formal guarantees with high confidence.
Figures & tables
Figure 1. Schematic of Algorithm 1 on a scenario with an obstacle O and a target T . The roots of the trees, learned with LearnAmbiguitySet , are highlighted. (1) The forest F stores the trees of the previous stages. (2) SelectRoot selects one of the available trees in F , and a node ν (circled) from this tree, at which the root of the new tree is learned (Lines 1 – 1 ). (3) Each iteration of the tree search uniformly samples a state x∈X (black point), selects the node closest to it, and steers it towards x with the best of K candidate controls (solid child; the others are dashed) (Lines 1 – 1 ). If no node passes TargetCheck , the tree is added to F and the next stage begins (Line 1 ). (4) Otherwise, backtrack returns the controller u leading to the node that passes (black path; Lines 1 – 1 ).
Switched System
AV Maze
Unicycle Maze
Tractor Trailer
Double Integrator
Dimension
2D
3D
3D
4D
6D
L2P-DR-Forest (20 trials)
Max. number of it. per stage
1,500
3,000
5,000
1,500
3,000
Max. number of stages
10
10
20
20
20
Succ. rate (%)
100.0
100.0
100.0
100.0
100.0
Total planning time ( s )
0.08±0.12
1.56±0.96
2.15±3.17
55.51±47.22
1.28±0.37
MILP time ( s )
0.0343±0.0096
0.0427±0.0107
0.0919±0.0554
0.1129±0.0356
0.0785±0.0180
Table 1. Summary of average selected statistics (with one standard deviation).
Figure 2. AV Maze , drawn through the means of the nodes’ center distributions in the positions (px,py) , with one color per stage. In each stage panel, the new tree is opaque, the tree drawn by SelectRoot is half faded, the other trees are faded, and circles mark the learned roots. At the third stage, SelectRoot draws the first tree: the third tree restarts at the same node as the second one, which is abandoned.
Figure 3. Double Integrator , in the positions (px,py,pz) , with the encoding of Figure 2 . The inset of (a) zooms on the first tree, only two steps deep, in the (px,py) plane. At the fifth stage, SelectRoot draws the third tree: the fifth tree restarts at the same node as the fourth one, which is abandoned.
Appendix figures & tables4 assets
Supplementary material from the paper’s appendix.
Appendix
Figure 4. Example of the bounding box of a given artificial dataset Dpre , partitioned into M cells Ci with centers ci as explained above, for M∈{1,4} .
Figure 5. Representation of the worst-case probability mass displacement given by Algorithm 7 .
Figure 6. Informed search heuristic for Double Integrator (only the three first state dimensions are depicted).
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.
Ibon Gracia, Qi Heng Ho, Luca Laurenti +1
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
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.
Qi Heng Ho, Zachary N. Sunberg, Morteza Lahijanian
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
We address sampling-based motion planning for continuous-time stochastic systems under process and measurement uncertainty, with probabilistic guarantees on safety and performance. The robot dynamics are modeled as a continuous-time linear stochastic differential equation, while sensor measurements arrive at discrete time instants. We derive an offline hybrid belief propagation model in which the belief evolves according to continuous-time ODEs between measurements and undergoes discrete Kalman filter update jumps at measurement times. To ensure safety, we introduce a belief-barrier-function-based safety checker for segment-level probabilistic verification. This enables the planner to certify safety over entire continuous trajectory segments and detect inter-sample chance-constraint violations that are missed by conventional node-based checks. Together, these components provide a principled framework for sampling-based belief planning that accounts for both continuous-time uncertainty propagation and continuous-time safety requirements. We integrate the method with RRT and SST planners and evaluate it across multiple benchmark environments. The results show that the proposed method achieves high success rates and robust enforcement of chance constraints, including in narrow-passage scenarios where discrete-time counterparts fail due to missed inter-sample unsafe behavior.
Rayan Mazouz, Qi Heng Ho, Zachary N. Sunberg +1
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