モバイルロボットを使用したパスプランニングシミュレーションの実行

ライブスクリプトを開く

シナリオを作成して、室内を移動するモバイルロボットをシミュレートします。この例では、シナリオを作成して剛体ツリーオブジェクトからロボットプラットフォームをモデル化し、シナリオからバイナリ占有グリッドマップを取得して、モバイルロボットが追従するパスを mobileRobotPRM パスプランニングアルゴリズムを使用して計画する方法を示します。

床面と静的メッシュで構成されるシナリオの作成

robotScenario オブジェクトは、一連の固定障害物と、プラットフォームと呼ばれる移動可能なオブジェクトで構成されます。robotPlatform オブジェクトを使用して、シナリオ内のモバイルロボットをモデル化します。この例では、床面とボックス型メッシュで構成されるシナリオを作成して部屋を作成します。

scenario = robotScenario(UpdateRate=5);

シナリオに床面として平面メッシュを追加します。

floorColor = [0.5882 0.2941 0];
addMesh(scenario,"Plane",Position=[5 5 0],Size=[10 10],Color=floorColor);

部屋の壁はボックス型メッシュとしてモデル化されます。静的メッシュは IsBinaryOccupied の値を true に設定して追加されるため、パスプランニング用のバイナリ占有マップにそれらの障害物が組み込まれます。

wallHeight = 1;
wallWidth = 0.25;
wallLength = 10;
wallColor = [1 1 0.8157];

% Add outer walls.
addMesh(scenario,"Box",Position=[wallWidth/2, wallLength/2, wallHeight/2],...
    Size=[wallWidth, wallLength, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength-wallWidth/2, wallLength/2, wallHeight/2],...
    Size=[wallWidth, wallLength, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/2, wallLength-wallWidth/2, wallHeight/2],...
    Size=[wallLength, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/2, wallWidth/2, wallHeight/2],...
    Size=[wallLength, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);

% Add inner walls.
addMesh(scenario,"Box",Position=[wallLength/8, wallLength/3, wallHeight/2],...
    Size=[wallLength/4, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/4, wallLength/3, wallHeight/2],...
    Size=[wallWidth, wallLength/6,  wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[(wallLength-wallLength/4), wallLength/2, wallHeight/2],...
   Size=[wallLength/2, wallWidth, wallHeight],Color=wallColor,IsBinaryOccupied=true);
addMesh(scenario,"Box",Position=[wallLength/2, wallLength/2, wallHeight/2],...
    Size=[wallWidth, wallLength/3, wallHeight],Color=wallColor,IsBinaryOccupied=true);

シナリオを可視化します。

show3D(scenario);
lightangle(-45,30);
view(60,50);

Figure contains an axes object. The axes object with xlabel East (m), ylabel North (m) contains 9 objects of type patch.

シナリオからのバイナリ占有マップの取得

パスプランニング用に、占有マップを binaryOccupancyMap オブジェクトとしてシナリオから取得します。マップ上の占有スペースを 0.3 m インフレートします。

map = binaryOccupancyMap(scenario,GridOriginInLocal=[-2 -2],...
                                           MapSize=[15 15],...
                                           MapHeightLimits=[0 3]);
inflate(map,0.3);

2 次元占有マップを可視化します。

show(map)

Figure contains an axes object. The axes object with title Binary Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains an object of type image.

パスプランニング

mobileRobotPRM パスプランナーを使用して、取得されたマップ上の開始位置からゴール位置までの障害物のないパスを見つけます。

モバイルロボットの開始位置とゴール位置を指定します。

startPosition = [1 1];
goalPosition = [8 8];

再現できるように rng シードを設定します。

rng(100)

バイナリ占有マップを使用して mobileRobotPRM オブジェクトを作成し、最大ノード数を指定します。接続された 2 つのノード間の最大距離を指定します。

numnodes = 2000;
planner = mobileRobotPRM(map,numnodes);
planner.ConnectionDistance = 1;

開始位置からゴール位置までのパスを見つけます。

waypoints = findpath(planner,startPosition,goalPosition);

軌跡の生成

waypointTrajectory System object™ を使用して、計画されたパスのウェイポイントにモバイルロボットが追従する軌跡を生成します。

% Robot height from base.
robotheight = 0.12;
% Number of waypoints.
numWaypoints = size(waypoints,1);
% Robot arrival time at first waypoint.
firstInTime = 0;
% Robot arrival time at last waypoint.
lastInTime = firstInTime + (numWaypoints-1);
% Generate waypoint trajectory with waypoints from planned path.
traj = waypointTrajectory(SampleRate=10,...
                          TimeOfArrival=firstInTime:lastInTime, ...
                          Waypoints=[waypoints, robotheight*ones(numWaypoints,1)], ...
                          ReferenceFrame="ENU");

シナリオへのロボットプラットフォームの追加

Clearpath Husky モバイルロボットをロボットライブラリから rigidBodyTree オブジェクトとして読み込みます。

huskyRobot = loadrobot("clearpathHusky");

rigidBodyTree オブジェクトとして指定されたロボットモデルと waypointTrajectory System object として指定されたその軌跡を使用して、robotPlatform オブジェクトを作成します。

platform = robotPlatform("husky",scenario, RigidBodyTree=huskyRobot,...
                         BaseTrajectory=traj);

ロボットを含むシナリオを可視化します。

[ax,plotFrames] = show3D(scenario);
lightangle(-45,30)
view(60,50)

モバイルロボットのシミュレーション

計画されたパスを可視化します。

hold(ax,"on")
plot(ax,waypoints(:,1),waypoints(:,2),"-ms",...
               LineWidth=2,...
               MarkerSize=4,...
               MarkerEdgeColor="b",...
               MarkerFaceColor=[0.5 0.5 0.5]);
hold(ax,"off")

シミュレーションを設定します。ロボットのすべての姿勢が事前にわかっているので、シミュレーションを単純にステップ実行し、ステップごとに可視化を更新します。

setup(scenario)

% Control simulation rate at 20 Hz.
r = rateControl(20);

% Status of robot in simulation.
robotStartMoving = false;

while advance(scenario)
    show3D(scenario,Parent=ax,FastUpdate=true);
    waitfor(r);

    currentPose = read(platform);
    if ~any(isnan(currentPose))
        % implies that robot is in the scene and performing simulation.
        robotStartMoving = true;
    end
    if any(isnan(currentPose)) && robotStartMoving
        % break, once robot reaches goal position.
        break;
    end
end

Figure contains an axes object. The axes object with xlabel East (m), ylabel North (m) contains 10 objects of type patch, line.

シミュレーションを再実行して結果を再度可視化するには、シナリオでシミュレーションをリセットします。

restart(scenario)

モバイル ロボットを使用したパス プランニング シミュレーションの実行