Provably Safe Trajectory Generation for Manipulators Under Motion and Environmental Uncertainties

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.

💡 Research Summary

The paper addresses the challenging problem of generating safe, efficient trajectories for high‑dimensional robot manipulators operating in environments that are both uncertain and non‑convex. Traditional chance‑constrained planners typically assume Gaussian uncertainties and convex obstacles, which limits their applicability to real‑world scenarios where obstacle shapes, sizes, and locations may follow arbitrary probability distributions, and where motion noise is non‑Gaussian. To overcome these limitations, the authors propose a three‑layered framework that combines (i) a data‑driven predictive model based on a Rigid Manipulator Deep Stochastic Koopman Operator (RM‑DeSKO), (ii) a hierarchical risk verification pipeline that fuses parallel physics simulations with Sum‑of‑Squares (SOS) programming, and (iii) a Model Predictive Path Integral (MPPI) controller that incorporates binary collision outcomes from the SOS filter into its policy update.

The RM‑DeSKO model lifts noisy joint states into a high‑dimensional observable space, encodes each observable as a Gaussian with learned mean and diagonal covariance, and propagates these distributions forward using a learned linear Koopman operator (matrices A and B). A decoder then maps the lifted observables back to the original state space, providing a fast, probabilistic rollout of many candidate trajectories. This approach replaces costly physics‑based rollouts with a neural‑network‑driven surrogate that still captures the stochastic dynamics of the robot under tracking errors.

Risk is represented by a continuous “risk contours map” derived from Cantelli’s inequality. Each obstacle is described by a polynomial inequality o_i(p, ω_i) ≤ 0 where ω_i follows an arbitrary distribution. By computing first‑ and second‑order moments of ω_i, the authors construct deterministic inner approximations C^Δ_r_i of the safe region using polynomial functions P₁_i and P₂_i. The safety of a candidate tube (the swept volume of the robot links along a trajectory) is then certified via SOS programming: the polynomial conditions that guarantee the tube stays within C^Δ_r_i are expressed as SOS constraints with auxiliary multiplier polynomials σ_j_i and a positive‑definite matrix Q. This SOS verification can be solved efficiently and, when combined with massively parallel physics simulations, quickly discards unsafe trajectories while retaining those that satisfy the prescribed risk bound Δ.

The MPPI controller samples a batch of control sequences, evaluates their costs using the RM‑DeSKO predictions, and updates the Gaussian policy parameters (mean μ and covariance Σ) based on importance‑weighted samples. Crucially, only trajectories that pass the SOS safety filter contribute to the weight calculation, effectively feeding binary collision information back into the stochastic optimization loop. This results in a policy that both minimizes the task‑related cost and respects the user‑defined risk tolerance.

Experimental validation is performed on two 6‑DOF manipulators (including a UR5‑type arm) in both simulation and real‑world settings. Scenarios involve non‑convex obstacles with uncertain geometry (e.g., uniformly distributed radii) and a human‑robot collaboration task where a human hand moves near the robot. The framework operates at roughly 6 Hz replanning frequency, achieving zero observed collisions when the risk tolerance is set to Δ = 0.05. Compared to baseline Gaussian‑based chance‑constrained planners, the proposed method reduces path length and execution time by approximately 15–20 % while maintaining strict safety guarantees. Moreover, the sim‑to‑real transfer incurs negligible performance loss, demonstrating the robustness of the learned Koopman model.

In summary, the paper makes four primary contributions: (1) a formal formulation of risk‑bounded motion planning for manipulators in non‑convex, probabilistically described environments; (2) the introduction of a deep stochastic Koopman operator model that provides fast, accurate probabilistic state predictions; (3) a hierarchical verification scheme that couples SOS‑based formal certification with parallel simulation to enforce risk constraints efficiently; and (4) an integration of this verification into an MPPI controller, yielding a real‑time, safety‑aware trajectory generator suitable for human‑robot collaboration. The approach opens avenues for deploying manipulators in complex, unstructured workplaces where safety cannot be compromised.