計算工学講演会論文集

Vol.6(2001 年 5 月)

計算工学会

３次元運動をする開リンク機構の有限要素法を用いた逆動力学計算
Calculation of Inverse Dynamics Using FEM for Open-Loop Link Mechanisms
in 3-D Motion今泉大作１），磯部大吾郎２）
Daisaku IMAIZUMI and Daigoro ISOBE

1)

筑波大学大学院 理工学研究科 (〒305-8573 茨城県つくば市天王台 1-1-1，dimaizum@kz.tsukuba.ac.jp)

2)

工博 筑波大学講師 機能工学系(〒305-8573 茨城県つくば市天王台 1-1-1，isobe@kz.tsukuba.ac.jp)

Finite Element Method (FEM) is a kind of parallel calculation scheme, which subdivide a continuum
system by discrete minute elements, and takes out a solution by calculating global equation constituted byelemental information. By applying FEM to the calculation of inverse dynamics of link mechanisms, it
may become easier to use, compared to other conventional schemes using dynamic equations where linkvariables depend upon each other. The purpose of this report is to establish the scheme using FEM for 3-D
motional link mechanisms. Some simulation tests are carried out using the scheme and the validity isverified.
Keywords : Link Mechanisms, Finite Element Method, 3-D Motion, Inverse Dynamics

1． はじめに効性を確認した。

古くから、様々なリンク機構の制御手法の開発や逆動力学問題の研究が盛んに行われている。従来、リンク機構の問題

2． ＳＩ法によるリンク機構のモデル化

はニュートン・オイラー法やラグランジュ法などにより運動方程本研究では、骨組構造の有限要素解析で実績のあるＳＩ式を導出し、それを使用した解析が行われていた。これらの手法を用いてリンク機構をモデル化する。この手法は塑性ヒ法によって導出される方程式は、各リンクの情報が他のリンクンジが発生する位置が明確であれば、それに伴いあらかじ情報に依存するという特徴を持っている。一方、連続体力学にめ数値積分点をシフトしておくという手法である。数値積基づく数値解析手法として広く利用される有限要素法（ＦＥＭ）

分点位置と塑性ヒンジ発生位置の関係は以下のように示は、系全体を微小要素に離散化し、それぞれの情報を全体方される２）。

程式にまとめて解を求めるという手法である。ゆえに、リンク系

ｓ＝−ｒ またはｒ＝−ｓ(１)を連続体の系として並列的に扱うことが可能となり、FEM を逆動力学計算に適用した場合、系の変化に対してソフトウェア自Ｌｉｎｋ ｍｅｍｂｅｒ

体に手を加えることなく入力データを変更するだけで柔軟か

Ｌｉｎｋ ｊｏｉｎｔ

Ｃｅｎｔｅｒ ｏｆ ｇｒａｖｉｔｙ

Ｎｏｄｅ Ｎｏ．１

Ｎｏｄｅ Ｎｏ．２

Ｌｉｎｋ ｊｏｉｎｔ１)

つ容易に対処可能となる 。このような特徴をもつ本手法は、開リンク機構以外にも様々な形態のリンク機構に対して適用できる、統一的な計算手法となり得る。
本研究では、骨組構造の有限要素解析に適用され、その有効性が立証済みの Shifted Integration 法（ＳＩ法）２)を用いてリンク機構をモデル化し、３次元運動をする開リンク機構に対する逆動力学計算アルゴリズムを構築した。
さらに、いくつかの３次元運動問題を解析し、本手法の有Ｆｉｎｉｔｅ ｅｌｅｍｅｎｔ

Ｎｏｄｅ Ｎｏ．３：Ｅｌｅｍｅｎｔ Ｎｏ．
：Ｎｕｍｅｒｉｃａｌ ｉｎｔｅｇｒａｔｉｏｎ ｐｏｉｎｔ

Fig.1 Finite element modeling of a link member

ΣＦjCZτ(i+1)ｙ

本研究ではリンク機構のモデル化に際し、Fig.1 のようτ(i+1)ｚＦiZに重心位置に節点を配してリンクを 2 つの線形チモシェ
ンコはり要素で分割し、重心位置の節点に数値積分点をシフトすることによりリンクの両端を関節として表現した。
さらに、ばね剛性値を正規化した値を 0 とすることによってその関節をピン状態として表現した。

Ｆ iφ Zτ(i+1)ｘ

τ iｚ

Ｆ iφ YＦ iφ X

τi

ΣＦjCYＦiY

ΣＦjCX

ＦiX

τ iｘ

3. リンク機構の関節トルク算出
本研究では時間積分スキーム ３)として陰解法を用いる：Nodal pointことにし、その中でも代表的な Newmark のβ法（δ＝1/2、β＝1/4）を使用した。まず、仮想仕事の原理より時刻 t+Δt における運動方程式は以下のように定式化される。

[M]{u&&}t +∆t + [C ]{u&}t +∆t + [K ]{∆u} = {F }t +∆t − {R}t{u}t + ∆t = {u}t + {∆u}(２)

： Nodal point expressing center of（ａ） Ｊｏｉｎｔ ｔｏｒｑｕｅ（ｂ） Ｎｏｄａｌ ｆｏｒｃｅｓ

Fig.2 Joint torque and nodal forces acting
on i‑th link in n‑link mechanism

(３)

 n

τ ix = liC Fiy + li  ∑ Fj  + Fiφx +τ (i+1) x
 j =i +1  y(７)ここで[Ｍ]は集中質量マトリックス、[Ｃ]は減衰マトリックス、[Ｋ]は剛性マトリックス、 {u&&}は節点加速度ベクトとなる。ここで、liC は関節から重心までの距離、li はリンルである。また、 {F } は節点力ベクトル、 {R}は要素内力要素座標系の各軸方向成分であることを示す。なお、大文ベクトルである。ここに Newmark のβ法によって求められ字の添字は全体座標系の各軸方向成分であることを示す。

ル、{u&}は節点速度ベクトル、{∆u} は節点変位増分ベクト

た速度 {u&}t + ∆t 、加速度 {u&&}t + ∆t として{u&}t + ∆t =同様に y 軸回りおよび z 軸回りの関節トルクを記述し、
i=１，・・・，ｎについてマトリックス形式に直して全体

1{∆u}−  1 − 1{u&}t −  1 − 4 β {u&&}t ∆t
2 β∆t
 2β

 4β (４)1{∆u} − 1 {u&}t −  1 − 1{u&&}t2β∆ tβ∆ t
 2β
(５)

{u&&}t + ∆t =クの長さ、関節トルクおよび節点力の右下の添字ｘ，ｙは座標系で整理すると、{τ } = [L ][T ]{P }n3 n×1nnn(８)3 n× 9 n 9 n× 9 n 9 n ×1と表現できる。ここで {τ n } は関節トルクベクトル、{P n }はを代入し、さらにモデル化の際にばね剛性値を正規化した

節点力に関するベクトルで、それぞれのｉ番目のリンクに値を０としたことより剛性マトリックスを、簡略化のため関する成分はに減衰マトリックスをそれぞれ省略する。以上のことから運動方程式を整理すると、以下のようになる。
τ ix  1{∆u} − 1 {u&}t −  1 −1{u&&}t 2ββt2
∆βt
∆


{τ i } = τ iy {∆f } = {R}t − {F }t + [M ]

(６)

(９)

および

 
τ iz ここに全体座標系に基づく変位増分 {∆u} 、時刻 t での速度 {u&}t 、時刻 t での加速度 {u&&}t を入力すれば、全体座標系の節点力増分ベクトル {∆f } を求めることができる。

次に、Fig.2(a)にｎリンク機構において i 番目のリンクF
 iX
 FiYF iZ n ∑ F jX  j = i +1 n{Pi } =  ∑ F jY  j = i +1 n ∑ F jZ j=i+1 FiφX FiφY FiφZ(１０)が必要とする関節トルク、Fig.2 (b)にそれを i 番目のリンクにかかる節点力によって表した図を示す。i 番目のリンと与えられる。また、[ T n ]は全体座標系からリンクの要素

クには３軸回りそれぞれの関節トルクが必要となるが、こ座標系に変換するマトリックス４)で、れらはｉ番目のリンクの重心に働く並進力に起因する回転モーメント、リンクの先端に働く並進力、つまり i+１〜ｎ番目までのリンクに働く並進力の和に起因する回転モーメント、および重心回りに作用する慣性モーメント、さ cos φ iXx cos φ iYx cos φ iZx [Ai ] =  cos φ iXy cos φ iYy cos φ iZy  cos φ iZx cos φ iZy cos φ iZz (１１)

らにこれらの和に i+１番目のリンクの関節トルクを加算というマトリックスが対角にｎ個並ぶマトリックスであしたものとなる。例えば、ｉ番目のリンクにおけるｘ軸回る。ここで、φは各座標軸間の角度を表している。また、りの関節トルクを節点力によって表すと[ Ln ]はリンクの重心までの距離や部材長などの情報を含む部材長マトリックスである。

このようにマトリックス表現をすることにより、簡潔で5．ねじり運動の逆動力学計算

かつ構成変化に対する柔軟性を有する逆動力学計算アル

次に、本手法を用いてねじり運動に関しての逆動力学計ゴリズムが構築された。

算を行う。

4. 平面内運動の逆動力学計算

なる腕型マニピュレータとし、その目標軌道を Fig.4(a)ここでも対象とするリンク機構は３つリンク部材から本アルゴリズムの有効性を確認する数値例として、Ｚ−に示す。これは根元のリンク１が先のリンクの面内回転とＸ平面内における運動についての逆動力学シミュレーシ同じ角度でねじれる運動である。計算時間、リンクの形状ョンを行い、その結果を従来の手法であるニュートン・オ等は前節と同様である。本手法によって求められた各軸回イラー法により求めた結果と比較・検討した。
計算対象のリンク機構は３つリンク部材からなる腕型りの関節トルク曲線を Fig.4(b)、(c)および(d)に示す。
リンク２およびリンク３は平面運動をしており、本手法マニピュレータで、目標軌道を Fig.3(a)に示す。両手法がこれらの関節トルクを正確に計算できることは既に確とも解析対象時間はＴ＝1.0s、時間増分Δｔ＝0.02s とし、認済みである。リンク１に関しては x 軸回りの関節は常に合計 50 ステップで計算した。すべてのリンクの形状は、リンク２、リンク３に作用する重力の影響を受けている。

断面が幅ｂ＝0.032m、高さｈ＝0.006m で、長さ l ＝0.400m

さらに、速度が速くなるにつれ、遠心力の影響で外向きにである。質量はｍ＝0.215kg で、重心はリンクの中心の位引っ張られる力を受ける。一方、y 軸回りの関節はリンク置にあるとする。また、この運動は鉛直面内運動であるた２、リンク３の慣性力の影響を受ける。つまり、最初と最め、重力を考慮した。リンク機構の速度変化は、静止した後にそれぞれ正負の加速度が大きくかかるため、それによ状態、つまり初速度 0 から徐々に加速し、対象時間の半分る慣性力が働いている。確認のため理論値を計算すると、の時点で最高速度をむかえ、その後徐々に減速して最後に

リンク１の x 軸回りの関節には、静止している状態のリン速度 0 となって静止するというものである。本手法によりク２、リンク３の重力によるモーメントが 1.686N･m、さ求められた y 軸回りの関節トルク曲線を Fig.3(b)、ニュらに t=0.5s においてはそれらの接線方向の速度から求めートン・オイラー法により求められた関節トルク曲線をた遠心力によるモーメントが 0.419N･m かかる。合計

Fig.3(c)に示す。また、この運動ではｘ軸回り、ｚ軸回り2.105N･m という値は、Fig.4(b)の t=0.5s における値と一の関節トルクは 0 となる。

致する。また、理論的には y 軸回りの関節にリンク２、リ両者を比較すると、関節トルク曲線はほぼ一致している

ンク３の慣性力によるモーメントが t=0.0s、t=1.0s の時ことが分かる。したがって本手法は、平面内運動に関してにそれぞれ±0.533N･m かかる。
また t=0.5s では 0 である。

は慣性項、重力項、さらにはコリオリ力や遠心力などの非これらも Fig.4(c)における値と一致する。ｚ軸回りの関線形項が正確に考慮された関節トルクを算出できること

節トルク曲線に関してはリンク２、リンク３を回転させるが確認できた。

ために必要な関節トルクが算出されるべきであり、正負の違いはあるが、リンク２の y 軸回りの関節トルク曲線と同5Link1
Link2
Link3

じ値が出ていることでそれが確認できる。

-50.2

0.4

0.6

0.8

1

Δt=0.02 [s]50step0-21Time[s]-3(ｂ) Joint torque curves obtainedby FEM
5Link2Link3

5

Δt=0.02 [s]50step4Link1
Link2
Link3

3

Δt=0.02 [s]50step

0.2

0.4

0.6
Time[s]0.81(ｃ) Joint torque curves
around ｙ-axis
Link1
Link2
Link30.5

Torque[N･m]00(ａ) Target trajectory for 3-link mechanismwith torsional motion in 3-D space
Link1

Torque[N･m]Torque[N･m]

Link1
Link2
Link3-10(ａ) Target trajectory for 3-linkmechanism on Z-X plane

20
Torque[N･m]Torque[N*m]3Δt=0.02 [s]50step2

Δ=0.02 [s]50step01-5-0.5000.51Time[s](ｃ) Joint torque curves obtained
by Newton-Euler method

Fig.3 Calculation of inverse dynamics forarm‑type manipulator on Z‑X plane-100.20.4
0.6Time[s]0.81(ｂ) Joint torque curves
around ｘ-axis

00.2

0.4

0.60.81

Time[s](ｄ) Joint torque curves
around ｚ-axis

Fig.4 Calculation of inverse dynamics for arm‑typemanipulator with torsional motion in 3‑D space以上より、本手法による計算結果は実際の物理現象と適のリンクにおける関節トルクの影響を多く受け、その分誤合したものであり、本手法はねじりを伴う運動に対しても差が蓄積されるためである。また、誤差の主な原因として数値的に精度良く計算できることが分かった。一般的な３は、今回作成した目標軌道が厳密には直線軌道を描いてい次元運動は平面運動とねじり運動との組み合わせで表現ないこと、さらに、物体の速度曲線が正確には三角形になできることから、本手法は様々な３次元運動に適用できるっていないことが挙げられる。つまり、本手法は目標軌道と言える。

さえ正確に与えることができれば、様々な３次元運動に対して精度の良い計算が可能であると考えられる。また、こ6. 複雑な３次元運動の逆動力学計算のような３次元運動の逆動力学計算をする際に必要とな最後に、複雑な３次元運動の計算例５）を取り上げ、本る作業が、目標軌道データおよびモデルの入力データの構手法による結果と比較･検討した。対象とするリンク機構

築のみであることから、本手法はリンク機構の構成変化には４リンク６自由度の腕型マニピュレータで、これを使っ対して柔軟性に富む手法であると言える。て質量 5kg の物体を運ぶ際の逆動力学シミュレーションを行った。Fig.5(a)、(b)および(c)はそれぞれリンク機構の初期状態、運ばれる物体の軌道および従来の手法により計7．結論
本研究では、３次元運動をする開リンク機構を対象とし、算された関節トルク曲線である。これらの情報は参考文献

ＳＩ法によるリンク機構のモデル化および FEM を用いた５）から抜粋したものである。本機構は先端のリンクにのトルク算出アルゴリズムの構築を行った。いくつかのシミみ３自由度の関節が取り付けられており、このリンクがいュレーションの結果、本手法は様々な３次元運動に対してわゆるグリッパの役割を果たす。リンクの形状は外径 R＝十分に精度の高い逆動力学計算法として適用でき、機構の0.020m、内径ｒ＝0.015m の円筒形で、リンクの密度は 2.8変化に対してもそのアルゴリズムを変えることなく柔軟33×10 kg/m である。軌道はグリッパの姿勢を一定にＡ→Ｂかつ容易に対応できることが確認できた。また本手法は、→Ｃ→Ａの順で直線的に描き、各区間での速度曲線を三角本研究で取り上げた開リンク機構のみならず、他の形態の形としている。例えばＡからＢに沿った軌道の半分まではリンク機構にも応用が可能であると考えられる。今後は、一定に加速し、その後一定に減速しＢ点に達するときに速本手法を閉リンク機構およびフレキシブルマニピュレー度が 0 になるといった具合である。全解析対象時間をタの逆動力学計算法に拡張することによりその実用性をT=3.0s とし、ＡＢ、ＢＣ、ＣＡの各区間は等しい時間 T/3さらに高めていきたい。

で通過するものとした。
この目標軌道をもとに本手法で計算した関節トルク曲

参考文献

線を Fig.5(d)に示す。従来の手法によるものと比較すると、

1) 磯部大吾郎、竹内 裕喜、上田 健夫： 超冗長マニピ
ュレータの関節トルク算出法 ‑FEM を用いた理論の展開‑、日本計算工学会論文集、 第 2 巻、pp.73‑78(2000)
2) 都井 裕： 骨組構造回転対称シェル構造の有限要素解析における Shifted Integration 法について、日時間経過に伴う関節トルクの変化の様子は良好に一致している。J１、J２の関節トルク曲線に関しては他と比べて誤差が大きいが、これは根元に近いリンクほどそれより先
J1

本造船学会論文集、第 168 号、pp.369〜377、（1991）

J23) 野田 安則： ITAS−板成形シミュレーション専用プ
0.3ｍ

ログラム、UNISYS 技報 50 号、vol16、No2、（1996）
4)J6

高野 政晴： 機械運動学、コロナ社(1978)

J5 J4

5.0ｋｇ

牧野 洋5) M・ブコブラトビッチ

0.3ｍ

V・ポトコニャック 著、

古田 勝久 監訳： ロボット工学の基礎 マニピュレ

0.15ｍ

ータの動力学、シュプリンガー・フェアラーク、(c) Joint torque curves(a) Initial state５）(1986)0.3m
0.3m

Torque[N･m]20
10J1J2J3J4J5J66) 杉本 浩一： 閉ループ機構の運動方程式の導出、日本ロボット学会誌、vol.15、No.3、pp.460〜467、（1997）

0-107) 吉川 恒夫 田村 正人： フレキシブルアームに対-20する仮想受動関節モデルの有効性の検討、日本ロボット学会誌、vol.17、No.2、pp.250〜259、(1999)

-30
0t[s]
123(b) Trajectory of the object (d) Joint torque curves obtained by FEM

Fig.5 Calculation of inverse dynamics forarm‑type manipulator in 3‑D space