Control and Simulate Multiple Warehouse Robots

This example shows how to control and simulate multiple robots working in a warehouse facility or distribution center. The robots drive around the facility picking up packages and delivering them to stations for storing or processing. This example builds on top of the Execute Tasks for a Warehouse Robot example, which drives a single robot around the same facility.

This package-sorting scenario can be modeled in Simulink® using Stateflow charts and Robotics System Toolbox™ algorithm blocks. A Central Scheduler sends commands to robots to pick up packages from the loading station and deliver them to a specific unloading station. The Robot Controller plans the trajectory based on the locations of the loading and unloading stations, and generates velocity commands for the robot. These commands are fed to the Plant, which contains a differential-drive robot model for executing the velocity commands and returning ground-truth poses of the robot. The poses are fed back to the scheduler and controller for tracking the robot status. This workflow is done for a group of 5 robots, which are all scheduled, tracked, and modeled simultaneously.

The provided Simulink model, multiRobotExampleModel, models the above described scenario.

Central Scheduler

The Central Scheduler uses a Stateflow chart to handle package allocation to the robots from the Package Dispenser. Each robot can carry one package at a time and is instructed to go from the loading to an unloading station based on the required location for each package. The scheduler also tracks the status of the packages and robots and updates the Status Dashboard. Based on robot poses, the scheduler also sends stop commands to one robot when it detects an imminent collision. This behavior can allow the robots to run local obstacle avoidance if available.

The For Each Robot and Package State subsystem is a For Each Subsystem (Simulink) which processes an array of buses for tracking the robot and package states as RobotPackageStatus bus object. This makes it easy to update this model for varying number of robots. For more information about processing arrays of buses using a For-Each Subsystem, see Work with Arrays of Buses (Simulink).

Scheduler

The following schematic details the signal values of the Scheduler Stateflow chart.

Robot Controller

The Robot Controller uses a For Each Subsystem (Simulink) to generate an array of robot controllers for your 5 robots.

The following schematic details the type of signal values associated with the For Each Robot Controller.

Each robot controller has the following inputs and outputs.

The controller takes delivery commands, which contains the package information, and plans a path for delivering it someone in the warehouse using mobileRobotPRM. The Pure Pursuit block takes this path and generates velocity commands for visiting each waypoint. Also, the status of the robot and packages get updated when the robot reaches its goal. Each robot also has its own internal scheduler that tells them the location of unloading stations based on the package information, and sends them back to the loading station when they drop off a package.

The robot controller model uses the same model, warehouseTasksRobotSimulationModel, shown in Execute Tasks for a Warehouse Robot.

Plant

The Plant subsystem uses a Differential Drive Kinematic Model block to model the motion of the robots.

Model Setup

Begin to setup various variables in MATLAB® for the model.

Defining the Warehouse Environment

A logical type matrix, logicalMap represents the occupancy map of the warehouse. The warehouse contains obstacles representing walls, shelves, and other processing stations. Loading, unloading, and charging stations are also given in xy-coordinates.

load multiRobotWarehouseMap.mat logicalMap loadingStation unloadingStations chargingStations
warehouseFig = figure('Name', 'Warehouse Setting', 'Units',"normalized", 'OuterPosition',[0 0 1 1]);
visualizeWarehouse(warehouseFig, logicalMap, chargingStations, unloadingStations, loadingStation);

Checking occupancy at stations

Ensure that the stations are not occupied in the map.

map = binaryOccupancyMap(logicalMap);
if(any(checkOccupancy(map, [chargingStations; loadingStation; unloadingStations])))
    error("At least one of the station locations is occupied in the map.")
end

Central Scheduler

The Central Scheduler requires the knowledge of the packages that are to be delivered so as to send the delivery commands to the robot controllers.

Defining Packages

Packages are given as an array of index numbers of the various unloading stations that the packages are supposed to be delivered to. Because this example has three unloading stations, a valid package can take a value of 1, 2, or 3.

load packages.mat packages
packages

packages = 1×11

     3     2     1     2     3     1     1     1     2     3     1

Number of Robots

The number of robots is used to determine the sizes of the various signals in the initialization of the Scheduler Stateflow chart

numRobots = size(chargingStations, 1); % Each robot has its own charging station;

Collision Detection and Goal-Reached Threshold

The Central Scheduler and the Robot Controller use certain thresholds for collision detection, collisionThresh, and a goal-reached condition, awayFromGoalThresh.

Collision detection ensures that for any pair of robots within a certain distance-threshold, the robot with a lower index should be allowed to move while the other robot should stop (zero-velocity command). The still moving robot should be able to avoid local static obstacles in their path. You could achieve this with another low-level controller like the Vector Field Histogram (Navigation Toolbox) block.

The goal-reached condition occurs if the robot is within a distance threshold, awayFromGoalThresh, from the goal location.

load exampleMultiRobotParams.mat awayFromGoalThresh collisionThresh

Bus Objects

The RobotDeliverCommand and RobotPackageStatus bus objects are used to pass robot-package allocations between the Central Scheduler and the Robot Controller.

load warehouseRobotBusObjects.mat RobotDeliverCommand RobotPackageStatus

Simulation

Open the Simulink model.

open_system("multiRobotExampleModel.slx")

Run the simulation. You should see the robots drive plan paths and deliver packages.

sim('multiRobotExampleModel');

### Searching for referenced models in model 'multiRobotExampleModel'.
### Found 1 model reference targets to update.
### Starting serial model reference simulation build.
### Successfully updated the model reference simulation target for: robotController

Build Summary

Model reference simulation targets:

Model            Build Reason                                        Status                        Build Duration
=================================================================================================================
robotController  Target (robotController_msf.mexa64) did not exist.  Code generated and compiled.  0h 1m 55.061s 

1 of 1 models built (0 models already up to date)
Build duration: 0h 2m 3.4159s

Metrics and Status Dashboard

For each of the packages, the dashboard in the model shows if the package is "InProgress", "Unassigned", or "Delivered". Robot Status displays the distance travelled, package location, and a package ID.

Extending the Model

This model is setup to handle modifying the number of robots in the warehouse based on availability. Adding more robots requires defining additional charging stations.

chargingStations(6, :) = [10, 15]; % Charging Station for the additional 6th robot
chargingStations(7, :) = [10, 17];  % Charging Station for the additional 7th robot

You can also add more unloading stations and assign packages to it.

unloadingStations(4, :) = [30, 50];
packages = [packages, 4, 4];

Additional Differential Kinematic Model blocks are also required to match the number of robots. The exampleHelperReplacePlantSubsystem adds these by updating numRobots.

numRobots = size(chargingStations, 1) % As before, each robot has its own charging station 

numRobots = 
7

exampleHelperReplacePlantSubsystem('multiRobotExampleModel/Robots', numRobots);

You can also redefine any existing locations. Modify the loading station location.

loadingStation = [35, 20];

Simulation

After making the modifications, run the simulation again. You should see the updated station locations and an increased number of robots.

sim('multiRobotExampleModel');

### Searching for referenced models in model 'multiRobotExampleModel'.
### Found 1 model reference targets to update.
### Starting serial model reference simulation build.
### Successfully updated the model reference simulation target for: robotController

Build Summary

Model reference simulation targets:

Model            Build Reason                                Status                        Build Duration
=========================================================================================================
robotController  Global variable unloadingStations changed.  Code generated and compiled.  0h 0m 55.432s 

1 of 1 models built (0 models already up to date)
Build duration: 0h 0m 56.93s

Visualization

The Visualization Helper offers some options for changing the view of the warehouse. Open the block mask to switch between various Preset Views of different stations. Toggle path visualization or update robot mesh types. Adjust the Sample time to change the rate of the visualization, which does not affect the execution of the actual robot simulation.