2P2-C02

有限要素法を用いた逆動力学計算によるリンク機構の制御実験
Control Experiments of Link Mechanisms Using Inverse Dynamics Calculated by FEM○ 筑後 陽一(筑波大院)

佐藤 俊介(筑波大院)

正 磯部大吾郎(筑波大)In this report, a new calculation scheme of inverse dynamics for link mechanisms using
Finite Element Method (FEM) is implemented into a control system. Joint torques of linkmechanisms are calculated in parallel, and also incrementally, in the calculation scheme.
Therefore, some investigations are made to verify the validity of the control system. This paper
explains the outline of the control system, in which the proposed scheme is used to supply afeed-forward control. Some control experiments are carried out, and the results are comparedwith those by a PD control system.
Keywords：Link Mechanisms, Inverse Dynamics, Control, Finite Element Method (FEM)

1.はじめに1.はじめにる．一方，FEM による逆動力学計算法では，まず時々刻々

ロボット工学の発展に伴い，ロボットに求められる動作はの節点力増分を算出し，それを足し合わせて逐次的に節多様化してきている．ロボットに高速，複雑な動作をさせようと点力を求め，最終的にトルクに換算する．また，従来のする際，遠心力やコリオリ力といった非線形力項の影響を受手法では各リンクにおける相対回転座標系を用いるのにけることが予想される．この問題に対処するには，非線形力

対し，FEM による手法では絶対直交座標系を用いる．こ項を考慮した動力学方程式を導出し，制御系に動力学補償のように，解の計算法や扱う座標系などが従来の手法とを組み込むことが大変有効な手段となる．

異なるため，本手法を使用する場合には，実際に制御を従来の逆動力学計算法，例えばニュートン･オイラー法行う際に重要となるフィードバックの導入や，計算時間などでは，リンク機構に対して内向き，外向き計算を行などについて検討を行う必要がある．そこで本報告では，い，リンク間で相互依存する変数による動力学方程式をFig.1 のような平面 3 リンク機構に対し，PD フィードバ導出している．そのため，多自由度を持つリンク機構やック制御と FEM 逆動力学計算によるフィードフォワード

リンク構成が変化するような機構，または系が閉じるよ制御を組み合わせた Fig.2 のような制御系を構築し，軌うな機構では式の導出が非常に困難になってしまう．

道追従制御の実験を行った．

一方，ロボットの動作が高速化，複雑化してくると，逆動力学計算部では目標軌道から FEM により逆動力学系の形態や拘束条件の変化などに対して柔軟に対応でき計算を行い，逐次的に全関節のトルクを算出する．リン

る統一的な逆動力学計算法の必要性が増すことが考えら

ク機構を実際に制御する際には，逆動力学によるトルクれる．そこで，並列解法の一種である，有限要素法(FEM)値の他に，モータに発生する摩擦力の影響やフィードバを適用した逆動力学計算法が提案された[1]．この手法でック値などを加え，次式のように計算した．

は，系全体を有限要素で離散化し，並列的にその節点力

τ = Jq& + Dq + f c + τ I . D. + τ feedbackを求める．そして，力学的関係より節点力をトルクに変換することで逆動力学を求める．本アルゴリズムには，直交(1)座標系に基づいて関節トルクを算出することや，運動方程式を厳密に解くのではなく，時間増分ごとに逐次的に解くことにただし，τは入力トルク，J はモータの慣性モーメント，よって算出するといった特徴がある．本研究では，以上の特徴を持つ FEM による逆動力学計算法を実際に制御系に組込み，簡単な開リンク機構の制御に適用することで，本手法の実用性を確認することを目的とした．本報告では，ギヤ無しモータを使用した平面開リンク機構に対し，逆動力学によるフィードフォワード制御と PD フィードバック制御を適用した場合についての検討結果を報告する．

２．制御系の構成
従来の逆動力学計算法では，動力学方程式に変位，速度，加速度を入力してその時刻のトルクを直接的に求めFig.1 3-link mechanism used in the control experiments

日本機械学会 [No.02-6] ロボティクス・メカトロニクス講演会'02 講演論文集
2P2-C02 (1)2002.6.7〜9

松江市

X , Z, qτ I.D.
Correction
considering
friction

Calc. of Inverse
Dynamics byFEM

d0.5

τ＋&q

dq＋＋d＋

DAｰboardVServoamp.

I0.3

τfeedbackEncoder

Kv＋－0.4

MotorZ[m]

PC

0.2

q&

Link3

0.1
Link2＋

Ku－X,Zq0： Coordinates in global Cartesian coordinates

0.1

0.2
0.3
X[m]

0.4

0.5

Fig.3 Target for control in 33-second motion

q , q& ： Actual angle, speed
q d , q& d ： Target angle, speed
Ku , Kv ：

Link1

02

Feed back gain for angle, speed

Rotational angle [rad]Fig.2 Outline of the control system

q はモータの角速度，D はモータの粘性抵抗，fc は動摩擦力，τI.D.は逆動力学計算により求めたトルク値，τfeedbackはフィードバック値である．本実験では，モータの慣性モーメント J はカタログ値から，モータの粘性抵抗 D，動摩擦力 fc は実験的に同定した．フィードバックは，前1.5

1Link 1
0.5

TargetFeed-forward + PD
PD

ステップの直交座標系における目標角度を要素座標系に座標変換した値と，計測された角度情報を用いて次式の0ように行った．

τ feedback = K u ( q d − q ) + K v (q& d − q& )04Time[s]68Fig.4 Control result of LinkLink-1 in 33-second motion(2)

ここで q d , q& d はそれぞれ目標角度および目標角速度，q , q& はそれぞれエンコーダより取得した実際の角度お2４．まとめ
本報告では，実際に平面３リンク機構の制御に FEM によび角速度， K u , K v はそれぞれ角度，角速度に対するゲよる逆動力学計算法を適用して実験を行い，フィードバインである．また，サーボアンプでは電流制御モードをック制御の導入やリアルタイム性についての検討を行っ使用した．

た．その結果，逐次的に解を算出する本手法の場合でも，PD フィードバック制御と組み合わせリアルタイムに使用

３．実験結果可能であることが確認できた．今後は，並列解法の一種

サンプリングタイム 10 ms で時々刻々のトルク値をフである本手法の特徴を最大限に利用し，本報告で構築しィードフォワード的に入力し，PD フィードバックと組みた制御系を閉リンク機構や開閉リンク系が交互に発生す合わせて制御した場合と，PD フィードバックのみで制御るような問題などに適用し，統一的な手法としての実用した場合の 2 種類の実験を行った．Fig.3 に制御時間 3 s性を高めていく予定である．また，逆動力学の算出が特の目標軌道を，Fig.4 にリンク１におけるモータの回転に重要となるような，リンク系と外界との間に力が作用角度の時刻歴を示す．

する場合などについても実験を進め，検討を行いたい．

実験において，10 ms のサンプリングタイムで逐次的なトルク値の計算から電流値への変換，指令電圧の出力

参考文献までなされており，十分にリアルタイム性が確保できる[1] 磯部大吾郎 他: 有限要素によるリンク機構のモデル化（その１〜その３）,日本機械学会ロボティク
ス・メカトロニクス講演会, (2000, 2001).

ことが分かった．また，フィードフォワード的にトルク値を与えた場合，PD フィードバックのみで制御を行った場合と比較して追従性，収束性共に優れていることが確認できた．フィードフォワードを加味した場合，フィー[2]ドバックのゲインを小さくでき，速い動作などの制御には特に有効であると考えられる．

2P2-C02 (2)小林尚登，増田良介，小森谷清 他，計測自動制御
学会編

ロボット制御の実際，コロナ社．