Organizations: Hong Kong Center for Construction Robotics, The Hong Kong University of Science and Technology, Hong Kong, China · Department of Mechanical and Automation Engineering, The Chinese University of Hong Kong, Hong Kong, China · Department of Electronic Engineering, The Chinese University of Hong Kong, Hong Kong, China · Shenzhen Key Laboratory of Robotics Perception and Intelligence and the Department of Electronic and Electrical Engineering at Southern University of Science and Technology, Shenzhen, China · Department of Electronic Engineering at The Chinese University of Hong Kong, Hong Kong, China
Robot manipulators operating in uncertain and non-convex environments present significant challenges for safe and optimal motion planning. Existing methods often struggle to provide efficient and formally certified collision risk guarantees, particularly when dealing with complex geometries and non-Gaussian uncertainties. This article proposes a novel risk-bounded motion planning framework to address this unmet need. Our approach integrates a rigid manipulator deep stochastic Koopman operator (RM-DeSKO) model to robustly predict the robot's state distribution under motion uncertainty. We then introduce an efficient, hierarchical verification method that combines parallelizable physics simulations with sum-of-squares (SOS) programming as a filter for fine-grained, formal certification of collision risk. This method is embedded within a Model Predictive Path Integral (MPPI) controller that uniquely utilizes binary collision information from SOS decomposition to improve its policy. The effectiveness of the proposed framework is validated on two typical robot manipulators through extensive simulations and real-world experiments, including a challenging human-robot collaboration scenario, demonstrating sim-to-real transfer of the learned model and its ability to generate safe and efficient trajectories in complex, uncertain settings.
Figures & tables
Fig. 1 : Risk-bounded motion planning framework for a manipulator under environmental and motion uncertainties. It combines a RM-DeSKO neural network model predicting states XN,H and costs C^N,H , with a MPPI refining control inputs u∗ . A supervisory logic enforces environment safety by computing contact force-based collision costs in simulation and checking bounded collision risk for the next predicted state before execution. Noisy state xt closes the perception-action loop. The new cost function c^(xt,u∗) quickly guides stochastic optimization while ensuring risk constraints.
Fig. 2 : Schematic diagram of proposed forward dynamics propagation in MPPI under RM-DeSKO model with key equations for N sampled trajectories over a horizon H .
Fig. 3 : Ellipsoidal body description example of a robot arm to be verified for collision risk.
Fig. 4 : Comparison results of the log10 of maximum (top) and mean (bottom) prediction errors for a robot arm under state uncertainty across the planning horizon.
Transformer
DKU
LSTM
MLP
RM-DeSKO
RM-DeSKO-N
RM-DeSKO-A
Max Err
0.801±0.086
0.860±0.077
0.816±0.091
0.852±0.078
0.789±0.097
0.816±0.119
0.827±0.093
Mean Err
−1.175±0.010
−0.980±0.014
−1.074±0.049
−0.893±0.022
−1.061±0.037
−1.082±0.040
−1.075±0.040
TABLE I : Statistical Results of State Prediction Errors Under State Uncertainty Over the Planning Horizon
Methods
Success Rate [%]
TTG [s]
Length
Baseline
89
47.249
2.273
Transformer
0
-
-
LSTM
0
-
-
Ours
94
34.587
1.168
TABLE II : Comparison Results of Risk-Bounded Motion Planning for Manipulator under Environment and State Uncertainty
Fig. 5 : Snapshots of the planned trajectories by the MPPI control using four kinds of nonlinear dynamical systems, i.e., baseline, Transformer, LSTM, and Ours (from top to bottom). The arm tries to reach the goal (green ball) while keeping the probability of collision with uncertain nonconvex obstacles (red heart-shaped) guaranteed. Only our method succeeds in time because of the correct guiding rollouts (cyan) benefited from our neural network dynamical model and collision risk verification.
Metric
N(0.2,0.2)
N(0.22,0.22)
N(0.25,0.25)
TTG [s]
34.587
65.992
100.210
Length
1.168
1.720
1.731
Success Rate [ % ]
94
91
83
TABLE III : Planning Results under Different Tracking Errors ωd
Metric
200
500
1000
TTG [s]
40.529
21.147
34.587
Length
1.723
0.953
1.168
Success Rate [ % ]
88
93
94
TABLE IV : Planning Results under Different Numbers of Samples N
o 0.49X[0.8,c] X[1.5,c] X[1,c] X[1,c] X[1,c] X[1,c] Methods
Metric
(αp,αo)
(10,0.1)
(5,0.1)
(1,0.1)
(10,1)
Ours
TTG [s]
34.587
46.839
49.586
48.325
Length
1.168
1.358
1.971
2.568
Success Rate [ % ]
94
87
11
8
baseline
TTG [s]
47.249
37.849
42.913
49.130
Length
2.273
2.074
2.065
2.692
TABLE V : Planning Results under Different Pairs of Weights
Fig. 6 : Snapshots of the rebar tying experiment under uncertainty from the left and front views (top and bottom). The rollouts in cyan on the screen stably point to the goal despite the eccentric payload and tracking errors of the manipulator. The robot successfully generates a risk-bounded trajectory to bypass the uncertain nonconvex human arms and reach the tying target using our method, which transfers directly from simulation to a real robot.
Safe motion planning develops methods for robots and vehicles to reach goals while avoiding collisions and respecting dynamic and uncertainty constraints.
Department of Biomedical Engineering, National University of Singapore, Singapore 117583 · Peng Cheng Laboratory, Shenzhen 518057, China · School of Electrical and Electronic Engineering, Nanyang Technological University, Singapore 639798