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

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

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

モバイル ロボットの運動学をモデル化する方法は多数あります。そのすべてで車輪速度とロボットの状態 [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);

その他のモデル

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

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)

ビークル コントローラーの作成

ビークルは単純追跡コントローラーを使用して一連のウェイポイントを追従します。一連のウェイポイント、ロボットの現在の状態、その他のいくつかのパラメーターを指定されたコントローラーは、ビークル速度と向首角速度を出力します。

% 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 つの出力として返す関数内に単純追跡コントローラーをラップしなければなりません。この目的で、例の補助関数 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");
axis equal
view(0,90)