3I13

機構形態に依存しない逆動力学計算法
磯部大吾郎 (筑波大学)
A Calculation Scheme of Inverse Dynamics Independent toSystem Configuration*Daigoro ISOBE, University of Tsukuba
Abstract −In this paper, a calculation scheme for inverse dynamics independent to system configurationof link mechanisms is constructed by using Finite Element Method (FEM). In this scheme, the entire system issubdivided into discrete elements and evaluated as a continuum. A single link structure of a pin joint and a rigidbar is expressed by two finite elements. The scheme calculates nodal forces by evaluating equations of motion in
matrix form, and thus information in the entire system can be handled in parallel. The obtained nodal forces arethen used to calculate joint torque in the system. Some numerical tests on open- and closed-loop link mechanisms are carried out, and it is verified that the scheme can be used as a unified calculation scheme independent
to system configuration.
Key Words: Link Mechanism, Inverse Dynamics, Finite Element Method, System Configuration

1. はじめにと，対象とするリンク系の構成が変化した場合にもソフトウェア内部の記述を変更する必要がなく，入力データを変更するのみで柔軟に対処することが可能となる 1)．
本稿では，有限要素法を利用した逆動力学計算法の概略と，本手法を開リンク機構と閉リンク機構に適用した場合の例を示す．

ロボットの歩行動作や複数のマニピュレータによる協調動作などでは，系内に開ループと閉ループが交互に発生し，動力学方程式（または計算アルゴリズム）を瞬時に入れ替える必要性が生じる場合がある．
しかし，ニュートン・オイラー法やラグランジュ法などの従来の手法によるリンク機構の逆動力学計算では，閉ループ系が存在する場合としない場合とではその動2. 有限要素法による逆動力学計算過程力学方程式（または計算アルゴリズム）が大きく異なるため，その実行に大きな困難を伴う．特にロボット
図1に示すように，モータとリンク部材によって構の動作が高速化・複雑化して力制御の重要性が増すと， 成されたリンク機構は，リンクの重心位置に節点を設系の変化に対して柔軟に対応できる統一的な逆動力け，関節位置の反対側に数値積分点をシフトした２つ学計算法の必要性が増すことが考えられる．の線形チモシェンコはり要素によって表現する1)．こ
本稿では，機構形態に依らない統一的な逆動力学計のような要素数の削減により，計算のリアルタイム処算法の構築を目的として，有限要素法を適用して開発理が可能となる．
したものについて述べる．有限要素法は，要素座標系計算アルゴリズムでは，まず目標軌道（すなわち変における個々の要素の離散情報を全体座標系の情報位）を入力とし，それを達成するために必要な節点力に変換してから重ね合わせる，いわゆる並列的なアプを求める逆解析を行う．そのため，以下の式を用いてローチをしている．また，リンク系を連続体とみなし， 時刻tにおける節点力増分ベクトル{Δf}を算出する．
これを有限要素により離散モデル化する．
そのため各
要素の節点力は並列的に求められ，これを関節トルク{∆f } = {R}t − {F }t + [M ] 1 2 {∆u} − 1 {u&}t −  1 −1{u&&}t (1)ββt2∆βt∆に変換することによって逆動力学が計算される．
節点
力を並列的に求解可能である本手法の特長を用いる
Joint (Servo motor)

Center ofgravity+

Numerical integration
pointLinkFinite
element+(a)Link mechanism

図1

ここで，[M]は集中質量マトリックス，{u&&}t は節点加速度ベクトル， {u&}t は節点速度ベクトル， {∆u} は節点変位増分ベクトルである．また， {F }t は外力ベクトル，{R}t は要素内力ベクトルである．βはニューマークのβ法のパラメータで，本研究ではβ=1/4 を用いている．本アルゴリズムでは，上式に変位増分および時刻t における速度，加速度を入力することにより節点力増分を算出し，これを逐次加算して目標の変形を得るために必要な節点力を求める．
次に，図 2 に示すような一般的なｎリンク機構において，節点力と関節トルクとの関係を考える．i 番目のリンクには，その重心に作用する並進力に起因する回転モーメント，先の関節に作用する i+1〜n リンク

Nodal point(b)Finite elements

有限要素によるリンク機構のモデル化

第１９回日本ロボット学会学術講演会（２００１年９月１８-２０日）-895-

FjC ZiC

k-1

Z

kk-2
35

j=i+1

k-1
ii-1

k

3

φi

FjCX

k+1k+1xk+2i+12z

j=i+1Torque [N m]k-2X

12n2n-1:Link No. 1~n2n2n-1

12n+1

12n:Element No. 1~2n0

Joint1
Joint2
Joint3
Joint4
Joint5
Joint6
Joint7
Joint8-5-10

none :Nodal No. 1~2n+1-15

図20n リンク機構に働く節点力
図3nnnn0.60.81(3)と表現できる 1)．ここで，{Pn}は節点力に関するベク
トル,[Tn]は全体座標系から要素座標系への変換マトリックス，[Ln]は部材長マトリックスである．

3. 数値例
本手法による逆動力学計算の数値例として，8 リン
ク開ループ系に適用した場合のトルク曲線を図 3 に示す．長さ 20cm のリンク部材（重心位置：中央，質量 107.5g）を 8 本連結し，図内にあるような 1 秒間での振り上げ運動を与えた．
遠心力やコリオリ力などの非線形項を考慮したニュートン・オイラー法による結果と良好に一致した．また計算時間も，十分にリアルタイム処理が可能な範囲内であった．
次に，本手法を 6 リンク閉ループ系に適用した場合
2)のトルク曲線を図 4 に示す．図内に示すように，対象モデルは 6 つの能動関節を有し，機構上部の受動関節に 1kg の質量が配されている．各リンク部材の長さを 50cm とし，それぞれの部材の重心位置にも部材の質量を分配した．トルク曲線では，非線形力が影響を与えていることが確認できる．この他，節点力レベル-896-

8 リンク開ループ系のトルク曲線

4Torque [N m]2(2)と与えられる．ここで，liC は関節から重心位置までの距離，li はリンクの長さ，(ΣFjC)は合力を示す．変数右下の添字 i,j などは対象とするリンクの番号を示
し，C は重心位置での成分，x,z は要素座標系の各軸方向成分，X,Z は全体座標系の各軸方向成分，φは Y軸回りの成分であることを示す．また，変数右上の添字 n は対象とする機構の総リンク数を示す．上式をi=1, …, n について並べてマトリックス形式に直し，全体座標系で整理すると，関節トルクベクトルは{τ } = [L ][T ]{P }0.4Time [s]の並進力の合力に起因する回転モーメント，および重心回りに作用する慣性モーメントが生じる．
したがって i 番目のリンクの関節トルクτi は，これらの和にi+1 番目のリンクの関節トルクτi+1 を加算したものとなり，要素座標系の節点力を用いると
 n
τ i = liC FiCx + li  ∑ FjC  + FiCφ +τ i+1
 j =i +1  x

0.2

Joint 1
Joint 2
Joint 3
Joint 4
Joint 5
Joint 60-2-4m = 1.0 kg
3-66m
52-84

1
●：Mass ○：Joint

-10

00.2

0.4

0.60.81Time [s]図46 リンク閉ループ系のトルク曲線で並列的に解が求まる本手法の特長を最大限に利用すると，各関節が受け持つトルク配分比や受動関節の位置を自由に設定することが可能である 2)．このような柔軟性は，閉リンク機構の様々なパラメータ設計をする際に有用となる．

4. 結論
本来は構造解析などのツールとして用いられる有限要素法を，開・閉リンク機構の逆動力学計算法に適用した．本手法では並列的に解が求まるため，機構の構成変化にも柔軟に対応可能である．また，開・閉ループ系の如何に関わらずそのアルゴリズムには大きな差異がなく，トルク算出過程も簡潔である．今後は，動作過程で開・閉ループ系が交互に生じる機構問題などに適用し，機構形態に依らない柔軟な逆動力学計算法としての実用性を高めていきたい．
参考文献
1)

2)

磯部 大吾郎, 竹内 裕喜, 上田 健夫: 超冗長マニピュ
レータの関節トルク算出法 −FEM を用いた理論の展開−, 日本計算工学会論文集, 第 2 巻, pp.73-78,(2000).
磯部大吾郎: 有限要素法を用いた閉リンク機構の逆動力学計算, 計算工学講演会論文集, 第 6 巻, 第 2 号,
pp.803-806, (2001).