Simulate Earth Moving with Autonomous Excavator in Construction Site

Initial Setup

Download Construction Site Scene

You can check if the map is available in the server.

sim3d.maps.Map.server

Download the Construction site scene from the server.

helperDownloadConstructionScene

You can also manually download the Construction site scene using

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

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

sim3d.maps.Map.local

             MapName                                        Description                              Version
    __________________________    _______________________________________________________________    _______

    "Suburban scene"              "a suburban area beyond the city's border"                           "1"  
    "Offroad pit mining scene"    "An open pit mine with haul roads and terraced levels"               "1"  
    "Construction site scene"     "A construction site scene with walls, buildings and materials"      "1"  

Initialize Sensor and Excavator Model Parameters

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.

helperDownloadExcavatorDesign

Adding ExcavatorDesignWithSimscape to MATLAB path ...OK
Copying Pin locations file to example folder ...OK

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.

helperInitializeSimulinkModelParameters

Loading Lidar Parameters and Mounting Positions...OK
Loading Robot Model and Parameters...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:

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

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:

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:

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

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"), OpenType='new-tab')

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 (Simulink).

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:

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.