CC-VPSTO: Chance-Constrained Via-Point-based Stochastic Trajectory Optimisation for Safe and Efficient Online Robot Motion Planning

TL;DR

CC-VPSTO tackles safe real-time robot motion planning under uncertainty by turning a general chance-constrained problem $\min J(\mathbf{x})$ s.t. $P_{\bm{\delta}}[g(\mathbf{x},\bm{\delta})>0]\le \eta$ into a tractable Monte-Carlo surrogate. It integrates this surrogate into the VP-STO framework within an MPC setting, using a confidence-bounded threshold $k_\beta$ (and theoretical $k_{\beta,\mathrm{rad}}$ bounds) to guarantee constraint satisfaction with probability $1-\beta$, without assuming a specific uncertainty distribution. The approach supports arbitrary inequality constraints, allows online re-planning, and scales to multiple obstacles and time steps via a joint trajectory formulation; it is validated in offline simulations and a real Franka arm experiment, showing favorable safety-efficiency trade-offs compared with baselines. The work demonstrates real-time applicability (e.g., 3–4 Hz MPC updates) and provides practical guidance on sample size and confidence calibration for reliable safe operation in uncertain, dynamic environments.

Abstract

Safety in the face of uncertainty is a key challenge in robotics. We introduce a real-time capable framework to generate safe and task-efficient robot motions for stochastic control problems. We frame this as a chance-constrained optimisation problem constraining the probability of the controlled system to violate a safety constraint to be below a set threshold. To estimate this probability we propose a Monte--Carlo approximation. We suggest several ways to construct the problem given a fixed number of uncertainty samples, such that it is a reliable over-approximation of the original problem, i.e. any solution to the sample-based problem adheres to the original chance-constraint with high confidence. To solve the resulting problem, we integrate it into our motion planner VP-STO and name the enhanced framework Chance-Constrained (CC)-VPSTO. The strengths of our approach lie in i) its generality, without assumptions on the underlying uncertainty distribution, system dynamics, cost function, or the form of inequality constraints; and ii) its applicability to MPC-settings. We demonstrate the validity and efficiency of our approach on both simulation and real-world robot experiments.

Figures (11)

• Figure 1: Real-robot experiment. The robot is tasked to move its ball-shaped end effector from a start point on one side to a goal point on the other side of a conveyor belt (indicated by the yellow balls in the right simulation view). Meanwhile, the ball end effector has to avoid the box obstacle on a moving conveyor belt which is controlled according to a stochastic policy. Depending on the anticipated box movement, the robot can either pass the box in front (left images) or behind (right images). This problem setting requires the robot to be reactive whilst being able to plan safe motions in real-time. It further poses a trade-off between safety and performance, i.e., reaching the other side of the conveyor belt in minimum time.
• Figure 2: CDF of the binomial distribution for different values of $N$ and $p$, given $k/N$ on the x-axis. The CDF values map to our confidence in observing $k/N$ constraint violations under the assumption that the true probability of constraint violation is $p$.
• Figure 3: Graph of the penalty function used in CC-VPSTO. When observing more than $k_{\mathrm{thresh}}$ constraint violations in the $N$ Monte--Carlo simulations, the penalty function takes value $J_{\mathrm{pen,min}}$ plus a quantity proportional to the number of extra constraint violations compared to $k_\mathrm{thresh}$. Note that we chose the minimum penalty term $J_{\mathrm{pen, min}}$ to be much larger than the largest cost objective without constraint violations.
• Figure 4: Offline Planning Experiment. We evaluate the heuristic $\eta_{\mathrm{binom}}$ for different values of $\eta$ and numbers of particles $N$ ($100$, $1000$) by running CC-VPSTO $N_{\mathrm{exp}}=10^5$ times and evaluating the solutions on a set of $N_{\mathrm{eval}}=10^4$ new unseen samples. We compare the heuristic $\eta_{\mathrm{binom}}$ to the Rademacher-complexity bound $\eta_{\mathrm{rad}}$. We also show the mean collision probability $\hat{\eta}_{\mathrm{avg}}$ and the $(1-\beta)$-percentile of the collision probabilities, i.e., $\hat{\eta}_{(1-\beta)}$ across experiments. Last, we show the empirical probability of chance constraint violation $\hat{\beta}$, which is the share of experiments that had a collision probability $\hat{\eta}_i>\eta$.
• Figure 5: Offline Planning Experiment. We show $N_{\mathrm{eval}}=10^4$ red circles for the uncertain obstacle position and the mean trajectory for the two local optima from CC-VPSTO, which used $N=100$ particles in the optimisation, for varying values of $\eta$ across $N_{\mathrm{exp}}=10^5$ experiments. The blue circle shows the robot's radius and starting position.

Theorems & Definitions (12)

• Proposition 1
• Corollary 1
• proof : Proof of Proposition \ref{['prop:cc_binom']}
• Proposition 2
• Proposition 3
• Corollary 2
• Proposition 4
• proof
• Proposition 5
• proof