計測自動制御学会東北支部 第 245 回研究集会(2008.10.24)
資料番号 245-25

終端状態制御による 3 リンクロボットアームの制御
Control of Three Link Robot Arm by Final-State Control○ 前田卓也 ∗ ，渡部慶二 ∗ ，村松鋭一 ∗ ，有我祐一 ∗ ，遠藤茂 ∗∗（山形大学）○Takuya Maeda ∗ , Keiji Watanabe ∗ , Eiichi Muramatu ∗ ,Yuichi Ariga ∗ ,Sigeru Endo ∗∗(Yamagata University)

キーワード：終端状態制御(Final-state control)，誤差学習(Error Learning)，
3 リンクロボットアーム(Three Link robot arm)
連絡先：〒992-0061 米沢市城南 4-3-16 山形大学工学部応用生命システム工学 渡部・村松・有我研究室
前田卓也，tell：080-1854-1591，E-mail：takujyun0722@co.jp

1 緒言

また，反射的動作の一つとして，翁らの研究 (3)では猫ひねりの動作を逆運動学の軌道生成なし

従来から，ロボットの運動制御に関する研究が多にフィードフォワード制御のみで運動制御が行くなされている．それらの多くでは，フィードバえることを立証している．

ック制御が用いられているため，逆運動学によるそこで，本研究では，逆運動学による軌道生成

軌道の生成と，それへの追従制御が一般的に行わなしにロボットアームをフィードフォワード制れている (1) ．しかしながら，人間や動物の運動制御するための手法として，西村らが提案している御では必ずしもフィードバック制御のみで行わ誤差学習を用いた終端状態制御 ( 4 )( 5) を採用し，反れているのではなく，反復学習動作や反射的動作復学習動作の一つである，投てき動作の制御を行はむしろフィードフォワード制御で行われていう．また，この制御方法を用いることによってロるのではないかという指摘もある．

ボットアームが生物的動作になるかを検証する．

その反復学習動作の一つとして，川人らの研究

人が物を投げる動作を獲得する過程では、時々のフィードバック誤差学習が挙げられる．この刻々の関節角度，ひいては手先の軌道を逐一求め

フィードバック誤差学習は，学習初期はフィードているわけではない．物を投げる目標位置を決め，バック制御で行われるが，反復学習するとフィーその飛距離に対して腕の初期状態とリリースポ( 2)ドフォワード制御のみで動作できるようになる． イント（終端状態），またリリースポイントでの
しかし，このフィードバック誤差学習では，逆運

手先の速度を調整することによって動作を獲得動学で求める軌道が必要になってくる．

している．このことから，終端状態制御を用いる

ことが適当であると考えられる．

本論文では，シミュレーションによりその有効性

本研究では，ロボットアームは 3 リンクとし，水を検証する．

平面内を動くものとする．このため，ロボットアームは横投げの動作をすることになる．また，投2

力学モデル

げる物は鉄

2.1 実システムと線形モデルの運動方程式球とし，手先にある電磁石で保持されており，リFig.1 に 3 リンクロボットアームの力学モデルをリースポイントで電磁石 OFF となり瞬時に離れる．ロボットアームの上から見た x 軸に平行に投示す．また，各パラメータは以下の通りである．
yげるモデルを Fig.1(a)に、横から見たモデルをFig.1(b)に示す．このとき，Fig.1(b)の B 点を目標位置とし，AB 間を飛距離と考え，その飛距離より水平投射の法則を用いてリリースポイントで

l1
M1
l2M2の手先の速度を求める．また，その速度より逆ヤコビ行列の法則を用いて各関節の角速度を算出する．算出された角速度とリリースポイント[Px , Py ] から得られる各関節の角度を終端の状態量

yl3
M3

L1L2

Link 3θ1L3x

Link 2

θ3

θ2x

Link1

Fig.2 Dynamic model of three rink robot armとし，誤差学習を用いた終端状態制御により，投M i [kg ] ：各リンクの質量(i = 1,2,3)

てきするためのフィードフォワード制御入力を

Li [m] ：各リンクの長さ(i = 1,2,3)求める。

l i [m] ：関節からリンクの重心距離 (i = 1,2,3)[yu i [ N ⋅ m] ：各モータへの入力], Pyy]Releasepoint [ PxP,x P
ﾘﾘｰｽﾎﾟｲﾝﾄ
v0d i [kgm 2 / s] ：各関節の減衰係数

θ3Robot arm

角変位 θ 1 ， θ 2 ， θ 3 を一般化座標として，ラグラθ2ンジュの運動方程式を解くことにより導出する．

J2xJ1

導出された運動方程式は次式で表される．

M (θ )θ&& + Dθ& + K (θ , θ&) = Fu(a) Top view
Release point(i = 1,2,3)

Fig.1に示したモデルの運動方程式を，リンクのJ3

θ1(i = 1,2,3)

M (θ ) ：慣性行列、v0(1)

D ：減衰係数

K (θ , θ&) ：非線形項、 F ：トルク係数

u ：制御入力、 θ = [θ1 θ 2

θ 3 ]Τ

Landing pointABx

この式(1)の非線形項 K (θ ,θ&) はこのままでは状(b) Side view

態方程式に組み込むことはできない．そこで，非Fig.1 Sidethrow Model

線形項を三角関数ごとにまとめ，さらに三角関数

の加法定理を用いて sin θ の項に整理して，以下のような式変形を施す．

p × sin θ = p ×sin θθ

3

終端状態制御本研究では，参考文献(4)(5)の手法により，終×θ(2)ただし， p は多項式を表し，θ = 0 の時 sin θ / θ = 1 とする．これより以下の式を満たす行列 K (θ ,θ&) が求端状態制御を実現する．この概略を以下で述べる．
3.1 線 形時変系に対する誤差学習による終端状態制御
本研究では非線形である実システムを時変系まる．

として扱い，誤差学習を取り入れた終端状態制御

K (θ , θ&) = G (θ , θ&)θ(3)

これより，式(1)を以下とすることができる．

M (θ )θ&& + Dθ& + G (θ , θ&)θ = Fu

(4)によってフィードフォワード入力を求める．
制御対象の状態方程式式(6)をオイラー法によって刻み時間 ∆t で離散化した離散システムは以下のように表される．

また，式(4)を鉛直下向きの近傍で線形化した線形モデルの運動方程式を以下のようにする．

x k +1 = Ak x k + B k u k(8)ただし，M (θ )θ&& + Dθ& + Eθ = Fu

(5)

2.2 実システムと線形モデルの状態方程式

Bk = B (θ k )∆tである．このシステムの拡大系は2.1 節の式(4)より実システムの状態方程式は

x& = A(θ , θ&) x + B (θ )uAk = A(θ k , θ&k )∆t + I(6)~~~x k +1 = Ak ~x k + B k wk
w = − K~x + u~kk(9)k

ただし，ただし，A~
Ak =  k
0 3×6

0 3×3
I 3×3


A(θ , θ&) = 
−1−1&
− M (θ ) G (θ , θ ) − M (θ ) D 
 0 3×3 
B (θ ) = −1 M (θ ) F と表され，さらにθ x =  &θ ~ ~F = Ak − Bk K

また，2.1 節の式(5)より線形モデルの運動方程式は以下のように表される．

である．

~~x k +1 = F~x k + B u~k(11)そして，鉛直上向きの近傍で線形化した線形時(7)不変の状態方程式を対象として求めた制御入力~ˆV L = [u~ 0 , u~1 , u~ 2 , L , u~ N − 1 , ]T を式(11)に順次与えた

ただし，I 3×3 
 0 3×3
A=
−1−1 
− M L E − M L D (10)とすれば，式(8)は以下のように表される．

である．

x& = AL x + BL u

Bk  ~
0 6×3  ~  x k , Bk =  , x k = u I 3×3  I 3×3  k 0BL =  3×−13 M L F 

ときの状態遷移行列 F0 , F1,L, F N −1 は次式で求められる．

~~x1 = F0 ~x0 + B u~0~~x =F~x + B u~
21 11M~ˆ
V + ∆V =~ Τ ~ ~ Τ −1 ~ 0U V′ U V′ U V′
x − FN′ −1 FN′ − 2 L F0′~x0

((12)

M

~~
x N = FN −1 ~x N −1 + B u~N −1)()(19)となる．式(19)はシステムが目標状態 ~x 0 に到達~x N は以下のように表される。

するための入力である式(16)と同じ形になる．式~ ~~x N = FN −1 FN −2 L F0 ~x0 + U V V(13)(19)を式(15)の V~ˆ に与えたとき，状態遷移が変わ~~~~ ~U V = FN −1 FN − 2 L F1 B , FN −1 FN − 2 L F2 B , L , FN −1 B , B~ΤV = [u~0 , u~1 , u~2 , L , u~N −1 ]] らなければ目標状態に到達する．ところが，入力[の変化が大きいと状態遷移の変化が大きくなり∆V による誤差補償は収束しない．そこで，学習目標状態を ~x 0 とすると式(13)の ~x N が ~x N = ~x 0 と係数 γ (0〈γ ≤ 1) を ∆V にかけて入力の変化を小さくなるとき終端状態制御が実現する．式(12)から得抑える．そのため，以下のように入力の更新を行られる状態遷移行列 F0 , F1,L, F N −1 と、式(13)によう．って実システムが初期状態 ~x 0 から，目標状態 ~x 0~に到達するための制御入力 Vˆ は次式となる．

()~ˆ ~ Τ ~ ~ Τ
V = U V (U V U V ) −1 ~x 0 − FN −1 FN −2 L F0 ~x0 (14)となる．

~ˆ~ˆ
V ← V + γ∆V(20)

式(20)で求められた V~ˆ = [u~0 , u~1 , u~2 , L, u~N −1 ]T を式(11)に与えると ~xk = [xk uk ]Τ が求められ，その要素から実システムへのフィードフォワード入力 u k が得以上で求めた制御入力 V~ˆ = [u~0 , u~1 , u~2 , L, u~N −1 ]T を拡られる．この一連の流れをフローチャートとして，大システム(11)式に与えると拡大システムの状

Fig.3 に示す。

態ベクトル ~xk = [xk uk ]Τ が求められ，その要素からSTART

実システムへのフィードフォワード入力 u k が得Calculate~ feedforward
input VL by using
linear as a time-varying

られる．しかし，式(14)で求めたV~ˆ を与えたシス

xN
Obtain Fi and ~byCalculation ofEq.(11) by using V~ˆ
~x0 −~xN → eテム式(11)に与えた結果，終端状態は~ ~ˆ
~x N′ = FN′ −1 FN′ −2 L F0′~x0 + U V′ V

Obtain Fi by calculationof Eq.(11) by using V~L(15)となって目標状態 ~x 0 とは異なり，結局 V~ˆ は(ACalculate feedforward
input V~ˆ by using Eq.(14))~ˆ ~ Τ ~ ~ ΤV = U V′ U V′ U V′ (~x N′ − FN′ −1 FN′ − 2 L F0 ~x 0 ) (16)

ALess thantoleranceerrorNO
By using Eq.(18),
calculate∆VYES
Obtain feedforward
input u k from augmented
state vector ∆VEND~ˆ~ˆ
V + γ ⋅ ∆V → Vと表される．ここで，上付きの’は状態遷移の変化に伴うベクトルおよび行列の変化を表す．このFig.3 Flow chart of design of feedforward inputときの終端誤差ベクトル e をe=~x0 − ~x N′(17)()

パラメータ導出方法
前章で述べた手法で制御入力を求めるためにとし，入力 ∆V 以下のようにする．

~ Τ ~ ~ Τ −1
∆V = U V′ U V′ U V′e4(18)は，状態量 ~x k の初期と終端での値が必要となる．
初期状態は任意で構わないが，所望の投てきを実そして，式(16)の V~ˆ に式(18)の ∆V を加えて現するためには，終端状態をあらかじめ求めておかなければならない。ここでは，その手法について述べる．

次に，手先の速度 V より各関節角速度を逆ヤコビ行列より求める．ロボットアームのベクトル方
程式は以下のようになる．

4.1 終端状態での各関節角度の導出方法
Fig.4 より逆運動学を用いて手先がリリースポイント達するための各関節角度を求める．
今回は，アームの先端にボールを取り付け投げることを考え，リンク 3 を x 軸に平行になるようにした．この時の関節 J 3 の座標は次式となる．

P3 x = Px

 cosθ 1 sin θ 1  0 cos(θ 1 + θ 2 ) sin (θ 1 + θ 2 ) 0
Pr = l1 + 
  l 2
− sin θ 1 cosθ 1  1
− sin (θ 1 + θ 2 ) cos(θ 1 + θ 2 ) 1
 cos(θ 1 + θ 2 + θ 3 ) sin (θ 1 + θ 2 + θ 3 ) 0+  l 3
− sin (θ 1 + θ 2 + θ 3 ) cos(θ 1 + θ 2 + θ 3 ) 1

 x   sin θ 1l1 + sin (θ 1 + θ 2 )l 2 + sin (θ 1 + θ 2 + θ 3 )l 3 ∴  =  y  cos θ 1l1 + cos(θ 1 + θ 2 )l 2 + cos(θ 1 + θ 2 + θ 3 )l 3 (26)となる．偏微分を行うことにより，微分関係は，(21)P3 y = Py − l 3
よって，角度 d 3 は以下のように求まる．

P 
d 4 = tan −1  3 x P 
 3y (22)また，関節 J 1 と J 3 を結ぶ直線を l 13 とすると，d1 と x&   cos θ 1l1 + cos(θ 1 + θ 2 )l 2 + cos(θ 1 + θ 2 + θ 3 )l 3
 y&  = − sin θ l − sin (θ + θ )l − sin (θ + θ + θ )l  
1 112 2123 3cos(θ 1 + θ 2 )l 2 + cos(θ 1 + θ 2 + θ 3 )l3(27)− sin (θ 1 + θ 2 )l 2 − sin (θ 1 + θ 2 + θ 3 )l 3
θ&1 cos(θ 1 + θ 2 + θ 3 )l3   & 
θ2− sin (θ 1 + θ 2 + θ 3 )l 3   & 
θ 3  d 2 と d 4 は余弦定理を用いて以下のように求まる． と求まる．よって，ヤコビ行列は以下のようになる．

222

−1  l1 + l 2 − l13 d=cos
 1
2ll
12

222l+l−l−11132d 2 = cos 2ll113222

−1  l 2 + l13 − l1 
d 3 = cos  2l 2 l13 (23) cosθ1l1 + cos(θ1 + θ 2 )l2 + cos(θ1 + θ 2 + θ 3 )l3J =− sin θ1l1 − sin (θ1 + θ 2 )l2 − sin (θ1 + θ 2 + θ 3 )l3cos(θ1 + θ 2 )l2 + cos(θ1 + θ 2 + θ 3 )l3− sin (θ1 + θ 2 )l2 − sin (θ1 + θ 2 + θ 3 )l3(28)cos(θ1 + θ 2 + θ 3 )l3 − sin (θ1 + θ 2 + θ 3 )l3 

ここで，ヤコビ行列が横長行列のため逆ヤコビ行
よって，各関節の角度は，以下のように求まる． 列を求めるために，擬似逆行列を用いて以下のよθ 1 = d 3 ± d1
 ± (d 2 − π ) d ±d4
 3うになる．
(24)( )J + = J Τ JJ Τ−1(29)y

4.2 終端状態の各関節角速度の導出[P , P ] = [0.3,0.2]x

ここでは，所望の投てきを実現するために必要

θ3な各関節角速度を求める．まず，Fig.2 のように物理学の水平投射より速度 v 0 を(25)式より求めることができる．

v0 = AB / 2 H / g
よって，手先の速度は V = [v0(25)0]Τ となる．

yθ1d1d2d3J3
θ2J2J1Fig.4 Final statexこれより，各関節角速度は以下のようになる．

θ&1 & +
θ 2  = J Vθ&3  

うな動作を得られた．
次に，より遠くへ投げようとしている Fig.6 の結(30)

果を検証する．Fig.5 のときよりも終端での角速度が大きく設定されているため，それを達成する
ためにフィードフォワード入力は先ほどより大5 シミュレーションきくなっていることがわかる．しかし，その変化量はさほど大きくならなかった．また，第二関節，シミュレーションに用いた力学モデルのパラメータを Table.1 に示す．今回飛距離が 1.5m と2.0m との二つのパターンのシミュレーションを行った．終端状態制御では初期状態と終端状態の状態量を任意に設定することができる．そこで，初期状態の状態量をすべて 0 とし，終端状態の状態量を 4 章の方法で求め Table.2 の通りにした．
また，初期状態から終端状態までの時間を 1secとし，3 章の方法で誤差学習をした．このときのオイラー法の刻み幅は 1.0ms とし，誤差の二乗和第三関節が大きく変化し，先ほどよりも大きく振りかぶっていることが Fig.5，Fig.6 の(d)よりわかった．
このことから，ただ単にフィードフォワード制
御入力だけを大きくして手先を加速するのではなく，大きく振りかぶることで手先が加速するための角度をかせぎ，その上で若干制御入力を増やしていることがわかった．
人も遠くに投げる場合に，投球動作自体も大きくしている．これにより過大な筋力を使って力まが 1 × 10 −5 以下になるまで学習を進めた．この目標
Table.1 Parameter of modelの達成までには，飛距離 1.5m の時 36 回，飛距離2.0m の時 46 回の学習を要した．
飛距離 1.5m のときのシミュレーション結果をFig.5 に，飛距離 2.0m のときのシミュレーション
結果を Fig.6 に示す．
Fig.5 より，1sec で角度，角速度ともに目標状

M 1[kg ]M 2 [kg ]M 3 [kg ]L1 [m]
L2 [ m ]L3 [m]

1.330.114
0.1030.138

l1 [m]
l 2 [ m]l 3 [ m]0.15

d1 [kgm2 / s]

0.247

0.872

0.12
0.13

0.079

20.07520.046d2 [kgm / s]d3[kgm / s]

態にほぼ到達していることがわかる．また，
Fig5(a)と(b)より，ロボットアームは初めはゆっTable.2 Values of final states

くり動き，ある瞬間から急に目標位置に到達する

Distance of a jump Distance of a jump
1.5[m]
2.0[m]まで速度をあげていることがわかる．そのため，Fig.5(c)では後半で急激にフィードフォワード入力が大きくなっている．なぜこのような挙動になるかは、Fig.5(d)を見るとよくわかる．ロボッ
トアームは，初めゆっくりと振りかぶってタメを作り，そこから一気に目標位置まで腕を振っている．このように，人が振りかぶってから投げるよθ1
θ2
θ3

1.077

1.077

-0.475-0.475-0.601-0.601

θ&-3.592-4.789

θ&2
θ&3

10.58

12.07

14.11

16.09

1

2
1.5

20
rink 1rink 2rink 3

Angle [rad]

1
0.5
0-0.5
-1
-1.5

105
0-5
-10
-15

-2
-2.5
0

rink 1rink 2rink 315
Angular velocity [rad/s]

2.5

0.2

0.4
0.6
Time [sec]

0.8

1-20
00.2

(a) Swing angles
2.5
1.5

1(b) Angular velocities
0.45

rink 1rink 2rink 30.4 Back

Swing

0.35

1

Swing

0.3

0.5

0.25

00.2

-0.5

0.15

-1

0.1

-1.5

0.05

-2-2.500.8

YFeedFoword Input u [V]

2

0.4
0.6
Time [sec]

0.2

0.4
0.6
Time [sec]0.8

0-0.25-0.2-0.15-0.1-0.05 0 0.05 0.1 0.15 0.2 0.25
X1(c) Feedforward inputs(d) Stick diagram

Fig.5 Simulation result of distance of a jump 1.5m
2.520

rink 1rink 2rink 32

Angular velocity [rad/s]

1.5
1Angle [rad]rink 1rink 2rink 3

15

0.5
0-0.5
-1-1.51050-5-10-15-2-2.5
00.2

0.4
0.6
Time [sec]

0.8

-20
010.2

(a) Swing angles
2.5(b) Angular velocities
0.45

rink 1rink 2rink 30.4 Back

Swing

0.35

1

0.3

0.5

0.25

0

Swing

0.2

-0.5

0.15

-1

0.1

-1.5

0.05

-2
-2.5
00.8

Y

FeedFoword Input u [V]

2
1.5

0.4
0.6
Time [sec]

0.2

0.4
0.6
Time [sec](c) Feedforward inputs

0.8

10-0.25-0.2-0.15-0.1-0.05 0 0.05 0.1 0.15 0.2 0.25
X(d) Stick diagram

Fig.6 Simulation result of distance of a jump 2.0m1かせに投げることを回避している．シミュレーシ

ので，角度に制限を与えることを検討する．さらョン結果において，これと似た現象がみられたこに，実験による検証を行う予定である．とから，終端状態制御を用いて獲得した動作は生物模倣的なものになっていると考えられる．

参考文献(1). 島田 明, 「モーションコントロール」, オ

6 結言ーム社 p177-178

本論文では，3 リンクロボットアームによる投て(2). 川人 光男,「脳の運動学習」,日本ロボット

き動作を誤差学習による終端状態制御を用いて導出し，シミュレーションによって検証した．その結果，以下の結論を得た。
誤差学習による終端状態制御によって導出した

学会誌,Vol.13,No.1,(1995),11-19(3). 翁 志強, 西村 秀和, 「フィードフォワードのトルク入力による2 リンク猫ひねりロボットの終端状態制御」日本機械学会論文集(C 編),

フィードフォワード制御入力を用いて，3 リンク

Vol.66,No.643,C,(2007-3),857-863

ロボットアームの投てき動作の制御を行うこと(4). 西村 秀和, 高崎 堅治, 舟木 厚司, 戸谷ができた．
獲得した動作は，人が物を投げるときと同様に，

隆美, 「誤差学習による終端状態制御を用いたブ
ラキエーションロボットの運動制御」, 日本機械

一度振りかぶってタメを作ってから投げる動作

学会論文集
（C 編）
,Vol63,No605,C,(1997-1),182-189に移行するものになった．

(5). 西村 秀和, 舟木 厚司, 戸田 隆美, 「フタメの大きさはは飛距離により変化することが示された．
今後は，手先の終端速度を早くし過ぎるとアームがタメを作る際に可動範囲を越える場合があるィードフォワード入力を用いた終端状態制御のパラメータ変動に対するロバスト性能」, （走行振り子の位置決め制御による検証）,日本機械学会
論文集(C 編)Vol.61,No.587,C,(1995-7),2938-2944