A Terrain-Adaptive epsilon-Constraint MPC for Uneven Terrain Kinodynamic Planning

Technical Summary: A Terrain-Adaptive $\epsilon$-Constraint MPC for Uneven Terrain Kinodynamic Planning

Problem Statement
Autonomous navigation for car-like vehicles in unstructured, uneven environments (e.g., construction sites, disaster zones, planetary exploration) presents a dual challenge: generating collision-free paths while respecting complex kinodynamic constraints. Traditional mesh-based planners, such as those utilizing the Fast Marching Method (FMM), excel at producing terrain-aware geometric paths but often fail to incorporate dynamic feasibility, such as velocity limits and vehicle stability. Conversely, kinodynamic planners frequently struggle to balance competing objectives—specifically path efficiency versus pose stability (preventing tip-over)—without falling into local optima or relying on overly simplified 2D assumptions. Existing data-driven approaches (e.g., Deep Reinforcement Learning) often lack generalization across diverse start-goal configurations and fail to integrate continuous global guidance structures. Furthermore, standard multi-objective optimization methods often rely on weighted scalarization, which can allow path length to dominate the solution, neglecting critical stability constraints on rough terrain.

Methodology
The authors propose a receding horizon control framework that integrates a semi-parametric vehicle-terrain model with a terrain-adaptive $\epsilon$-constraint Model Predictive Control (MPC).

1. Vehicle-Terrain Modeling:

   • Semi-Parametric Approach: The system combines an analytical kinematic model for car-like vehicles with a data-driven residual correction. The nominal model predicts state evolution based on linear and angular velocities.
   • Sparse Gaussian Process (SGP): To account for unmodeled terrain effects (e.g., slip, complex contact dynamics), an SGP is trained on local terrain descriptors to predict the residual error between the nominal model and actual system behavior.
   • Terrain Descriptors: The model utilizes a rich set of descriptors including area-weighted normal vectors, surface roughness ($\sigma_z$), Gaussian and mean curvatures, and total neighborhood area.
   • Height Function: A projection-based method determines terrain height $h(x,y)$ by mapping query points to the nearest face on a triangular mesh, ensuring accurate orientation calculations.

2. Optimization Framework:

   • Multi-Objective Formulation: The planner optimizes two competing objectives:
     • Path Efficiency ($f_1$): Minimizes directional misalignment with a global FMM vector field, weighted by a goal-proximity scaling factor.
     • Pose Stability ($f_2$): Minimizes cumulative orientation deviations (roll and pitch) relative to the local terrain normal, penalizing deviations beyond a safe threshold ($\alpha_{safe}$) and enforcing a hard limit ($\alpha_{max}$) to prevent tip-over.
   • Terrain-Adaptive $\epsilon$-Constraint: Instead of weighted scalarization, the method converts the problem into a constrained single-objective optimization. The stability constraint ($f_2 \leq \epsilon_{stab}$) is dynamically adjusted based on local terrain roughness and slope. In challenging terrain, the stability budget tightens; in benign terrain, it relaxes, allowing for more aggressive exploration.
   • Sampling Strategy: Candidate control sequences are generated via a hybrid approach:
     • Warm-starting: Shifting previous successful control sequences with added noise to ensure temporal continuity.
     • FMM-Biased Sampling: Blending controls suggested by the global FMM vector field with random exploration to escape local minima.

Key Contributions
The paper claims four primary contributions:

1. A semi-parametric model for car-like vehicle motion on uneven triangular meshes, combining analytical dynamics with SGP-based residual learning.
2. A cost function for mesh navigation derived from a continuous FMM vector field.
3. A solution approach utilizing terrain-adaptive $\epsilon$-constraint optimization within a receding horizon framework to generate non-dominated trajectories that balance efficiency and stability.
4. A ROS-compatible planner (Move Base Flex) designed for real-time deployment.

Experimental Results
The proposed method ($\epsilon$-MPC) was evaluated in simulation (Gazebo) and real-world experiments using an AgileX Scout Mini robot on two distinct 3D mesh terrains. It was compared against Model Predictive Path Integral (MPPI) and Genetic Algorithmic Kinodynamic (GAKD) baselines.

• Performance Metrics: The proposed method achieved a 94% navigation success rate in simulation, outperforming MPPI (87%) and GAKD (89%).
• Stability: It reduced maximum orientation deviation by 24% compared to MPPI and improved the multi-objective trade-off metric ($T$) by 23%.
• Ablation Studies:
  • FMM Bias: An FMM bias strength of $\alpha_{FMM}=0.7$ yielded optimal results, improving success rates by 11–27% over pure random sampling.
  • Adaptivity: The terrain-adaptive constraint approach eliminated all tip-over incidents observed with fixed epsilon constraints and reduced maximum orientation deviation by 32%.
  • Real-World Deployment: In physical experiments, the method maintained an 80% success rate (vs. 67% for MPPI), successfully navigating steep sections where baselines failed due to excessive roll angles. Planning times remained within real-time constraints (~35–48 ms per iteration).

Significance and Claims
The paper asserts that its primary significance lies in providing a principled framework that explicitly addresses the multi-objective nature of kinodynamic planning on uneven terrain. By dynamically adjusting stability constraints based on local terrain descriptors, the approach achieves a superior balance between path optimality and physical safety without relying on complex, computationally expensive evolutionary searches (like NSGA-II, which was found to be too slow for real-time use). The authors conclude that while weighted scalarization methods can optimize individual objectives, the terrain-adaptive $\epsilon$-constraint method is more effective for ensuring feasible, safe navigation in unstructured outdoor environments, offering a practical solution that scales from simulation to real-world deployment.