柔軟リンク系のモデルベースド・フィードフォワード制御
Model-based feed-forward control of flexible link systems○非 山中 浩司（筑波大院） 正 加藤 昭博（ジャトコエンジニアリング）
正 磯部 大吾郎（筑波大）
Kouji Yamanaka, Graduate School, Univ. of Tsukuba, 1-1-1 Tennodai, Tsukuba-shi, Ibaraki
Akihiro KATO, JATOCO, Ltd., Fuji-shi, Shizuoka
Daigoro ISOBE, Univ. of Tsukuba, 1-1-1 Tennodai, Tsukuba-shi, Ibaraki
１．はじめに近年，ロボットのタスクが複雑かつ高速になり，また，エネルギー削減の観点から機構の軽量化が図られている．
しかし，軽量化に起因する部材剛性の低下により，部材に発生する曲げ振動が無視できなくなるため，その振動を制御することが重要な問題となる．また，柔軟リンク系の複雑な動特性を改善するためには，逆動力学計算によって算出される関節トルクを用いて動力学補償を与えること，すなわちフィードフォワード制御が有効となる．柔軟リンク系をフィードフォワード制御する際には，動力学方程式にたわみ量を付加する方法，センサを用いてたわみに関する量をフィードバック的に与える方法などがあるが，いずれも汎用性が乏しいのが欠点である．
そこで，リンク系を有限要素によりモデル化し，有限要素法(FEM)に基づいて逆動力学計算を行う並列的逆動力学計算法（以下，並列的解法と記す）[1]が開発された．並列的解法では，各変数が完全に分離されたマトリックス形式で表現されるため，対象とする機構が形態変化する場合にも，入力データの変更のみで柔軟に対応可能である．また，並列的解法は，FEMによって弾性たわみを考慮した軌道計算アルゴリズムを付加することで，柔軟リンク系に対しても適用可能となる[2]．
本研究では，並列的解法により柔軟リンク系の逆動力学計算および制御実験を行い，モデルベースド制御法としての有効性を検証した．
２．軌道計算アルゴリズム
並列的解法では，動的な効果を含めた節点力を求めれば全関節のトルクが算出されるため，その計算過程は部材剛性に依らず同一のものとなる．したがって，部材剛性や減衰を考慮した正確な軌道を作成し，並列的解法の入力値とすればよい．以下に FEM を用いた軌道計算アルゴリズムについて記す．
機構の動作によって生じる体積力を考慮すると，仮想仕事の原理より，時刻 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}が算出され，最終的にはこの２つの変位量の和として剛性や減衰を考慮した軌道が得られる．時間積分法としては
Newmark のβ法(δ=1/2,β=1/4)を使用した．
３．並列的逆動力学計算法
並列的解法は，剛性および減衰に関する項を無視した(1)
式に，前節で得た軌道を入力して部材剛性や減衰を考慮した節点力を算出し，それを力学的な関係に基づいてトルクに換算する．得られた各節点力から各関節回りのトルクを算出する式を，全体座標系のマトリクス形式で表したものが次式である[1]．

{τ }= [L ][T ]{P }nnnn(2)

ここで，{τn}は関節トルクベクトル，{Pn}は節点力に関するベクトルである．また，[Tn]は全体座標系からリンクの要素座標系への座標変換マトリクス，[Ln]は系の形態を表す部材長マトリクスである．このように，各変数をマトリクス形式に分離することにより，機構の構造が変化した場合にも入力値の変更のみで柔軟に対応可能となる．また，軌道計算と逆動力学計算の双方を同一のモデルで解くことにより，包括的に一つのアルゴリズムとして扱うことを可能とした．
４．数値的および実験的評価
表１に示すような条件で，5 リンク柔軟リンク機構に対し解析を行った．リンクは曲げ変形精度の高い 3 次はり要素 1要素でモデル化した．与えるタスクは，図 1(a)に示すような水平面内を 3.0 s で動く軌道とし，その後 3.0 s 静止させるものとする．図 1(b)は剛体リンク系の軌道である．図 1(d)Table 1 Link parameters for numerical test
Link

Flexural

Link

Extra

lengthstiffness

mass

mass

[m]

[Nm2]

[kg]

[kg]

Link 10.200

0.5384

0.021

Link 20.200

0.4376

Link 30.200

Link 4
Link 5Damping coefficients

αβ0.500

0.100

2.0×10-4

0.021

0.200

0.100

2.0×10-4

0.2917

0.021

0.100

0.100

2.0×10-4

0.200

0.1459

0.021

0.050

0.100

2.0×10-4

0.200

0.02917

0.021

0.010

0.100

2.0×10-4

Y [m]

0.4

Torque [N*m]

1

0.5

Y [m]

0Kinematics computation

0.5

J1 25-0.5

34
X [m]

0 0.2 0.4 0.6 0.8(a) Initial trajectory-0.5

Parallel solution scheme

0X [m]
0 0.2 0.4 0.6 0.8(b) Trajectory for rigid-body model
1

Y [m]

0.500.2

0-0.2
0123Time [s]456(c) Torque curves for rigid-body model
0.4

Torque [N*m]1

Joint 1
Joint 2
Joint 3
Joint 4
Joint 5

Joint 1
Joint 2
Joint 3
Joint 4
Joint 5

0.2

0-0.2

-0.5

X [m]

00 0.2 0.4 0.6 0.8 1

123Time [s]456(e) Torque curves for flexible model(d) Trajectory for flexible modelFig. 1 Inverse dynamics calculation flow for rigid-body and flexible models

0.5

Y[m]

Y[m]
0.4]
da[reglna 2
alnoi
atot 1R]m 0*N[uqeort0.200-0.2-0.5

Rigid body model
Flexible model
Control torque (FB)
Control torque (FF+FB)

-0.4
-0.5

X[m]
0 0.1 0.2 0.3 0.4 0.5

TargetFB
FF+FB

3X[m]
00.2

01(a)Rigid-body model (b)Flexible body modelFig.2 Obtained trajectories

2340.4

Joint 1]m[ 0.2X

0

Joint 2050time[s]0.4

Analysis
Experiment(FF+FB)(a)Torque curves (Joint1)

12

3

time[s]405(b)Rotational angle

123

45Time[s](c)X-coordinate of link headFig.3 Control results for flexible model

Table 2 Link parameters for experimentParameter

Link 1

Link 2

Link length [m]

0.25

0.25

Young’s modulus [GPa]

200

4.38

Moment of inertia [m4]2.92×10-12

7.72×10-12

Link mass [kg]

6.83×10-2

6.20×10-3

Extra mass [kg]5.81×10-1

4.20×10-2

は剛体リンク系の軌道を軌道計算アルゴリズムに入力して得られた，たわみを考慮した柔軟リンク系の軌道である．図1(b)の軌道を並列的解法に入力すると，図 1(c)に示す剛体リンク系のトルク曲線を得ることができ，図 1(d)に示した軌道を本解法に入力すると，図 1(e)に示す柔軟リンク系のトルク曲線を得ることができる．
次に，本解法の有効性を確認するために，2 関節柔軟リンク系のフィードフォワード制御実験を行った．図 2 は目標軌道とたわみを考慮した柔軟リンク系の軌道を示す．また，

図 3 には，(a)には根元関節のトルク曲線，(b)には関節回転角，(c)にはリンク先端の座標値Ｘを制御結果として示す．
実験結果から、本解法によって得られたトルク曲線と，関節角のフィードバック値を加え実際の入力値とした制御トルク(FF+FB)がほぼ一致することがわかる．また，関節角やリンク先端の座標値においても，良好に一致した．以上より，本解法により得られた柔軟リンク系の軌道およびトルクは定量的に妥当であると確認された．
５．おわりに並列的解法により得られる２関節柔軟リンク系の軌道とトルク曲線，およびフィードフォワード制御結果から，モデルベースド制御手法としての有効性を確認した．
参考文献[1] 磯部,“有限要素法を用いたリンク機構の逆動力学計算”, 日本
ロボット学会誌, 20(6), 647-653, 2002.
[2] 磯部，今泉，“リンク系の部材剛性に依存しない統一的逆動力学計算法”，日本機械学会論文集（C 編）,70(691), 728-735,2004.