derivative

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

R2019b 以降

構文

stateDot = derivative(motionModel,state,cmds)

説明

stateDot = derivative(motionModel,state,cmds) は、運動モデルが bicycleKinematics、differentialDriveKinematics、または unicycleKinematics オブジェクトの場合、現在の状態微分 stateDot を 3 要素ベクトル [xDot yDot thetaDot] として返します。運動モデルが ackermannKinematics オブジェクトの場合は、state を 4 要素ベクトル [xDot yDot thetaDot psiDot] として返します。xDot および yDot はメートル/秒単位で指定されるビークルの速度を参照します。thetaDot はビークルの向きの角速度、psiDot はビークルのステアリングの角速度であり、どちらもラジアン/秒単位で指定します。

例

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

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

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

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

ステアリング角度に拘束があるアッカーマン運動学モデルのシミュレーション

ステアリング角度に拘束があるアッカーマン ステアリングを使用するモバイル ロボット モデルをシミュレートします。シミュレーション時にステアリング制限に達すると、モデルは最大ステアリング角度を維持します。ステアリングの飽和の影響を確認するには、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 構造体を作成するには、odesetの Events オプションと、作成されたイベント関数 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

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

イベント関数の定義

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

入力引数

motionModel — 移動運動学モデル オブジェクト

ackermannKinematics オブジェクト | bicycleKinematics オブジェクト | differentialDriveKinematics オブジェクト | unicycleKinematics オブジェクト

運動モデルのプロパティを定義する移動運動学モデル オブジェクト。ackermannKinematics、bicycleKinematics、differentialDriveKinematics、または unicycleKinematics オブジェクトとして指定します。

state — 現在のビークルの状態

3 要素ベクトル | 4 要素ベクトル

現在のビークルの状態。motionModel 入力に応じて 3 要素または 4 要素ベクトルとして返されます。

• unicycleKinematics –– [x y theta]

• bicycleKinematics –– [x y theta]

• differentialDriveKinematics –– [x y theta]

• ackermannKinematics –– [x y theta psi]

x および y はメートル/秒単位で指定されるビークルの位置を参照します。theta はビークルの向き、psi はビークルのステアリング角度であり、どちらもラジアン/秒単位で指定されます。

cmds — 運動モデルへの入力コマンド

2 要素ベクトル

運動モデルへの入力コマンド。運動モデルに依存する 2 要素ベクトルとして指定します。

ackermannKinematics オブジェクトの場合、コマンドは [v psiDot] です。

その他の運動モデルの場合、motionModel の VehicleInputs プロパティによってコマンド ベクトルが決定します。

• "VehicleSpeedSteeringAngle" –– [v psiDot]

• "VehicleSpeedHeadingRate" –– [v omegaDot]

• "WheelSpeedHeadingRate" (unicycleKinematics のみ) –– [wheelSpeed omegaDot]

• "WheelSpeeds" (differentialDriveKinematics のみ) –– [wheelL wheelR]

v はメートル/秒単位の運動方向のビークル速度です。psiDot はラジアン/秒単位のステアリング角速度です。omegaDot は後車軸の角速度です。wwheelL および wheelR はそれぞれ左と右の車輪速度です。

出力引数

stateDot — 現在の状態の状態微分

3 要素ベクトル | 4 要素ベクトル

motionModel 入力に応じて 3 要素または 4 要素ベクトルとして返される現在の状態微分:

• unicycleKinematics –– [xDot yDot thetaDot]

• bicycleKinematics –– [xDot yDot thetaDot]

• differentialDriveKinematics –– [xDot yDot thetaDot]

• ackermannKinematics –– [xDot yDot thetaDot psiDot]

xDot および yDot はメートル/秒単位で指定されるビークルの速度を参照します。thetaDot はビークルの向きの角速度、psiDot はビークルのステアリングの角速度であり、どちらもラジアン/秒単位で指定します。

参照

[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 で導入

参考

ackermannKinematics | bicycleKinematics | differentialDriveKinematics | unicycleKinematics

トピック

• モバイル ロボットの運動学的方程式