derivative
ビークルの状態の時間微分
説明
stateDot = derivative(motionModel,state,cmds)bicycleKinematics、differentialDriveKinematics、または unicycleKinematics オブジェクトの場合、現在の状態微分 stateDot を 3 要素ベクトル [xDot yDot thetaDot] として返します。
運動モデルが ackermannKinematics オブジェクトの場合は、stateDot を 4 要素ベクトル [xDot yDot thetaDot psiDot] として返します。xDot および yDot はメートル/秒単位で指定されるビークルの速度を参照します。thetaDot はビークルの向きの角速度、psiDot はビークルのステアリングの角速度であり、どちらもラジアン/秒単位で指定します。
articulatedSteeringKinematics 運動モデルについては、stateDot を 4 要素ベクトル [xDot yDot thetaDot gammaDot] として返します。xDot および yDot はメートル/秒単位で指定されるビークルの速度を参照します。thetaDot は前車体の角速度、gammaDot は後車体を基準とした前車体の角速度であり、どちらもラジアン/秒単位で指定します。
例
ステアリング角度に拘束があるアッカーマン ステアリングを使用するモバイル ロボット モデルをシミュレートします。シミュレーション時にステアリング制限に達すると、モデルは最大ステアリング角度を維持します。ステアリングの飽和の影響を確認するには、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),LineWidth=2) hold on plot([pose2(:,1); pose3(:,1)],[pose2(:,2);pose3(:,2)]) 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
二輪の運動学モデルを作成します。
model = bicycleKinematics;
状態を 4 つ設定し、それぞれの状態の制御コマンドを設定します。
state = [0 0 0;
1 1 0;
2 2 0;
3 3 0];
control = [0.1 pi/10;
1.0 pi/10;
5.0 pi/10;
9.0 pi/10];すべての制御コマンドの状態微分を計算します。
stateDot = model.derivative(state,control)
stateDot = 3×4
0.1000 1.0000 5.0000 9.0000
0 0 0 0
0.0325 0.3249 1.6246 2.9243
入力引数
出力引数
参照
[1] Lynch, Kevin M., and Frank C. Park. Modern Robotics: Mechanics, Planning, and Control. 1st ed. Cambridge, MA: Cambridge University Press, 2017.
