Safe Learning Predictive Control for Ego-World Robotic Systems

Technical Summary: Safe Online World Policy Learning Model Predictive Control (SOWL-MPC)

Problem Statement
The paper addresses the challenge of safe autonomous navigation for an "ego" robot operating in a shared environment with a "world" robot whose control policy is unknown and potentially non-stationary. This scenario, termed the EGO–WORLD framework, assumes the ego robot has access only to noisy state measurements of the world robot ($\tilde{z}_t = z_t + w_t$) and prior knowledge of the world robot's nominal dynamics (drift vector $g(z)$ and control matrix $H(z)$), but lacks knowledge of the world robot's actual control policy $\pi(z)$. Traditional reactive methods (e.g., Dynamic Window Approach, Control Barrier Functions) are often myopic, while existing predictive approaches typically rely on offline-trained models or fixed motion assumptions that fail to adapt to unseen behaviors. The core problem is to design an online control framework that can iteratively learn the latent world policy from streaming data and utilize this learning to perform safe, collision-free maneuvers via Model Predictive Control (MPC).

Methodology
The proposed solution, SOWL-MPC, integrates online learning with a receding-horizon control scheme through three main stages:

1. Policy Modeling and Offline Pretraining:
   The unknown world policy $\pi(z)$ is modeled as a set of independent Sparse Variational Gaussian Processes (SVGPs), one for each component of the policy vector. Initially, hyperparameters are selected via offline pretraining on a dataset of noisy state-input pairs. The input data (world control inputs) are reconstructed online from consecutive noisy state observations using a least-squares approach based on the known nominal dynamics.

2. Online Learning via Online Variational Conditioning (OVC):
   To adapt to new data in real-time without the prohibitive computational cost of retraining, the authors employ Online Variational Conditioning (OVC). Instead of storing the entire history of data, OVC maintains sufficient statistics (inducing locations, variational means, and covariances) to update the SVGP parameters recursively. The algorithm projects the current posterior into "pseudo-data," combines it with the new observation, and updates the inducing points and variational parameters. This allows the system to learn the latent policy directly from streaming noisy measurements.

3. Uncertainty-Aware Trajectory Prediction and Control:
   The learned policy is propagated through the nonlinear world dynamics using a first-order Gaussian moment propagation scheme (analogous to an Extended Kalman Filter prediction step). This generates a multi-step probabilistic forecast of the world robot's future trajectory, including a measure of predictive uncertainty.
   These predictions are fed into a nonlinear MPC controller. The MPC formulation includes:

   • Cost Function: A standard tracking cost with penalties for control effort and input smoothness.
   • Safety Constraints: Collision avoidance is enforced by inflating the safety distance between the robots. The safety margin is dynamically adjusted based on the predictive uncertainty (trace of the covariance matrix) scaled by a user-defined safety parameter $n_\sigma$. Hard constraints are applied to the immediate next step, while slack variables allow for temporary relaxation in the later prediction horizon to ensure feasibility.

Key Contributions
The paper outlines three primary contributions:

1. SOWL-MPC Framework: A novel control strategy for EGO–WORLD systems that learns the latent state-feedback policy of an uncooperative world robot online, rather than learning the system dynamics or relying on offline predictors. This formulation leverages the lower dimensionality of the input space compared to the state space for data efficiency.
2. Application of OVC to Robotics: The first application of Online Variational Conditioning to online policy identification and trajectory prediction in robotic systems. This enables the SVGP models to update continuously on streaming data with fixed computational complexity ($O(M^3)$ per update, where $M$ is the number of inducing points).
3. Real-Time Validation: A full-stack implementation in ROS 2 (integrated with the CHOIRBOT toolbox) demonstrating real-time feasibility. The approach was validated through extensive Monte Carlo virtual experiments and real-world hardware tests involving overtaking and crossing maneuvers.

Experimental Results
The strategy was evaluated in both virtual (Webots/ROS 2) and real-world (indoor arena with NVIDIA JetRacer cars) settings:

• Prediction Accuracy: In virtual experiments on the ETHZ MOBIL and LEMNISCATE tracks, SOWL-MPC significantly outperformed an offline-trained GP baseline and a Constant Velocity (CV) baseline. The Average Displacement Error (ADE) for SOWL-MPC stabilized at approximately 0.05 m after the initial learning phase, compared to errors exceeding 0.4 m for the CV baseline and periodic spikes above 0.7 m for the offline baseline.
• Safety Guarantees: Monte Carlo simulations demonstrated that tuning the safety parameter $n_\sigma$ allows for a trade-off between aggressiveness and safety. With $n_\sigma = 3$, the method achieved a 100% success rate in avoiding collisions across 50 trials, whereas a deterministic approach ($n_\sigma = 0$) failed in 36% of trials.
• Real-Time Performance: The average computation time per control step remained well below the 50 ms sampling time even for prediction horizons up to $T=40$, confirming real-time feasibility.
• Real-World Validation: On physical hardware, the system successfully executed anticipatory avoidance maneuvers, with the ADE stabilizing around 0.05 m, confirming the transferability of the virtual results to real-world dynamics.

Significance and Claims
The paper claims that SOWL-MPC represents a significant advancement by bridging the gap between reactive safety and predictive planning in environments with unknown, adaptive agents. By explicitly modeling and learning the policy of the world robot rather than just its dynamics, and by utilizing OVC for efficient online updates, the framework achieves safety guarantees that adapt to the observed behavior of the world robot. The authors emphasize that this is the first application of OVC to this specific robotic problem, enabling data-efficient learning of latent policies that can be directly integrated into uncertainty-aware control loops. The work demonstrates that safe, anticipatory navigation is achievable even when the world robot's control logic is completely unknown and only noisy state observations are available.