リンク機構の並列的逆動力学計算法の開発（統一的手法の構築を目指して）
Development of a Parallel Solution Scheme of Inverse Dynamics for Link Mechanisms(Aiming Construction of a Unified Scheme)○正磯部大吾郎（筑波大） 今泉 大作（NEC）

筑後 陽一（キヤノン）

Daigoro ISOBE, University of Tsukuba, 1-1-1 Tennodai, Tsukuba-shi, Ibaraki
Daisaku IMAIZUMI, NEC Co.
Youichi CHIKUGO, Canon Inc.
A new era of computational mechanics, where the Finite Element Method (FEM) is applied to the field ofcontrol, has arrived. A parallel solution scheme of inverse dynamics developed by using the FEM, can copewith most of link systems as a unified scheme, regardless of their boundary conditions. The torque curves canbe obtained seamlessly without changing the numerical algorithm, even in such cases where the dynamics ofthe link systems would continuously change. A feed-forward control experiment of a continuouslytransforming link mechanism is carried out to verify the validity of the scheme, and some numerical results
when applied to flexible manipulators are also shown. It is one of the cases where the application of the FEM
can be well demonstrated.
Key Words: Parallel Solution Scheme, Inverse Dynamics, Feed-Forward Control, Link Mechanisms, FEM
1. 緒言
連続体力学に基づく数値解析手法として広く利用される有限要素法（FEM）は，系全体を微小要素に離散化し，全体方程式にまとめて解を求める手法である．そのため，各要素の情報が並列的に取り扱われ，マトリックス計算によって節点力や変位・ひずみなどが求められる．この特長を利用し，連続体の制御法[1]，形態や部材剛性に依存せずに様々なリンク系に対応できる並列的逆動力学計算法[2]などが開発された．並列的逆動力学計算法では，力の次元の運動方程式で並列的に求められる節点力を，力学的な関係に基づいて関節トルクに変換する．その際，力，座標変換，長さの次元に関する成分が個々のマトリックスに分離される．これは，従来の動力学方程式が全ての成分が混在した形であるのとは大きく異なる点であり，そのためリンク系のダイナミクス変化への柔軟性だけでなく，力制御の際にヤコビ行列を必要としないなどの簡便性をも持ち合わせる．
本解法は，ロボットの動作が多様化し開・閉ループ系が高速に切り替わる，あるいは混在するといった状況が発生した場合に，逆動力学の統一的解法としての威力を発揮することが予想される．本報告では，並列的解法の有効性を検証するため，これを導入したフィードフォワード制御系を構築し，ダイナミクスが連続的に変化する剛体リンク系に対する制御実験を行った．さらに，リンク系のモデル化に使用する有限要素の剛性を変化させ，柔軟リンク系に対して行った逆動力学計算結果について報告する．

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

Link material: Aluminum
Link length : 14.2 cm
Link mass: 102g
Motor mass: 536g①: Joint No.

Fig.1 Variable constraint link mechanism
Joint1
Joint2
Joint3
Joint4

20.2

Joint1
Joint2
Joint3
Joint4Torque[Nm]1Angle[rad]2. 並列的逆動力学計算法
リンク系のモデル化には，骨組構造の有限要素解析などで主に用いられる線形チモシェンコはり要素を使用する．要素内の質量分布は剛体リンク系ではリンク重心に集中，柔軟リンク系では要素両端に 2 等分ずつとする．
本解法では，力の次元の運動方程式を増分的に解き，求められた節点力を次式によりトルクに変換する[2]．

0-1② ① ③④

0.10-2{τ } = [L ][T ]{P }nnnn(1)① Joint No.
◎ Passive joint● Active joint― Link member-3-1nn｛P ｝は節ここで｛τ ｝は求めるべき関節トルクベクトル，0123456Time[s]Fig.2 Target trajectory

7-0.1
0

1234Time[s]Fig.3 Torque curves

5012345

TargetResult-0.5-1

Joint 301032304545

TargetResult-0.5-1-1.5

21Time[s]Time[s]0TargetResult0Joint 40Time[s]12345Time[s]Fig.4 Control results
ある．
3. 可変形態剛体リンク系のフィードフォワード制御実験
本解法によるトルク算出ループと PID フィードバックループとを導入し，フィードフォワード制御系を構築した．直交座標系に基づく全体座標系での目標軌道を入力とし，有限要素法による逆動力学計算を経て，逐次的に全リンクのトルクτI.D.が算出される．これにモータを駆動する際の慣性力および動摩擦力，そして PID フィードバック値を足し合わせ，最終的な制御トルクτが出力される．制御過程中の逆動力学計算の占有時間は，一般的に用いられるサンプリングタイムと比較しても十分に短く，実用的な観点から問題ないことが確認された．
次に，開リンク系から閉リンク系へと連続的に形態を変化させる機構に対し，軌道追従制御実験を行った．本実験で用いたリンク機構の概観を図 1 に示す．逆動力学の影響を顕著にするため，各モータにはギアを介していない．また，2 本の開リンク系の先端に配置した拘束機構を連結することで，閉リンク系を構成させることが可能となっている．拘束機構は並進方向の動作を拘束し，ピン状態の受動関節となる．本
実験で設定した目標軌道を図 2 に示す．動作開始後 3 秒間は2 本の独立した開リンク系として動作し，3 秒後に両者の先端が拘束され，その後は 1 つの閉リンク系として動作する．
この際，リンク系の境界条件が変化するにも関わらず，本解法では連続的にトルク値の計算が行われた．算出されたトル
ク曲線を図 3 に示す．2.3s～2.6s の間に大きめのトルク値が算出されているのは，拘束機構を連結するのに余剰な外力が必要となり，意図的にトルクを発生させているためである．
具体的には，関節 4 とそのリンク先端に並進方向の力を発生させた．本解法では力の次元で方程式を解くため，このように発生力などを扱う際には，ヤコビ行列を使った変換を一切必要としない．これも本解法の特長の一つである．
各関節角の追従結果を図 4 に示す．2s～3s の間で軌道からのズレが見られるが，これは拘束機構を連結した際の発生力の影響である．この結果から，形態が変化するリンク系に対しても適切なトルク値が連続的に算出され，良好な軌道追従性を示すことが確認された．なお，フィードバック制御のみで実験を行った場合には，拘束動作を実現できなかった．
4. 柔軟リンク系の逆動力学計算
リンクの剛性を考慮した軌道を有限要素法により求め，これを入力して得られた逆動力学計算結果例を示す．ここでは，減衰は無視する．図 5(a)には，剛体リンク系を水平面内にて5. 結言
有限要素法を用いて開発された逆動力学計算法は，リンク系の動作を力の次元で定式化しているため，並列性を有し，ダイナミクスや部材剛性の変化に対し柔軟に対応することが可能である．本解法を実際にフィードフォワード制御系に組込み，可変形態リンク系の制御実験を行った結果，ダイナミクスの変化に対して柔軟であることが確認された．また，柔軟リンク系に対する数値計算の結果．剛体リンク系の場合と同様に逆動力学が算出されることが確認された．
本解法は，未だに解析ツールとしての範疇を超えない有限要素法を，制御ツールとして活用した一つの例である．今後，連続体の離散化制御手法として有限要素法が益々活用されることを期待したい．CPU の高速化とともに敷居は低くなっているはずである．
参考文献[1] 磯部大吾郎, 中村 博，清水 隆太:“ 圧電アクチュエータ集合体のリアルタイム FEM 制御システムの開発” ,日本機械学会論文集 (A編), 第 66巻, 第 645号, pp.861-866, 2000.
[2] 磯部大吾郎: “ 有限要素法を用いたリンク機構の逆動力学計算”，日本ロボット学会誌, Vol.20, No.6, pp.647-653,
2002.

Y[m]

Y[m]
0.4

0.4

0.2

0.2

00-0.2

-0.2

-0.4

X[m]
00.2

：Joint-0.4

X[m]
00.4

0.2

0.4

(a) Rigid body model(b) Flexible modelFig.5 Kinematics of a manipulator
Flexible model
Rigid body model

1Flexible model
Rigid body model3

Axial force[N]0.5

TargetResult0-1.5

Joint 2
10.2 s で 180 度回転させる動作を示す．そして，これを目標タスクとして運動学計算（軌道作成）アルゴリズムに入力すると部材剛性が考慮され，図 5(b)の軌道が得られる．ここで，リンクの長さは 0.4 m，質量は 0.0224 kg，曲げ剛性 EI は(a)が完全剛，(b)は 0.46 Nm である．2 つの軌道を比較すると分かるように，柔軟リンク系の場合にはリンク部材が大きく変形している．次に，これらの軌道を並列的解法に入力し，計算された関節トルクの時刻歴を図 6 に示す．また図 7 には，軌道と同時に算出されたリンクの根元の要素に作用する軸力を示す．両結果とも固有周期は理論値(0.0998 s)とほぼ一致し，それぞれその時刻の速度･加速度に即した計算結果が算出されていることが確認された．

Torque[N･m]1

Joint Angle[rad]Joint 1

Joint Angle[rad]Joint Angle[rad]Joint Angle[rad]202

10-1-100.1

0.2
Time[s]0.3

0.4

Fig.6 Joint torque

00.1

0.2
Time[s]0.3

0.4

Fig.7 Axial force