2P1-3F-F3

並列的解法によるフレキシブルマニピュレータの逆動力学計算
Calculating Inverse Dynamics for Flexible Manipulators by Using a Parallel Solution Scheme○正磯部大吾郎（筑波大）

今泉大作（筑波大院）

Daigoro ISOBE (Univ. of Tsukuba) and Daisaku IMAIZUMI (Graduate school, Univ. of Tsukuba)This paper describes a parallel solution scheme of inverse dynamics for flexible manipulators. In this scheme,
the entire system is subdivided into finite elements and evaluated as a continuum. It calculates nodal force byevaluating the equation of motion in a matrix form, and the information from the entire system can be handledin parallel. Therefore, this parallel solution scheme may turn into a scheme independent of the system
configuration of link mechanisms. Moreover, the kinematics of flexible manipulators is calculated in order toobtain a target trajectory required for calculating the inverse dynamics, by considering stiffnesses of the finiteelements. Both algorithms for calculating kinematics and inverse dynamics are combined into a single program.
Some numerical tests 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]．本解法では，並列的に求められた節点力を，力学的な関係に基づいて関節トルクに変換する．
その際，力の次元に関する成分，座標変換に関する成分，長さの次元に関する成分が個々のマトリックスに分離される．
これは，従来の動力学方程式が全ての成分が混在した形であるのとは大きく異なる点であり，そのため系の変化への柔軟性だけでなく，力制御の際にヤコビ行列を必要としないなどの簡便性をも持ち合わせる．
他方では，近年のロボット産業の急速な進歩により，ロボットのさらなる高速化・軽量化の要求が高まっている．その達成には，リンク部の剛性低下によって発生する振動やたわみが無視できなくなる．本研究では，FEM を用いた並列的逆動力学計算法に対し，有限要素で弾性たわみを考慮した運動学計算アルゴリズムを付加し，フレキシブルマニピュレータに対する逆動力学計算法を開発した．その際に，運動学と逆動力学を同一のモデルで解くことにより，リンク機構に任意のタスクを与えた際に，その軌道を作成し，動作に必要な関節トルクを算出するまでの過程を包括的に一つのアルゴリズムとして扱うことを可能とした．本報告では，開発したアルゴリズムについて簡単な数値例とともに示す．

：Joint：Nodal point：Numerical integration point：Finite element(a) Link member (b) Finite element modelFig.1 Finite element modeling of a link member

2. 有限要素によるリンク機構のモデル化
本研究では，骨組構造の有限要素解析で主に用いられる，線形チモシェンコはり要素をフレキシブルマニピュレータのモデル化に使用した．部材内の質量分布や応力評価点位置などについては議論の余地があるが，本報告では各要素につき質量配分は要素両端に 2 等分ずつ，数値積分点および応力評価点は中央に配置した．例として 1 リンクを 2 要素で分割した様子を図 1 に示す．また本来，FEM は要素分割を細かくするほど高精度の近似解が得られる一方，その分，計算量が多くなってしまうという問題がある．そこで本研究では，リンク部の曲げ変形および固有周期をある程度高い精度で計算でき，計算量も比較的少なくてすむ 1 リンク 4 要素分割を採用した．
3. 逆動力学計算アルゴリズム
モデル化されたリンク機構に対し，目標軌道に追従するために必要な関節トルクを算出するアルゴリズムについて述べる．関節トルクを求める逆動力学計算には，入力データとして機構の動作量と変形量の和から成る目標軌道が必要となるが，フレキシブルマニピュレータの目標軌道を求める運動学アルゴリズムについては次節で述べる．
まず，仮想仕事の原理より時刻 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(a)は n リンク機構において i 番目のリンクが必要とする関節トルク，
図 2 (b)はそれを i 番目のリンクに作用する節点力によって表現した図である．i 番目のリンクには 3 軸回り各々の関節トルクが必要となるが，例えば x 軸回りの関節トルクを節点力によって表すと，

日本機械学会 [No.03-4] ロボティクス・メカトロニクス講演会’03 講演論文集

2P1-3F-F3(1)2003.5.23〜25 函館市

ΣＦjZＦjφZ

τ(i+1)ｚτ(i+1)ｙ

τiｙＦiφZ

τiｚ

Y[m]

0.4

0.4

0.2

0.2

00-0.2-0.2：JointjYＦiZ
τ(i+1)ｘY[m]ＦjφY ΣＦＦiφY Ｆ

ＦiY ＦjφX ΣＦjXＦiXi φX：Nodal pointτiｘ(a) Joint torque(b) Nodal force-0.4

X[m]Fig.2 Joint torque and nodal forces actingon i-th link in n-link mechanism nτ ix = liC Fiy + li  ∑ F j  + Fiφx + F(i +1)φx + τ (i +1) x(2){τ } = [L ][T ]{P }
nnn

X[m]
00.4

Flexible model
Rigid body model

10.2

0.4

Flexible model
Rigid body model3

Axial force[N]yとなる．ここで liC は関節から重心までの距離，li はリンクの長さ，関節トルクおよび節点力の右下の添字 x，y は要素座標系の各軸方向成分であることを示す．また同様に，大文字の添字は全体座標系の各軸方向成分を示す．式(2)と同様に y軸回りおよび z 軸回りの関節トルクを記述し，i=1,･･･, n についてマトリックス形式に直して全体座標系で整理すると，n0.2(a) Rigid body model(b) Flexible model
Fig.3 Kinematics of a manipulator

Torque[N･m] j =i +10-0.4

02

10-1-100.1

0.2
Time[s]0.3

Fig.4 Joint torque

0.4

00.1

0.2
Time[s]0.3

0.4

Fig.5 Axial force(3)と表現できる[1]．ここで｛τn｝は求めるべき関節トルクベ
クトル，｛P n｝は節点力に関するベクトルである．また，[T n]は全体座標系からリンクの要素座標系に変換する座標変換マトリックス，[L n] はリンク長や重心までの距離などの情報を含む部材長マトリックスである．このように，それぞれの項をマトリックス形式で独立に分割することにより，その形が簡潔に表現されるだけでなく，機構形態や系の一部の情報が変化した場合にも，その情報を入力段階で変更するだけで対応が可能となる．また本手法は，目標軌道さえ与えられていれば，関節トルクを求める過程において機構の剛性や減衰を考慮する必要がない．そのため，リンク部を剛体として取り扱う場合，あるいは弾性変形を伴う柔軟体として取り扱う場合にも，全く同一のアルゴリズムを使用することが可能となる．このことは，異なる剛性を有する部材が混在する機構に対しても同様である．
4. 運動学計算アルゴリズム
フレキシブルマニピュレータの逆動力学計算に必要となる目標軌道を作成するために，対象とする機構に任意のタスクを与えた際に，その機構の動作形態を求める運動学アルゴリズムを構築した．
本研究で開発した運動学計算アルゴリズムは，骨組構造を解析する有限要素解析手法を，能動的に動作する機構に適用したものである．その際に，モデルとして前節と同一のものを使用することで，任意のタスクを与える段階から関節トルクを算出するまでを，同一のモデルで包括的に取り扱うことが可能となった．運動方程式には式(1)を使用し，これに機構の能動的な動作量の時刻歴を入力することにより，機構の剛性および減衰を考慮した変形量，さらには部材内に働く内力などが算出される．

5. 逆動力学計算例
本アルゴリズムによって得られた計算結果の例を示す．ここでは，減衰は無視する．図 3(a)には，剛体リンク系を水平面内にて 0.2 s で 180 度回転させる動作を示す．そしてこれを目標タスクとして入力した場合には，運動学計算アルゴリズムを経て部材剛性が考慮され，図 3(b)の軌道が得られる．
ここで，リンクの長さは 0.4 m，質量は 0.0224 kg，曲げ剛性
EI は(a)が完全剛，(b)は 0.46 Nm である．2 つの軌道を比較すると分かるように，柔軟リンク系の場合にはリンク部材が大きく変形している．次に，これらの軌道を並列的逆動力学計算法に入力し，計算された関節トルクの時刻歴を図 4 に示す．
また図 5 には，リンクの根元の要素に働く軸力を示す．両結果とも固有周期は理論値(0.0998 s)とほぼ一致し，それぞれその時刻の速度･加速度に即した計算結果が算出されていることが確認された．
6. 結言
本研究では，フレキシブルマニピュレータを有限要素でモデル化し，簡潔かつ機構の部材剛性に依らない統一的な逆動力学計算法を構築した．また，フレキシブルマニピュレータの運動学計算も，同一のモデルを用いることにより，任意のタスクからその実現に必要な関節トルクを算出する過程を包括的に扱うことが可能となった．
本手法は，複雑な機構や閉リンク系など，様々なリンク機構に対して適用が可能である．さらに，フィードフォワード制御への適用も期待できる．今後の課題としては，実機を用いた検証や減衰項の考慮などが挙げられる．
参考文献[1] 磯部大吾郎: “ 有限要素法を用いたリンク機構の逆動力 学 計 算 ”， 日 本 ロ ボ ッ ト 学 会 誌 , Vol.20, No.6,
pp.647-653, 2002.

2P1-3F-F3(2)