䢴䣃䢳䢯䣈䢳䢸

リンク系に対する汎用的フィードフォワード制御システムの開発
Development of General-Purpose Feed-Forward Control System for Link Systems
非 上田 浩正 (日野自動車)○正 磯部 大吾郎 (筑波大)Hiromasa UEDA, Hino Motors, LtdDaigoro ISOBE, University of Tsukuba
In this paper, a general-purpose feed-forward control system for link systems is developed using the parallel
solution scheme of inverse dynamics. The dynamic equations of complicated link systems are hard to conduct bygenerally 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 byconverting the nodal forces, which is evaluated by equations of motion in a matrix form. Therefore, information of theentire system is handled in parallel, and it makes easier to deal with such complicated systems as structure-varying
multi-armed manipulators. Some experiments are carried out using a structure-varying multi-armed manipulator toverify the practicability of the feed-forward control system using the parallel solution scheme.
Key Words: Inverse dynamics, Multi-armed manipulator, Feed-forward control, Parallel solution scheme

1.

緒言

宇宙空間など人間が活動することが困難な環境下で,マニピュレータは貴重な作業手段として用いられている．さらに，ペイロードの低減や限られたスペースの活用，構造を冗長系にすることにより迅速な作業を実現できる効果を狙い，リンクを付け替えて構造を変化させることが可能なマニピュレータが JAXAにより提案されている［1］．このようなマニピュレータは無重力下で使用されるため，動作が低速であっても遠心力やコリオリ力など非線形力項が無視できなくなる．そのため，動力学補償を行うべく逆動力学計算が必要となってくる．しかし，現在使用されている Newton-Euler 法や Lagrange 法といった方法［2］では，相対回転座標系を用いリンク間で相互に依存した，系固有の動力学方程式を導出することとなる．そのため，複雑な構造をもつ系や構造が変化する系においては，式の導出が特に煩雑になるといった問題点がある．そこで，有限要素法(FEM)を適用した逆動力学計算法である並列的逆動力学計算法（以下，並列的解法と記す）が開発された［3］．FEM では全体直交座標系で系全体を微小要素に離散化し，全体方程式にまとめて解を出す．そこでは，各要素の情報は並列的に取り扱われる．
並列的解法ではこの特徴を使って節点力を出し，それを力学的な関係に基づいてトルクに換算するという 2 段階の計算を行う．トルクに換算する過程においては，節点力，座標変換，リンク長に関する各々の成分はそれぞれ独立したマトリクスで表されている．そのため，例えば枝分かれ系といった複雑な構造への対応は入力データの変更のみで可能であり［4］，さらには構造が変化する際でもその都度適切なデータを入力すれば対応することができる．
本研究では，並列的解法を用いたリンク系に対する汎用的フィードフォワード制御システムの開発を行い，JAXA が提案したマニピュレータに準じた構造可変型マニピュレータの制御実験を行うことにより，制御システムの有効性を検証する．

2.

全体座標系で表される外力ベクトルである．次に，n リンク系の各々のリンクにおいて式(1)で得られた節点力を使って関節のトルクを表すと{τ } = [L ] [T ] {P }nnn(2)nという簡潔な式で表現できる［3］．ここで， {τ } は要素座標系nにおける関節トルクベクトル， {P } は全体座標系における節点n
力に関するベクトル， T は全体座標系を各リンクの要素座標n系に変換する座標変換マトリクス， [L ] は部材長やリンク構造の情報を含む部材長マトリクスである．
n[ ]

3.

制御システム検証用マニピュレータの概要

構造可変型多肢マニピュレータの仕様としては，構造の組換えが容易にでき，さらに動作時に変形が生じないようにモータを小さくし，できるだけ堅牢かつ軽量にすることとした．さらに，リンクの付け替えを容易にするために構成要素をモジュール化し，リンクユニットとコネクタユニットに分けた．リンクユニットは，主にリンク部材とモータで構成される．リンク部材は交換可能な構造とし，剛体系とフレキシブル系の実験を可能とした．コネクタユニットは，リンクユニット同士を機械的かつ電気的に結合するものである．また，動力学補償の影響を大とするため，モータのギア比はなるべく小さくした．具体的には，重量が動作に影響しない根元部分のモータに限ってギアレスモータ(maxon,RE75)にし，他の箇所には減速比 18:1 のギア付モータ(maxon,RE25)を使用した．根元部分以外の各ユニットを図 1 に示す．

並列的逆動力学計算法

並列的解法では，最初に運動学計算によって算出された目
標軌道の加速度情報をもとに運動方程式を解き，系内の自由な節点に作用する節点力を逐次的に算出する．

[M ]{u&&}t + ∆t = {F }t + ∆t

(1)

[M ] は全体質量マトリクス， {u&&}は節点加速度ベクトル， {F} はFig. 1 Structure-varying multi-armed manipulator䣝䣐䣱䢰䢢䢲䢻䢯䢶䣟䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢲䢻䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣈䣷䣭䣷䣱䣭䣣䢮䢢䣌䣣䣲䣣䣰䢮䢢䣏䣣䣻䢢䢴䢶䢯䢴䢸䢮䢢䢴䢲䢲䢻䢴䣃䢳䢯䣈䢳䢸䢪䢳䢫

4.

制御実験

Table 3(a) Feedback gain of series link (FF+FB)

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

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

Kp [Nm/rad]

KD [Nm/rad]Joint 115.00.000

Joint 2

0.50

0.005

Joint 30.400.005

Joint 40.300.005

Joint 50.250.003

Table 3(b) Feedback gain of series link (FB)

(3)

Kp [Nm/rad]

KD [Nm/rad]こ こ で ， τ I .D. は 逆 動 力 学 計 算 に よ り 求 め る ト ル ク ， τ motor ，τ feedback はそれぞれ，次式で求められるモータ特性トルクおよびPID フィードバックトルク， κ はギア比である．

Joint 1

13.0

0.030

Joint 2

0.50

0.002

Joint 30.500.001

τ motor = Jq&&d + Dq& d + f c

Joint 4040

0.001

Joint 50.300.001(4)τ feedback = K P (q d − q / κ ) + K D (q& d − q& / κ )(5)

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

τ FB = τ motor / κ + τ feedback(6)

なお，フィードバックゲインについては実験毎に最適なものを求めた．用いた J ， D ， fc の値を表 1 に示す．関節 1 のみ他のTable 1 Dynamics parameters of each motor
D [kgm2/s]
J [kgm2]
f c [N]
Joint 1
Joint 2~51.350×10-4

1.910×10-5

1.662×10-2-6-6

1.778×10-3

1.053×10

3.618×10

Table 2(a) Parameter of the link system
Parameter

ValueLength of rigid link unit [m]0.120

Length of connector unit [m]0.020

Link mass [kg]0.183

Connector and motor mass [kg]0.713

Extra mass[kg]0.157Table 2(b) Parameter of the flexible link
Length of link unit [m]Length ofconnector unit [m]Link mass [kg]Stainless Link

Aluminum Link

0.31

0.22

0.02

0.02

0.061
2

2.0×100.017
116.9×1010

Young’s modulus [N/m ]Geometrical moment ofinertia [m4]Damping coefficient α

2.917×10-12

2.5×10-120.10.0005

Damping coefficient β0.10.001

関節とは違うモータであるため，異なる値をとる．また，関節 1のモータ以外はギアを使用するため，関節 1 のモータでは κの値を 1 とし，それ以外の関節では 18 としている．また，実験に用いたリンク系のパラメータを表 2(a)と 2(b)に示す．
剛体リンクのみの直列構造の実験について，目標軌道を図2 に示す．リンクの先端部に 0.157 [kg] の錘を配置した．また，動作時間は 3 [s]とした．実験におけるフィードバックゲインについて，FF+FB 制御の場合のものを表 3(a)に，FB 制御のみのものを表 3(b)に示す．目標角度と計測角度を図 3(a)～3(c)に，FF+FB 制御の場合の全関節についての τ FF + FB ， τ I .D. およびFB 制御のみの場合の τ FB を図 4(a)~4(c)に示す．回転角を見ると，FF 制御を併用することによって特に根元側の追従性が向上していることが分かる．トルク値は関節 1 と 3 では τ I .D. がτ FF + FB と良好に一致している．一方，関節 5 では， τ I .D. のみでは制御トルク値に十分に近い値が出ていないことが分かる．これは，ベアリングの動摩擦抵抗などの外乱に起因するものと考えられる．
続いて剛体リンクのみを使用した枝分かれ構造の実験について，目標軌道を図 5 に示す．枝分かれ部分の先端部に各々0.157 [kg] の錘を配置した．また，動作時間は 0.5 [s]とした．
実験におけるフィードバックゲインについて，FF+FB 制御の場合のものを表 4(a)に，FB 制御のみのものを表 4(b)に示す．目
標角度と計測角度を図 6(a)～6(c)に，FF+FB 制御の場合の全関節についての τ FF + FB ， τ I .D. および FB 制御のみの場合の τ FBを図 7(a)~7(c)に示す．回転角を見ると，直列リンクの実験と同様に FF 制御を併用することによって特に根元側の追従性が向上していることが分かる．トルク値は関節 1 では τ I .D. が τ FF + FB と良好に一致している．一方，関節 4，5 では， τ I .D. のみでは制御トルク値に十分に近い値が出ていないことが分かる．これは先ほどの実験と同様に，関節で使用しているベアリングによる摩擦抵抗など，予測が困難な外乱によると思われる．
ここまでに示した実験は直列剛体リンクのみを取り扱っているものの，構造が複雑であり，従来の逆動力学計算法では対応が困難なものである．本実験においては並列的解法を用いたため，入力データを入れ替えるのみで，容易に対応ができた．
さらに実験結果から，並列的解法による逆動力学計算結果は妥当な値を出力しているといえる．
次に，2 箇所から分岐した枝構造にフレキシブルリンクを用䣝䣐䣱䢰䢢䢲䢻䢯䢶䣟䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢲䢻䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣈䣷䣭䣷䣱䣭䣣䢮䢢䣌䣣䣲䣣䣰䢮䢢䣏䣣䣻䢢䢴䢶䢯䢴䢸䢮䢢䢴䢲䢲䢻䢴䣃䢳䢯䣈䢳䢸䢪䢴䢫

0.6Table 4(a) Feedback gain of rigid branch link (FF+FB): Joint: Extra mass

Y [m]

J5
J4

0.4

J30.2

J2J10-0.2

0.2

0.4

Target
FBFF+FB
Rotational Angle [rad]Rotational Angle [rad]

0.6

0.4

0.2

0.001

Joint 3

0.70

0.001

Joint 4

0.30

0.001

Joint 5

0.30

0.001

2KD [Nm/rad]Joint 1

10.0

0.100
0.001

Joint 3

0.70

0.001

0.4

Joint 4

0.30

0.001

0.2

Joint 5

0.30

0.001

03

1Time [s]2of J1of J3

TargetFB
FF+FB

Table 5(a) Feedback gain of flexible branch link (FF+FB)

3

Fig. 3(b) Rotational angle

1Torque [N*m]0.60.4

Kp [Nm/rad]
1.50

Fig. 3(a) Rotational angle
0.80.001

1.00

Joint 20Time [s]1.50

Joint 2

0.60

Rotational Angle [rad]Target
FB
FF+FB

0.8

1

Joint 1

Table 4(b) Feedback gain of rigid branch link (FB)

0.6

Fig. 2 Target trajectory

0KD [Nm/rad]

X [m]
00.8

Kp [Nm/rad]0Kp [Nm/rad]

KD [Nm/rad]Joint 11.50.001

Joint 2

1.00.001

Joint 31.00.001

Joint 40.10.001Joint 50.10.001

0.2

0Computed torque (τI.D.)
Control torque (κτFB)
Control torque (κτFF+FB)-101

Time [s]2031Time [s]2Fig. 3(c) Rotational angle

Fig. 4(a) Torque curveof J5of J1

0.6

Table 5(b) Feedback gain of flexible branch link (FB)

0.2

Torque [N*m]Torque [N*m]

0.4
0.2
0-0.2

01

Time [s]20.1

3KD [Nm/rad]Joint 18.00.001

Joint 2

3.00.001

Joint 33.00.001

Joint 40.10.001

Joint 50.10.001

0Computed torque (τI.D.)
Control torque (κτFB)
Control torque (κτFF+FB)-0.1

Computed torque (τI.D.)
Control torque (κτFB)
Control torque (κτFF+FB)-0.4

Kp [Nm/rad]30

1Time [s]2Fig. 4(b) Torque curve

Fig. 4(c) Torque curveof J3of J5

3

いた実験について記す．フレキシブルリンクのパラメータは，先に表 2(b)に示した通りである．関節 4 側にはアルミ製のリンクを，関節 5 側にはステンレス製のリンクを用いる．リンクの剛性を考慮した目標軌道を図 8 に示す．枝リンクの先端部に 0.157 [kg]の錘を配置した．また，動作時間は 1 [s]とし，動作終了後 1 [s]の間，回転角を保持するようにした．実験におけるフィードバックゲインについて，FF+FB 制御の場合のものを表 5(a)に，FB制御のみのものを表 5(b)に示す．目標角度と計測角度を図9(a)～9(c)に，FF+FB 制御の場合の全関節についての τ FF + FB ，
τ I .D. および FB 制御のみの場合の τ FB を図 10(a)~10(c)に示す．
回転角を見ると，FF 制御を併用した場合は，剛体リンクを使用した実験よりも追従性が良いことがわかる．さらにトルク値は関

節 1，3，5 全てにおいて τ I .D. が τ FF + FB と良好に一致しており，フレキシブルリンク系においても妥当なトルク値を出力していることがわかる．
以上の結果より，並列的解法による逆動力学計算を用いた制御システムが，長大な構造のリンク系や枝分かれ構造のリンク系，さらにはフレキシブルリンクを用いた構造のリンク系においても妥当なフィードフォワードトルクを供給でき，良好な制御を実現できることが示された．

5.

結論

本研究では，並列的解法を利用した汎用的フィードフォワー
ド制御システムを開発し，その検証実験として，従来より使用されてきた方法では困難とされるような多肢マニピュレータの FF制御を行った．その結果，複雑な構造である上に弾性たわみを考慮しなければならないような系の場合についても，正確に制御を行うことができることが確認された．今後は，制御システ
ムをロボット機構に適用してその実用性を高めた上で，ウェブ上に公開する予定である．

䣝䣐䣱䢰䢢䢲䢻䢯䢶䣟䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢲䢻䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣈䣷䣭䣷䣱䣭䣣䢮䢢䣌䣣䣲䣣䣰䢮䢢䣏䣣䣻䢢䢴䢶䢯䢴䢸䢮䢢䢴䢲䢲䢻䢴䣃䢳䢯䣈䢳䢸䢪䢵䢫

0.5

Y [m]

: Joint: Extra mass

Y [m]0.4: Joint: Extra mass

0.6

J50.3

J30.4

J4

J50.2
J20.1
J1-0.2-0.1

0.1

J10X [m]00.2-0.2

X [m]
00.2

Fig. 8 Target trajectory

Fig. 5 Target trajectory
0.01

0.1

-0.01

-0.02

10.5

Target
FBFF+FB-0.03
00.1

0.05

010.5

-0.05
000.20.3
Time [s]

0.4

00.5

Fig. 6(a) Rotational angle

0.1

0.2
0.3
Time [s]

0.4

00.5

1.5

2of J4

of J1

00.5

1Time [s]1.5

2Fig. 9(b) Rotational angleof J4
Computed torque (τI.D.)
Control torque (κτFB)
Control torque (κτFF+FB)

0.5
Torque [N*m]

Rotational Angle [rad]0.20Target
FB
FF+FB

1.5

TargetFB
FF+FB
Torque [N*m]1Time [s]Fig. 9(a) Rotational angle

0.4

10.5

Fig. 6(b) Rotational angle

of J1
1.5

Target
FB
FF+FB

1.5
Rotational Angle [rad]Rotational Angle [rad]0TargetFB
FF+FB

Rotational Angle [rad]Target
FB
FF+FB

1.5

Rotational Angle [rad]

J4

J2

0

Rotational Angle [rad]

J3

0.2

100.5

0.5
-0.5
Computed torque (τI.D.)
Control torque (κτFB)
Control torque (κτFF+FB)

-0.2

000.1

0.2
0.3
Time [s]

0.4

00.5

0.1

0.2
0.3
Time [s]000.40.5Fig. 6(c) Rotational angle

Fig. 7(a) Torque curveof J5of J1

0.4

0.4

00.2

1Time [s]1.5200.1

0.20.3Time [s]0.51Time [s]of J5of J1
Computed torque (τI.D.)
Control torque (κτFB)
Control torque (κτFF+FB)

0.5

0

1.5

Computed torque (τI.D.)
Control torque (κτFB)
Control torque (κτFF+FB)
00.1

0.2
0.3
Time [s]

0-0.5

00.4

Fig. 7(b) Torque curve

Fig. 7(c) Torque curveof J4of J5

0.5

[4]

謝辞

1Time [s]1.5

200.51Time [s]1.5of J4of J5

D. Isobe, A. Yagi, S. Sato：“General – Purpose Expression ofpp. 789-798, 2006．
[5]

小野繁：ころがり軸受の応用設計，pp.69-71，大河出版，1979．

文 献
[1]

若林靖史，稲場典康，下田孝幸，狼嘉彰：“再構成型宇宙用多[2]J.J.Craig, 三浦宏文, 下山勲 訳：ロボティクス-機構・力学・制御

2Fig. 10(c) Torque curve

Application”, JSME International Journal Series C, Vol.49, No.3,費補助金基盤研究 C（課題番号: 19560250）の助成を受けた．
ここに謝意を表する．

0.5

Fig. 10(b) Torque curve

Structural Connectivity in the Parallel Solution Scheme and Its

本研究を遂行するに当たり、（独）日本学術振興会科学研究

肢マニピュレータ・システム”，特許第 3435081 号，2000-7-11．
-，pp.155-188, 共立出版, 1991．
[3]

20-0.20.4

0.5

Computed torque (τI.D.)
Control torque (κτFB)
Control torque (κτFF+FB)-0.5
Computed torque (τI.D.)
Control torque (κτFB)
Control torque (κτFF+FB)-0.2

0Fig. 10(a) Torque curve

Torque [N*m]Torque [N*m]Torque [N*m]Torque [N*m]

0.5

0.2

0.5

Fig. 9(c) Rotational angle

磯部大吾郎：“有限要素法を用いたリンク機構の逆動力学計算”，
日本ロボット学会誌，Vol.20，No.6，pp.647-653，2002．

䣝䣐䣱䢰䢢䢲䢻䢯䢶䣟䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢲䢻䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣈䣷䣭䣷䣱䣭䣣䢮䢢䣌䣣䣲䣣䣰䢮䢢䣏䣣䣻䢢䢴䢶䢯䢴䢸䢮䢢䢴䢲䢲䢻䢴䣃䢳䢯䣈䢳䢸䢪䢶䢫