このページは前リリースの情報です。該当の英語のページはこのリリースで削除されています。

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

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

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

はじめに

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

ワークスペースでのロボット モデルの設定

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

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);

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

exampleHelperPolynomialTrajectory を使用して、各ジョイントについて目的とする動作の軌跡を生成します。exampleHelperPolynomialTrajectory は、初期コンフィギュレーション、ターゲット コンフィギュレーション、および軌跡の持続時間に基づいて、目的のジョイント位置、ゼロ速度およびゼロ加速度を満たす軌跡の多項式係数を生成します。

p = exampleHelperPolynomialTrajectory(q0,q1,tDuration);

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

u=M(q)(q¨d-Kdq˙e-Kpqe)+C(q,q˙)q˙+G(q)

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

q¨e+Kdq˙e+Kpqe=0

Kd および Kp を適切に選択すると、追跡誤差 qe は、時間が無限大に近づくとゼロに収束します。

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

open_system('robotSafeTrajectoryTracking/Computed Torque Controller');

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

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

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

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

Kp=ωn2

Kd=2ωnξ

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

軌跡のスケーリング

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

  • Trajectory Scaling

  • Desired Motion

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

  • fs()>0 では、ロボットが前進します (fs()=1 が通常速度)。

  • fs()=0 では、ロボットが停止します。

  • fs()<0 では、ロボットが後退します。

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

Desired Motion ブロックでは、多項式係数が与えられ、指定された timeStamp 入力を使用して目的の軌跡が生成されます。この入力は、軌跡スケーリング アルゴリズムに基づいて調整できます。qd(t)q˙d(t)q¨d(t) は軌跡多項式に基づく出力で、Computed Torque Controller サブシステムに入力されます。

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

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

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

モデルの実行

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

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

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

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

  • ここで、軌跡スケーリングのスイッチを [On] に切り替えて、モデルを再実行します。衝突後、計算されたトルクの値が変わり、反力が抑えられていることに注意してください。

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

  • Simulink の [停止] ボタンをクリックしてシミュレーションを停止します。

まとめ

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