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

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

この例では,金宝app仿真软件®と机器人系统工具箱™マニピュレーターアルゴリズムブロックを使用して,Simscape™多体™で実行中のシミュレートされたロボットに対して安全な軌跡追従制御を実現する方法を示します。

この例を実行するには,机器人系统工具箱とSimscape多体の両方が必要です。

はじめに

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

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

この例では7自由度のロボットマニピュレーターである反思索耶のモデルを使用します。importrobot机器人を呼び出して、统一描述格式(URDF)ファイルに保存されている記述からrigidBodyTreeモデルを生成します。Simscapeと一致するように、DataFormatプロパティおよび重力プロパティを設定します。

索耶= importrobot (<年代p一个n年代tyle="color:#A020F0">“sawyer.urdf”）;sawyer.removeBody (<年代p一个n年代tyle="color:#A020F0">“头”）;索耶。DataFormat =<年代p一个n年代tyle="color:#A020F0">“列”；索耶。Gravity = [0, 0, -9.80665];

軌跡生成と関連設定

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

tStart = 0.5;tDuration = 3;

次に,初期コンフィギュレーションとターゲットコンフィギュレーションを割り当てます。q0处が初期コンフィギュレーションで,第一季度がターゲットコンフィギュレーションです。

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

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

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

p = exampleHelperPolynomialTrajectory (q0, q1, tDuration);

金宝app仿真软件モデルの概要

次に,金宝app仿真软件モデルを開きます。上記で生成された変数は,金宝app仿真软件モデルワークスペースに既に保存されています。

open_system (<年代p一个n年代tyle="color:#A020F0">“robotSafeTrajectoryTracking.slx”）;

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

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

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

ロボット工学マニピュレーターブロックを使用して計算トルクコントローラーを構築

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

$米（问）问_{}^{¨} + C （问，问_{}^{˙} ）问_{}^{˙} + G （问）＝ u$

ここで<年代p一个nclass="inlineequation"> $问$ 、<年代p一个nclass="inlineequation"> $问_{}^{˙}$ 、<年代p一个nclass="inlineequation"> $问_{}^{¨}$ 、<年代p一个nclass="inlineequation"> $\in R^{n}$ は一般化された座標の位置,速度および加速度,<年代p一个nclass="inlineequation"> $u \in R^{n}$ は制御入力(トルク),<年代p一个nclass="inlineequation"> $米（问）$ はジョイント空間の質量行列,<年代p一个nclass="inlineequation"> $C （问，问_{}^{˙} ）问_{}^{˙}$ は速度積トルク,<年代p一个nclass="inlineequation"> $G （问）$ は重力トルクです。目的の位置<年代p一个nclass="inlineequation"> $问_{d}$ 、速度<年代p一个nclass="inlineequation"> ${问_{}^{˙}}_{d}$ 、加速度<年代p一个nclass="inlineequation"> ${问_{}^{¨}}_{d}$ をもつ目的のジョイント軌跡に沿って追跡するために,計算トルクコントローラーが,与えられたロボットダイナミクス変数<年代p一个nclass="inlineequation"> $米（问）$ 、<年代p一个nclass="inlineequation"> $C （问，问_{}^{˙} ）问_{}^{˙}$ および<年代p一个nclass="inlineequation"> $G （问）$ によって,指定されたコンフィギュレーションと速度の取得に必要なトルクを計算します。金宝app仿真软件では,机器人系统工具箱のロボット工学マニピュレーターブロックを使用してこれらの変数を簡単に派生させ,次の計算トルクコントローラーを設計することができます。

$u ＝米（问）（ {问_{}^{¨}}_{d} - K_{d} {问_{}^{˙}}_{e} - K_{p} 问_{e} ） + C （问，问_{}^{˙} ）问_{}^{˙} + G （问）$

ここで,<年代p一个nclass="inlineequation"> $问_{e} ＝问 - 问_{d}$ は位置誤差,<年代p一个nclass="inlineequation"> ${问_{}^{˙}}_{e} ＝问_{}^{˙} - {问_{}^{˙}}_{d}$ は速度誤差です。この制御入力によって,システムダイナミクスは2次颂歌になります。

${问_{}^{¨}}_{e} + K_{d} {问_{}^{˙}}_{e} + K_{p} 问_{e} ＝ 0$

$K_{d}$ および<年代p一个nclass="inlineequation"> $K_{p}$ を適切に選択すると,追跡誤差<年代p一个nclass="inlineequation"> $问_{e}$ は,時間が無限大に近づくとゼロに収束します。

计算转矩控制器サブシステムは,关节空间质量矩阵、速度的产品转矩、重力力矩という3つのロボット工学マニピュレーターブロックで構築されています。関連付けられたrigidBodyTreeモデルである索耶がこれらすべてのブロックに割り当てられていることと,コンフィギュレーションと速度を列ベクトルとして挿入する必要があることに注意してください。

open_system (<年代p一个n年代tyle="color:#A020F0">“robotSafeTrajectoryTracking /计算力矩控制器”）;

计算力矩控制器内には調整可能なパラメーターが2つあります(色つきのブロックで示されます)。

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

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

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

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

ここで,<年代p一个nclass="inlineequation"> $ω_{n}$ と<年代p一个nclass="inlineequation"> $ξ$ は2次颂歌の固有振動数と減衰比です。この例で,KpとKdの既定値は,固有振動数と減衰比を<年代p一个nclass="inlineequation"> $ω_{n} ＝ 10$ および<年代p一个nclass="inlineequation"> $ξ ＝ 1$ として設定して派生させ,2次颂歌が臨界減衰系になります。

軌跡スケーリング

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

轨道伸缩
想要运动

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

$f_{年代} （） > 0$ では,ロボットが前進します(<年代p一个nclass="inlineequation"> $f_{年代} （）＝ 1$ が通常速度)。
$f_{年代} （）＝ 0$ では,ロボットが停止します。
$f_{年代} （） < 0$ では,ロボットが後退します。

轨道伸缩ブロックでは,外部トルク<年代p一个nclass="inlineequation"> ${τ_{}^{ˆ}}_{e x t} ＝ τ_{m e 一个} - τ_{}^{ˆ}$ を推定して<年代p一个nclass="inlineequation"> $f_{年代} （）$ を計算する必要があります。ここで<年代p一个nclass="inlineequation"> $τ_{m e 一个}$ は,Simscapeモデルから測定されたトルク,<年代p一个nclass="inlineequation"> $τ_{}^{ˆ}$ は,前のタイムスタンプでの,目的の動作の予測トルクです。モデルの外部扭矩观察者セクションで,逆动力学ブロックは測定トルクから減算される予測トルクを計算します。予測トルクは<年代p一个nclass="inlineequation"> $τ_{}^{ˆ} ＝米（问_{d} ）（ {问_{}^{¨}}_{d} ） + C （问_{d} ， {问_{}^{˙}}_{d} ） {问_{}^{˙}}_{d} + G （问_{d} ）$ です。

想要运动ブロックでは,多項式係数が与えられ,指定された时间戳入力を使用して目的の軌跡が生成されます。この入力は,軌跡スケーリングアルゴリズムに基づいて調整できます。<年代p一个nclass="inlineequation"> $问_{d} （ t ）$ 、<年代p一个nclass="inlineequation"> ${问_{}^{˙}}_{d} （ t ）$ 、<年代p一个nclass="inlineequation"> ${问_{}^{¨}}_{d} （ t ）$ は軌跡多項式に基づく出力で,计算力矩控制器サブシステムに入力されます。

Simscape多体机器人模型と単純な接触機構

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

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

モデルの実行

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

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

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

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

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

シミュレーションの実行中,スライダーを調整してオブジェクトをロボットの近くまたはロボットから遠くに動かします。ロボットは,軌跡の安全な実行を続けようとしながら,オブジェクトの位置に反応するはずです。
金宝app仿真软件の[停止]ボタンをクリックしてシミュレーションを停止します。

まとめ

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