並列的解法によるフレキシブルマニピュレータのフィードフォワード制御
Feed-Forward Control of Flexible Manipulators by Using Parallel Solution Scheme○非 加藤 昭博（筑波大院）
正 磯部 大吾郎（筑波大）
Akihiro KATO, Graduate School, University of Tsukuba, Tennodai 1-1-1, Tsukuba-shi, Ibaraki
Daigoro ISOBE, University of Tsukuba, Tennodai 1-1-1, Tsukuba-shi, Ibaraki

１．はじめにリンク機構に目標軌道を与え，その実現に必要となる関節トルクを算出することを逆動力学計算という．一般的な逆動力学計算法では，ニュートンオイラー法などによりリンク機構の動力学方程式を導き出し，その式から直接的にトルクを算出する．しかし，動力学方程式は相対回転座標系に基づいて定式化され，各変数が相互に依存し合うために，対象となる機構のダイナミクスが変化した場合には式の再導出が必要となる．そこで，リンク機構のダイナミクスに依存しない統一的な逆動力学計算法の開発を目的とし，有限要素法(FEM)を用いた並列的解法(PSS)が考案され[1]，開･閉ループが連続的に発生する機構のフィードフォワード制御においても有効であることが確認された[2]．
他方，近年のロボットにはタスクを高速化し，かつ消費エネルギーを削減する目的で部材の軽量化が求められている．しかし，軽量化に起因する部材剛性の低下により，部材に発生する曲げ振動が無視できなくなるため，その振動を制御することが重要な問題となる．また，柔軟リンク系の複雑な動特性を改善するためには，逆動力学計算によって算出される関節トルクを用いて動力学補償を与えることが有効となる．そのため，フレキシブルアームの軌道制御や逆動力学計算などの研究[3]が行われている．しかし，従来の動力学方程式が剛体リンク系を想定し，しかも相対回転座標系に基づいていることにより，その逆動力学計算過程は煩雑なものとなっている．一方，並列的解法では，FEMによって弾性たわみを考慮した軌道計算アルゴリズムを付加することで，柔軟リンク系に対しても適用可能となる[4]．
本報告では，並列的解法により得られるフレキシブルマニピュレータのトルク曲線の妥当性を示すことを目的として，解析および実験による検証を行った．
２．軌道計算アルゴリズム
並列的解法では，動的な効果を含めた節点力を求めれば全関節のトルクが算出されるため，その計算過程は部材剛性に依らず同一のものとなる．したがって，部材剛性や減衰を考慮した正確な軌道を作成し，並列的解法の入力値とすればよい．以下に 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]は系の形態を表す部材長マトリクスである．このように各変数をマトリクス形式に分離することにより，機構の構造が変化した場合にも入力値の変更のみで柔軟に対応可能となる．また，軌道計算と逆動力学計算の双方を同一のモデルで解くことにより，包括的に一つのアルゴリズムとして扱うことを可能とした．

Fig.1 1-link flexible manipulator used in
the experiments

Y[m]

Y[m]

0.4

0.4

0.05

0.2

0.200]m*
N[ 0euqort-0.2-0.2-0.4-0.4

X[m]0-0.050.1 0.2 0.3 0.4

X[m]Rigid body model
Flexible model
Control torque in the experiment
0-0.1 0 0.1 0.2 0.3 0.4

1time[s]23

Fig.3 Torque curve(b) Flexible model(a) Rigid body modelFig.2 Target trajectory obtained by FEM
４．数値的および実験的評価
図１および表１に示すような，アクリル製 1 リンクフレキシブルマニピュレータに対し解析および実験を行った．リンク部材は曲げ変形精度の高い 3 次はり要素 1 つでモデル化した．部材先端に 24[g]の質点を付加し，水平面内を 1.5[s]でπ[rad]回転させるタスクを与えた．
図２には，マニピュレータの軌道を 0.1[s]ごとに描画したものを示す．図 2(b)は，図 2(a)の剛体リンク系の入力軌道から軌道計算アルゴリズムで算出した柔軟リンク系の軌道であり，部材の剛性が考慮された結果になっていることがわ
かる．次に，得られた柔軟リンク系の軌道から算出したトルク曲線を図３に示す．トルク曲線は，リンク部材の剛性が考慮されているため剛体リンク系のトルク曲線を中心に振動しており，その振動周期は，部材のもつ 1 次固有振動周期と一致することが確認された．さらに，実際に実験で用いた，このトルク値と関節角のフィードバック値を足し合わせて得られる制御トルク曲線と比較しても，柔軟リンク系のトルク曲線は定性的および定量的に妥当であると言える．図 4には，実験での関節角の追従結果を示す．関節角フィードバ
ックのみの制御結果は目標関節角度からの遅れやオーバーシュートが大きいのに対し，リンク系のトルクをフィードフォワード的に（動力学補償を）与えた制御結果は追従性が向上しているのがわかる．よって，実験結果から得られた柔軟リンク系のトルクは定量的に妥当であるといえ，本解法の有Table 1 Parameter of link member (Acrylic plastic)
Parameter

ValueLength: L [m]0.42

Sectional area: A [m ]0.2×10-4

Density: ρ [kg/m3]

1.2×103

Young’s modulus: E [GPa]3.726

Moment of inertia: Iy [m4]6.667×10-12

Head mass: m [kg]0.024

1st natural period of vibration T1 [s]

0.944

3]
dar[eglna 2alnoiatort 1
Target
FBFF+FB

00

1time[s]23

Fig.4 Rotational angle
効性が示されたといえる．
５．おわりに
本報告では，様々なリンク系に対する統一的な逆動力学計算法の開発の一環として，並列的解法により得られたフレキシブルマニピュレータのトルク曲線を数値的および実験的に評価し，その妥当性を確認した．今後は，多関節フレキシ
ブルリンク系のフィードフォワード制御への適用が課題となる．
参考文献[1] 磯部大吾郎,“有限要素法を用いたリンク機構の逆動力学計算”, 日本ロボット学会誌, 20(6), 647-653, 2002.
[2] D. Isobe, Y. Chikugo, D. Imaizumi, S. Sato and A. Yagi,
Feed-forward control of link mechanisms under various
boundary conditions by using a parallel solution scheme,
Proc. 2003 IEEE/RSJ International Conference onIntelligent Robots and Systems(IROS 2003), 2003,
2773-2778, Las Vegas U.S.A.
[3] H. Asada, Z. –D.Ma and H. Tokumaru. Inverse Dynamics ofFlexible Robot Arms: Modeling and Computation for
Trajectory Control, Trans. of ASME, J. of Dynamics Systems.
Measurement, and Control, 112, 177-185, 1990.
[4] 磯部大吾郎，今泉大作，“リンク系の部材剛性に依存しない統一的逆動力学計算法”，日本機械学会論文集（C編）,70(691), 728-735,2004.