モバイル ロボットのさまざまな運動学モデルのシミュレーション

この例では、単一の環境内でさまざまなロボット運動学モデルをモデル化して比較する方法を説明します。

運動学的拘束をもつモバイル ロボットの定義

モバイル ロボットの運動学をモデル化する方法は多数あります。そのすべてで車輪速度とロボットの状態 [x y theta] との関係が指定されます。これは xy 座標と、ロボット進行方向 theta (ラジアン単位) です。

一輪車の運動学モデル

モバイル ロボット車両の運動学を表す最も簡単な方法は、中央車軸周りの回転によって車輪速度が設定され、z 軸周りにピボットできる一輪車モデルを使用することです。差動駆動型および自転車の運動学モデルは、入力を車両の速度と回頭角速度として指定し、かつ他の拘束を考慮しない場合には、いずれも一輪車の運動学に簡略化されます。

unicycle = unicycleKinematics("VehicleInputs","VehicleSpeedHeadingRate");

差動駆動型の運動学モデル

差動駆動型モデルは、後駆動車軸を使用して車両速度と回頭角速度の両方を制御します。駆動車軸に接続した車輪は、両方向に回転することができます。ほとんどのモバイル ロボットは低水準の車輪コマンドに対する何らかのインターフェイスをもつため、このモデルでも入力として車両速度と回頭角速度を使用し、車両制御を簡略化します。

diffDrive = differentialDriveKinematics("VehicleInputs","VehicleSpeedHeadingRate");

一輪車モデルの動作と区別するため、差動駆動型運動学モデルに車輪速度の拘束を追加します。

diffDrive.WheelSpeedRange = [-10 10]*2*pi;

自転車の運動学モデル

自転車モデルでは、ロボットを後駆動車軸と z 軸周りに旋回する前車軸の 2 本の車軸をもつ自動車型モデルとして扱います。自転車モデルは、各車軸に取り付けられた複数の車輪を中央に配置された単一の車輪としてモデル化でき、前輪の回頭角を自転車のように直接設定できるという仮定に基づいて動作します。

bicycle = bicycleKinematics("VehicleInputs","VehicleSpeedHeadingRate","MaxSteeringAngle",pi/8);

その他のモデル

アッカーマン運動学モデルは、アッカーマン ステアリングを前提とする修正自動車型モデルです。ほとんどの自動車型車両では、複数の前輪が同じ軸の周りに旋回するのではなく、車両の旋回中心と同心円上を走行するように、少し異なる複数の軸の周りを旋回します。この旋回角度の差はアッカーマン ステアリングと呼ばれ、通常は実際の車両の機構によって実現されます。車両と車輪の運動学という観点からは、ステアリング角度を速度入力として扱うことによってこれを実現できます。

carLike = ackermannKinematics;

シミュレーション パラメーターの設定

これらのモバイル ロボットは、さまざまな運動学によって発生する何らかの差異を示すように設計された、一連のウェイポイントを追従します。

waypoints = [0 0; 0 10; 10 10; 5 10; 11 9; 4 -5];
% Define the total time and the sample rate
sampleTime = 0.05;               % Sample time [s]
tVec = 0:sampleTime:20;          % Time array

initPose = [waypoints(1,:)'; 0]; % Initial pose (x y theta)

車両コントローラーの作成

車両は Pure Pursuit コントローラーを使用して一連のウェイポイントを追従します。一連のウェイポイント、ロボットの現在の状態、その他のいくつかのパラメーターを指定されたコントローラーは、車両速度と回頭角速度を出力します。

% Define a controller. Each robot requires its own controller
controller1 = controllerPurePursuit("Waypoints",waypoints,"DesiredLinearVelocity",3,"MaxAngularVelocity",3*pi);
controller2 = controllerPurePursuit("Waypoints",waypoints,"DesiredLinearVelocity",3,"MaxAngularVelocity",3*pi);
controller3 = controllerPurePursuit("Waypoints",waypoints,"DesiredLinearVelocity",3,"MaxAngularVelocity",3*pi);

ODE ソルバーを使用したモデルのシミュレーション

状態を更新する関数 derivative を使用してモデルをシミュレートします。この例では常微分方程式 (ODE) ソルバーを使用して解を生成します。別の方法では、差動駆動型ロボットのパス追従で説明されているようにループを使用して状態を更新します。

ODE ソルバーでは、すべての出力を 1 つの出力として指定する必要があるため、線形速度と回頭角速度を 1 つの出力として返す関数内に Pure Pursuit コントローラーをラップしなければなりません。この目的で、例の補助関数 exampleHelperMobileRobotController を使用します。また、例の補助関数によって、ゴールから指定の半径内にあるロボットは確実に停止します。

goalPoints = waypoints(end,:)';
goalRadius = 1;

モデルのタイプごとに ode45 が 1 回呼び出されます。導関数は、initPose により設定された初期状態を使用して状態出力を計算します。各導関数は、対応する運動学モデル オブジェクト、現在のロボットの姿勢、およびその姿勢におけるコントローラーの出力を受け入れます。

% Compute trajectories for each kinematic model under motion control
[tUnicycle,unicyclePose] = ode45(@(t,y)derivative(unicycle,y,exampleHelperMobileRobotController(controller1,y,goalPoints,goalRadius)),tVec,initPose);
[tBicycle,bicyclePose] = ode45(@(t,y)derivative(bicycle,y,exampleHelperMobileRobotController(controller2,y,goalPoints,goalRadius)),tVec,initPose);
[tDiffDrive,diffDrivePose] = ode45(@(t,y)derivative(diffDrive,y,exampleHelperMobileRobotController(controller3,y,goalPoints,goalRadius)),tVec,initPose);

結果のプロット

ODE ソルバーの結果は、すべての軌跡の結果を一度に可視化する plotTransforms を使用して、1 つのプロット上に簡単に表示できます。

まず、姿勢の出力を、平行移動と四元数のインデックス付き行列に変換しなければなりません。

unicycleTranslations = [unicyclePose(:,1:2) zeros(length(unicyclePose),1)];
unicycleRot = axang2quat([repmat([0 0 1],length(unicyclePose),1) unicyclePose(:,3)]);

bicycleTranslations = [bicyclePose(:,1:2) zeros(length(bicyclePose),1)];
bicycleRot = axang2quat([repmat([0 0 1],length(bicyclePose),1) bicyclePose(:,3)]);

diffDriveTranslations = [diffDrivePose(:,1:2) zeros(length(diffDrivePose),1)];
diffDriveRot = axang2quat([repmat([0 0 1],length(diffDrivePose),1) diffDrivePose(:,3)]);

その後、すべての変換のセットをプロットして上から確認できます。一輪車、自転車、差動駆動型ロボットのパスは、それぞれ赤、青、緑です。プロットを簡略化するために、10 点につき 1 つの出力のみを表示します。

figure
plot(waypoints(:,1),waypoints(:,2),"kx-","MarkerSize",20);
hold all
plotTransforms(unicycleTranslations(1:10:end,:),unicycleRot(1:10:end,:),'MeshFilePath','groundvehicle.stl',"MeshColor","r");
plotTransforms(bicycleTranslations(1:10:end,:),bicycleRot(1:10:end,:),'MeshFilePath','groundvehicle.stl',"MeshColor","b");
plotTransforms(diffDriveTranslations(1:10:end,:),diffDriveRot(1:10:end,:),'MeshFilePath','groundvehicle.stl',"MeshColor","g");
view(0,90)

×

例を開く

この例の変更されたバージョンがあります。編集された方の例を開きますか?

いいえ、変更されたバージョンを上書きします。 はい