Main Content

ackermannKinematics

乗用車型ステアリング ビークル モデル

説明

ackermannKinematics はアッカーマン ステアリングを使用する乗用車型ビークル モデルを作成します。このモデルは、距離 WheelBase だけ離れた 2 本の車軸をもつビークルを表します。ビークルの状態は、4 要素ベクトル [x y theta psi] として定義され、グローバルの xy 位置をメートル単位で指定します。xy 位置は、後車軸の中心にあります。ビークルの向き theta とステアリング角度 psi はラジアン単位で指定されます。ビークルの向きは後車軸の中心で定義されます。角度はラジアン単位で指定されます。モデルの時間微分状態を計算するには、入力ステアリング コマンドと現在のロボットの状態を指定して、関数 derivative を使用します。

作成

説明

kinematicModel = ackermannKinematics は、アッカーマン運動学モデル オブジェクトを、既定のプロパティ値で作成します。

kinematicModel = ackermannKinematics(Name,Value) は、追加のプロパティを指定された値に設定します。複数のプロパティを任意の順序で指定できます。

プロパティ

すべて展開する

ホイール ベースは前後の車軸間の距離を示し、メートル単位で指定されます。

ビークル速度範囲は、ビークルの最小速度と最大速度を指定する 2 要素ベクトル [MinSpeed MaxSpeed] です。メートル/秒単位で指定します。

オブジェクト関数

derivativeビークルの状態の時間微分

すべて折りたたむ

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

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

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

Figure contains an axes object. The axes object contains 493 objects of type patch, line.

ステアリング角度に制約があるアッカーマン ステアリングを使用するモバイル ロボット モデルをシミュレートします。シミュレーション時にステアリング制限に達すると、モデルは最大ステアリング角度を維持します。ステアリングの飽和の影響を確認するには、1 台はステアリング角度に制約があり、もう 1 台はステアリングに制約が何もない、2 台のロボットの軌跡を比較します。

モデルの定義

アッカーマン運動学モデルを定義します。この自動車型モデルでは、前輪は指定された距離だけ離れています。これらが同心円で確実に旋回するように、車輪は異なるステアリング角度をもちます。旋回時に、前輪はステアリング角度の変化率としてステアリング入力を受け取ります。

carLike = ackermannKinematics; 

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

定数線形速度に従って一定のステアリング速度を入力として受け取るようにモバイル ロボットを設定します。制約付きのロボットをより長時間シミュレートして、ステアリングの飽和を示します。

velo = 5;    % Constant linear velocity 
psidot = 1;  % Constant left steering rate 

% Define the total time and sample rate 
sampleTime = 0.05;                  % Sample time [s]
timeEnd1 = 1.5;                     % Simulation end time for unconstrained robot 
timeEnd2 = 10;                      % Simulation end time for constrained robot 
tVec1 = 0:sampleTime:timeEnd1;      % Time array for unconstrained robot 
tVec2 = 0:sampleTime:timeEnd2;      % Time array for constrained robot  

initPose = [0;0;0;0];               % Initial pose (x y theta phi) 

ODE ソルバーのオプション構造体の作成

この例では、options 構造体を ODE ソルバーの引数として渡します。options 構造体には、ステアリング角度の制限に関する情報が格納されています。options 構造体を作成するには、odesetEvents オプションと、作成されたイベント関数 detectSteeringSaturation を使用します。detectSteeringSaturation は最大ステアリング角度を 45 度に設定します。

detectSteeringSaturation を定義する方法の詳細については、この例の最後にある "イベント関数の定義" を参照してください。

options = odeset('Events',@detectSteeringSaturation);

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

次に、関数 derivative と ODE ソルバーode45を使用して、モデルを解決し、解を生成します。

% Simulate the unconstrained robot 
[t1,pose1] = ode45(@(t,y)derivative(carLike,y,[velo psidot]),tVec1,initPose);

% Simulate the constrained robot 
[t2,pose2,te,ye,ie] = ode45(@(t,y)derivative(carLike,y,[velo psidot]),tVec2,initPose,options);

ステアリングの飽和の検出

ステアリング制限に達すると、モデルはそのイベントのタイムスタンプを登録します。制限に達するまでの時間は te に保存されます。

if te < timeEnd2
    str1 = "Steering angle limit was reached at ";
    str2 = " seconds";
    comp = str1 + te + str2; 
    disp(comp)
end 
Steering angle limit was reached at 0.785 seconds

新しい初期条件を使用した制約付きロボットのシミュレーション

次に、統合の終了前に、2 回目のシミュレーションの初期条件として制約付きロボットの状態を使用します。入力ベクトルを変更して、ステアリング速度がゼロに設定されたステアリングの飽和を表します。

saturatedPsiDot = 0;             % Steering rate after saturation 
cmds = [velo saturatedPsiDot];   % Command vector 
tVec3 = te:sampleTime:timeEnd2;  % Time vector 
pose3 = pose2(length(pose2),:); 
[t3,pose3,te3,ye3,ie3] = ode45(@(t,y)derivative(carLike,y,cmds), tVec3,pose3, options);

結果のプロット

plot と、pose. に保存されたデータを使用してロボットの軌跡をプロットします。

figure(1)
plot(pose1(:,1),pose1(:,2),'--r','LineWidth',2); 
hold on; 
plot([pose2(:,1); pose3(:,1)],[pose2(:,2);pose3(:,2)],'g'); 
title('Trajectory X-Y')
xlabel('X')
ylabel('Y') 
legend('Unconstrained robot','Constrained Robot','Location','northwest')
axis equal

Figure contains an axes object. The axes object with title Trajectory X-Y contains 2 objects of type line. These objects represent Unconstrained robot, Constrained Robot.

ステアリングの制限に達すると、制約なしロボットは曲率の半径が減少するらせん状の軌跡を追従し、制限付きロボットは曲率の半径が一定である円形の軌跡を追従します。

イベント関数の定義

4 番目の状態 theta が最大ステアリング角度と等しくなると統合が終了するようにイベント関数を設定します。

function [state,isterminal,direction] = detectSteeringSaturation(t,y)
  maxSteerAngle = 0.785;               % Maximum steering angle (pi/4 radians)
  state(4) = (y(4) - maxSteerAngle);   % Saturation event occurs when the 4th state, theta, is equal to the max steering angle    
  isterminal(4) = 1;                   % Integration is terminated when event occurs 
  direction(4) = 0;                    % Bidirectional termination 

end

参照

[1] Lynch, Kevin M., and Frank C. Park. Modern Robotics: Mechanics, Planning, and Control 1st ed. Cambridge, MA: Cambridge University Press, 2017.

拡張機能

C/C++ コード生成
MATLAB® Coder™ を使用して C および C++ コードを生成します。

バージョン履歴

R2019b で導入