Vehicle-Specific Configuration Space Distance Fields for Terrain-Aware Predictive Control of Heavy Robots

=== SUMMARY ===
Technical Summary: Vehicle-Specific Configuration Space Distance Fields for Terrain-Aware Predictive Control of Heavy Robots

Problem Statement
Heavy mobile machines, such as 6,000-kg skid-steer robots, operate in unstructured environments where terrain acts as both a support surface and a source of obstacles. A critical challenge in navigating these environments is that a planar map often misclassifies terrain as "occupied" when, in reality, low obstacles may fit beneath the robot's chassis while still providing wheel support. Conversely, an obstacle might be passable for the wheels but cause a collision with lower body components (e.g., bogie strips or the central chassis). Traditional navigation models often fail to distinguish between wheel-contact regions and components with varying ground clearances, leading to overly conservative paths or unsafe maneuvers. The core problem addressed is how to express these vehicle-specific geometric requirements—wheel support, cross-wheel consistency, and component-wise underside clearance—within a representation suitable for real-time predictive control.

Methodology
The authors propose a framework that integrates a vehicle-specific terrain representation with Nonlinear Model Predictive Control (NMPC) for a heavy skid-steer robot. The methodology consists of three primary components:

1. Vehicle-Specific Terrain Admissibility:
   The system converts a Truncated Signed Distance Field (TSDF) from LiDAR into a height map. It then evaluates position-heading configurations on a grid by testing four specific criteria:

   • Wheel-Patch Support: A 5×3 sample patch is fitted to each of the four wheels using least-squares planes to ensure local planar support and limit inclination.
   • Cross-Wheel Consistency: A common support plane is fitted to the first three wheel patches; the fourth patch is tested for consistency against this plane to detect geometric inconsistencies (e.g., a wheel xuankong or a rock under a single wheel).
   • Component-Wise Clearance: The vehicle footprint is partitioned into regions (central chassis, bogie strips, outer envelope, and stowed manipulator). Each region has a distinct permissible terrain height threshold relative to the support plane. This allows the system to accept configurations where an obstacle fits under the chassis but is rejected if it strikes the lower bogie strips.
   • Attitude Limits: Geometric roll and pitch limits are enforced on the fitted support planes.

2. Configuration Space Distance Field (CDF):
   The binary classification of admissible and rejected configurations is transformed into a continuous, signed Configuration Space Distance Field (CDF). This field is periodic in the heading dimension to account for the cyclic nature of orientation. The distance metric combines translation and heading, weighted by a scaling factor. The CDF provides a scalar cost and a gradient to the controller, representing the proximity to geometric violations. A conditional grid-level margin is derived to relate the interpolated field value to the execution deviation.

3. Predictive Control and Estimation:
   The CDF is integrated into an NMPC framework. The cost function includes terms for reference tracking, wheel-speed smoothing, and a one-sided penalty based on the CDF value (active only when the distance to a rejected configuration is below a threshold).

   • Online Kinematic Estimation: An Extended Kalman Filter (EKF) estimates effective skid-steer parameters (effective wheel radius, half-track, and longitudinal offset of the rotation center) using measured wheel speeds and external pose data. These parameters update the prediction model in real-time.
   • Execution: The system runs on a GPU-accelerated pipeline for terrain processing (classification and distance transform) and a CPU-based solver (HPIPM) for NMPC. The controller updates wheel commands at approximately 150 Hz, while terrain updates occur at roughly 5 Hz.

Key Contributions
The paper makes three specific contributions:

1. A Terrain Field Built from Wheel Support and Component Clearance: The authors develop a heading-periodic CDF that distinguishes surfaces supporting the wheels from the space available beneath specific underside components. This classification enables the navigation system to exploit configurations where obstacles pass under the chassis but are avoided by the wheels. A theoretical result (Proposition 1) provides a conditional grid margin, explicitly relating the field value, grid resolution, and execution deviation.
2. Direct Integration with Skid-Steer Predictive Control: The CDF is incorporated as a soft penalty into the NMPC cost function. This formulation allows the optimizer to trade reference tracking against movement toward geometrically admissible configurations. The system couples this with bounded wheel-speed commands, slew-rate limits, and online effective steering parameter estimation.
3. Complementary Recorded Simulation and Full-Scale Experimental Evidence: The work provides extensive validation through MuJoCo simulations and full-scale outdoor experiments on a 6,000-kg robot. The evaluation covers tracking performance, body attitude, computational timing, and the impact of kinematic calibration on prediction accuracy.

Results

• Simulation (MuJoCo): Comparisons on simple and rocky courses show that the full terrain-aware controller achieves significantly lower tracking errors and smaller peak body pitch angles compared to variants without terrain costs. However, on rocky terrain, a variant without wheel checks achieved lower tracking RMSE but higher peak pitch, illustrating a trade-off between reference following and body attitude.
• Activation Distance: In circular tracking simulations, increasing the CDF activation distance (the threshold for applying the terrain penalty) resulted in increased tracking deviation, confirming that the controller prioritizes terrain margin over strict reference tracking when the margin is tight.
• Continuous Traversal: A continuous multi-lap run demonstrated sustained traversal over 11 nominal reference periods with a mean tracking RMSE of 0.742 m.
• Outdoor Experiments: Field tests on gravel with scattered rocks confirmed the system's ability to execute complex maneuvers. The online kinematic estimator reduced position prediction errors by 63–73% compared to using nominal geometry alone. The system achieved mean controller update times of 6.5–6.7 ms, with the complete terrain-processing pipeline taking approximately 67 ms on an RTX 3050 GPU (with the distance transformation stage alone accounting for roughly 50 ms).
• Kinematic Calibration: Replay of field data showed that estimating effective steering parameters online significantly improved prediction accuracy over fixed calibration, particularly for position errors.

Significance and Claims
The authors claim that the central contribution is the construction and use of a vehicle-specific interface (the CDF) within an executable navigation hierarchy. The paper argues that by expressing wheel support and underside clearance as a configuration space distance field, the system can directly optimize terrain negotiation and reference tracking within the same control problem. The significance lies in the ability to navigate heavy robots through terrain that appears occupied in planar maps but is actually traversable due to the vehicle's specific geometry. The work supports the integration of detailed, component-level terrain reasoning into predictive navigation, validated by both simulation and full-scale field data. The authors remain modest regarding collision avoidance, noting that the CDF acts as a soft penalty rather than a hard constraint, and that the evaluation focuses on recorded outcomes rather than inferring collision frequencies from the data.