Provably Safe Trajectory Generation for Manipulators Under Motion and Environmental Uncertainties

This paper presents a provably safe motion planning framework for robot manipulators in uncertain, non-convex environments by integrating a deep stochastic Koopman operator for state prediction with a hierarchical sum-of-squares verification filter within a Model Predictive Path Integral controller to generate certified, collision-free trajectories.

The Gist

Imagine you are trying to guide a clumsy, long-armed robot through a cluttered room to pick up a cup. But there's a catch: the robot's joints are a bit wobbly (it doesn't move exactly where you tell it to), and the furniture in the room might be slightly different than you think (maybe a chair is a bit closer than your map says, or a person is waving their arms unpredictably).

If you tell the robot to move in a straight line, it might crash. If you tell it to move super slowly to be safe, it will take forever and be useless.

This paper presents a new "brain" for the robot that solves this problem. It allows the robot to move fast and efficiently while still having a mathematical guarantee that it won't crash, even when things are uncertain.

Here is how they did it, broken down into simple concepts:

1. The "Crystal Ball" (RM-DeSKO)

Most robots try to guess where they will be in the future based on simple physics equations. But when a robot is wobbly, those simple guesses are often wrong.

The authors built a special AI "Crystal Ball" called RM-DeSKO.

• The Analogy: Imagine you are throwing a ball. A simple model says, "It will go in a perfect arc." But in reality, the wind is gusty, and your hand shakes.
• What the AI does: Instead of predicting one spot where the robot will be, this AI predicts a cloud of possibilities. It says, "There is a 90% chance the robot's hand will be here, a 5% chance it will be there, and a tiny chance it will be over there."
• Why it matters: By understanding this "cloud" of where the robot might actually end up, the planner can avoid areas where the cloud overlaps with obstacles.

2. The "Safety Net" (Hierarchical Verification)

Even with a good crystal ball, you don't want to trust it 100% without checking. The paper uses a two-step safety check, like a bouncer at a club.

• Step 1: The Quick Scan (Physics Simulation): The robot runs a super-fast, rough simulation in its head (using a tool called IsaacGym). It asks, "If I move this way, do I look like I'm about to hit something?" If the answer is a loud "YES," it immediately throws that idea away. This is fast but not perfectly precise.
• Step 2: The Strict Math Check (SOS Programming): If the robot passes the quick scan, it runs a rigorous mathematical proof (called Sum-of-Squares or SOS).
  • The Analogy: Think of the quick scan as a security guard looking at your ID. The math check is like a forensic expert analyzing the ID to make sure it's 100% real.
  • This step proves, with mathematical certainty, that the probability of hitting an object is below a specific limit (e.g., less than 5%). If it passes, the robot is allowed to move.

3. The "Dance Instructor" (MPPI Controller)

The robot needs to decide which path to take. The authors use a method called MPPI (Model Predictive Path Integral).

• The Analogy: Imagine a dance instructor trying to teach a robot to dance through a crowd. The instructor throws out 1,000 random dance moves (trajectories).
• Most of these moves result in a crash (or a near-miss). The instructor looks at the "bad" moves and says, "Okay, don't do that."
• Because the robot has the "Crystal Ball" (RM-DeSKO) and the "Safety Net" (SOS), the instructor can quickly learn which moves are safe and which are risky, refining the dance until the robot finds the perfect, smooth path through the crowd.

4. The Real-World Test: Tying Rebar

To prove it works, they didn't just test it in a video game. They tested it on a real robot arm (a UR5e) in a construction setting.

• The Task: The robot had to tie two steel bars together while a human worker stood nearby holding the bars.
• The Challenge: The human's arms were moving (uncertainty), the robot's payload was slightly heavy and shifting (uncertainty), and the robot's own movements were slightly inaccurate.
• The Result: The robot successfully navigated the tight space, dodging the human's arms without stopping or crashing, even though the human was moving and the robot was "wobbly." It did this by constantly updating its "cloud of possibilities" and checking the math safety net before every move.

Why This is a Big Deal

• Old Way: Robots were either too slow (playing it safe by moving inches at a time) or too risky (crashing because they didn't account for wobbly movements).
• New Way: This system gives the robot a mathematical promise of safety. It can say, "I am 95% sure I won't hit you," and it can prove it. This allows robots to work faster and closer to humans, which is essential for things like assembly lines, construction, and helping people at home.

In short, they taught the robot to imagine all the ways it could mess up, check the math to ensure it won't actually mess up, and then move confidently through a chaotic world.

Technical Summary

Here is a detailed technical summary of the paper "Provably Safe Trajectory Generation for Manipulators Under Motion and Environmental Uncertainties."

1. Problem Statement

The paper addresses the challenge of safe and optimal motion planning for high-dimensional robot manipulators operating in environments characterized by two types of uncertainty:

• Motion Uncertainty: Imperfect tracking, joint position/velocity noise, and dynamic errors during execution.
• Environmental Uncertainty: Non-convex obstacles with probabilistic geometry, size, and location (e.g., human workers moving unpredictably or objects with uncertain dimensions).

Key Challenges:

• Existing chance-constrained planners often assume Gaussian uncertainties and convex obstacles, which limits their applicability to complex, real-world scenarios.
• Methods handling non-convex obstacles often rely on conservative safety margins, sacrificing efficiency, or require computationally expensive collision checks that prevent real-time replanning.
• Neural network-based planners often lack formal safety guarantees (certifiability).

The goal is to generate a trajectory sequence $\{u_0, \dots, u_{T-1}\}$ that steers a manipulator from a start to a goal while ensuring the probability of collision with any uncertain obstacle remains below a user-defined risk tolerance $\Delta$.

2. Methodology

The authors propose a novel framework integrating a learned dynamics model, a sampling-based controller, and a hierarchical verification system. The workflow consists of three main components:

A. Rigid Manipulator Deep Stochastic Koopman Operator (RM-DeSKO)

To handle motion uncertainty without relying on perfect physical models, the authors train a neural network based on the Deep Stochastic Koopman Operator (DeSKO) theory.

• Mechanism: The model lifts the non-linear state space (joint positions and velocities) into a higher-dimensional "lifted" linear space where the dynamics are approximately linear.
• Stochasticity: It models the observables in the lifted space as Gaussian distributions. It uses encoders to predict the mean and covariance of these observables, allowing it to propagate the state distribution forward over a planning horizon $H$.
• Integration: This model is embedded within the Model Predictive Path Integral (MPPI) controller to generate "rollouts" (future state predictions) that account for tracking errors, providing more accurate cost estimation than standard linear models.

B. Hierarchical Collision Risk Verification

To ensure formal safety guarantees without the computational burden of checking every trajectory point with Sum-of-Squares (SOS) programming, a two-stage verification process is used:

1. Fast Filtering (Physics Simulation): The MPPI controller uses IsaacGym to simulate the robot's interaction with obstacles. It calculates a primary collision cost based on contact forces ($F_{contact}$) to guide the optimization. This handles the bulk of the trajectory search efficiently.
2. Formal Certification (SOS Programming): Before executing a selected control signal, the system performs a rigorous check:
   • The robot's links are approximated as a sequence of ellipsoids.
   • Obstacles are represented by polynomial inequalities (with uncertain parameters).
   • Sum-of-Squares (SOS) decomposition is used to mathematically prove that the probability of the robot's ellipsoids intersecting the uncertain obstacle region is $\le \Delta$.
   • If the SOS condition fails, the trajectory is rejected, and the cost is updated to force the MPPI to re-optimize.

C. Control Loop

The system operates in a receding-horizon manner:

1. Sample control sequences.
2. Predict future states using RM-DeSKO.
3. Compute costs using IsaacGym contact forces.
4. Update MPPI policy.
5. Verify the best candidate trajectory using SOS decomposition.
6. Execute the control if verified; otherwise, re-plan.

3. Key Contributions

1. First Risk-Bounded Planning for Non-Convex/Probabilistic Obstacles: The paper formulates and solves the risk-bounded motion planning problem for manipulators facing non-convex obstacles with probabilistic geometry, size, and location, a scenario previously difficult to handle with formal guarantees.
2. RM-DeSKO Model: Development of a specialized neural network model that robustly predicts the state distribution of rigid manipulators under motion uncertainty, outperforming standard baselines (LSTM, Transformer, DKU) in trajectory prediction accuracy.
3. Hierarchical Verification Framework: A novel method combining fast physics-based simulation (for efficiency) with SOS programming (for formal certification). This allows the system to handle non-Gaussian uncertainties and provide mathematical guarantees on collision probability.
4. Sim-to-Real Transfer: Successful deployment of the learned policy on a real-world UR5e robot in a human-robot collaboration task (rebar tying) without retraining, demonstrating robustness to real-world noise and environmental uncertainty.

4. Experimental Results

The framework was validated on a Franka Emika Panda (simulation) and a UR5e (real-world).

• State Prediction: RM-DeSKO achieved the lowest maximum error (0.789) compared to Transformers, LSTMs, and DKU, proving its superiority in handling unknown noise for MPPI rollouts.
• Motion Planning Performance (Simulation):
  • Success Rate: 94% (vs. 89% for baseline, 0% for Transformer/LSTM baselines in complex tasks).
  • Time to Goal (TTG): 34.59s (vs. 47.25s for baseline).
  • Trajectory Length: 1.168 (vs. 2.273 for baseline), indicating more direct and efficient paths.
  • The method successfully navigated narrow gaps between heart-shaped, uncertain obstacles where other methods failed or got stuck.
• Real-World Experiment (Human-Robot Collaboration):
  • Task: A UR5e robot tied rebar while avoiding a human worker's arms (modeled as uncertain non-convex obstacles).
  • Results: Achieved a 90% success rate (9/10 trials) across different noise levels.
  • Efficiency: Replanned at 6 Hz, allowing for responsive adjustments to the human's movement.
  • Safety: The robot successfully avoided collisions while maintaining a collision probability bound of $\Delta = 10\%$.

5. Significance and Impact

• Safety vs. Efficiency Trade-off: The paper demonstrates that it is possible to achieve formal safety guarantees (mathematical proof of low collision probability) without resorting to overly conservative safety margins that degrade performance.
• Handling Real-World Complexity: By addressing non-convex geometries and non-Gaussian uncertainties, the method bridges the gap between theoretical planning and messy real-world applications like construction and assembly.
• Computational Feasibility: The hierarchical approach (using SOS only as a filter rather than for every sample) makes formally certified planning computationally feasible for real-time control (6 Hz).
• Generalization: The ability to transfer a model trained in simulation directly to a real robot (zero-shot sim-to-real) for complex tasks highlights the robustness of the Koopman-based learning approach.

In conclusion, this work provides a robust, mathematically grounded framework for deploying autonomous manipulators in dynamic, uncertain, and cluttered environments, significantly advancing the state of the art in safe robotic motion planning.