䣔䣕䣌䢴䢲䢲䢺䣃䣅䢵䣇䢵䢯䢲䢸

構造可変型多肢マニピュレータのフィードフォワード制御○上田浩正(筑波大院) 磯部大吾郎(筑波大)Feed-Forward Control of Structure-Varying Multi-Armed Manipulator*Hiromasa UEDA (Graduate School, Univ. of Tsukuba), Daigoro ISOBE (Univ. of Tsukuba)
Abstract － In this paper, feed-forward control of structure-varying multi-armed manipulator is doneusing the parallel solution scheme of inverse dynamics. The dynamic equations of such manipulator are
hard to conduct by generally used schemes such as the Newton-Euler method or the Lagrangian method,since the equations include interdependent variables between the constituting links. In the parallel solution
scheme, the joint torque is calculated by converting the nodal forces, which is evaluated by equations ofmotion of finite elements in a matrix form. Therefore, information of the entire system is handled inparallel, and it makes easier to deal with structure-varying multi-armed manipulator. Some numerical tests
are carried out to verify the adequacy of the parallel solution scheme.
Key Words: Inverse dynamics, Multi-armed manipulator, Feed-forward control, Parallel solution scheme
節点力を出し，それを力学的な関係に基づいてトルクに換算するという 2 段階の計算を行う．トルクに換算する過宇宙空間など人間が活動することが困難な環境下で,程においては，節点力，座標変換，リンク長に関するマニピュレータは貴重な作業手段として用いられている． 各々の成分はそれぞれ独立したマトリクスで表されていペイロードの低減や限られたスペースの活用，構造を冗る．そのため，例えば枝分かれ系といった複雑な構造へ長系にすることにより迅速な作業を実現できる効果を狙の対応は入力データの変更のみで可能であり［4］，さらい，リンクを付け替えて形状が変化できる，即ち構造を変には構造が変化する際でもその都度適切なデータを入化させることが可能なマニピュレータ（Fig. 1）が JAXA に力すれば対応することができる．
より提案されている［1］．このようなマニピュレータは無重
本研究では，JAXA の提案したマニピュレータに準じ力下で使用されるため，動作が低速であっても遠心力やた構造可変型マニピュレータを製作し，その制御に並列コリオリ力など非線形力項が無視できなくなる．そのため的解法を用いることによって解法の有効性を検証する．
動力学補償を行うべく逆動力学計算が必要となってくる．
し か し ， 現 在 使 用 さ れ て い る Newton-Euler 法 や
2. 並列的逆動力学計算法
Lagrange 法といった方法［2］では，相対回転座標系を用
並列的解法では，最初に運動学計算によって算出さいリンク間で相互に依存した，系固有の動力学方程式をれた目標軌道の加速度情報をもとに運動方程式を解き，導出することとなる．そのため，複雑な構造をもつ系や構系内の拘束されていない節点に作用する節点力を逐次造が変化する系においては，式の導出が特に煩雑にな的に算出する．
るといった問題点がある．そこで，有限要素法(FEM)を1.

緒言適用した逆動力学計算法である並列的逆動力学計算法（以下，並列的解法と記す）が開発された［3］．FEM では
全体直交座標系で系全体を微小要素に離散化し，全体方程式にまとめて解を出す．よって，各要素の情報は並列的に取り扱われる．並列的解法ではこの特徴を使って[M ]{u&&}t + ∆t = {F }t + ∆t

[M ] は全体質量マトリクス， {u&&} は節点加速度ベクトル，{F}は全体座標系で表される外力ベクトルである．次に，n リンク系の各々のリンクにおいて(1)式で得られた節点力を使って関節のトルクを表すと{τ } = [L ] [T ] {P }nFig. 1 Concept of variable multi-armed manipulator
proposed by JAXA［1］➨䢴䢸ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢺ᖺ䢻᭶䢻᪥ࠥ䢳䢳᪥凛(1)nnn(2)という簡潔な式で表現できる［3］．ここで， {τ n } は要素座標系における関節トルクベクトル， {P n } は全体座標系における節点力に関するベクトル， [T n ] は全体座標系を各リンクの要素座標系に変換する座標変換マトリクス，
Ln は部材長やリンク構造の情報を含む部材長マトリクスである．

[ ]䣔䣕䣌䢴䢲䢲䢺䣃䣅䢵䣇䢵䢯䢲䢸3.に，ユニットを接続したときの全体概要を Fig. 3 に示す．
また，実機の仕様を Table 1 に示す．

構造可変型多肢マニピュレータ

構造可変型多肢マニピュレータの仕様としては，構造の組換えが容易にでき，さらに動作時に思わぬ変形が生じないようにモータを小さくし，できるだけ堅牢かつ軽量にすることとした．これらのことを考慮し，マニピュレータのリンクにおける構成要素をモジュール化し，リンクユニットとコネクタユニットに分けた．リンクユニットは，主にリンク部材とモータで構成される．コネクタユニットは，リンクユニット同士を機械的かつ電気的に結合するものである．また，動力学補償の影響を大とするため，モータはギアレスであることを目指した．一方では全体を軽量化する必要性があることから，重量が動作に影響しない根元部分のモータに限ってギアレスモータ(maxon, RE40)にし，他の箇所には減速比 18:1 のギア付モータ(maxon,RE25)を使用した．根元部分以外の各ユニットを Fig. 2

4.

制御実験

実験は合計 4 リンクのマニピュレータを枝分かれ構造，そして直列に並べた構造について行った．なお，その際にはフィードフォワード（FF）制御を補助する目的で PIDフィードバック(FB)制御を併用した．これは実機の組み付け誤差による抵抗や，関節で使われるベアリングの抵抗は定式化が困難であり［5］，補償する必要が生じたためである．この場合の制御トルク τ FF + FB は次式で表される．

τ FF + FB = τ motor + τ I .D. + τ feedback(3)

ここで， τ I .D. は逆動力学計算により求めるトルク， τ motor ，τ feedback はそれぞれ，次式で求められるモータ特性トル
クおよび PID フィードバックトルクである．
(4)
τ motor = Jq&&d + Dq& d + f c
τ feedback = K P (qd − q) + K D (q& d − q& ) + K I Σ(qd − q)(5)Fig. 2 Connector and link units

ここで J ， D ， f c はモータの慣性モーメントと粘性係数&&d は目標の角度と角速度と角加と動摩擦力， q d ， q& d ， q
速度， q ， q& は実際に検出された角度と角速度， K P ，K D ， K I は比例，微分，積分に対するフィードバックゲインである．一方，FB 制御のみを使用した場合は次の制御トルク τ FB を使用した．

τ FB = τ motor + τ feedback(6)

実験に用いた J ， D ， f c の値を Table 2 に示し，各ゲイン値を Table 3 に示す．関節 1 のみ他の関節とは違うモータを使用しているため，異なる値をとる．
枝分かれ構造の実験について，目標軌道を Fig. 4 に示す．図で A，B と記された部分に 0.160 [kg] の錘を配置した．また，動作時間は 1 [s]とした．関節 1，3，5 の全体座標系における目標角度，FF+FB 制御の場合ならびFig. 3 Structure-varying multi-armed manipulator
Table 1 Parameter of the link system
Parameter

Value

Length of link unit [m]0.120

Length of connector unit [m]0.020

Table 2 Dynamics parameters of each motor

Joint 1
Joint 2~5J [kgm2]

D [kgm2/s]-4

1.910×10-5

1.662×10-2

3.618×10-6

1.778×10-3

1.350×10-6

1.053×10

f c [N]

Table 3 Feedback gain for each motor

Link mass [kg]0.183

KPKIKDConnector and motor mass [kg]0.713

Joint 10.10000

0.00100

0.00001

Extra mass[kg]0.160

Joint 2~50.06000

0.00001

0.00001➨䢴䢸ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢺ᖺ䢻᭶䢻᪥ࠥ䢳䢳᪥凛3TargetFF+FB
FB

Rotation angle [rad]Rotation angle [rad]䣔䣕䣌䢴䢲䢲䢺䣃䣅䢵䣇䢵䢯䢲䢸

Joint 52

1.5

Joint 4

Target
FF+FB
FB

1

0.5

Joint 3

1

Joint 1

0

Joint 1

0-0.5
00.2

0.4

0.6

0.8

10Time [s]

0.05

00

-0.05

-0.05
00.2

0.4

0.6

0.8

01

: τFF+FB: τI.D.
: τFB

-0.05

-0.05
0.4

0.6

0.8

1Time [s]Fig. 6(c) Torque curveof J30.800.20.40.60.8Fig. 6(d) Torque curveof J5に FB 制御のみの場合の計測角度を Fig. 5 に示す．
FF+FB 制御の場合の全関節についての τ FF + FB ， τ I . D . ，および FB 制御のみの場合の τ FB を Fig. 6(a)～(d)に示す．なお，関節 4 と 5 の間はコネクタユニットであり，関節4 に制御トルクは発生しない．回転角を見ると，FF 制御を併用することによって追従性が向上していることが分かる．
トルク値は関節 1 と 2 では τ I . D . が τ FF + FB と良好に一致している．一方，関節 3 と 4 では， τ I . D. のみでは制御トルク値に十分に近い値が出ていないことが分かる．これは，ベアリングの動摩擦抵抗の計測，実験時誤差に起因するものと思われる．
次に，直列に並べた構造での実験について，目標軌
道を Fig. 7 に示す．図で A と記された部分に 0.160 [kg]の錘を配置した．動作時間は 2 [s]とした．関節 1 と 4 の全体座標系における目標角度，FF+FB 制御の場合ならびに FB 制御のみの場合に計測された角度を Fig. 8 に示す． τ FF + FB ， τ I . D . ， τ FB を Fig. 9(a)～(d)に示す．回転角を見ると，FB 制御のみの場合は，フィードバックゲイ➨䢴䢸ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢺ᖺ䢻᭶䢻᪥ࠥ䢳䢳᪥凛

0.1

0-0.1

-0.1

00.5

11.520Time [s]: τFF+FB: τI.D.
: τFB

0.20.1-0.1

1.5

2Time [s]Fig. 9(c) Torque curveof J320.1-0.111.5: τFF+FB: τI.D.
: τFB

0.2

00.5

1Fig. 9(b) Torque curveof J2000.5Time [s]Fig. 9(a) Torque curveof J1

1Time [s]2: τFF+FB: τI.D.
: τFB

0.201

: τFF+FB: τI.D.
: τFB

0.05

00.2

0.6

0.1

000.4

0.1

Fig. 6(b) Torque curveof J2
Torque [N*m]Torque [N*m]Fig. 6(a) Torque curveof J1
0.10.2: τFF+FB: τI.D.
: τFB0.2

Time [s]Time [s]0.05: τFF+FB: τI.D.
: τFBTorque [N*m]

0.1

1.5

Fig. 8 Rotational angle

Torque [N*m]0.05Torque [N*m]: τFF+FB: τI.D.
: τFB

Fig. 7 Target trajectory

Torque [N*m]0.11Time [s]Fig. 5 Rotational angle

Torque [N*m]Torque [N*m]Fig. 4 Target trajectory

0.5

00.5

11.5

2Time [s]Fig. 9(d) Torque curveof J4ンを FF+FB 制御用に小さく設定しているため当然ではあるが，追従性が大幅に悪化している．一方，FF+FB 制御の場合には，回転角の追従性は良好に保たれている．
フィードバックゲインの設定幅が狭まる意味でも，FF 制御は有効である．また，関節 1 と 2 でのトルク値 τ I . D. は
τ FF + FB と良好に一致している．
以上の結果より，並列的解法による逆動力学計算が，構造可変型多肢マニピュレータに対しても良好なフィードフォワードトルクを提供できることが示された．

5.

結論

本研究では，汎用性を有する並列的解法を利用し，従来より使用されてきた方法では困難とされるような多肢マニピュレータの FF 制御を行った．今後は長編成化，構造の複雑化，そしてリンクを柔軟化した場合に対しても検証を行っていく予定である．

䣔䣕䣌䢴䢲䢲䢺䣃䣅䢵䣇䢵䢯䢲䢸謝辞
本研究を遂行するに当たり，（独）日本学術振興会科学研究費補助金基盤研究 C（課題番号: 19560250）の援助を受けた．ここに謝意を表する．

参考文献[1] 若林靖史，稲場典康，下田孝幸，狼嘉彰：“再構成型宇宙用多肢マニピュレータ・システム”，特許第3435081 号，2000-7-11．

➨䢴䢸ᅇ᪥ᮏ兑兀儧儬Ꮫ఍Ꮫ⾡ㅮ₇఍凚䢴䢲䢲䢺ᖺ䢻᭶䢻᪥ࠥ䢳䢳᪥凛[2] ［2］J.J.Craig, 三浦宏文, 下山勲 訳：ロボティクス機構・力学・制御-，pp.155-188, 共立出版, 1991．
[3] 磯部大吾郎：“有限要素法を用いたリンク機構の逆動力学計算”，日本ロボット学会誌，Vol.20，No.6，pp.647-653，2002．
[4] D. Isobe, A. Yagi, S. Sato：“General – Purpose
Expression of Structural Connectivity in the
Parallel Solution Scheme and Its Application”,
JSME International Journal Series C, Vol.49, No.3,
pp. 789-798, 2006．
[5] 小野繁：ころがり軸受の応用設計，pp.69-71，大河出版，1979．