ロボットアームのマルチループ PI 制御

この例では次を使用します。

この例では、looptune を使用して、6-DOF ロボットアームマニピュレーターのマルチループコントローラーを調整する方法を説明します。

ロボットアームモデルとコントローラー

この例では、以下に示す 6 自由度のロボットアームを使用します。このアームは、ベースから先端へとラベル付けされた 6 つのジョイント:"Turntable"、"Bicep"、"Forearm"、"Wrist"、"Hand"、および "Gripper" で構成されます。2 つの DC モーターを並行して使用する Bicep ジョイントを除いて、各ジョイントは 1 つの DC モーターによって作動します。

図 1: ロボットアームマニピュレーター。

ファイル cst_robotarm.slx には、このシステムの電気系と機械系のコンポーネントの Simulink® モデルが含まれています。

open_system("cst_robotarm")

図 2: ロボットアームの Simulink モデル。

"Controller" サブシステムは 6 つ (ジョイントごとに 1 つ) のデジタル PI コントローラーで構成されます。各 PI コントローラーは、Simulink ライブラリの "2-DOF PID Controller" ブロックを使用して実装されます。詳細については、設定点の追従と外乱の抑制の PID 調整を参照してください。制御のサンプル時間は Ts=0.1 (10 Hz) です。

図 3: コントローラーの構造。

一般的に、このようなマルチループコントローラーは、PID ループを 1 つずつ調整し、全体の動作が満足できる状態になるまでループを周期的に繰り返すことで順番に調整されます。このプロセスは時間がかかることがあります。また、必ずしも全体が最適に調整された状態に収束するとは限りません。あるいは、systune または looptune を使用し、応答時間や最小相互干渉といったシステムレベルの要件に従って、6 つすべての PI ループを一緒に調整することもできます。

この例では、各ジョイントでの滑らかな角運動により、アームを特定の構成に約 1 秒で移動させなければなりません。アームは、Bicep の 90 度の角度以外は、すべてのジョイントの角度がゼロである完全に伸ばされた垂直位置から動き始めます。最終的な構成は次の角度位置で指定されます。Turntable = 60 度、Bicep = 80 度、Forearm = 60 度、Wrist = 90 度、Hand = 90 度、Gripper = 60 度。

Simulink モデルの "再生" ボタンをクリックして、モデルに指定された PI ゲイン値の角度の軌跡をシミュレートします。最初に青いボタンをダブルクリックして、ロボットアームの 3D アニメーションを表示することもできます。以下に角度応答と 3D アニメーションを示します。応答が非常に遅く、不正確であることは明らかです。

図 4: 調整されていない応答。

プラントの線形化

ロボットアームのダイナミクスは非線形です。アームを 1 セットの PI ゲインで制御できるかどうかを把握するには、目的の軌跡に沿ってさまざまな点 (スナップショット時間) でプラントを線形化します。ここで、"プラント" は制御信号 (PID ブロックの出力) と測定信号 ("6 DOF Robot Arm" ブロックの出力) の間のダイナミクスを示しています。

SnapshotTimes = 0:1:5;

% Plant is from PID outputs to Robot Arm outputs
LinIOs = [...
   linio("cst_robotarm/Controller/turntablePID",1,"openinput"),...
   linio("cst_robotarm/Controller/bicepPID",1,"openinput"),...
   linio("cst_robotarm/Controller/forearmPID",1,"openinput"),...
   linio("cst_robotarm/Controller/wristPID",1,"openinput"),...
   linio("cst_robotarm/Controller/handPID",1,"openinput"),...
   linio("cst_robotarm/Controller/gripperPID",1,"openinput"),...
   linio("cst_robotarm/6 DOF Robot Arm",1,"output")];
LinOpt = linearizeOptions(SampleTime=0);  % seek continuous-time model
G = linearize("cst_robotarm",LinIOs,SnapshotTimes,LinOpt);

size(G)

6x1 array of state-space models.
Each model has 6 outputs, 6 inputs, and 19 states.

t=0、1、2、3、4 秒での線形化されたモデルと t=5 秒での最終モデルの間のギャップをプロットします。

G5 = G(:,:,end);  % t=5
G5.SamplingGrid = [];
sigma(G5,G(:,:,2:5)-G5,{1e-3,1e3})
grid
title("Variation of linearized dynamics along trajectory")
legend("Linearization at t=5 s","Absolute variation",...
       Location="southwest")

低周波数と高周波数ではダイナミクスが大幅に変化しますが、その変化は 10 rad/s 付近で 10% 未満まで低下します。大まかに言うと、これが目的の制御帯域幅です。ターゲットゲイン交差周波数付近の小さなプラント変化は、単一セットの PI ゲインでアームを制御することができ、ゲインスケジューリングを使用する必要がないことを示しています。

LOOPTUNE を使用した PI コントローラーの調整

looptune を使用すると、6 つすべての PI ループを直接調整し、最小限のループ相互作用と適切な MIMO 安定余裕で目的の応答時間を実現することができます。PI ゲインを Simulink に書き戻すときに、コントローラーは連続時間で調整され、自動的に離散化されます。slTuner インターフェイスを使用して、調整しなければならないブロックを指定し、プラントやコントローラーの境界を特定します。

% Linearize the plant at t=3s
tLinearize = 3;

% Create slTuner interface
TunedBlocks = {"turntablePID","bicepPID","forearmPID",...
               "wristPID","handPID","gripperPID"};
ST0 = slTuner("cst_robotarm",TunedBlocks,tLinearize);

% Mark outputs of PID blocks as plant inputs
addPoint(ST0,TunedBlocks)

% Mark joint angles as plant outputs
addPoint(ST0,"6 DOF Robot Arm")

% Mark reference signals
RefSignals = {...
   "ref Select/tREF",...
   "ref Select/bREF",...
   "ref Select/fREF",...
   "ref Select/wREF",...
   "ref Select/hREF",...
   "ref Select/gREF"};
addPoint(ST0,RefSignals)

最も簡単な使用方法では、looptune に必要な情報はターゲットの制御帯域幅のみであり、目的の応答時間の逆数の少なくとも 2 倍でなければなりません。ここでは、目的の応答時間が 1 秒であるため、ターゲットの帯域幅を 3 rad/s で試します (プラントダイナミクスが少なくとも 10 rad/s 付近で変化することを忘れないでください)。

wc = 3;  % target gain crossover frequency
Controls = TunedBlocks;      % actuator commands
Measurements = "6 DOF Robot Arm";  % joint angle measurements
ST1 = looptune(ST0,Controls,Measurements,wc);

Final: Peak gain = 0.957, Iterations = 10
Achieved target gain value TargetGain=1.

1 付近または 1 より小さい最終値は、looptune が要求された帯域幅を達成したことを示しています。最初のコントローラーと調整コントローラーに関して、角度位置のステップコマンドへの応答を比較します。

T0 = getIOTransfer(ST0,RefSignals,Measurements);
T1 = getIOTransfer(ST1,RefSignals,Measurements);

sp = stepplot(T0,"b--",T1,"r",4);
sp.IOGrouping = "all";
legend("Initial","Tuned",Location="southeast")
grid

y=1 の付近で整定された 6 つの曲線は各ジョイントのステップ応答を表しています。また、y=0 の付近で整定された曲線は相互干渉項を表しています。調整されたコントローラーは明らかに改善されていますが、オーバーシュートがあるため Bicep 応答が安定するまで時間がかかります。

2 自由度の活用

2-DOF PI コントローラーには、フィードフォワードコンポーネントとフィードバックコンポーネントがあります。

図 5: 2 自由度 PID コントローラー。

既定では、looptune はフィードバックループを調整するだけで、フィードフォワードコンポーネントを "認識" しません。これを確認するには、PI コントローラーの $b$ パラメーターがその初期値 $b=1$ に設定されたままであることを検証します (調整された値を表示するには「showTunable(ST1)」と入力します)。フィードフォワードアクションを利用してオーバーシュートを削減するには、明示的なステップ追従要件による帯域幅のターゲットを基準角からジョイントの角度に置換します。

TR = TuningGoal.StepTracking(RefSignals,Measurements,0.5);
ST2 = looptune(ST0,Controls,Measurements,TR);

Final: Peak gain = 0.766, Iterations = 13
Achieved target gain value TargetGain=1.

2-DOF 調整によってオーバーシュートを除去し、Bicep の応答を改善します。

T2 = getIOTransfer(ST2,RefSignals,Measurements);
sp = stepplot(T1,"r--",T2,"g",4);
sp.IOGrouping = "all";
legend("1-DOF tuning","2-DOF tuning",Location="southeast");

調整コントローラーの検証

調整された線形応答は満足できるものに見えるため、PI ゲインの調整された値を Simulink ブロックに書き戻し、全体的な操作をシミュレートします。シミュレーション結果を図 6 に示します。

writeBlockValue(ST2)

図 6: 調整された角度応答。

Bicep ジョイントの非線形応答はアンダーシュートが顕著です。さらに詳しく調査すると、その原因には次の 2 つが考えられます。まず、PI コントローラーがあまりにアグレッシブであり、モーターを飽和させるため (入力電圧は ± 5 V に制限されます)。

図 7: DC モーターへの入力電圧 (制御信号)。

次に、Wrist と Bicep 間の相互干渉効果をスケーリングすると、Bicep の応答に有意で持続的な影響を及ぼすため。これを確認するには、操作 (Bicep ジョイントに -10 度、Wrist ジョイントに 90 度) 時に発生する実際のステップ変化に対して、この 3 つのジョイントのステップ応答をプロットします。

H2 = T2([2 4],[2 4]) * diag([-10 90]);  % scale by step amplitude
H2.u = ["Bicep","Wrist"];
H2.y = ["Bicep","Wrist"];
step(H2,5)
grid

設計の改良

この特定のアーム操作に対する Bicep の応答を改善するには、相互干渉の影響を各ジョイントでの最終的な角変位 "に対して" 小さく維持しなければなりません。これを行うには、ステップ追従要件の相互干渉項を基準角の振幅でスケーリングします。

JointDisp = [60 10 60 90 90 60];  % commanded angular displacements, in degrees
TR.InputScaling = JointDisp;

アクチュエータの飽和を削減するには、基準信号から制御信号へのゲインを制限します。

UR = TuningGoal.Gain(RefSignals,Controls,6);

これらの改良された調整目標でコントローラーを再調整します。

ST3 = looptune(ST0,Controls,Measurements,TR,UR);

Final: Peak gain = 1.13, Iterations = 204

スケーリングされた応答を以前の設計と比較します。Wrist と Bicep の動作間の干渉が、ピーク値および合計エネルギーのいずれにおいても大幅に削減されていることがわかります。

T2s = diag(1./JointDisp) * T2 * diag(JointDisp);
T3s = diag(1./JointDisp) * getIOTransfer(ST3,RefSignals,Measurements) * diag(JointDisp);
sp = stepplot(T2s,"g--",T3s,"m",4);
sp.IOGrouping = "all";
legend("Initial 2-DOF","Refined 2-DOF",Location="southeast");