Simulink での一輪ロボットのパスの計画

この例では、与えられたマップ上の 2 点の位置間で障害物のないパスを Simulink® で実行する方法を説明します。パスは確率的ロード マップ (PRM) の計画アルゴリズム (mobileRobotPRM)) を使用して生成されます。このパスを通るための制御コマンドは、Pure Pursuit コントローラー ブロックを使用して生成されます。一輪の運動学的運動モデルは、これらのコマンドに基づいてロボットの運動をシミュレートします。

マップと Simulink モデルの読み込み

マップの範囲とマップ内の障害物を定義する占有マップを読み込みます。exampleMaps.mat には、この例で使用する simpleMap を含む複数のマップがあります。

load exampleMaps.mat

マップ内に開始位置と終了位置を指定します。

startLoc = [5 5];
goalLoc = [12 3];

モデルの概要

Simulink モデルを開きます。

open_system('pathPlanningUnicycleSimulinkModel.slx')

モデルは 3 つの主要部分で構成されています。

• 計画

• 制御

• プラント モデル

計画

MATLAB® Function ブロック Planner は mobileRobotPRM パス プランナーを使用し、入力として開始位置、ゴール位置およびマップを受け入れます。ブロックはロボットが追従するウェイポイントの配列を出力します。計画されたウェイポイントは、下流の Pure Pursuit コントローラー ブロックによって使用されます。

制御

Pure Pursuit

Pure Pursuit コントローラー ブロックは、ウェイポイントと現在のロボットの姿勢に基づいて線形速度と角速度のコマンドを生成します。

ゴール到達のチェック

Check Distance to Goal サブシステムによってゴールまでの現在の距離が計算され、その距離がしきい値内にある場合はシミュレーションが停止します。

プラント モデル

Unicycle Kinematic Model ブロックは、簡略化されたビークル運動をシミュレートするビークル モデルを作成します。このブロックはコマンド入力として Pure Pursuit コントローラー ブロックから線形速度と角速度を受け取り、現在の位置と速度の状態を出力します。

モデルの実行

モデルをシミュレートするには、次を行います。

simulation = sim('pathPlanningUnicycleSimulinkModel.slx');

ロボットの運動の可視化

モデルをシミュレートした後、マップ内で障害物のないパスを走行するロボットを可視化します。

map = binaryOccupancyMap(simpleMap)

map = 
  binaryOccupancyMap with properties:

              LayerName: 'binaryLayer'
               DataType: 'logical'
           DefaultValue: 0
             Resolution: 1
               GridSize: [26 27]
    GridLocationInWorld: [0 0]
      GridOriginInLocal: [0 0]
     LocalOriginInWorld: [0 0]
           XLocalLimits: [0 27]
           YLocalLimits: [0 26]
           XWorldLimits: [0 27]
           YWorldLimits: [0 26]

robotPose = simulation.UnicyclePose

robotPose = 428×3

    5.0000    5.0000         0
    5.0000    5.0000   -0.0002
    5.0001    5.0000   -0.0012
    5.0006    5.0000   -0.0062
    5.0031    5.0000   -0.0313
    5.0156    4.9988   -0.1569
    5.0707    4.9707   -0.7849
    5.0945    4.9354   -1.1140
    5.1075    4.9059   -1.1828
    5.1193    4.8759   -1.2030
    5.1343    4.8364   -1.2081
    5.1529    4.7880   -1.2013
    5.1746    4.7327   -1.1914
    5.2059    4.6556   -1.1786
    5.2488    4.5539   -1.1643
      ⋮

numRobots = size(robotPose, 2) / 3;
thetaIdx = 3;

% Translation
xyz = robotPose;
xyz(:, thetaIdx) = 0;

% Rotation in XYZ euler angles
theta = robotPose(:,thetaIdx);
thetaEuler = zeros(size(robotPose, 1), 3 * size(theta, 2));
thetaEuler(:, end) = theta;

for k = 1:size(xyz, 1)
    show(map)
    hold on;
    
    % Plot Start Location
    plotTransforms([startLoc, 0], eul2quat([0, 0, 0]))
    text(startLoc(1), startLoc(2), 2, 'Start');
    
    % Plot Goal Location
    plotTransforms([goalLoc, 0], eul2quat([0, 0, 0]))
    text(goalLoc(1), goalLoc(2), 2, 'Goal');
    
    % Plot Robot's XY locations
    plot(robotPose(:, 1), robotPose(:, 2), '-b')
    
    % Plot Robot's pose as it traverses the path
    quat = eul2quat(thetaEuler(k, :), 'xyz');
    plotTransforms(xyz(k,:), quat, 'MeshFilePath',...
        'groundvehicle.stl');
    
    pause(0.01)
    hold off;
end

© Copyright 2019 The MathWorks, Inc.