複数の倉庫ロボットの制御とシミュレーション

この例では、倉庫施設や配送センターで作業する複数のロボットの制御とシミュレーションを行う方法を説明します。ロボットは施設内を走行してパッケージをピックし、保管または加工のためにステーションに搬送します。この例は、同じ施設内で 1 台のロボットを駆動する倉庫ロボットのタスクの実行の例に基づいています。

このパッケージ仕分けシナリオは、Simulink® で Stateflow チャートと Robotics System Toolbox™ のアルゴリズム ブロックを使用してモデル化することができます。Central Scheduler は、"荷積みステーション" からパッケージをピックして特定の "荷降ろしステーション" に搬送するコマンドをロボットに送信します。Robot Controller は、荷積みステーションと荷降ろしステーションの位置に基づいて軌跡を計画し、ロボットへの速度コマンドを生成します。これらのコマンドは Plant に入力されます。これは、速度コマンドを実行してロボットのグラウンド トゥルース姿勢を返す差動駆動型ロボット モデルを含んでいます。ロボットのステータスを追跡する目的で、姿勢がスケジューラとコントローラーに返されます。このワークフローが 5 台のロボットからなる 1 グループに対して実行されます。すべてのロボットのスケジューリング、追跡およびモデル化が同時に行われます。

提供されている Simulink モデル multiRobotExampleModel は、前述のシナリオをモデル化したものです。

Central Scheduler

Central Scheduler は Stateflow チャートを使用して、Package Dispenser からロボットへのパッケージの割り当てを処理します。各ロボットは一度に 1 つのパッケージを搬送可能で、各パッケージに必要な移動先に基づいて、荷積みステーションから荷降ろしステーションへ移動するように命令されます。また、スケジューラはパッケージとロボットのステータスを追跡して、Status Dashboard を更新します。あるロボットが差し迫った衝突を検知した場合、スケジューラはロボットの姿勢に基づいて、そのロボットに停止コマンドを送信することも行います。この動作によって、利用可能な場合はロボットが局所的な障害物回避を実行できます。

For Each Robot and Package State サブシステムは、ロボットとパッケージの状態を追跡するためのバスの配列を RobotPackageStatus バス オブジェクトとして処理するFor Each Subsystem (Simulink)です。これにより、さまざまな台数のロボットに合わせて、このモデルを簡単に更新できます。For-Each サブシステムを使用したバス配列の処理の詳細については、バス配列の操作 (Simulink)を参照してください。

Scheduler

次の概略図は、Stateflow チャート Scheduler の信号値の詳細を示しています。

Robot Controller

Robot Controller はFor Each Subsystem (Simulink)を使用して、5 台のロボット用にロボット コントローラーの配列を生成します。

次の概略図は、For Each Robot Controller に関連付けられている信号値のタイプの詳細を示しています。

各ロボット コントローラーは次の入力と出力をもちます。

コントローラーは、パッケージ情報が含まれている搬送コマンドを受け取り、mobileRobotPRM を使用して、倉庫内の人員に配達するためのパスを計画します。Pure Pursuit ブロックはこのパスを受け取り、各ウェイポイントを通過するための速度コマンドを生成します。また、ロボットがゴールに到達した時点でロボットとパッケージのステータスが更新されます。各ロボットには固有の内部スケジューラもあり、これはパッケージ情報に基づいて荷降ろしステーションの位置を指定し、パッケージを降ろした後でロボットを荷積みステーションに戻します。

ロボット コントローラー モデルは、倉庫ロボットのタスクの実行に示されているものと同じモデル warehouseTasksRobotSimulationModel を使用します。

Plant

Plant サブシステムは、Differential Drive Kinematic Model ブロックを使用してロボットの運動をモデル化します。

モデルの設定

MATLAB® で、モデルのさまざまな変数の設定を開始します。

倉庫の環境の定義

logical 型の行列 logicalMap は、倉庫の占有マップを表します。倉庫には、壁、棚、その他の加工ステーションを表す障害物が含まれています。荷積み、荷降ろし、および充電の各ステーションも xy 座標で指定されています。

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);

ステーションの占有状態の確認

マップ内でステーションが占有されていないことを確認します。

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

Central Scheduler は、ロボット コントローラーに搬送コマンドを送信するために、搬送するパッケージの知識を必要とします。

パッケージの定義

パッケージは、パッケージの届け先となるさまざまな荷降ろしステーションを表すインデックス番号の配列として指定されます。この例には荷降ろしステーションが 3 か所あるため、有効なパッケージは 1、2、3 のいずれかの値を取ることができます。

load packages.mat packages
packages

packages = 1×11

     3     2     1     2     3     1     1     1     2     3     1

ロボットの台数

ロボットの台数は、Stateflow チャート Scheduler の初期化でさまざまな信号のサイズを決定するために使用されます。

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

衝突検出とゴール到達のしきい値

Central Scheduler と Robot Controller は、衝突検出およびゴール到達条件のために特定のしきい値として collisionThresh と awayFromGoalThresh をそれぞれ使用します。

衝突検出によって、特定の距離しきい値内にある任意のロボットのペアについて、インデックス値が低い方のロボットの移動を許可し、もう一方のロボットを必ず停止させる (速度ゼロのコマンド) ことができます。移動し続けるロボットは、パス上にある局所的な静的障害物を回避できなければなりません。これを実現するために、Vector Field Histogram (Navigation Toolbox)ブロックのような他の低水準コントローラーを使用できます。

ゴール到達条件は、ロボットのゴール位置からの距離がしきい値 awayFromGoalThresh 内にある場合に発生します。

load exampleMultiRobotParams.mat awayFromGoalThresh collisionThresh

バス オブジェクト

RobotDeliverCommand と RobotPackageStatus のバス オブジェクトは、Central Scheduler と Robot Controller の間でロボットとパッケージの割り当てを受け渡すために使用されます。

load warehouseRobotBusObjects.mat RobotDeliverCommand RobotPackageStatus

シミュレーション

Simulink モデルを開きます。

open_system("multiRobotExampleModel.slx")

シミュレーションを実行します。ロボットが計画パスを走行してパッケージを搬送する様子が表示されるはずです。

sim('multiRobotExampleModel');

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

Build Summary

Simulation targets built:

Model            Action                        Rebuild Reason                              
===========================================================================================
robotController  Code generated and compiled.  robotController_msf.mexa64 does not exist.  

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

メトリクスおよびステータスのダッシュボード

各パッケージについて、モデルのダッシュボードにパッケージが "InProgress"、"Unassigned"、"Delivered" のいずれであるかが表示されます。Robot Status には、移動した距離、パッケージの位置およびパッケージ ID が表示されます。

モデルの拡張

このモデルは、可用性に基づいて倉庫内のロボットの台数を変更できるように設定されています。ロボットを追加するには、追加の充電ステーションを定義する必要があります。

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

また、荷降ろしステーションを追加して、これにパッケージを割り当てることもできます。

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

ロボットの台数に一致するように、追加の Differential Kinematic Model ブロックも必要です。exampleHelperReplacePlantSubsystem は、numRobots を更新することでこれらを追加します。

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

numRobots = 7

exampleHelperReplacePlantSubsystem('multiRobotExampleModel/Robots', numRobots);

任意の既存の位置を再定義することもできます。荷積みステーションの位置を変更します。

loadingStation = [35, 20];

シミュレーション

変更を行った後に、シミュレーションを再び実行します。ステーションの位置が更新され、ロボットの台数が増えたことが確認できます。

sim('multiRobotExampleModel');

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

Build Summary

Simulation targets built:

Model            Action                        Rebuild Reason                              
===========================================================================================
robotController  Code generated and compiled.  Global variable unloadingStations changed.  

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

可視化

Visualization Helper には、倉庫のビューを変更するオプションがいくつかあります。ブロック マスクを開いて、さまざまなステーションの各種の [Preset Views] を切り替えることができます。パスの可視化の切り替え、またはロボットのメッシュ タイプの更新を行います。表示速度を変更するには [Sample time] を調整します。これは実際のロボット シミュレーションの実行には影響しません。