Nonplanar Model Predictive Control for Autonomous Vehicles with Recursive Sparse Gaussian Process Dynamics

1. Problem Statement

Autonomous vehicle (AV) research has historically focused on flat, planar terrain. However, real-world applications (e.g., off-road driving, search and rescue) require navigation over nonplanar, unstructured terrain characterized by slopes, hills, banks, and bumps.

• The Challenge: Standard vehicle dynamics models (like the single-track bicycle model) assume a flat surface and fail to capture the complex interactions between the vehicle and 3D terrain geometry.
• Limitations of Existing Solutions:
  • Analytical Models: Difficult to derive analytically for complex, varying terrain topologies.
  • Neural Networks: Require large offline training datasets and lack the ability to adapt online to new or changing terrain uncertainties.
• Goal: Develop a control framework that enables real-time, high-accuracy trajectory tracking on 3D surfaces by learning and adapting to terrain-induced dynamics online.

2. Methodology

The authors propose a Nonplanar Model Predictive Control (MPC) framework that integrates a physics-based nominal model with a data-driven learning component.

A. Terrain Modeling

The terrain is modeled as a smooth height field $h(x, y)$. From this, the system derives:

• Surface Normal ($n$): The unit vector orthogonal to the local tangent plane.
• Slope Angle ($\theta$) and Orientation ($\phi$): Derived from the normal vector to define the local inclination and azimuth.
• Roll ($\alpha$) and Pitch ($\gamma$): Computed to transform the global frame to the local tangent frame of the vehicle.

B. Hybrid Dynamics Model

The core innovation is a residual learning approach combining two components:

1. Nominal Model: A standard single-track (bicycle) dynamic model ($f_{ST}$) representing planar vehicle dynamics.
2. Residual Model: A Sparse Gaussian Process (GP) that learns the "residual" dynamics caused by nonplanar effects.
   • Input Vector ($\xi$): Includes vehicle states ($\psi, \delta, v, \beta, r$), control inputs ($a, v_\delta$), and crucially, the terrain geometry ($\alpha, \gamma$).
   • Output: The correction term for velocity, side-slip, and yaw rate ($[v, \beta, r]$).
   • Recursive Updates: Unlike static GP models, this framework uses online recursive least squares to update the GP posterior mean and covariance as new data is collected in real-time. This allows the model to adapt to changing terrain uncertainties without retraining.

C. Control Formulation (MPPI)

The resulting hybrid dynamics model is highly nonlinear, making standard gradient-based MPC solvers difficult to apply. The authors employ Model Predictive Path Integral (MPPI) control:

• Sampling-Based Optimization: MPPI samples thousands of control sequences (trajectories) from a truncated normal distribution.
• Rollout: For each sample, the system propagates the state forward using the hybrid model (Nominal + GP Residual) and the terrain map (interpolating $\alpha$ and $\gamma$ at each step).
• Weighting: Control sequences are weighted based on their cost (tracking error + control effort) using a temperature parameter ($\tau$).
• Execution: The optimal control input is the weighted average of all samples, applied in a receding horizon manner.

3. Key Contributions

1. Geometry-Aware Residual Learning: A novel framework that explicitly conditions a Gaussian Process on terrain geometry (roll/pitch) to learn nonplanar dynamics, rather than just learning generic noise.
2. Online Recursive Adaptation: Implementation of a recursive sparse GP that updates its parameters in real-time, enabling the controller to adapt to varying terrain conditions during operation, overcoming the static nature of neural network approaches.
3. Integration with MPPI: Successfully embedding the learned, non-linear dynamics into an MPPI solver to handle the complexity of 3D trajectory planning.
4. Open-Source Simulation Environment: Development and release of a custom NVIDIA Isaac Sim environment with ROS2 support, featuring various nonplanar tracks (Kidney, L-shaped, Oval) to facilitate reproducibility.

4. Results and Evaluation

The framework was validated in the Isaac Sim environment on three distinct nonplanar tracks with varying slopes (up to 75°) and elevation changes.

• Tracking Performance:
  • Kidney & L-shaped Tracks: The proposed Recursive GP model maintained tight tracking of the reference trajectory. In contrast, the baseline single-track model exhibited significant fluctuations and deviations.
  • Oval Track (High Curvature/Slope): The baseline single-track model failed completely, losing stability and departing the track. The proposed framework maintained stability and traversability.
• Error Metrics:
  • The Recursive GP model demonstrated significantly lower Cross-Track Errors (CTE) compared to the baseline.
  • Histograms of absolute CTE showed a tighter distribution centered near zero for the proposed method.
• Computational Efficiency:
  • Despite the added complexity of GP predictions during trajectory rollouts, the system achieved a planning frequency of ~50 Hz (20ms per cycle). This confirms the feasibility of real-time control on embedded hardware.

5. Significance

This work bridges the gap between theoretical nonplanar dynamics and practical autonomous control.

• Robustness: It proves that learning-based methods can outperform purely physics-based models in unstructured environments without requiring massive offline datasets.
• Adaptability: The recursive update mechanism is critical for real-world deployment where terrain properties (e.g., soil friction, surface compliance) may change dynamically.
• Scalability: By utilizing sparse GPs and GPU-parallelized MPPI, the approach remains computationally tractable for real-time applications, paving the way for safer autonomous off-road navigation.

The authors conclude with plans to validate the framework on physical off-road vehicles and extend the system to handle dynamic obstacles.