Simulate Earth Moving with Autonomous Excavator in Construction Site

Since R2024b

This example uses:

This example shows how to simulate earth moving workflows for an excavator by using Simulink®, in which you excavate the ground to create a depression and then move the spoil to a dump truck. This workflow has a modular design and you can modify it per your requirements.

This example uses these toolboxes for visualizing the scene, collecting sensor data, motion planning, and simulating the environment along with other vehicles in Unreal Engine.

Robotics System Toolbox Offroad Autonomy Library™ — Adds and simulates the dump truck in Simulink® and Unreal Engine.
Robotics System Toolbox™ — Models and simulates the excavator in Simulink® and Unreal Engine, plans path, and generates trajectory in multiple steps of the workflow.
Simulink 3D Animation™ — Provides the Unreal Engine simulation environment.
Simscape™ Multibody™ — Simulates a high-fidelity excavator model using low-level controls.

The steps listed below provide a comprehensive overview of the example workflow. In these steps of the workflow, the output of one module becomes the input for the next module.

FollowExcavatorTrajectoryInUnrealWithSimscapeExample_01.png

To efficiently demonstrate the example workflow, certain stages are extracted into standalone examples for clearer explanations. The standalone examples use additional toolboxes for the following purposes:

Image Processing Toolbox™ and Computer Vision Toolbox™ — Visualize a point cloud using pcshow (Computer Vision Toolbox) and merge multiple point clouds using pcmerge (Computer Vision Toolbox).
Navigation Toolbox™ — Represent the environment as an occupancy map with the occupancyMap3D (Navigation Toolbox) object.
Optimization Toolbox™ — Generate the trajectory using the contopptraj function from Robotics System Toolbox.

To use the Simulation 3D Physics Dump Truck block, you must download and install the Robotics System Toolbox Offroad Autonomy Library support package from Add-On Explorer. For more information about installing add-ons, see Get and Manage Add-Ons.

Initial Setup

Download Construction Site Scene

Check the maps available in the server.

sim3d.maps.Map.server

Download the Construction site scene from the server.

sim3d.maps.Map.download("Construction site scene")

Check if the downloaded map is available in your local machine.

sim3d.maps.Map.local

Initialize Simulink Model Parameters

Load information about the lidar sensors, such as the mounting location on the excavator, detection range, and vertical field of view.
Load the rigidBodyTree model for the excavator.
Download the excavator model and add it to the path. This example uses an excavator modeled in Simscape. For details on the physical model, see Excavator Design with Simscape.
Use the Run Inverse Kinematics on Humanoid Robot Arm example in KinematicsSolver (Simscape Multibody) documentation to find piston values for a given configuration. To compute initial piston positions for this excavator model, use helperIKForExcavator.

helperInitializeSimulinkModelParameters

Loading Lidar Parameters and Mounting Positions...OK
Loading Robot Model and Parameters...OK
Adding ExcavatorDesignWithSimscape to MATLAB path ...OK
Initializing Piston Positions in Excavator model and controllers...OK

Create Robot Model from CAD Data

The excavator design originally came from a CAD model, which is used in three places:

A Simscape model that adds hydraulics to the rigid body dynamics for a complete physical model
A kinematic model for collision checking and motion planning
An Unreal Engine visualization that simulates the motion of the excavator

While there are many ways to provide the CAD model to these various parts, this example executes this workflow:

The Simscape model uses Multibody Link to import the CAD files and then add the dynamics to create a plant model for the excavator. The excavator joints are revolute joints. These joints use pistons for actuation, modeled primarily using cylindrical joints and universal joints. The controller in this example manages only the mechanical aspects of the excavator. However, you can use this model to test a more advanced controller that regulates the hydraulic systems. For more information about designing Simscape Multibody models, see Heavy Equipment Design with Simscape.
The kinematic model was created by extracting a URDF file from SolidWorks®, using a third-party tool and importing it as a rigidBodyTree object in MATLAB. The CAD files are converted to STL formats. You can load the URDF file into MATLAB using importrobot as a rigidBodyTree object and use it to Plan Collision-Free Path for Excavator Arm in MATLAB with Lidar Data. You can find one of the tools for conversion in this resource.
There are several ways to import to Unreal Engine. You can reuse a rigidBodyTree object that is used for kinematic modeling by using the Simulink 3D Robot block. This step enables excavator motion simulation in Unreal Engine with minimal additional effort.

Extract Lidar Sensor Data

This example shows a Simulink model for collection of sensor data from four lidar sensors. The lidar sensors are mounted on the excavator in the front, left, rear, and right to allow for a full 360 degree view. Simulation 3D Lidar blocks are used to interface with Unreal Engine and receive the point clouds.

lidarParams

lidarParams = struct with fields:
    lidarPhysicalParams: [1×1 struct]
     lidarSensorOffsets: [1×1 struct]
        lidarSampleTime: 0.1000

open_system("collectLidarPointCloud.slx");

The model logs the four point clouds to the workspace variable lidarData. The four point clouds can be saved as a MAT file to load in Extract Scene from Lidar Data.

Process Extracted Lidar Data

You merge the four point clouds collected earlier into one. Each point cloud is initially in the lidar sensor frame. First, you transform the points into the excavator frame of reference using translation and rotation from lidarParams. Then, you combine these transformed point clouds into a single point cloud in the excavator frame.

Because the lidar sensor is mounted on the excavator, some points in the point cloud are due to reflections from the excavator itself. In this example, the lidar sensor detects parts of the excavator body and arm. You need to remove these points during postprocessing to avoid false collision detection. The collision checker assumes all points are obstacles, so removing these points ensures valid poses are not mistakenly flagged as collisions.

To learn about post-processing of the collected lidar data, see the Extract Scene from Lidar Data example.

Plan Excavator Trajectory

In this section, the excavator moves spoil into a dump truck bed at a construction site. To learn about generating an collision-free path while moving the arm, see the Plan Collision-Free Path for Excavator Arm in MATLAB with Lidar Data example.

The excavator motion has three stages:

Excavation closes the bucket from an open position. During this motion, rest of the joints remain static. Trajectory to execute this motion is simple and you can use trapveltraj function(ToDo: Check) to generate the trajectory.
Spoil transfer moves the bucket from the ground to the top of the dump truck bed. Motion planning for this is more complex as the path must be collision free.
Spoil loading opens the closed bucket over the dump truck. This motion mirrors the motion discussed in the first stage, with only a different start and goal configuration.

Combine the three trajectories for use in the From Workspace Simulink block.

startcfg = deg2rad([0 -8 50 -90]); % swing, boom, stick, bucket
goalcfg = deg2rad([120 45 20 -70]);
tgap = 0.5; % gap in seconds before the trajectory starts
numSamples = 100;

% Generate trajectory for excavation.
bucketAngleStart = startcfg(end);
bucketAngleGoal = 0;
[q, ~, ~, t] = trapveltraj([bucketAngleStart bucketAngleGoal], numSamples, PeakVelocity=pi/8);
% Insert joint angle configuration for swing, boom, and stick. Repeat the
% startcfg as the remaining joints are static. Add the generated bucket configuration.
trajCloseBucket = [t', repmat(startcfg(1:end-1), numSamples, 1), q'];

% Load the trajectory for the spoil transfer.
load("excavatorArmTraj.mat");
% Start this trajectory tgap after previous trajectories end
trajGroundTruckBed = [trajCloseBucket(end,1) + tgap + t', q'];

% Generate trajectory for the spoil loading to dump truck.
bucketAngleStart = 0;
bucketAngleGoal = goalcfg(end);
[q, ~, ~, t] = trapveltraj([bucketAngleStart bucketAngleGoal], numSamples, PeakVelocity=pi/8);
trajOpenBucket = [trajGroundTruckBed(end,1) + tgap + t', repmat(goalcfg(1:end-1), numSamples, 1), q'];

% Combine the three trajectories
trajectory = [trajCloseBucket; trajGroundTruckBed(2:end,:); trajOpenBucket(2:end,:)];

This example presents a simple motion planning subsystem and can be replaced with more sophisticated subsystems taking the current and goal configuration as input to generate a trajectory. You can manage different stages by incorporating a task scheduler using Stateflow™. You can see a similar architecture in Intelligent Bin Picking System in Simulink®.

Follow Excavator Path Using Simscape and Unreal Engine™

Open Simulink Model

The Simulink model consists of following subsystems:

Plan excavator trajectory: Loads the generated trajectory and adjusts the configurations using the offsets to bring angles in Simscape reference frame.
Control excavator motion: The subsystem shows a PI controller to control pistons for actuating the boom, stick, and bucket joints.
Simulate excavator motion using Simscape model: A plant model for the excavator, takes in actuator state and generates expected joint configuration.
Visualize scene and validate motion with Unreal Engine: Takes in a configuration and sends it to Unreal Engine to visualize the motion and potential issues like collision.

open_system("followPlannedTrajectorySimscapeUnreal.slx");

To enable better understanding of the controller subsystem outputs, this example first details the Simscape model subsystem.

Simulate Excavator Motion Using Simscape Model

This example uses the joint configuration for the four revolute joints to control the excavator arm with bucket as an end effector. The model has five input ports that activate the actuators and change the excavator state

Base Position defines excavator base position with respect to the world frame.
Swing Motor Angle controls joint angle between the chassis and base.
Boom Piston position controls joint angle between the boom and chassis.
Stick Piston Position controls joint angle between the stick and boom.
Bucket Piston position controls joint angle between the bucket and stick.

First output port reads joint position and forces from the actuator sensors.

Machine subsystem contains different bodies, joints and their links. To learn more about it see Excavator Design with Simscape.

open(Simulink.BlockPath("followPlannedTrajectorySimscapeUnreal/Excavator Simscape model/Machine"))

Control Excavator Motion

This example shows a simple path tracking controller for the excavator arm and is designed to control the mechanics of the excavator. The controllers control the actuators using different methods.

Control Base and Chassis

This example shows the excavator stationary near a dig site, and hence Base Position is set to [0 0 0]. Because the actuator state maps directly to the joint state, you can control the chassis by sending the desired state obtained from the trajectory to the actuator. This mode of controlling the vehicle can be helpful to test the simulation framework without complicated algorithms involved in the initial development stages.

Control Excavator Arm

The Simscape model uses cylinders and pistons to actuate the revolute joints for the boom, stick, and bucket. To set the desired joints between the bodies in the model, set the prismatic joint in the actuator. Because the mapping is not straightforward, use a PI controller to command the piston positions and achieve the desired configuration in the joint space.

Use the pose from the trajectory to determine the configurations of the boom, stick, and bucket joint angles. The error values for PI controller are calculated the between desired state and the state calculated by the Simscape model. For quick convergence of the controller, set the correct initial piston values as you see in Initialize Simulink Models Parameters.

To ensure smooth simulation, break the algebraic loop by adding a Transfer Fcn block before passing the error to the controllers. For more information about algebraic loops, see Algebraic Loop Concepts.

Visualize Scene and Validate Motion with Unreal Engine

This subsystem enables visualization of collision or errors that occur when the excavator executes the planned trajectory. The Unreal Engine scene used in this example has three parts:

Construction Scene: The Unreal Engine Construction Scene is selected with the Simulation 3D Scene Configuration block in Unreal Execution mode.
Dump truck actor: The Simulation 3D Physics Dump Truck block, which is included in the Offroad Autonomy Library support package, is used to import dump truck Unreal assets into the construction scene. The SteeringAngle and Velocity inputs are set to zero to keep the dump truck stationary, reflecting real-world conditions.
Excavator Actor: The Simulation 3D Robot block is used to add the excavator, available as a rigidBodyTree object, to the construction scene in Unreal Engine. The block is used to visualize the joint configuration generated from Simscape block.

Simulate and Control Vehicles in Unreal Engine

After building the physical model, controller, and Unreal Engine model, you can simulate the complete system and validate the planned trajectories.

To perform simulation and visualize the scene in Unreal Engine and Simscape Multibody Mechanics Explorer, click Run or execute the following code:

sim("followPlannedTrajectorySimscapeUnreal.slx");

This example tracks the trajectory of an excavator using motion planners, enhancing controller design through high-fidelity simulation and realistic visualization with Unreal Engine. The workflow focuses on automating the excavator to move the spoil from the ground to a dump truck. Offroad Navigation for Autonomous Haul Trucks in Open Pit Mine (Navigation Toolbox) shows an extended workflow to autonomously navigate the haul truck to the dump site. This example uses dump truck synonymous to the haul truck.

The Simulate Construction Vehicles in Unreal Engine for Material Handling example provides a related workflow in which you can quickly prototype a construction scene with standard vehicles using ready-made assets. In this example, a backhoe loads bricks into a dump truck.