1P1-B18

並列的逆動力学計算法を用いた多関節柔軟リンク系の制御
Control of Multi-Joint Flexible Link System Using Parallel Solution Scheme of Inverse Dynamics
加藤昭博（ジヤトコエンジニアリング）○正磯部大吾郎（筑波大）

Akihiro KATO, JATCO Engineering
Daigoro ISOBE, Univ. of Tsukuba, isobe@kz.tsukuba.ac.jp
In this paper, verification is done on the control scheme of multi-joint flexible link system using
parallel solution scheme of inverse dynamics. The solution scheme is developed using the finite element
method, which evaluates the entire system as a continuum by subdividing it into finite elements. It calculatesnodal forces that reflect the effects of stiffness and damping of link systems, and converts nodal forces into
joint torques. Therefore, the inverse dynamics of link systems can be obtained regardless of member stiffness.
Some numerical tests and feed-forward vibration control of 2-joint flexible link system are carried out toconfirm the expansibility and flexibility of the proposed scheme.
Key Words: Multi-Joint Flexible Link System, Parallel Solution Scheme, Inverse Dynamics, Feed-Forward Control

1. 緒言Y近年，タスクの高速化およびエネルギ消費量の削減を図り，ロボット機構に対して軽量化の要求が高まっている．軽量化に伴って剛性が低下した柔軟リンク系の制御問題を取り扱う場合，機構に発生する弾性たわみや曲げ振動を抑制・補償することが必要となる．そのため，柔軟リンク系の動力学を考慮したモデル化や振動制御などの研究が精力的に行われている．しかし，従来から使用される動力学方程式は，一般的に相対的な回転座標系に基づいて定式化され，様々な変数が混在した形となっているため，その逆動力学計算過程を複雑なものとしている．
他方，連続体力学に基づく数値解析手法として広く利用される有限要素法(FEM)の特徴を，リンク機構の逆動力学計算に利用した並列的逆動力学計算法（以下，並列的解法と記す）が提案された．並列的解法は，各要素内の情報が集約された節点力を並列的に算出し，それを力学的な関係に基づいて関節トルクに換算する手法である．したがって，柔軟リンク系に適用した場合にも，リンク内に発生する弾性変形の影響が反映された節点力をトルクに換算することで，リンクの剛性に拠らずに逆動力学計算が可能となる[1]．また，並列的解法の節点力‐トルク換算式は，各変数が完全に分離されたマトリクス形式で表現されるため，対象となる機構の構造が変化する場合にも，入力データの変更のみで柔軟に対応可能となる．このように並列的解法は，従来の解法には無い高い汎用性を有しており，既に様々なリンク系に対してアルゴリズムを変更することなく逆動力学計算が可能となることが確認されている[2]．
本研究では，並列的解法が持つ汎用性についてさらに検証を進めることを目的とし，2 関節柔軟リンク系に対する制振実験に適用した．その際，発生する変形を予測し，それを打ち消すような軌道とトルクを算出し制御入力するといったフィードフォワード的な制振手法を用いることで，本解法の有効性を検証した．

2. 軌道計算アルゴリズム
並列的解法では，動的な効果を含めた節点力を求めれば全関節のトルクが算出されるため，その計算過程はリンク剛性に依らず同一のものとなる．したがって，剛性や減衰を考慮した正確な軌道を作成し，並列的解法の入力値とすれば良い．
以下に FEM を用いた軌道計算アルゴリズムについて記す．
機構の動作によって生じる慣性力を考慮すると，仮想仕事Xθv=0
θ

: Joint: Rigid body: Flexible body: Control angle

Fig. 1 Control angle in vibration controlの原理より，時刻 t + ∆t における増分型運動方程式は次式のように定式化される．

[M]{u&&m}t+∆t +[M]{u&&d}t+∆t +[C]{u&d}t+∆t +[K]{∆ud}={F}t+∆t −{R}t (1)
ここで[M]は全体質量マトリクス,[C]は全体減衰マトリクス，[K]は全体剛性マトリクス，{F}は外力ベクトル，｛R｝は内力ベクトル，{um}は系の動作量ベクトル，{ud}は系の変形量ベクトルである．また，減衰には比例減衰を用いた．部材に発生する空気抵抗は，形状抗力を外力{F}に加えることで考慮した．式(1)に時刻毎の{um}を入力することで逐次的に{ud}が算出され，最終的には{um}と{ud}の和として剛性と減衰を考慮した軌道が得られる．時間積分法としては，Newmark のβ法(δ=1/2,β=1/4)を使用した．

3. 制振手法
本研究では，並列的解法により得られる柔軟リンク系の軌道とトルク曲線の精度を検証することを目的とし，柔軟リンク系の制振を実施した．すなわち，リンクの変形量を予測し，フィードフォワード的に制振を行う手法をあえて採り，軌道とトルク曲線の精度を検証しようというものである．以下に前述の軌道計算アルゴリズムを用いた制振手法について述べる．
軌道計算アルゴリズムは，剛体リンク系の入力軌道から柔軟リンク系の軌道を出力する．柔軟リンク系の軌道が目標手先位置の近傍にあって速さが 0，すなわちリンクの変形が最大とみなせる場合に制振軌道を軌道計算アルゴリズムにより作成し，これを制御信号として出力し制振を行う．ここで制振軌道とは，図 1 に示すようにリンク部材の変形が最大になった時点（先端の速さが 0）で，リンクが真直となるように関節を制御角θだけ回転させる軌道である．

[No. 06-4] Proceedings of the 2006 JSME Conference on Robotics and Mechatronics, Waseda, Japan, May 26-28, 2006
1P1-B18(1)Y[m]Joint 2
Joint 1

Fig. 2 2-link flexible manipulator used in the experiment
Table 1 Link parameters
Parameter
Length: L [m]Young’s modulus: E [GPa]Moment of inertia: I [m4]Mass of link: M [kg]
Added mass: m [kg]Link 1
0.25
200
2.92×10-12
6.83×10-2
5.81×10-1

2

3Time[s]

0.2

0.2

0.2

000-0.2

-0.2

-0.2

-0.4

-0.4

-0.4

X[m]
00.4

X[m]
0(a) Rigid body model

0.2

X[m]

0.40(b) Flexible body model
without vibration control

0.20.4(c) Flexible body modelwith vibration control

Fig. 3 Trajectory obtained by the proposed scheme

Analysis
Experiment(FF+FB)

Joint 10.4

20.2

100450Fig. 4 Torque curve of Joint 1

1245Fig. 5 Rotational angle

並列的解法は，剛性および減衰に関する項を無視した式(1)に，前節で得た軌道を入力して剛性や減衰を考慮した節点力を算出し，それを力学的な関係に基づいてトルクに換算する．
得られた各節点力から各関節回りのトルクを算出する式を，マトリクス形式で表すと次式のようになる[2]．

{τ } = [L ][T ]{P }n3Time[s]4. 並列的逆動力学計算法

n0.2

X[m]

Torque[N*m]

Rotational angle[rad]10.4

Joint 2

Flexible model
Control torque (FB)
Control torque (FF+FB)
00.4

Target
FB
FF+FB

3-0.5

Y[m]

0.4

Link 2
0.25
4.38
7.72×10-12
6.20×10-3
4.20×10-2

0

Y[m]

nn(2)

ここで，{τn}は求めるべき関節トルクベクトル，{Pn}は節点力に関するベクトル，[Tn]は全体座標系からリンクの要素座標系に変換する座標変換マトリクス，[Ln]は系の連結情報を表す部材長マトリクスである．このように各変数をマトリクス形式に分離することにより，機構の構造が変化した場合にも入力値の変更のみで柔軟に対応可能となる．また，軌道計算と逆動力学計算の双方を同一のモデルで解くことにより，包括的に一つのアルゴリズムとして扱うことを可能とした．

5. 数値的および実験的評価
図 2 および表 1 に示すような，剛性の異なる部材で構成された 2 関節柔軟リンク系に対し制振実験を行った．リンクは曲げ変形精度の高い 3 次はり要素 1 要素でモデル化し，分布質量マトリックスを使用した．与えるタスクは水平面内を 2[s]でπ [rad]回転させ，その後静止させるものである．また，
本実験では簡単のためにリンク 2 のみを制振することとした．
図 3 には，リンクの軌道を 0.1 [s]ごとに描画したものを示す．軌道計算アルゴリズムに図 3(a)の剛体リンク系の軌道を入力すると，図 3(b)および図 3 (c)の柔軟リンク系の軌道が出0123Time[s]45Fig. 6 X-coordinate of link head

力される．制振を行わない場合の図 3(b)に比べ，制振を行う
図 3(c)では曲げ振動が比較的速く収束する様子がわかる．次に，図 3(c)の軌道から得られた根元の関節(Joint1)におけるトルク曲線を図 4 に黒の実線で示す．図において，本解法により得られたトルク曲線と，それに関節角のフィードバック値を加え実際の入力とした制御トルク(FF+FB)を比較すると，両者はほぼ一致した．また，図 5 に示す関節角度曲線では，フィードバック制御のみの制御結果(FB)に比べ，フィードフォワード制御を加えた制御結果(FF+FB)では追従性が明らかに向上した．さらに，図 6 のリンク先端の座標値 X も解析値と実験値は良好に一致した．以上より，本解法により得られた 2 関節柔軟リンク系の軌道およびトルクは定量的に妥当であり，本解法の有効性が示されたといえる．

6. 結言
解析結果と実験結果を比較し，並列的逆動力学計算法によって得られる 2 関節柔軟リンク系の軌道とトルク曲線の妥当性，およびその汎用性を確認した．また，リンクの変形量を検出するセンサを用いずに制振が実現できたことから，model-based の制振手法として新たに発展する可能性が見込めるものと考える．

文 献
[1]

[2]

1P1-B18(2)磯部大吾郎，今泉大作：“リンク系の部材剛性に依存しない統一的逆動力学計算法”，日本機械学会論文集(C 編), 70(691),728-735, 2004.
Isobe, D., Chikugo, Y., Imaizumi, D., Sato, S. and Yagi, A.,
Feed-Forward Control of Link Mechanisms under Various
Boundary Conditions by Using a Parallel Solution Scheme, Proc. ofIROS’03, Las Vegas U.S.A., (2003).