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 36.677s 1 of 1 models built (0 models already up to date) Build duration: 0h 1m 44.04s
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 44.92s 1 of 1 models built (0 models already up to date) Build duration: 0h 0m 46.269s
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.