ロボティクスマニピュレーターブロックを使用した安全な軌跡追従制御の実行

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

この例では、Simulink® とマニピュレーターアルゴリズムブロックを使用して、シミュレートされたロボットの安全な軌跡追従制御を実現する方法を示します。

この例を実行するには、Robotics System Toolbox™ と Simscape Multibody™ の両方が必要です。

はじめに

この例では、マニピュレーターアルゴリズムブロックを使用して、ジョイントの位置および速度のフィードバックを備えた、計算トルクコントローラーを実装するモデルを使用します。コントローラーはシミュレートされたロボット (Simscape Multibody を使用して実装したもの) からジョイント位置と速度の情報を受信し、トルクコマンドを送信して、与えられたジョイント軌跡に沿ってロボットを動かします。軌跡が実行されたときにロボットアームのエンドエフェクタが衝突するように、平面オブジェクトがロボットの前に配置されています。追加の設定を行わないと、オブジェクトとの衝突によるトルクにより、ロボットまたはオブジェクトが損傷する可能性があります。安全な軌跡追従を達成するため、コントローラーに目的の動作を割り当てるときにタイムスタンプを調整するための軌跡スケーリングブロックが構築されています。モデルの実行中にいくつかのパラメーターを調整したり、ロボットを操作したりして、シミュレートされたロボットでの効果を観察できます。

ロボットモデルの設定

この例では、7 自由度のロボットマニピュレーターである Rethink Sawyer のモデルを使用します。importrobotを呼び出して、Unified Robot Description Format (URDF) ファイルに保存されている記述からrigidBodyTreeモデルを生成します。Simscape と一致するように、DataFormat プロパティおよび Gravity プロパティを設定します。Simulink モデルはシミュレーション中に、ワークスペースからこのロボットモデルにアクセスします。

sawyer = importrobot('sawyer.urdf');
sawyer.removeBody('head');
sawyer.DataFormat = 'column';
sawyer.Gravity = [0, 0, -9.80665];

軌跡の生成

まず、軌跡に開始時間と持続時間を割り当てます。

tStart = 0.5;
tDuration = 3;

次に、初期コンフィギュレーションとターゲットコンフィギュレーションを割り当てます。q0 が初期コンフィギュレーションで、q1 がターゲットコンフィギュレーションです。

q0 = [0; -1.18; 0; 2.18; 0; -1.0008; 3.3161];
q1 = zeros(7,1);

次の図は、平面オブジェクトの位置に関連する、ロボットの初期コンフィギュレーションとターゲットコンフィギュレーションの可視化を示します。平面オブジェクトは、軌跡追従中にロボットが衝突するように配置されます。

Simulink モデルでは、Polynomial Trajectoryブロックで 5 次の多項式を使用して、軌跡内の任意の時点におけるロボットの位置、速度、加速度を計算します。

Simulink モデルの概要

次に、Simulink モデルを開きます。上記で生成された変数は、Simulink モデルワークスペースに既に保存されています。

open_system('robotSafeTrajectoryTracking.slx');

robotSafeTrajectoryTracking モデルは、安全な軌跡追従を行うための軌跡スケーリングを備えた、計算トルクコントローラーを実装します。このモデルには主なサブシステムが 3 つあります。

計算トルクコントローラー
軌跡スケーリングと目的の動作
単純な接触メカニズムをもつ Simscape Multibody モデル

各タイムステップでは、軌跡スケーリングのスイッチがオンの場合、目的のジョイント位置、速度、および加速度を評価するために、変更されたタイムスタンプが使用されます。次に、計算トルクコントローラーは rigidBodyTree モデルに関連付けられたマニピュレーターブロックを使用して目的の動作を追跡します。派生した制御入力が Simscape Multibody の Sawyer モデルに入力されます。ここにはロボットを操作するための平面オブジェクトが含まれています。

計算トルクコントローラーの作成

$n$ 個の非固定ジョイントをもつマニピュレーターでは、システムダイナミクスを次のように表すことができます。

$M (q) q_{}^{¨} + C (q, q_{}^{˙}) q_{}^{˙} + G (q) = u$

ここで $q$ 、 $q_{}^{˙}$ 、 $q_{}^{¨}$ 、 $\in R^{n}$ は一般化された座標の位置、速度および加速度、 $u \in R^{n}$ は制御入力 (トルク)、 $M (q)$ はジョイント空間の質量行列、 $C (q, q_{}^{˙}) q_{}^{˙}$ は速度積トルク、 $G (q)$ は重力トルクです。目的の位置 $q_{d}$ 、速度 ${q_{}^{˙}}_{d}$ 、加速度 ${q_{}^{¨}}_{d}$ をもつ目的のジョイント軌跡に沿って追跡するために、計算トルクコントローラーが、与えられたロボットダイナミクス変数 $M (q)$ 、 $C (q, q_{}^{˙}) q_{}^{˙}$ および $G (q)$ によって、指定されたコンフィギュレーションと速度の取得に必要なトルクを計算します。Simulink では、Robotics System Toolbox のロボティクスマニピュレーターブロックを使用してこれらの変数を簡単に派生させ、次の計算トルクコントローラーを設計することができます。

$u = M (q) ({q_{}^{¨}}_{d} - K_{d} {q_{}^{˙}}_{e} - K_{p} q_{e}) + C (q, q_{}^{˙}) q_{}^{˙} + G (q)$

ここで、 $q_{e} = q - q_{d}$ は位置誤差、 ${q_{}^{˙}}_{e} = q_{}^{˙} - {q_{}^{˙}}_{d}$ は速度誤差です。この制御入力によって、システムダイナミクスは 2 次 ODE になります。

${q_{}^{¨}}_{e} + K_{d} {q_{}^{˙}}_{e} + K_{p} q_{e} = 0$

$K_{d}$ および $K_{p}$ を適切に選択すると、追跡誤差 $q_{e}$ は、時間が無限大に近づくとゼロに収束します。

Computed Torque Controller サブシステムは、Joint Space Mass Matrix、Velocity Product Torque、Gravity Torqueという 3 つのロボティクスマニピュレーターブロックで構築されています。関連付けられたrigidBodyTreeモデルである sawyer がこれらすべてのブロックに割り当てられていることと、コンフィギュレーションと速度を列ベクトルとして指定する必要があることに注意してください。

open_system('robotSafeTrajectoryTracking/Computed Torque Controller');

"Computed Torque Controller" 内には調整可能なパラメーターが 2 つあります (色つきのブロックで示されます)。

ゲイン Kp: ロボットコンフィギュレーションを修正する際の比例ゲイン
ゲイン Kd: ロボットコンフィギュレーションを補正する際の微分ゲイン

Kp と Kd を決定する標準的な方法では、次のように計算します。

$K_{p} = ω_{n}^{2}$

$K_{d} = 2 ω_{n} ξ$

ここで、 $ω_{n}$ と $ξ$ は、2 次 ODE の固有振動数と減衰比です。この例で、Kp と Kd の既定値は、固有振動数と減衰比を $ω_{n} = 10$ および $ξ = 1$ として設定して派生させ、2 次 ODE が臨界減衰系になります。

Trajectory Scaling

このサブシステムには 2 つのメインブロックがあります。

Trajectory Scaling
Desired Motion

Trajectory Scaling は、この例で安全な軌跡追従のために展開されているメインブロックです。各タイムステップ $t_{i}$ で、元のタイムスタンプは $t_{i} = t_{i - 1} + Δ t$ で計算されます。しかし、ロボットが予期しないオブジェクトと衝突した場合、増大するトルクと、計画した軌跡からの逸脱によって、ロボットとオブジェクトの両方が破壊される場合があります。軌跡スケーリングの主な考えは、目的の動作と測定されたトルク $τ_{m e a}$ の関数である $f_{s} (q_{d}, {q_{}^{˙}}_{d}, {q_{}^{¨}}_{d}, τ_{m e a}) \in [- 1, 1]$ を導入して、タイムスタンプを $t_{i} = t_{i - 1} + f_{s} (q_{d}, {q_{}^{˙}}_{d}, {q_{}^{¨}}_{d}, τ_{m e a}) Δ t$ として計算することです。関数 $f_{s} ()$ はロボットの動作の速度を制御し、ロボットが感知した干渉に基づいて決まります。測定されたトルクが予測より大きい場合、 $f_{s} ()$ を減らしてロボットの速度を下げるか、目的のトルクに達するまで後退させます。 $f_{s} ()$ のこれらの値は、ロボットの動作に次の影響を与えます。

$f_{s} () > 0$ では、ロボットが前進します ( $f_{s} () = 1$ が通常速度)。
$f_{s} () = 0$ では、ロボットが停止します。
$f_{s} () < 0$ では、ロボットが後退します。

Trajectory Scaling ブロックでは、外部トルク ${τ_{}^{ˆ}}_{e x t} = τ_{m e a} - τ_{}^{ˆ}$ を推定して $f_{s} ()$ を計算する必要があります。ここで $τ_{m e a}$ は、Simscape モデルから測定されたトルク、 $τ_{}^{ˆ}$ は、前のタイムスタンプでの、目的の動作の予測トルクです。モデルの External Torque Observer セクションで、Inverse Dynamicsブロックは測定トルクから減算される予測トルクを計算します。予測トルクは $τ_{}^{ˆ} = M (q_{d}) ({q_{}^{¨}}_{d}) + C (q_{d}, {q_{}^{˙}}_{d}) {q_{}^{˙}}_{d} + G (q_{d})$ です。

Desired Motion の領域で、軌跡スケーリングアルゴリズムの出力が Polynomial Trajectory ブロックの入力として送られます。このブロックは、既知の値 q0 と q1、およびその境界条件 (ゼロ速度とゼロ加速度) を基に、5 次多項式軌跡を計算します。これにより位置、速度、加速度 ( $q_{d} (t)$ 、 ${q_{}^{˙}}_{d} (t)$ 、 ${q_{}^{¨}}_{d} (t)$ ) が出力され、"Computed Torque Controller" サブシステムに送られます。

Simscape Multibody Robot Model と単純な接触機構

Simscape Multibody Robot Model は同じ .urdf ファイルから smimport を使用してインポートします。ここでは、ジョイントトルク、ジョイント位置、速度用の一連のトルクアクチュエータとセンサーが追加されます。接触機構ブロックにより、球面と平面としてのエンドエフェクタと障害物の間に働く反力がシミュレートされます。ここでは、単純な線形バネ-ダンパーモデルが使用されています。

メモ: 接触機構はエンドエフェクタと平面オブジェクトの間にのみ実装されています。したがって、ロボットアームのその他のパーツは引き続き障害物を通過できます。

モデルの実行

モデルを実行して、ロボットシミュレーターにおける Sawyer の動作を観察して操作します。

まず、以下に示すように Simscape モデルブロックの左にあるスコープアイコンをクリックしてビューアーを開きます。ジョイントトルク、エンドエフェクタと平面オブジェクトの間の反応接触力、および軌跡追従の目的の動作を計算するためのタイムスタンプを含む信号がスコープに表示されます。

軌跡スケーリングのスイッチを [Off] に切り替えます。

以下のコマンドを使用するか、[実行] をクリックしてシミュレーションを開始します。

sim('robotSafeTrajectoryTracking.slx','StopTime','5');

アームがオブジェクトと衝突し、大きなトルク入力と大きな反力が生じているのがわかります。このケースでは元のタイムスタンプを使用していることに注意してください。その後、シミュレーションを停止します。

ここで、軌跡スケーリングのスイッチを [On] に切り替えて、モデルを再実行します。

衝突後、計算されたトルクの値が変わり、反力が抑えられていることに注意してください。

シミュレーションの実行中、スライダーを調整してオブジェクトをロボットの近くまたはロボットから遠くに動かします。ロボットは、軌跡の安全な実行を続けようとしながら、オブジェクトの位置に反応するはずです。

まとめ

この例では、Simulink でロボティクスマニピュレーターブロックを使用して計算トルクコントローラーを設計し、Simscape Multibody の軌跡スケーリングおよび動的シミュレーションと統合して、安全な軌跡追従を実現する方法を説明しました。その結果として生じるトルク、反力、およびタイムスタンプにより、安全な軌跡追従を行うための軌跡スケーリングの機能を示しました。

ロボティクス マニピュレーター ブロックを使用した安全な軌跡追従制御の実行