ロボット工学ブロック使用た安全な追従制御の実行

この例はを使用ます：

simuli金宝appnk®ととととアルゴリズムして，シミュレートされれたロボットの安全な轨迹追従追従制御制御制御を実现実现

机器人系统工具箱™simscape MultiBody™ののの両方がが必要ですですですです

はじめに

この例，アルゴリズムブロック使用して，ジョイントの位置速度速度のののフィードバックフィードバックフィードバックをををを备え备え备えたた，，计算计算トルクトルクトルクコントローラーコントローラーコントローラーコントローラーをををををををを実装実装実装するするするするするするするモデルモデルモデルモデルモデルモデルモデルをををををををを使用使用）からから位置との情报受信し，，を送信しししてて，，与えられたジョイントジョイント轨迹轨迹に沿っ沿ってロボットロボットロボットをを动かし动かし动かしますますます轨迹ががが実行実行，平面ロボット前に配置されていますのの设定设定をを行わ行わ行わないないとオブジェクトオブジェクトととのの冲突冲突によるトルクトルクにより，，ロボットロボットロボットロボットオブジェクトががががするするする可能可能可能可能可能に目的をときときタイムスタンプをするするためスケーリングスケーリングブロックブロックがが构筑构筑构筑さされれいいますます。。モデルモデルの実行実行中中ににににいくついくついくついくつをを调整调整调整调整调整たロボットの效果観察できます。

ロボットモデルの设定

この例は，，自由自由度のロボットマニピュレーターであるであるであるであるsaw saw saw saw sawのののを使用Importrobotunified机器人说明格式（urdf）ファイルファイルファイルにされているからから刚性木板simscapeとととと一致するよう，，，，DataFormatプロパティおよび重力simulinkモデルモ金宝appデルモデルモデル中，ワークからこのこのロボットモデルににアクセス。。。

sawyer = incortrobot（'sawyer.urdf'）；sawyer.emovebody（'头'）；sawyer.dataformat ='柱子';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 =零（7,1）;

次図平面オブジェクト位置に关连，ロボットのととターゲットコンフィギュレーションコンフィギュレーションのの可视可视化化を示します。。平面はは，轨迹追従追従

金宝appsimulinkモデルで，，<一个href="//www.tatmou.com/jp/jp/help/robotics/ref/polynomialtrajectory.html" data-docid="robotics_ref#mw_59e05d0a-835f-4d56-85a6-ee509b10771a" class="a" hreflang="en">多项式轨迹5次次多项式使用し轨迹内のの时点におけるロボットのの位置，速度速度，加速度をを计算

金宝appsimulinkモデルの概要

sim金宝appulink simulinkモデルを开きます。でされた変数変数は，，，，，，，，モデルモデルモデルスペーススペースにに既に保存保存さささ

open_system（'RobotsafetrajectoryTracking.slx'）；

RobotsafetrajectoryTrackingモデルは安全追従を行うためのを备えた，计算计算トルクトルクコントローラーコントローラーをを実装ししますます。このこのこのこのモデルモデルモデルモデルににににににははは主主主主なななサブサブサブシステム

计算トルクコントローラー
轨迹スケーリングとの动作
単纯な接触をもつもつもつもつもつもつもつ

各タイムは轨迹スケーリングのがオン场合，，，ジョイント位置位置，，，速度速度速度速度，，および加速度度ををを评価评価评価するするためためためにに，，変更変更変更変更さささされれたIgidbodytreeモデル关连られマニピュレーターをを使用して目的目的の动作动作しますますますます。。。。派生派生派生派生派生派生ししし制御制御入力ががががががががががががががががががががががががががががががががががががががががががががががががががががががが。

计算トルクコントローラー作成

$n$ 个のジョイントもつマニピュレーターは，ダイナミクスをのように表すことができます。

$m （（问）问_{}^{-} + C （（问，，，，问_{}^{-} ）问_{}^{-} + G （（问） = 你$

ここで $问$ ， $问_{}^{-}$ ， $问_{}^{-}$ ， $\in r^{n}$ は一般れたの位置，および，，， $你 \in r^{n}$ は制御（トルク）， $m （（问）$ はジョイントの质量行列， $C （（问，，，，问_{}^{-} ）问_{}^{-}$ は速度积トルク， $G （（问）$ は重力です。目的位置位置 $问_{d}$ ，速度 ${问_{}^{-}}_{d}$ ，加速度 ${问_{}^{-}}_{d}$ をもつジョイントに沿っ追迹ために，トルクコントローラーが，，与えられられたロボット $m （（问）$ ， $C （（问，，，，问_{}^{-} ）问_{}^{-}$ および $G （（问）$ によって指定れコンフィギュレーションと速度速度の取得必要必要なトルクトルクを计算ま金宝appすますますますsimulinkでは机器人系统工具箱の设计することができ。

$你 = m （（问）（（ {问_{}^{-}}_{d} - k_{d} {问_{}^{-}}_{e} - k_{p} 问_{e} ） + C （（问，，，，问_{}^{-} ）问_{}^{-} + G （（问）$

ここで， $问_{e} = 问 - 问_{d}$ は位置误差， ${问_{}^{-}}_{e} = 问_{}^{-} - {问_{}^{-}}_{d}$ は速度。この制御入力によって，ダイナミクスダイナミクスはは次次次次次次ににになります

${问_{}^{-}}_{e} + k_{d} {问_{}^{-}}_{e} + k_{p} 问_{e} = 0$

$k_{d}$ および $k_{p}$ を适切选択すると误差误差 $问_{e}$ は，が大近づくとゼロ收束します。

计算的扭矩控制器サブシステムは，关节空间质量矩阵，速度产品扭矩，重力扭矩3つのつのマニピュレーターで构筑さてます。关连付けたた刚性木板モデルである索耶がこれらブロック割り当てられいるとコンフィギュレーション速度をを列ベクトルとして指定するする必要ががあることにに注意注意

open_system（“ RobotsafetrajectoryTracking/计算扭矩控制器”）；

“计算的扭矩控制器”内に调整なパラメーターパラメーターががつありつありつありつあり（色つきので示さます）。

ゲインKP：ロボットコンフィギュレーション修正するのゲインゲイン
ゲインKD：ロボットコンフィギュレーション补正するのゲインゲイン

KPとKDを决定的方法では，ように计算し。。

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

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

ここで， $ω_{n}$ と $ξ$ は，次odeのの振动数と比ですこの例，，，KPとKDの既定は固有振动と减衰比を $ω_{n} = 10$ および $ξ = 1$ として设定派生さ，，次次次次次が减衰系になり。。

轨迹スケーリング

2つのははつのつのブロックがあり。。。

轨迹缩放
所需的运动

轨迹缩放は，で安全轨迹のため展开さているメインブロックブロックです。。ステップステップステップ $t_{一世}$ で，のタイムスタンプは $t_{一世} = t_{一世 - 1} + δ t$ 计算さ。しかしロボットが予期しないオブジェクトととた场合场合，，増大増大増大するトルク，，计画计画したた轨迹からのの逸脱によってによってによってととののの両方両方両方両方両方両方両方両方両方が，目的动作とされたトルク $τ_{m e 一个}$ の关数である $F_{s} （（问_{d} ，，，， {问_{}^{-}}_{d} ，，，， {问_{}^{-}}_{d} ，，，， τ_{m e 一个} ） \in [[- 1 ，，，， 1 这是给予的$ を导入て，タイムをを $t_{一世} = t_{一世 - 1} + F_{s} （（问_{d} ，，，， {问_{}^{-}}_{d} ，，，， {问_{}^{-}}_{d} ，，，， τ_{m e 一个} ） δ t$ として计算ことです。关数 $F_{s} （（）$ はロボット速度を制御，，し干渉干渉に基づいて决まります。。测定されれたトルクトルクトルクがが $F_{s} （（）$ を减らロボット速度をか，のトルク达するまで后退させます。 $F_{s} （（）$ のこれらは，の动作にのををますます。

$F_{s} （（） > 0$ では，が前进ます（ $F_{s} （（） = 1$ が通常速度）。
$F_{s} （（） = 0$ では，が停止し。。
$F_{s} （（） < 0$ では，が后退し。。

轨迹缩放ブロックでは外部トルク ${τ_{}^{ˆ}}_{e X t} = τ_{m e 一个} - τ_{}^{ˆ}$ を推定して $F_{s} （（）$ を计算必要がます。ここで $τ_{m e 一个}$ simscapeモデルモデル测定された，， $τ_{}^{ˆ}$ は，のスタンプでの，目的动作の予测予测トルクです。モデルモデルモデルモデルモデルモデルのののののののののの逆动力学ブロックはからされる予测トルクします。予测はは $τ_{}^{ˆ} = m （（问_{d} ）（（ {问_{}^{-}}_{d} ） + C （（问_{d} ，，，， {问_{}^{-}}_{d} ） {问_{}^{-}}_{d} + G （（问_{d} ）$ です。

所需的运动の领域，轨迹アルゴリズムの出力が多项式轨迹ブロックのとしてれ。このブロック，既知の値Q0とQ1，および（ゼロゼロゼロ速度速度速度速度速度速度速度速度基に，，，次次次多项式式轨迹を计算しますます。。。これによりにより位置，速度速度 $问_{d} （（ t ）$ ， ${问_{}^{-}}_{d} （（ t ）$ ， ${问_{}^{-}}_{d} （（ t ）$ ）が出力さ，“计算的扭矩控制器”サブシステム送られます。

SIMSCAPE多体机器人模型とと接触机构机构

SIMSCAPE多体机器人模型は同じ.urdfファイルからSmimport使用しし。。では，トルクトルク，，，速度速度用用ののの一连一连ののアクチュエータアクチュエータととセンサーセンサーがが追加さされれますますますます。。。。によりにより，，，，，反力がます。ここは単纯线形-ダンパー-ダンパー-ダンパーモデルが使用ささてて。。。。

メモ：接触机构エフェクタ平面オブジェクトの间実装さていいます。。したがっしたがってて，，アームアームそのそのその他他他ののの

モデルの実行

sawyerのにおけるにおけるのののののをして操作します。

simscape simscapeモデルモデルモデルの左あるあるクリッククリッククリックししててビューアービューアーを开き开きます。ジョイントジョイントトルク，，，エフェクタエフェクタと平面平面オブジェクトののの间の反応接触力ためのスタンプ含むがスコープにされます。

轨迹スケーリングスイッチ[off]にに切り替え。。

以下のを使用する，，[実行]をクリックてシミュレーション开始します。

sim（'RobotsafetrajectoryTracking.slx'，，，，“停止时间”，，，，'5'）；

アームオブジェクト冲突，大きな入力と大きな反力が生じののががわかりわかりますますます。。このケースでではは元元ののタイムタイムスタンプスタンプをを使用使用ししししているに注意注意注意注意

ここで轨迹スケーリングのをを[上]に切り替え，を再実行し。。

冲突后れたトルク値変わり，反力抑えられてていることことに注意注意し。

の実行スライダー调整しててロボットの近くから远く远くにに动かし动かしますます。ロボットはは，轨迹轨迹の安全な実行をををを

まとめ

simuli金宝appnk simulinkでで工学マニピュレーターを使用使用しててトルクコントローラーををを设计设计しししし，，，，，，，，，，，，，，，，，，，，，，，，，，，，のののののの轨迹スケーリングスケーリング动动的的シミュレーションシミュレーションシミュレーションととと统合ししてとして生じる反力およびタイムにより安全な追従を行うための轨迹スケーリングスケーリングの机能机能ましましまし