Main Content

Simulink での差動駆動型ロボットのパスの計画

この例では次を使用します:

ライブ スクリプトを開く

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

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

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

loadexampleMaps.mat

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

startLoc = [5 5]; goalLoc = [20 20];

モデルの概要

Simulink モデルを開きます。

open_system('pathPlanningSimulinkModel.slx')

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

  • 計画

  • 制御

  • プラント モデル

計画

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

制御

Pure Pursuit

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

ゴール到達のチェック

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

プラント モデル

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

モデルの実行

simulation = sim('pathPlanningSimulinkModel.slx');

ロボットの運動の可視化

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

map = binaryOccupancyMap(simpleMap); robotPose = simulation.Pose; thetaIdx = 3;% Translationxyz = robotPose; xyz(:, thetaIdx) = 0;% Rotation in XYZ euler anglesθ= robotPose (:, thetaIdx);thetaEuler = 0 (size(robotPose, 1), 3 * size(theta, 2)); thetaEuler(:, end) = theta;% Plot the robot poses at every 10th step.fork = 1:10:size(xyz, 1) show(map) holdon;% Plot the start location.plotTransforms([startLoc, 0], eul2quat([0, 0, 0])) text(startLoc(1), startLoc(2), 2,'Start');% Plot the goal location.plotTransforms([goalLoc, 0], eul2quat([0, 0, 0])) text(goalLoc(1), goalLoc(2), 2,'Goal');% Plot the xy-locations.plot(robotPose(:, 1), robotPose(:, 2),'-b')% Plot the robot pose as it traverses the path.quat = eul2quat(thetaEuler(k, :),'xyz'); plotTransforms(xyz(k,:), quat,'MeshFilePath',...'groundvehicle.stl'); light; drawnow; holdoff;end

Figure contains an axes object. The axes object with title Binary Occupancy Grid contains 16 objects of type patch, line, image, text.

© Copyright 2019 The MathWorks, Inc.