計算工学講演会論文集 Vol.6(2001年5月)

計算工学会

有限要素法を用いた閉リンク機構の逆動力学計算
CALCULATION OF INVERSE DYNAMICS FOR CLOSED-LOOP LINK MECHANISMS USING FEM
磯部 大吾郎1)

Daigoro ISOBE
1) 工博 筑波大学講師 機能工学系（〒305‑8573 茨城県つくば市天王台1‑1‑1, isobe@kz.tsukuba.ac.jp）

In this paper, a scheme using Finite Element Method (FEM) for calculating inverse dynamics is applied to closed-loop
link mechanisms. In this scheme, the entire system is subdivided into discrete elements and evaluated as a continuum. A single
link structure of a pin joint and a rigid bar is expressed by using the Shifted Integration technique. It calculates nodal forces byevaluating equations of motion in matrix form, and thus information in the entire system can be handled in parallel. The obtained
nodal forces are then used to calculate joint torque in the system. A simple numerical test on a closed-loop link mechanism iscarried out, and it is verified that the scheme can be used as a unified calculation scheme independent to system configuration.
Key words: Closed-Loop Link Mechanism, Inverse Dynamics, Finite Element Method, Shifted Integration Technique

１． はじめにニュートン・オイラー法やラグランジュ法などの従来の手法によるリンク機構の逆動力学計算法では，閉ルる手法である．すなわち，個々の有限要素の性状を表す構成式さえ良好に近似されていれば，系全体の挙動を把握し制御することが可能となる．

ープ系が存在する機構について計算する際には大きな困

次に文献[5]では，FEM を剛体マニピュレータの逆動力難を伴う．これは，前者では外向き・内向き計算を経て学計算法として適用することを提案した．ここでは 2 次リンク間の相互依存変数を求めるため，系が閉じると動元の開リンク系を連続体とみなし，目標軌道上を動くた力学方程式の導出が不可能となるからであり，後者ではめに必要な節点力を FEM により算出し，これを利用し拘束条件を加味した方程式の導出過程が非常に煩雑となて関節トルクを求めるアルゴリズムを構築した．節点力るからである．そこで，閉ループの一部を切断して開ルを並列的に求解可能である本手法の特長を用いると，リープ系の集合として拘束条件を設定し，ニュートン・オンク数が多い場合にも容易に計算できるだけでなく，系イラー法により力学的に解く方法[1]などが考案されていに構成変化が生じた場合にも柔軟に対応できることが確るが，複雑な機構ではパラメータ数が増大し，その拘束認された．

条件を導出する過程も複雑となる．また，実際のロボッFEM を用いた手法では，系全体を有限要素で離散化し，トの動作では，系内に開ループと閉ループが交互に発生

要素座標系における各要素の情報を直交座標系に基づくし，動力学方程式（または計算アルゴリズム）を瞬時に全体座標系の情報に変換してから重ね合わせる，いわゆ入れ替える必要性が生じる場合がある．特に，ロボット

る並列的なアプローチをしている．そのため開リンク系の動作が高速化して力制御の重要性が増すと，このようの場合と同様に，系が閉じた場合にも各要素の節点力はな場合に柔軟に対応できる統一的な逆動力学計算法が必

並列的に求められ，これを関節トルクに変換することに要となることが考えられる．

よって逆動力学が計算される．また，この特長を利用す

筆者らは，冗長性の高い系の制御を並列的に処理する手法として有限要素法(FEM)を適用することを提案し，ると，トルク配分比や受動関節位置の変化などにも柔軟に対応した計算が実施できるものと思われる．

圧電アクチュエータ集合体を連続体とみなした系の本報告では，リンク機構に対する統一的逆動力学計算

FEM による制御を試みた[2][3][4]．その結果，連続体法の開発の一環として，FEM による手法を閉リンク機構の一部としてアクチュエータに必要な電圧が算出され，の問題へ適用する．そして，受動関節が１つの簡単な 2

個々のアクチュエータが独立にではなく，系の一部とし次元閉リンク機構のトルク算出問題について考察し，本て制御されることが実験によって確認された．FEM は，手法の有効性について検証する．

節点を介した微小要素ごとの情報を全体系のものに重ね合わせ，マトリックス形式の全体方程式により解を求め

２．有限要素法による節点力の導出過程
２．有限要素法による節点力の導出過程

リンク機構のモデル化には，本来は骨組構造の有限要素解析に用いられる Shifted Integration 法(SI 法)[6]を適用

i+1 番目のリンクの関節トルクτi+1 を加算したものとなり，要素座標系の節点力を用いるとした．モデル化の詳細については文献[5]を参照されたい．
SI 法を用いると要素端に関節を表現することが可能となるため，１本のリンクを２つの線形チモシェンコはり要úi = liC FiCx + li (FjC )x + FiCû + úi+1(4)j=i+1

素によって表現し，要素数の削減により計算時間の大幅な短縮化を図ることが可能となる．

nXと与えられる．ここで，liC は関節から重心位置までの距

仮想仕事の原理より，時刻 t＋Δt における増分型の運動方程式は以下のようになる．

離，li はリンクの長さ，(SFjC)は合力を示す．変数右下の添字 i,j などは対象とするリンクの番号を示し，C は重心位置での成分，x,z は要素座標系の各軸方向成分，X,Z[M ]f°
ugt+Å t +[C]fug_ t+Å t +[K]fÅug = fF gt+Å t ÄfRgt
(1)
fugt+Å t = fugt + fÅug(2)は全体座標系の各軸方向成分，φは Y 軸回りの成分であることを示す．
式(4)を i=1, …, n について並べてマトリックス形式に直し，全体座標系で整理すると，関節トルクベクトルはここで，[M]は集中質量マトリックス，[C]は減衰マトリug は節点加速度ベクックス，[K]は剛性マトリックス， f°

fún g = [Ln ][T n ]fP n g

クトルである．また，{F }は外力ベクトル，{R }は要素と表現できる[5]．ここで，{Pn}は 5n×1 の全体座標系に内力ベクトルである．本報告では剛体リンク系を取り扱おける節点力に関するベクトル， [Tn]は 5n×5n の全体座うため，[K]および[C]を無視して上式を整理すると，節標系からリンク（または要素）座標系への変換マトリッ

点力増分は以下の式で求められる．

クス，[Ln]は n×5n の部材長マトリックスである．成分数_ は節点速度ベクトル，fÅug は節点変位増分ベトル，fug(5)

は全て２次元の場合である．また，添字 n は対象とする

fÅf g = fRgt Ä fF gt +[M ](機構の総リンク数を示す．２次元リンク機構の場合の部

11
1fug_ tÄ(Ä 1)f°ugt ) (3)fÅug Ä2åÅtåÅt2å

ここでは，直接時間積分法の一種である Newmark のβ法（β=1/4）を使用した．上式によって算出した節点力増分を逐次加算することにより，各リンクが目標軌道上を動作するために必要な節点力を算出する．

３．閉
３．閉リンク機構のトルク算出過程
有限要素法の逆解析アルゴリズムにより節点力を求めた後，これを関節トルクに換算する．n 本のリンクによって構成される閉リンク機構内の i 番目のリンクには，図 1に示すようにその重心に作用する並進力に起因する回転モーメント，先の関節に作用する i+1〜n リンクの並進力の合力に起因する回転モーメント，および重心回りに作材長マトリックスは，次式のような上三角行列として表現される．

26666hi 66Ln = 666664
ただし，

hLiL1L2L2L3L3L30i=

hliC03Å Å Å Ln7Å Å Å Ln 77Å Å Å Ln 777Å Å Å Å 77Å Å Å 777Å Å 5Lnli01i(6)(7)用する慣性モーメントが生じる．したがって i 番目のリンクに必要な X‑Z 平面内の関節トルクτi は，これらの和に

k-2j=i+1k-1k-23i-1

Zki

k3FjCXk+1k+1k+22φiる．さらに，部材長マトリックスを次式のように複数のz領域に分け，受動関節の位置，および各能動関節が受け

j=i+1i+1

ル[Tn]{Pn}が上記のマトリックスと掛けられることによって，対象とするリンクより先の節点力情報が加算され

FjC ZiC

k-1である．要素座標系に変換された節点力に関するベクトx持つトルク配分を操作する．

X12n2n-12n2n-1

1
12n+12n:Link No. 1~n:Element No. 1~2n
none :Nodal No. 1~2n+1

Fig.1 Nodal force acting on i -th link in closed-looplink mechanismn[L ] ="La00Lb#(8)の質量を配している．各リンク部材の長さを 0.5m とし，1.5それぞれの部材の重心位置にも部材の質量を分配した．

m = 1.0 kg
1Z [m]●：Mass●：Mass-0.5-0.5

す．図 3(a)の case1 の場合には非線形項の影響が少ないため，初期トルクが関節 3 および 6 で質量 m と部材の質量を支えるのに必要なトルクと一致している．また，機構

410について解析を行い，算出されたトルク曲線を図 3 に示

520.5

動作時間が 10 秒間の場合(case1)と 1 秒間の場合(case2)

6m

3が傾くにつれてトルク値が負の方向に増大している（ト○：Joint○：Joint

00.5

ルクは時計回りが正）．最終的なトルクの値も，理論的な

11.5

X [m]

Fig.2 Target trajectory for closed-loop linkmechanism値と一致した．図 3(b)の case2 の場合には，動作時間が短いために初期トルクや曲線の上下幅などに変動が見られ，非線形項の影響が考慮された妥当な結果であることが分かる．
次に，本モデルの向かって左側と右側の関節のモータ4が受け持つトルク配分比を変え，このときのトルク曲線

Torque [N m]2の変化を調べた．図 4 に配分比を 4:1 にした場合のトルク0曲線を示す．左側の関節 1,2,3 の負担が全体的に増加し，-2一方，右側の関節 4,5,6 の負担が減少していることが確認できる．FEM を用いた手法では，個々の節点に作用する-4Joint 1
Joint 2
Joint 3
Joint 4
Joint 5
Joint 6-6-8-10024

節点力を独立に求めるため，このように各関節が受け持つトルク配分比を自由に変化させることが可能である．
これは，例えば複雑な運動をする歩行ロボットの機構設
6810Time [s]Fig.3(a) Calculated torque curves (case 1)

計をする際に有用な特長であると考えられる．
さらに，図 5 のように質点を配した受動関節を右側の節点に配置した場合の解析を行った．この場合にはトルTorque [N m]ク配分比を 1:1 に戻した．図 6 にこの場合のトルク曲線4を示すが，質点までの距離が長くなった分だけ左側の関2節の負担が増大していることが分かる．逆に右側の関節0は，質点までの距離が短くなった分だけ負担が減少している．関節 4 では，質点および部材の重量を支えるため-2のトルクが算出されている．このように，受動関節の位-4-8-10置が変化するなど機構に構成変化が生じた場合にも，本Joint 1
Joint 2
Joint 3
Joint 4
Joint 5
Joint 6-600.20.4

手法では入力データを書き換えるのみで対処が可能である．また，受動関節での拘束条件を考慮する必要がなく，0.60.81Time [s]Fig.3(b) Calculated torque curves (case 2)

ソフトウェア内の運動方程式などの記述を変える必要がない．本手法が有するこの簡便性は，様々な閉リンク機構のパラメータ設計をする際に有用となる．
4n)，その連結部に受動関節がある場合の部材長マトリッ

2クスである．式(3)で逐次的に算出された節点力増分を式(5)に代入してトルクの次元に変換することにより，機構内の n 個の関節トルクを並列的に求めることが可能となる．

Torque [N m]

上式は，閉リンク機構を a 本と b 本のリンクに分け(a+b =Joint 1
Joint 2
Joint 3
Joint 4
Joint 5
Joint 60-2-4-6４．数値例
４．数値例
図 2 に示すようなはり中央部に質点を載せた簡単な閉-8-1000.20.40.60.81

リンク機構に目標軌道を与え，算出されるトルク曲線をTime [s]考察して本アルゴリズムの有効性について検証した．モ

Fig.4 Calculated torque curves(torque distribution ratio = 4:1)

デルは 6 つの能動関節を有し，機構上部の受動関節に 1kg

1.54m = 1.0 kg
34m

20.5

65

10

●：Mass●：Mass-0.5-0.502Torque [N m]Z [m]11-4-6-8
Joint 1
Joint 2
Joint 3
Joint 4
Joint 5
Joint 6-10-12○：Joint○：Joint
0.5

0-2-14-16

1.5

X [m]

00.2

0.4

0.60.81Time [s]Fig.5 Target trajectory (passive joint atdifferent point)

Fig.6 Calculated torque curves(passive joint at different point)

５．結論
開リンク機構の逆動力学計算に適用しその有効性が立証されている FEM による手法を，本報告では閉リンク機構の逆動力学計算に適用した．一般的なラグランジュ法や，複数の仮想的な開リンク機構に分割して拘束条件を操作する方法ではその実施に困難が伴うが，本手法は開リンク機構に対するアルゴリズムをほとんど変更することなく容易に閉リンク機構に適用でき，機構形態に依らない統一的な逆動力学計算法となり得ることが確認できた．今後は，３次元の閉リンク系への適用，複数の受動関節が存在する閉リンク系への適用，および動作過程で開・閉ループ系が交互に生じる機構問題への適用を検3) Isobe, D., Nakamura, H. and Shimizu, R.: AReal‑Time Control System for ConnectedPiezoelectric Actuators: A Finite‑ElementApproach, Journal of Robotics and Mechatronics,
Vol.12, No.2, (2000), pp.172‑179.
4) 磯部大吾郎, 中村 博, 清水 隆太: 圧電アクチュエータ集合体のリアルタイム FEM 制御システムの開発,
日本機械学会論文集 (A 編), 第 66 巻, 第 645 号,(2000), pp.861‑866.
5) 磯部 大吾郎, 竹内 裕喜, 上田 健夫: 超冗長マニピ
ュレータの関節トルク算出法 −FEM を用いた理論の展開−, 日本計算工学会論文集, 第 2 巻，(2000),
pp.73‑78.

討していきたい．
6)

参考文献
1) 中村 仁彦：パラレルメカニズムの動力学，日本ロボット学会誌, Vol.10, No.6, pp.709-714, (1992).
2) 磯部大吾郎, 中川 恒: FEM による圧電アクチュエー
タ集合体のリアルタイム並列制御, 日本機械学会論文集 (A 編), 第 65 巻, 第 629 号, (1999),
pp.120‑126.

都井裕: 骨組構造および回転対象シェル構造の有限要素解析における Shifted Integration 法について,
日本造船学会論文集, 第 168 号, pp. 369-377,(1990).