Simulate Ground Following and Visualize Robot on Terrain

R2026b

Since R2026b

This example demonstrates how to simulate a four-wheeled robot following terrain using the Four-Wheel Ground Following block in Simulink®, without requiring Unreal Engine or any other 3-D simulation environment.

The example workflow consists of:

1. Generating a terrain heightmap from peaks function

2. Deriving chassis parameters from a rigidBodyTree object

3. Configuring and running the Four-Wheel Ground Following block

4. Interpreting the block outputs

5. Simulating the robot on the terrain surface

Generate Terrain Heightmap

The helper script generateTerrainHeightmap creates a smoothed peaks-based terrain and encodes it as a 16-bit PNG heightmap. The heightmap uses the Unreal Engine encoding convention where pixel value 32768 corresponds to zero elevation:

pixel = Z x (128/zScale) + 32768

The script produces a workspace variable terrainMeta containing the heightmap file path, resolution, terrain size, origin, and elevation scale, all of which are used to configure the block.

generateTerrainHeightmap

Terrain generated:
  Grid size:    201 x 201 pixels
  World extent: 40 x 40 m
  Resolution:   0.20 m/pixel
  Origin:       [-20.0, -20.0, 0.0] m
  Z range:      [-2.56, 3.18] m
  Heightmap saved: C:\Users\user\OneDrive - MathWorks\Documents\MATLAB\ExampleManager\user.Bdoc26b.j3322667\offroad_autonomy-ex81148826\terrain_heightmap.png

Extract Chassis Parameters from Robot Model

The helper script extractChassisParams loads the Clearpath Husky rigid body tree model and derives the wheel layout directly from the model transforms and collision geometry. This produces a robot struct used by the block.

You can derive these parameters from any rigidBodyTree object or URDF:

extractChassisParams

Robot: Clearpath Husky
  WheelBase:    0.5120 m
  TrackWidth:   0.5708 m
  WheelRadius:  0.1651 m
  RearAxlePose: T = [-0.2560, 0.0000, 0.0328] m

Chassis Parameters (derived from rigidBodyTree):
  WheelBase:    0.5120 m (front-to-rear axle distance)
  TrackWidth:   0.5708 m (left-to-right wheel distance)
  WheelRadius:  0.1651 m (from collision cylinder)
  RearAxlePose: translation = [-0.2560, 0.0000, 0.0328] m

disp("  WheelBase:   " + robot.wheelBase + " m")

  WheelBase:   0.512 m

disp("  TrackWidth:  " + robot.trackWidth + " m")

  TrackWidth:  0.5708 m

disp("  WheelRadius: " + robot.wheelRadius + " m")

  WheelRadius: 0.1651 m

disp("  RearAxlePose:   translation = [" ...
    + robot.rearAxlePose(1,4) + ", " ...
    + robot.rearAxlePose(2,4) + ", " ...
    + robot.rearAxlePose(3,4) + "] m")

  RearAxlePose:   translation = [-0.256, 0, 0.03282] m

Open Simulink Model

The model groundFollowingModel contains four key blocks:

• Constant block (2.0 m/s) — Forward velocity input

• Sine Wave block (0.5 amplitude, 0.15 Hz) — Steering rate oscillation

• Ackermann Kinematic Model — Computes 2D planar pose (X, Y, Yaw)

• Four-Wheel Ground Following — Projects the planar pose onto the 3-D terrain surface

open_system("groundFollowingModel");

Configure Block Parameters

The Four-Wheel Ground Following block mask parameters reference workspace variables set by the helper scripts:

Block Parameter	Workspace Variable	Description
Wheelbase (m)	robot.wheelBase	Front-to-rear axle distance
Track width (m)	robot.trackWidth	Left-to-right wheel distance
Elevation scale (m/pixel)	terrainMeta.zScale	Maps pixel values to meters
Wheel radius (m)	robot.wheelRadius	Wheel radius from collision geometry
Rear axle pose	robot.rearAxlePose	4x4 transform from base_link to rear-axle frame
Resolution (m/pixel)	terrainMeta.resolution * [1 1]	Pixel spacing in X and Y
Origin (m)	terrainMeta.origin	World position of terrain grid start
File path	terrainMeta.heightmapFile	Path to 16-bit PNG

Inputs: X, Y, and Yaw of the rear axle frame in the world frame (from Ackermann kinematic model).

Outputs: Translation (1x3 vector in meters) and Rotation (3x3 rotation matrix) of the vehicle origin frame, base_link, on the terrain. The block internally applies wheel radius to lift above the contact plane and rear axle pose to transform from the rear-axle frame to the vehicle origin.

Run Simulation

Simulate the model. The Ackermann Kinematic Model drives the vehicle at constant speed with oscillating steering, while the Four-Wheel Ground Following block computes the terrain-following pose at each timestep.

out = sim("groundFollowingModel.slx");
logTranslation = out.logTranslation;
logRotation = out.logRotation;

Interpret Block Outputs

The block outputs the vehicle origin frame (base_link), which accounts for wheel radius and rear axle pose internally. Extract position and Euler angles from the logged data.

time = logTranslation.Time;
nSteps = length(time);
pos = squeeze(logTranslation.Data);
if size(pos, 2) ~= 3
    pos = pos';
end
roll = zeros(nSteps, 1);
pitch = zeros(nSteps, 1);
yaw = zeros(nSteps, 1);
rotData = squeeze(logRotation.Data);
for k = 1:nSteps
    if ndims(rotData) == 3
        R = rotData(:,:,k);
    else
        R = reshape(rotData(k,:), 3, 3)';
    end
    pitch(k) = asin(-R(3,1));
    roll(k) = atan2(R(3,2), R(3,3));
    yaw(k) = atan2(R(2,1), R(1,1));
end

Visualize Block Outputs

Elevation Profile

The Translation output Z component shows how the vehicle origin (base_link) follows the terrain elevation along its driven path..

figure("Position", [100, 100, 800, 400]);
plot(time, pos(:,3), "b-", "LineWidth", 1.5);
xlabel("Time (s)"); ylabel("Z (m)");
title("Rear-Axle Elevation (Translation Output)");

grid on;

Roll and Pitch

The Rotation output encodes terrain-induced body tilts. Roll and pitch arise from the ground plane fit through the four wheel contact points, not from physics simulation.

figure("Position", [100, 100, 800, 400]);
plot(time, rad2deg(roll), "b-", time, rad2deg(pitch), "r-", "LineWidth", 1.5);
xlabel("Time (s)"); ylabel("Angle (deg)");
title("Terrain-Induced Roll and Pitch from Rotation Output");
legend("Roll", "Pitch");
grid on;

Vehicle Heading

The yaw component of the Rotation output tracks the vehicle heading, driven by the Ackermann steering input.

figure("Position", [100, 100, 800, 400]);
plot(time, rad2deg(yaw), "k-", "LineWidth", 1.5);
xlabel("Time (s)"); ylabel("Yaw (deg)");
title("Vehicle Heading (Yaw) from Rotation Output"); grid on;

Review Simulation Results

totalDist = sum(sqrt(diff(pos(:,1)).^2 + diff(pos(:,2)).^2));
disp("Simulation Results:")

Simulation Results:

disp("  Duration:  " + time(end) + " s")

  Duration:  30 s

disp("  Distance:  " + totalDist + " m")

  Distance:  62.1547 m

disp("  Max roll:  " + max(abs(rad2deg(roll))) + " deg")

  Max roll:  25.582 deg

disp("  Max pitch: " + max(abs(rad2deg(pitch))) + " deg")

  Max pitch: 23.542 deg

disp("  Z range:   [" + min(pos(:,3)) + ", " + max(pos(:,3)) + "] m")

  Z range:   [-0.29073, 1.5317] m

Simulate Robot on Terrain

The helper script visualizeGroundFollowing builds a floating-base rigid body tree and animates the Husky traversing the terrain surface. It uses the block outputs (Translation and Rotation) directly to place the robot mesh.

visualizeGroundFollowing

Simulation Results:
  Duration:   30.0 s
  Distance:   62.2 m
  Max roll:   25.58 deg
  Max pitch:  23.54 deg
  Z range:    [-0.29, 1.53] m