2C26

並列的解法を用いた枝分かれリンク系の逆動力学計算○ 八木 淳(筑波大院)，磯部 大吾郎(筑波大)Calculating Inverse Dynamics for Multi-branch Link Systems
by a Parallel Solution Scheme*Atsushi YAGI, Graduate School, Univ. of Tsukuba, Daigoro ISOBE, Univ. of Tsukuba
Abstract － In this paper, a parallel solution scheme is applied to calculation of inverse dynamics for a
multi-branch link system. In this scheme, the entire system is subdivided into discrete elements and evaluatedas a continuum. A single link structure consists of a pin joint and a rigid bar. It calculates nodal forces byevaluating equations of motion in a matrix form, and thus information in the entire system can be handled inparallel. The obtained nodal forces are then converted to joint torque in the system. Therefore, inverse
dynamics for complex link systems such as multi-branch link systems can be easily obtained. A simple
numerical test is carried out on a multi-branch link system to verify the validity of the scheme.

Key Words: Multi-branch Link Systems, Inverse Dynamics, Parallel Solution Scheme, Finite Element Method

1. はじめに
近年におけるロボットの動作の高速化や目標軌道の複雑化にしたがい，ロボットに働く遠心力やコリオリ力といった非線形力項が，無視できない大きな影響を及ぼす要因となっている．そこで，あらかじめ算出した関節トルクをフィードフォワード的に入力することにより，動力学補償を与える必要性が高まっている 1)．
現在，リンク系の関節トルクを算出する場合，ニュートン・オイラー法やラグランジュ法などを用いて動力学方程式を導出し，解を求めている 2)．しかし，これらの方法は一般に相対回転座標系に基づいているため，各変数が相互に依存し合う式となる．
そのため，閉リンク系や枝分かれリンク系などではその導出が困難となる．一方，一種の並列的解法として開発された逆動力学計算法 3)では, リンク系全体を有限要素に離散化し，要素座標系における各要素の情報を，各変数が相互に独立な絶対直交座標系に基づく全体座標系の情報に変換する．その上で重ね合わせを行い，節点力情報をマトリックス形式により算出する．そのため，枝分かれリンク系などといった複雑な系に対しても，動力学方程式に相当する部分のアルゴリズムを変えることなく，入力値の変更のみで柔軟に対応が可能である．
本報告では，並列的解法を枝分かれリンク系に適用し，本解法がそのような系に対しても，入力値の変更のみで柔軟に対応可能であることを確認した．

2. 並列的逆動力学計算アルゴリズム
重心位置で２つの有限要素に分けることによりモデル化したリンク系に対し，関節トルクを算出する
逆動力学計算アルゴリズムについて説明する．まず，仮想仕事の原理によって定式化された時刻 t + ∆t
における運動方程式に，Newmark のβ法(β=1/4)によって求められた速度・加速度を代入する．ここで簡Fig.1 Nodal forces acting in multi-branch n-link system

略化のために減衰項を省略し，さらに剛体・リンク系を表現するために弾性変形を無視し，剛性項を省略する．以上のことをまとめると次式が導き出され，各節点における節点力が求まる．


 2β



{∆f } = {R}t − {F}t + [M ]  1 2 {∆u} − 1 {u&}t +  1 − 1{u&&}t  β∆tβ∆t(1)

ただし， {∆ f } は節点力増分ベクトル，{R } は内力ベクトル， {F } は外力ベクトル， {∆ u } は節点変位増分ベクトル， {u& } は節点速度ベクトル， {u&&} は節点加速度ベクトル，[ M ]は全体質量マトリックスである．
次に，式(1)によって求められた節点力と関節トルクとの関係を説明する．Fig.1 に n リンク系内の i 番目のリンクに働く節点力を示す． i 番目のリンクには 3 軸回りそれぞれの関節トルクが必要となるが，第21回日本ロボット学会学術講演会（2003年9月20日〜22日）

これらはリンクの先端に働く並進力の合力に起因する回転モーメント，リンクの重心に働く並進力に起因する回転モーメント，および重心回りに作用する慣性モーメント，そしてこれらの和に先端に接続されたリンクの関節トルクを加算したものである 3)．
以上を x，y，z 軸回りの関節トルクに関してまとめ，i=1,…,n についてマトリックス形式に直して全体座標系で整理すると，{τ }= [L ][T ]{P }n3 n×1nnn(2)3 n×9 n 9 n×9 n 9 n×1と表現できる．ここで{ τ n }は求める関節トルクベク
トル，{ P n }は節点力に関するベクトル，[ L n ]はリンクの重心までの距離などの情報を含む部材長マトリnックス，[ T ]は全体座標系からリンクの要素座標系に変換する座標変換マトリックスである．また，右上添え字 n は対象とする系の総リンク数を示す．
従来の動力学方程式では，位置に関するパラメータが相互に依存しあう相対回転座標系を用いる．そのため，位置・速度・加速度に関するパラメータと，系の形態に関するパラメータが混在した形をとる．
これに対し本解法では，位置に関するパラメータが相互に独立な，絶対直交座標系に基づく全体座標系を用いる．そのため，位置・速度・加速度に関するパラメータは節点力に関するベクトルに，系の形態に関するパラメータは部材長マトリックスに全て集約される．そして，これらは座標変換マトリックスによって結ばれ，変数分離された形をとる．よって，系の形態の変化への対応，部材長マトリックスの操作のみで可能となる．枝分かれリンク系では，以下のような形の部材長マトリックスで表現される．

 Laa

Ln =  00[ ]LbaLbb0Lca 0  ， (a + b + c = n) (3)Lcc 

上式は，根元から a 本目のリンクで b 本と c 本のリンクに枝分かれをする n リンク系 (a +b+c = n) の部材長マトリックスである．ただし，右下添え字はマトリックスの行数を示す．

3. 数値例
枝分かれリンク系の逆動力学計算結果を示す．動力学方程式より算出したトルク曲線を比較対象として，本解法の妥当性を検証する．従来では，動力学方程式の導出に相対回転座標系を用いる．そのため，枝分かれリンク系など複雑な系の形態に対しては，その導出自体が困難となる．それに対し本解法では，絶対直交座標系に基づく全体座標系で情報の重ね合わせを行う．そのため，入力値の変更のみで柔軟に

Fig.2(a) Torque curves obtained Fig.2(b) Torque curves obtainedby a parallel solution schemeby Lagrangian method

系の形態に対応できる高い汎用性を持つ，という特長がある．本稿では，ラグランジュ法においても動力学方程式の導出が比較的容易に行うことのできる，3 リンクの枝分かれリンク系を解析対象とした．
解析対象のモデルを Fig.2(a)に示す．各リンクの部
材長は 0.4[m]，質量は 0.215[kg]とし，リンク番号を図のようにつける．リンク 2,3 の先端に 0.5[kg]の質点を配置する．枝に分かれる部分の関節は，異なるトルクを独立に発生することのできる，理想的な軸を有した関節であるものとする．根元の関節を Joint 1，リンク 2 を支える関節を Joint 2，リンク 3 を支える関節を Joint 3 と呼ぶ．このモデルに対し，1 秒間で 0.5π[rad]回転する水平面内運動を目標軌道として与えた．
Fig.2(a)は動力学方程式から，Fig.2(b)は本解法から算出したトルク曲線である．両グラフの比較より，本解法が枝分かれリンク系においても，妥当な解を算出できていることが分かる．これにより，従来の解法では動力学方程式の導出が困難である枝分かれリンク系といった複雑な系に対しても，本解法はアルゴリズムを変更することなく，入力値の変更のみで柔軟に対応可能であることを確認した．

4. 結論
一般的に使用される動力学方程式は，位置・速度・加速度に関するパラメータと，系の形態に関するパラメータが混在している．これに対し，本解法ではそれぞれが独立して表現される．そのため，複雑な系の形態に関しても，アルゴリズムを変更することなく，部材長マトリックスの操作のみで表現が可能となるという，高い汎用性を持つ．本報告では，枝分かれリンク系に対しても，本解法が入力値の変更のみで柔軟に対応できることを確認した．今後は、より複雑な系への適用と，実機での確認を考えたい．

参考文献
1)Michael Brady 他 4 名 編，高野政春 監訳：ロボット・モーションⅡ，ホルト・サウンダース，(1985)．
2)吉川恒夫 著：ロボット制御基礎論，コロナ社，(1992)．
3)磯部大吾郎：有限要素法を用いたリンク機構の逆動力学
計算，日本ロボット学会誌，Vol.20，No.6，pp.647-653，(2002)．