MATLAB でのジョイント空間の軌跡追従のシミュレーション

この例では、閉ループ制御下におけるロボット マニピュレーターのジョイント空間運動をシミュレートする方法を説明します。

ロボットと初期状態の定義

関数 loadrobot を使用して、ロボット ライブラリから ABB IRB-120T を読み込みます。

robot = loadrobot("abbIrb120T","DataFormat","column","Gravity",[0 0 -9.81]);
numJoints = numel(homeConfiguration(robot));

軌跡をシミュレートする時間範囲、初期状態 [joint configuration; jointVelocity]、およびジョイント空間の設定点などのシミュレーション パラメーターを定義します。

% Set up simulation parameters
tSpan = 0:0.01:0.5;
q0 = zeros(numJoints,1);
q0(2) = pi/4; % Something off center
qd0 = zeros(numJoints,1);
initialState = [q0; qd0];

% Set up joint control targets
targetJointPosition = [pi/2 pi/3 pi/6 2*pi/3 -pi/2 -pi/3]';
targetJointVelocity = zeros(numJoints,1);
targetJointAcceleration = zeros(numJoints,1);

ゴール位置を可視化します。

show(robot,targetJointPosition);

ジョイント空間制御を使用した動作のモデル化

jointSpaceMotionModelオブジェクトを使用して、さまざまなコントローラーの下でモデルの閉ループ運動をシミュレートします。この例ではそれらのいくつかを比較します。各インスタンスは関数derivativeを使用して状態導関数を計算します。ここで、状態は 2n 要素ベクトル [joint configuration; joint velocity] で、n は関連付けられたrigidBodyTreeオブジェクト内のジョイント数です。

計算トルク制御

計算トルク制御は、逆ダイナミクス計算を使用してロボットのダイナミクスを補正します。コントローラーは 2 次応答に基づいて、各ジョイントの閉ループ誤差ダイナミクスを駆動します。

jointSpaceMotionModel を作成してロボット モデルを指定します。"MotionType" を "ComputedTorqueControl" に設定します。updateErrorDynamicsFromStepを使用して誤差ダイナミクスを更新し、目的の整定時間とオーバーシュートをそれぞれ指定します。あるいは、減衰比と固有振動数をオブジェクト内で直接設定することもできます。

computedTorqueMotion = jointSpaceMotionModel("RigidBodyTree",robot,"MotionType","ComputedTorqueControl");
updateErrorDynamicsFromStep(computedTorqueMotion,0.2,0.1);

この運動モデルには、位置、速度、加速度を指定する必要があります。

qDesComputedTorque = [targetJointPosition; targetJointVelocity; targetJointAcceleration];

このコントローラーの Simulink での実用例については、ロボティクス マニピュレーター ブロックを使用した安全な軌跡追従制御の実行の例を参照してください。

独立ジョイント制御

独立ジョイント制御では、各ジョイントを 2 次追従応答をもつ個別のシステムとしてモデル化します。このタイプのモデルは理想的な動作をし、応答の遅い場合、またはダイナミクスが結果の軌跡に大きく影響しない場合の使用に非常に適しています。このようなケースでは計算トルク制御と同じように動作しますが、計算オーバーヘッドがより小さくなります。

運動タイプ "IndependentJointMotion" を使用して、joinSpaceMotionModel をもう 1 つ作成します。

IndepJointMotion = jointSpaceMotionModel("RigidBodyTree",robot,"MotionType","IndependentJointMotion");
updateErrorDynamicsFromStep(IndepJointMotion,0.2,0.1);

この運動モデルには、位置、速度、加速度を指定する必要があります。

qDesIndepJoint = [targetJointPosition; targetJointVelocity; targetJointAcceleration];

比例-微分制御

比例-微分制御 (PD 制御) は、重力補正を比例ゲインおよび微分のゲインと組み合わせたものです。PD コントローラーは、他の閉形式のモデルに比べてシンプルなタイプにもかかわらず、すべての正のゲイン値に対して安定になり得るため、望ましいオプションです。ここで、PD ゲインは n 行 n 列の行列として設定されます。n は、関連付けられた rigidBodyTree オブジェクト内のジョイント数です。このロボットの場合、n = 6 です。さらに、PD 制御は加速度プロファイルを必要としないため、その状態ベクトルは、ジョイント コンフィギュレーションとジョイント速度からなるわずか 2n 要素のベクトルです。

pdMotion = jointSpaceMotionModel("RigidBodyTree",robot,"MotionType","PDControl");
pdMotion.Kp = diag(300*ones(1,6));
pdMotion.Kd = diag(10*ones(1,6));

この運動モデルには、位置と速度を指定する必要があります。

qDesPD = [targetJointPosition; targetJointVelocity];

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

関数 derivative は状態導関数を出力します。これはode45などの常微分方程式 (ODE) ソルバーを使用して積分することができます。各運動モデルについて ODE ソルバーは、tspan をカバーする m 要素の列ベクトルと、各時点での 2n 要素の状態ベクトルからなる 2 行 m 列の行列を出力します。

各システムに最も適した ODE ソルバーを使用して、各運動モデルの軌跡を計算します。

[tComputedTorque,yComputedTorque] = ode45(@(t,y)derivative(computedTorqueMotion,y,qDesComputedTorque),tSpan,initialState);
[tIndepJoint,yIndepJoint] = ode45(@(t,y)derivative(IndepJointMotion,y,qDesIndepJoint),tSpan,initialState);
[tPD,yPD] = ode15s(@(t,y)derivative(pdMotion,y,qDesPD),tSpan,initialState);

結果のプロット

シミュレーションが完了したら、結果を並べて比較します。各プロットの上部にジョイント位置、下部に速度が表示されます。破線は基準軌跡を示し、実線はシミュレートされた応答を示します。

% Computed Torque Control
figure
subplot(2,1,1)
plot(tComputedTorque,yComputedTorque(:,1:numJoints)) % Joint position
hold all
plot(tComputedTorque,targetJointPosition*ones(1,length(tComputedTorque)),"--") % Joint setpoint
title("Computed Torque Motion: Joint Position")
xlabel("Time (s)")
ylabel("Position (rad)")
subplot(2,1,2)
plot(tComputedTorque,yComputedTorque(:,numJoints+1:end)) % Joint velocity
title("Joint Velocity")
xlabel("Time (s)")
ylabel("Velocity (rad/s)")

次のプロットでは独立ジョイント制御を使用して、いくつかの簡略化仮定の下で計算トルク運動が同等の動作を示すことを確認します。

% Independent Joint Motion
figure
subplot(2,1,1)
plot(tIndepJoint,yIndepJoint(:,1:numJoints))
hold all
plot(tIndepJoint,targetJointPosition*ones(1,length(tIndepJoint)),"--")
title("Independent Joint Motion: Position")
xlabel("Time (s)")
ylabel("Position (rad)")
subplot(2,1,2);
plot(tIndepJoint,yIndepJoint(:,numJoints+1:end))
title("Joint Velocity")
xlabel("Time (s)")
ylabel("Velocity (rad/s)")

最後に、PD コントローラーは同様の立ち上がり時間を実現するためにかなり大きいゲインを使用しますが、他の方法とは独立ジョイントの動作が異なります。これは、各ジョイントとその関連付けられたボディの若干異なる動的プロパティが、コントローラーによって補正されないためです。

% PD with Gravity Compensation
figure
subplot(2,1,1)
plot(tPD,yPD(:,1:numJoints))
hold all
plot(tPD,targetJointPosition*ones(1,length(tPD)),"--")
title("PD Controlled Joint Motion: Position")
xlabel("Time (s)")
ylabel("Position (rad)")
subplot(2,1,2)
plot(tPD,yPD(:,numJoints+1:end))
title("Joint Velocity")
xlabel("Time (s)")
ylabel("Velocity (rad/s)")

軌跡をアニメーションとして可視化

この動作が 3 次元でどのように見えるかを確認するには、次の例の補助関数を使用してロボットの運動を同期してプロットします。3 番目の入力は各サンプル間のフレーム数です。

exampleHelperRigidBodyTreeAnimation(robot,yComputedTorque,1,"Computed Torque Control");

exampleHelperRigidBodyTreeAnimation(robot,yIndepJoint,1,"Independent Joint Motion");

exampleHelperRigidBodyTreeAnimation(robot,yPD,1,"PD-Controlled Joint Motion");