計算工学講演会論文集

Vol.8 (2003年5月)

計算工学会

有限要素法を用いたフレキシブルマニピュレータの運動学・逆動力学計算
Calculating Kinematics and Inverse Dynamics of Flexible Manipulators by Using Finite Element Method○ 今泉 大作1)， 磯部大吾郎2)
Daisaku IMAIZUMI and Daigoro ISOBE1)工修
2)工博

日本電気（株）（〒277-8539 千葉県柏市新十余二5番地）

筑波大学助教授 機能工学系（〒305-8573 茨城県つくば市天王台1-1-1, isobe@kz.tsukuba.ac.jp）

In this paper, a parallel solution scheme of inverse dynamics for flexible manipulators is proposed. In thisscheme, the entire system is subdivided into finite elements and evaluated as a continuum. It calculates nodal force
by evaluating the equation of motion in a matrix form, and the information from the entire system can be handled inparallel. Therefore, this parallel solution scheme may turn into a scheme independent of the system configuration oflink mechanisms. Moreover, the kinematics of flexible manipulators is calculated in order to obtain a target
trajectory required for calculating the inverse dynamics, by considering stiffnesses of the finite elements. Bothalgorithms for calculating kinematics and inverse dynamics are combined into a single program. Some numericaltests are carried out to confirm the validity of the proposed scheme.
Key Words: Parallel Solution Scheme, Flexible Manipulators, Inverse Dynamics, Kinematics, FEM

1．
．はじめに連続体力学に基づく数値解析手法として広く利用される有限要素法（FEM）は，系全体を微小要素に離散化し，全体方程式にまとめて解を求める手法である．そのため，各要素の情報が並列的に取り扱われ，系の形態に依存せずに節点力や変位・ひずみなどが求められる．これは，ニュートン・オイラー法が再帰的な処理によって動力学方程式を求める直列的なアプローチであるのに対し，並列的なアプローチであるといえる．この特長を利用し，様々な形態のリンク系に対応できる並列的逆動力学計算法が開発された[1]．本解法では，並列的に求められた節点力を，力学的な関係に基づいて関節トルクに変換する．
その際，力の次元に関する成分，座標変換に関する成分，
長さの次元に関する成分が個々のマトリックスに分離される．このように，従来の動力学方程式が全ての成分が混在した形であるのに対し，本解法でのトルク算出式は，成分ごとに分離されたマトリックス形式によって表現されるため，系のダイナミクスが変化した場合にも入力データの変更のみで対応できるという柔軟性を持つ．このことにより，従来の手法では困難であった閉リンク機構の逆動力学計算[2,3]や，連続的に開→閉状態へと形態が移行するリンク系などに対する計算も，シームレスに実行可能となった[4]．これはすなわち，歩行ロボットの歩行動作や複数マニピュレータによる協調搬送など，開・閉ループが交互に発生するような状況下でも，動力学方程式や計算アルゴリズムを変更することなく，連続的かつ安定的に求解できることを意味する．ロボットの動作をさらに高速化させる上で，これは大変重要な意味を持つものである．
他方では，そのエネルギ消費量の削減と安全性の向上を狙い，ロボットのさらなる軽量化の要求が高まっている．部材が軽量化するにつれ，リンク部の剛性低下によって発生する振動やたわみが無視できなくなり，その制御は非常に困難となる．そのため，弾性たわみを考慮したロボットアームのモデル化[5,6]や逆動力学計算[7]などの研究が精力的に行われている．しかし，元来の動力学方程式が変数分離された形ではなく，さらにそれに加えて弾性たわみの影響を考慮しなければならないため，未だにその逆動力学計算には大変複雑な処理を要する．一方，前述の並列的解法は，動的な効果を含めた節点力を求めれば全関節のトルクが算出されるため，その逆動力学計算過程は剛体リンク系のものと変わらない．そこで
本研究では，この並列的解法に対し，有限要素で弾性たわみを考慮した運動学計算アルゴリズムを付加し，フレキシブルマニピュレータに対する逆動力学計算法を開発した．その際に，運動学と逆動力学を同一のモデルで解くことにより，リンク機構に任意のタスクを与えた際に，その軌道を作成し，動作に必要な関節トルクを算出するまでの過程を包括的に一つのアルゴリズムとして扱うことを可能とした．本報告では，開発したアルゴリズムについて簡単な数値例とともに示す．

２．逆動力学計算アルゴリズム
２．逆動力学計算アルゴリズム
本研究では，フレキシブルマニピュレータのモデル化に対し，骨組構造の有限要素解析で主に用いられる線形チモシェンコはり要素を使用した．部材内の質量分布や応力評価点位置などについては議論の余地があるが，本報告では各要素につき質量配分は要素両端に 2 等分ずつ，数値積分点および応力評価点は中央に配置した．例として 1 リンクを 2 要素で分割した様子を図 1 に示す．また
本来，FEM は要素分割を細かくするほど高精度の近似解が得られるが，その分計算量が多くなってしまうという問題点がある．そこで本研究では，リンク部の曲げ変形および固有周期をある程度高い精度で計算でき，計算量も比較的少なくて済む 1 リンク 4 要素分割を採用した．
次に，モデル化されたリンク機構に対し，目標軌道への追従に必要な関節トルクを算出するアルゴリズムについて述べる．関節トルクを求める逆動力学計算には，入となる．ここで liC は関節から重心までの距離，li はリンクの長さ，関節トルクおよび節点力の右下の添字 x, y, zは要素座標系の各軸方向成分であることを示す．また同様に，大文字の添字は全体座標系の各軸方向成分を示す．
式(2)と同様に y 軸回りおよび z 軸回りの関節トルクを記述し，i=1,… , n についてマトリックス形式に直して全体座標系で整理すると，：Joint：Nodal point：Numerical integration point：Finite element(a) Link member(b) Finite element modelFig. 1 Finite element modeling of a link member{τ } = [L ][T ]{P }n

力データとして機構の動作量と変形量との和から成る目標軌道が必要となるが，その運動学計算アルゴリズムについては次節で述べる．
まず，仮想仕事の原理より時刻 t+Δt における運動方程式は以下のように定式化される．

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

ここで，[M]は全体質量マトリックス，[C]は全体減衰マ
トリックス，[K]は全体剛性マトリックス，{F}，{R}はそ
れぞれ外力および内力ベクトルである．{u}は各節点での機構の動作量と変形量の総和である．また，{ud}は変形量のみのベクトルである．上式の剛性および減衰に関する項は，運動学上の変形量として考慮されるため，逆動力学計算では無視する．すなわち，式(1)は大幅に簡略化され，各節点の加速度情報のみから節点力増分が求まることになる．
次に，節点力と関節トルクとの関係について記す．図 2
は，n リンク機構を任意の軌道上で動作させる際に，i 番目のリンクに作用する節点力を示した図である．i 番目のリンクには 3 軸回り各々の関節トルクが必要となるが，例えば x 軸回りの関節トルクを節点力によって表すと，

 n j =i +1yτ ix = liC Fiy + li  ∑ F j  + Fiφx + F(i +1)φx + τ (i +1) xPnPn(2)j=i+1 FjZ

j=i+1 FjYFiZFiûZ
Link iFiYFiûY

ZPn

Link i + 1
j=i+1 FjXFiûXzYnn(3)と表現できる[1-3]．ここで｛τn｝は求めるべき関節トルクベクトル，｛P n｝は節点力に関するベクトルである．まn
た，[T ]は全体座標系からリンクの要素座標系に変換する座標変換マトリックス，[L n] はリンク長や重心までの距離などの情報を含む部材長マトリックスである．このように，それぞれの項をマトリックス形式で独立に分割することにより，その形が簡潔に表現されるだけでなく，機構形態や系の一部の情報が変化した場合にも，その情報を入力段階で変更するだけで対応が可能となる．また本手法は，正確な目標軌道さえ与えられていれば，関節トルクを求める過程において機構の剛性や減衰を考慮する必要がない．そのため，リンク部を剛体として取り扱う場合，あるいは弾性変形を伴う柔軟体として取り扱う場合にも，全く同一のアルゴリズムを使用することが可能となる．このことは，異なる剛性の部材が混在する機構に対しても同様である．

３．運動学計算アルゴリズム
フレキシブルマニピュレータの逆動力学計算に必要となる目標軌道を作成するために，対象とする機構に任意のタスクを与えた際に，その機構の動作形態を求める運動学計算アルゴリズムを構築した．
本研究で開発した運動学計算アルゴリズムは，骨組構造を解析する有限要素解析手法を，能動的に動作する機構に適用したものである．その際に，前節と同一のモデルを使用し，任意のタスクを与える段階から関節トルクを算出するまでを連続的に実行可能とした．運動方程式には式(1)を使用し，これに機構の能動的な動作量の時刻歴を入力することにより，機構の剛性および減衰を考慮した変形量，さらには部材内に働く内力などが算出される．

４．数値的評価

yFiXnxElemental coordinatesX
Global coordinates：Nodal point：Nodal point expressing center of gravity

Fig. 2 Nodal forces acting on i -th link in an
open-loop n-link mechanism

前述の逆動力学計算アルゴリズムにより計算されるトルク値の精度を評価するため，図 3 に示すような 3 関節
平面剛体リンク系（単リンク長：40cm，単リンク質量：215g，中央点に重心配置）に対する計算を実施し，従来の手法による計算結果と比較した．本数値例では，鉛直面内で動作時間 1.0s の目標軌道（図 3(a)参照）を与えた．
図 3(b)に従来の動力学方程式で求めたトルク曲線，図 3(c)に本解法で求めたトルク曲線を示す．厳密解を与える動力学方程式を解く場合は，増分ステップ数が少なくても解の精度は落ちない．一方，増分方程式を解いて近似解を求める本解法の場合は，ステップ数が少ないと精度が落ちることが確認できる．軌道によってはトルク曲線の収束性に多少の相違が出るが，本数値例が動作時間の短５．フレキシブルマニピュレータの逆動力学計算

Torque[N･ m]

5Z[m]1100 steps
50 steps
20 steps
10 steps

03

Joint 1-50.5

0200.20.4

0.60.81Time[s]1(b) Conventional scheme

X[m]
00.5

1(a) Target motion

Torque[N･ m]

5-0.5

100 steps
50 steps
20 steps
10 steps

本節では，フレキシブルマニピュレータに対して行った逆動力学計算結果を示す．ここでは，減衰は無視する．
図 5(a)には，1 関節剛体リンク系を水平面内にて 0.2 s で180 度回転させる動作を示す．そしてこれを目標タスクとして入力した場合には，運動学計算アルゴリズムを経て部材剛性が考慮され，図 5(b)の軌道が得られる．ここで，リンクの長さは 0.4 m，質量は 0.0224 kg，曲げ剛性 EI は(a)が完全剛，(b)は 0.46 Nm である．2 つの軌道を比較す
ると分かるように，柔軟リンク系の場合にはリンク部材が大きく変形している．次に，これらの軌道を並列的逆動力学計算法に入力し，計算された関節トルクの時刻歴
Y[m]

Y[m]
0.4

0.4

0.2

0.2

00-0.2-0.2○：Joint0

Joint 1-50

0.2

0.4

0.6

0.8

1

Time[s](c) Proposed scheme
Fig. 3 Accuracy of torque curves against number of steps-0.4

Sampling time
10.0ms

010.0ms

X[m]
00.4

0.2

0.1304ms
0.4038ms
10.0 10.2 10.4 10.6

Time [ms]Calculation of inverse dynamics
Calculation of control and output function
Waiting time

Fig. 4 Time possession of each process in control
procedure

い大きな動作であることを考慮すると，リンク機構に必要なトルク値を正確に求めるには，１秒当り 50～100 ス
テップで十分であると判断できる．
次に，本解法を実際の制御系に組み込む際に，精度の保証されるステップ数を取ることができ，かつサンプリングタイムと比較して十分に短い時間で計算できるかどうかを調べた．フィードフォワード制御系では，主に２つの計算処理が必要となる．１つは逆動力学計算処理で，手法が異なれば自ずとこの過程は異なる．もう１つは制御・出力関数の計算処理で，これは手法に依らず共通の過程となる．図 4 に制御過程での各処理に要する時間を示す．本解法による計算時間は従来の手法の 3 倍程度であるが，例えばサンプリングタイムを実用的な値の 10 msと設定した場合でも，十分に短い時間であることが分かる．さらにこのことは，図 3 の計算結果と合わせると，サンプリングタイムごとに１ステップずつ（1 秒間で 100ステップ）計算すれば十分な精度が得られることを示している．本研究での計算は，全て Pentium PC (CPU: 500MHz, memory: 512 MB)で実施した．

Torque[N･m]

0.0 0.2 0.4 0.6

Flexible model
Rigid body model

10-1
00.1

0.2
Time[s]0.3

0.4

Fig. 6 Torque curves

Flexible model
Rigid body model

32

10-1
00.1

0.4

(b) Flexible modelFig. 5 Kinematics of a 1-link manipulator

Axial force[N]Proposedscheme0.2-0.4(a) Rigid body model

0.1695msConvent.
schemeX[m]0.2Time[s]0.3

Fig. 7 Axial force

0.4

Y[m]
0.40.2

θ[rad]

322101

Link1
Link2

000.1

○：Joint-0.2
0.2

0.4

0.5

Fig. 9 Target trajectory (rotational angle)

X[m]
00.2
0.3
Time[s]

0.4
Joint1
Joint2

Fig. 8 Target trajectory for a 2-link manipulator withdifferent stiffness values for each link

６．結論
本研究では，フレキシブルマニピュレータを有限要素でモデル化し，簡潔かつ機構の部材剛性に依らない統一的な逆動力学計算法を構築した．また，その運動学計算についても同一のモデルを用いることにより，任意のタスクからその実現に必要な関節トルクを算出する過程を包括的に扱うことが可能となった．
本手法は，複雑な機構や閉リンク系など，様々なリンク機構に対して適用が可能である．さらに，フィードフォワード制御への適用も期待できる．今後の課題としては，実機を用いた検証や減衰項の考慮などが挙げられる．

参考文献[1] 磯部大吾郎, 竹内 裕喜，上田 健夫:“ 超冗長マニピ
ュレータの関節トルク算出法 -FEM を用いた理論の展開- ”，日本計算工学会論文集, 第 2 巻，2000, 73-78.

[2] D.Isobe, A Unified Numerical Scheme forCalculating Inverse Dynamics of Open/Closed LinkTorque[N･m]を図 6 に示す．また図 7 には，リンクの根元の要素に働く軸力を示す．このように，トルクだけではなく軸力などの部材内力を算出できることも，本解法の特徴の一つである．両結果とも固有周期は理論値（0.0998 s）とほぼ一致し，それぞれその時刻の速度･加速度に即した計算結果が算出されていることが確認された．
次に，剛性の異なる 2 本の部材を用いたリンク系の計算を行った．単リンク長は 20cm，単リンク質量は 11.2g，曲げ剛性 EI は 0.46 Nm（リンク 1），0.046 Nm（リンク 2）である．0.2 s で関節 1 を 0.5 rad，関節 2 を 1.0 rad 回転させると，前述の運動学計算アルゴリズムによって図 8 に示す軌道が得られた．図から，曲げ剛性の小さいリンク 2の方が大きく変形していることが分かる．さらに，異なった振動モードの合成波形が角度の時刻歴（図 9）およびトルク曲線（図 10）で観察される．これは，リンク 1 とリンク 2 が異なる固有周期を有することに起因する．以上の結果から，本解法は異なる剛性を有するフレキシブルマニピュレータにも適用可能であり，かつ剛体リンク系の場合と同一のアルゴリズムで逆動力学が計算できることが確認された．

0.50-0.500.1

0.20.3Time[s]0.40.5Fig. 10 Torque curves

Mechanisms, Proc. 27th Annual Conf. IEEE Industrial
Electronics Society IECON'01, Denver, 2001, 341-344.
[3] 磯部大吾郎: “ 有限要素法を用いたリンク機構の逆動 力 学 計 算 ”， 日 本 ロ ボ ッ ト 学 会 誌 , 20(6), 2002,
647-653.
[4] D.Isobe, D.Imaizumi, Y.Chikugo and S.Sato, A
Parallel Solution Scheme for Inverse Dynamics and Its
Application in Feed-forward Control of Link
Mechanisms, J. of Robotics and Mechatronics, 15(1),2003, 1-7.
[5] E.Boya, A Finite-Element Approach to Control theEnd-Point Motion of a Single-Link Flexible Robot, J. ofRobotic Systems, 4(1), 1987, 63-75.
[6] A.A.Shabana, Dynamics of Flexible Bodies Using
Generalized Newton-Euler Equations, Trans. of ASME, J.
of Dynamic Systems, Measurement, and Control, 112,1990, 496-503.
[7] H.Asada, Z.-D.Ma and H.Tokumaru, Inverse
Dynamics of Flexible Robot Arms: Modeling andComputation for Trajectory Control, Trans. of ASME, J.
of Dynamic Systems, Measurement, and Control, 112,
1990, 177-185.