受動性に基づく視覚フィードバックシステムに対する同調制御の一考察○村尾俊幸（産業技術大学院大学） 河合宏之（金沢工業大学） 藤田政之（東京工業大学）

A Study on Passivity-based Synchronized Control for Visual Feedback Systems∗T. Murao (Advanced Institute of Industrial Technology), H. Kawai (Kanazawa Institute ofTechnology) and M. Fujita (Tokyo Institute of Technology)
Abstract– This paper investigates passivity based synchronized visual feedback control for eye-to-handsystems. Firstly the brief summary of the estimation and the control error systems with an eye-to-handconfiguration is given with the basic representation of a relative rigid body motion. Secondly we construct
the synchronized visual feedback system with an eye-to-hand configuration by combining a synchronization
error system. Based on passivity, stability analysis are discussed. Finally the validity of the proposed control
law can be confirmed by the simulation results.
Key Words: Visual feedback control, Synchronization, Passivity, Lyapunov stability

1

はじめに

近年，視覚フィードバック制御の研究はますます盛んに行われており，工場だけでなくより複雑なシステ
ムや幅広い分野へも適用されるようになってきている.
この視覚フィードバック制御は古くから Eye-in-Hand
構造と Eye-to-Hand (固定カメラ) 構造に大別できることがよく知られているが, 次々とそれらを発展させた手法も研究されている 1) . 例えば, Occlusion 回避のために Eye-in-Hand と固定カメラを組み合わせた複数のカ
メラを用いる研究 2) や, 積極的に広い視野を手に入れるために可動カメラを用いる研究 3) など, 複数台のマ
ニピュレータから構成されるシステムが最近では提案されている.
一方, 一台では限界のある作業や広範囲に渡る作業を行うため, 複数台のマニピュレータからなる同調制御も
近年では研究されており, 他のマニピュレータに同調しながら目標軌道に追従する適応制御則 4) や, 角速度を推定することで角度の測定のみで構成できる同調制御
則 5) , 角加速度をフィードバック項に用いない同調制御則 6) などが提案されている. これらの制御則は, マニ
ピュレータに外乱が入ったときに, 同調効果のために外乱の影響を抑えることができるなどの利点が挙げられる. 複数台のマニピュレータで構成される視覚フィー
ドバックシステムに対しても, 目的に応じてこのような同調制御を適用することは効果的である.
そこで, 本稿では視覚フィードバックシステムに対する同調制御として, 二台のマニピュレータからなる受動性に基づくシンクロ視覚フィードバック制御則を提案する. 提案した制御則に対して安定性解析を行い, シ
ミュレーションにて制御則の検証を行う.

2

推定偏差システムと制御偏差システム

2.1

目標相対位置姿勢の生成

本稿では, Fig. 1 に示すような, 七つの座標系によって表される二台のマニピュレータを有する Eye-to-Hand
構造の視覚フィードバックシステムについて考える. 本
稿では簡単のため, 構成されるマニピュレータは二台とし, さらにカメラは固定されているとして考えるが,Camera

gch1

5c
gwc
gco
gww1
5w

World
Frame

gwo
gww2

gch2

5o

gh2o
TargetObject

5h1
gw1h1

gh1o

5w1
Manipulator 1

5h2
gw2h2
5w2
Manipulator 2

Fig. 1: Synchronized visual feedback system with eyeto-hand conﬁguration.
より多くのマニピュレータや可動カメラを用いたシス
テムに拡張することは可能である. Fig. 1 に示すように, 基準座標系 Σw , カメラ座標系 Σc , 観測対象座標系
Σo , 各マニピュレータの台座と手先にそれぞれ台座座
標系 Σwi , 手先座標系 Σhi を定義する. ここで, 本稿では二台のマニピュレータを考慮しているので i = 1, 2となる.
本システムの制御目的はマニピュレータの動きを同調させつつ運動する観測対象にマニピュレータの手先を追従させることである. 言い換えると, 手先座標系
Σhi からみた観測対象座標系 Σo の相対位置姿勢 ghi oを目標位置姿勢 ghi d (本稿では一定値であるとする) に常に一致させておくこととなる. ここで, gab は同次表
現と呼ばれ, 任意の二つの座標系 Σa から Σb の位置
pab ∈ R3 と姿勢を表す回転行列 eξ̂θab ∈ SO(3) から構成され, 4 × 4 行列として以下のように定義される.
eξ̂θab pab
gab =
(1)
0
1
ただし, 演算子 ∧(wedge) は 3 次元ベクトルを 3 × 3の歪対称行列へ写像する演算子であり, その逆写像は∨(vee) で定義される 8) . 本稿では複数台のマニピュレー

Manipulator 1

gww1
5w1

gh1d1

5o
Reference 1
Target Object 1

5h2

5h
gw1h1

gwo

Manipulator 2

Reference Manipulator

5h1

gww1
gh1d

gww1

gww2

gwh
ghd

gh2d2

5w

gwo

gw2h2
gww2

gww2

5o

TargetObject

5w2

gwo

gh2d
5o
Reference 2
Target Object 2

Fig. 2: Generation of desired relative rigid body motion.
タの同調を目的としているので, それぞれの目標相対位
置姿勢 ghi d 間である関係を持つ. 本節ではその目標相
対位置姿勢の生成について議論する.
はじめに, 観測対象に追従するために基本となるマ
ニピュレータとして参照マニピュレータを Fig. 2 のよ
うに考える. これは本システムの目標相対位置姿勢を決めるためのもので, このマニピュレータと実際の観測対象との間に, 目標相対位置姿勢 ghd を設ける. さらに, gwi oi = gwo (gooi = gwwi ) を満たすように座標系
Σoi を有する参照観測対象を Fig. 2 のように定める.
一方, 本システムの制御目的はすべてのマニピュレー
タに対して同じ動きをさせることであり, そのときには gwi hi = gwh となることから, ghhi = gwwi が満たされる. 以上の関係より, それぞれの目標相対位置姿勢
ghi di は設定する一つの目標相対位置姿勢 ghd を用いることで, つぎのように定めることができる.
−1
ghi di = gww
g g(2)
i hd wwi
この目標相対位置姿勢 ghi di と手先からみた参照観
測対象の相対位置姿勢 ghi oi の偏差を, 制御偏差 gehi
としてつぎのように定義する.
gehi := gh−1
g hi o i(3)i di
これは言い換えると, gehi = I のときに ghi o が ghi dに一致することを意味する. さらに, この制御偏差に対する制御偏差ベクトルを ehi := [pTehi eTR (eξ̂θehi )]T ∈R6 と定義する. ただし eR (eξ̂θ ) := sk(eξ̂θ )∨ ∈ R3 ,sk(eξ̂θ ) := 12 (eξ̂θ − e−ξ̂θ ) である.

2.2 推定偏差システムと制御偏差システム
制御偏差に用いた手先からみた参照観測対象の相対位置姿勢 ghi oi は以下のようにして導出される.
−1
ghi oi = gch
gco gwwi(4)i(4) 式において, gchi と gwwi は既知情報および測定可能な情報となるので, gco の運動についてつぎに考える.
gco は Fig. 1 における同次表現の合成則から−1
gco = gwc
gwo(5)で表される. また, カメラ座標系からみた観測対象の相対速度は, gco を時間微分することで,
b
b
b
b
−1 V
Vco
= −Ad(gco) wc + Vwo = Vwo

(6)

−1
b
のように導かれる 7)1 . ここで, Vab= (gabġab )∨ ∈ R6
は剛体運動のボディ速度を表しており, Ad(gab ) ∈ R6×6
は同次表現 gab の随伴写像である 8) .

る.

1 本稿では固定カメラシステムを考えているため V b = 0 とな
wc

上述の相対位置姿勢 gco は観測対象の位置姿勢 gwo
が測定不可能であるために未知情報となる. そこで, 相対位置姿勢 gco を含んでいる視覚情報 f ∈ R2m を利用し, 非線形オブザーバを構成することで, 相対位置姿
勢の推定値 ḡco を得ることを考える. (6) 式に基づき,推定値 ḡco の運動モデルをつぎのように構成する.
b
b
−1 V
V̄co
= −Ad(ḡco) wc + ue = ue(7)ue ∈ R6 は推定偏差の振舞いを安定にするために加えられる入力であり, 後に制御則の一部として提案する.
ここで, 相対位置姿勢の真値 gco と推定値 ḡco の偏差を, 推定偏差 gee として−1
gee := ḡcogco(8)と定義する. さらに, 推定偏差ベクトルを ee :=[pTee eTR (eξ̂θee )]T ∈ R6 と定義する. 姿勢推定偏差の回転量 θee を十分に小さいとし, さらにカメラの画像特徴量をその推定値の周りで 1 次テーラー展開することで, この推定偏差ベクトル ee は, カメラから得られる
視覚情報 f と推定モデルにより得られる視覚情報 f¯, さらには画像ヤコビアンの擬似逆行列 J † (ḡco ) ∈ R6×2m
を用いることでつぎのように導かれる 7) .
(9)
ee = J † (ḡco )(f − f¯)
この推定偏差ベクトル ee が 0 になれば, 相対位置姿勢の真値と推定値が一致することとなる.
推定偏差 gee を時間微分し, (6), (7) 式を用いることで, 推定偏差システムは以下のように導出される.
bb−1 ue + V
= −Ad(gee
Vee
wo
)

(10)

つぎに制御目的を達成するための制御偏差システムについて議論する. (3) 式より, 制御偏差 gehi は推定偏差 gee を用いて以下のように表すことができる.

g −1 gco gwwi = gh−1
g −1 ḡco gee gwwi (11)gehi = gh−1
i di chii di chi(11) 式において, 推定偏差 gee 以外は既知情報となるため, gee が得られれば相対位置姿勢 gco を直接得なくとも制御偏差 gehi は求まる. このとき, − π2 ≤ θee ≤ π2の範囲内で, 以下の関係式が成り立つ 3) .
ξθee =

sin−1 eR (eξ̂θee )eR(eξ̂θee )

eR (eξ̂θee )(12)

したがって, 推定偏差ベクトル ee から推定偏差 gee が求められ, 制御偏差 gehi を導出することが可能となる.
(3) 式で定義した制御偏差 gehi を時間微分することで, 制御偏差システムは以下のように導出される.
bbb= −Ad(g−1 ) Ad(g−1 ) Vwh
+ Vwo
Veh
i
i
ehi

hi di

(13)

3

シンクロ視覚フィードバック制御

Mi (qi )q̈i + Ci (qi , q̇i )q̇i + gi (qi ) = τi + τid

前節で構成した推定偏差システムと制御偏差システ
ムのみでも固定カメラ構造の視覚フィードバックシス
テムは構成できるが, マニピュレータの同調性に対しては考慮されない. 工場における製造業や宇宙での組立作業に対して, 目標値への追従だけでなく各マニピュレータ間の同調制御を行うことは非常に有効となる 4) .
そこで本節では視覚フィードバックシステムに対する同調制御則を提案する.

3.1 シンクロ偏差システム
本節では, 同調制御を保証するために必要となるシンクロ偏差システムを構成する. はじめに, i 番目のマ
ニピュレータの手先から観測対象までの相対位置姿勢
ghi oi と j 番目の相対位置姿勢 ghj oj の間の偏差として, gehij = (pehij , eξ̂θehij ) をつぎのように定義する.

gehij = gh−1
ghi oi , (i, j) = (1, 2), (2, 1)
j oj

(14)

さらに, 位置と回転に対するカップリングゲイン kpij ,kRij と前述した偏差 (14) の要素を用いることで, シン
クロ偏差として以下のものを定義する.
ξ̂θehij
e
k
p
k
R
p
eh
ij
ij
ij
(15)
gesij =
01
他の偏差ベクトルと同様に, シンクロ偏差ベクトルを
esij := [pTesij eTR (eξ̂θesij )]T ∈ R6 と定義する.
シンクロ偏差システムは, シンクロ制御偏差 gesij を
時間微分することで以下のように導出される.
bb
Ves= −Ksij Ad(g−1 ) Ad(g−1 ) Vwh
ij
i
ehi

hi di

b
+Ksij Ad(g−1 ·gh d ) Ad(g−1 ) Vwh
j
hi oi
hj dj
j j

b
−1
+Ksij Ad(gww) I − Ad(g−1 ·gh o ) Vwo (16)
i

hi o

j

2
ここで, Ksij := diag{kpij kRij I, kR
I} である.
ij

3.2 シンクロ視覚フィードバックシステムと受動性
本節ではさらにつぎのマニピュレータダイナミクスを考慮する.

(17)qi , q̇i , q̈i ∈ Rn はそれぞれの各関節の角度, 角速度, 角
加速度を表し, τi ∈ Rn は入力トルク, τid ∈ Rn はト
ルク外乱, Mi ∈ Rn×n は正定な慣性行列, Ci ∈ Rn は
遠心力・コリオリ力項, gi ∈ Rn は重力項を表す.
また, マニピュレータの手先の速度はボディマニピュレータヤコビアン Jib (qi ) ∈ R6×n を用いることでVwbi hi = Jib (qi )q̇i として表される 8) . 一方, 手先の速度の目標値を uhi d とし, マニピュレータの関節角
速度の目標値を q̇id で表すと, uhi d = Jib (qi )q̇id と表すことができる. ここで, 関節角速度に関する偏差を
ξi := q̇i − q̇id ∈ Rn と定義する. この関節角速度偏差ξi が 0 になると手先の速度がその目標値に一致することとなる.
このとき, マニピュレータへの入力トルクとして
τi = Mi (qi )q̈id + Ci (qi , q̇i )q̇id + gi (qi )TT(qi )AdT(g−1 ) ehi + Jib(qi )AdT(g−1 ) Ksij esij
+Jib
hi di

hj oj

T
−Jib
(qi )AdT(g−1 ) Ksji esji + uξi

(18)

hi oi

を考える. q̈id は関節角加速度の目標値である. uξiは関節角速度偏差をなくすための新たな入力であり, 後に提案する.
このとき, (10), (13), (16)–(18) 式を用いることで Eye-to-Hand 構造のシンク
ロ視覚フィードバックシステムは (19) 式のように表される. ただし, 入力, 外乱, 状態をそれぞれ
u := [uTξ1 uTξ2 (Ad(g−1 ) uh1 d )T (Ad(g−1 ) uh2 d )T uTe ]T ,
h1 d1

h2 d2

T
T
b T T
τ2d
(Vwo
) ] , x := [ξ1T ξ2T eTh1 eTh2 eTs12 eTs21
w := [τ1deTe ]T と定義している. Eye-to-Hand 構造のシンクロ視覚フィードバックシステムにおいて, 状態 x を 0 にとどめておくことで制御目的が達成される.
このシンクロ視覚フィードバックシステム (19) に対して, つぎのエネルギー関数を用いることで下記に示す
受動性が成立する.
1
1
V = ξ1T M1 ξ1 + ξ2T M2 ξ2 + E(geh1 ) + E(geh2 )2211E(ges12 ) +E(ges21 ) + E(gee ) (20)
+
kR12
kR21

„
«3
2
−1
−1 T
T
T
T
−M
Ad
C
ξ
+
M
J
e
+
Ad
K
e
−
Ad
K
e
1
1
−1
−1
s
s
−1
s
s
h
1b
12
12
21
21
1
1
1
2
3 6
(gh d )
(gh o )
(gh o )
1 1
2 2
1 1
„
«7
ξ̇1
6
7
6
7
−1
−1
T
T
T
T
6 ξ̇2 7 6−M C2 ξ2 + M J
2b Ad(g −1 ) eh2 + Ad(g −1 ) Ks21 es21 − Ad(g −1 ) Ks12 es12 722
6 b 7 6
7
h2 d2
h1 o1
h2 o2
6V
7 6
7
6 eh1 7 6−Ad(g−1 ) J1b ξ1
7
6 V b 7=6h1 o1
76 eh2 7 6
7
−Ad
J
ξ
−1
6V b 7 6(gh o ) 2b 2
7
6 es12 7 6
2 2
7
b
4Ves
5 6
−K
7
12 Ad(g −1 ) (J1b ξ1 − J2b ξ2 )
21
h1 o1
6
7
b
4
Vee
5
−K21 Ad −1 (J2b ξ2 − J1b ξ1 )(gh o )2 20
3 2 −1
3
2 −1
M1
0
0
0
0
0
0
M1
−1
0
7 6 0 M2
7
6 0 M −1
0
0
0
2
7 6 0
7
6
0
I
7 6
7
6 0
0
−Ad(g−1 )
0
0
7 6 0
7
6
eh1
0
I
„
«
7 6
7
6 0
0
0
−Ad
0
−1
7
6
7w(19)
6
(g
)
+6
eh2
7u+6 07
0K
s12 Ad(g −1 ) I − Ad(g −1 ·g
)
7
6
7
6 0
ww1
h2 o
0
−Ks12 Ad(g−1 ) Ks12 Ad(g−1 ·g
0
h
o
1
)
„
«7
7 6
6
eh1
h1 o1 h2 d2
7 6
7
6 0
0
K
Ad
−K
Ad
0
−1
−1
s
s
21
21
5 4 0
4
(gh o ·gh1 d1 )(geh )I − Ad(g−1 ·g ) 5
0 Ks21 Ad(gww
−1
)
2 2
2
h2 o h1 o20000−Ad(gee
−1
)
0
0
I

eh2

0 0 0 00
Ks = diag{I, I, I, I, Ks12 , Ks21 , I}

h1 o1

0

2 2

−I

とするとき, シンクロ視覚フィードバックシステム (19)の入出力間に T
0uT νdτ ≥ −β0 , ∀ T > 0(21)が成り立つ. ただし β0 はある非負の定数である.

3.3 シンクロ視覚フィードバック制御則と安定性
シンクロ視覚フィードバックシステム (19) に対し, 補
題 1 の受動性に基づくことで, 外乱がない場合に平衡点 x = 0 を安定とする制御則として次式を提案する.
u = −Kν = −KN Ks x

(22)

K := diag{Kξ1 , Kξ2 , Kh1 , Kh2 , Ke }ただし, ゲインにおける各要素はすべて正とする. このとき安定性に関してつぎの定理が導かれる.
定理 1 外乱がない (w = 0) とき, シンクロ視覚フィー
ドバックシステム (19) と (22) 式の入力で構成される閉ループ系の平衡点 x = 0 は漸近安定である.
定理 1 は, (20) 式のエネルギー関数を Lyapunov 関
数候補とすることで証明される. 推定偏差と制御偏差の両システムのみで構成される従来法 3) を複数台のマ
ニピュレータが扱えるように単純に発展させた制御則に対して, 本手法ではシンクロ偏差を考慮しているために, 同調性が必要なタスクにおいて性能が向上すると考えられる. 視覚フィードバックシステムでは観測対象の動きが未知なために, 角度空間で目標軌道を与えることは基本的に難しい. そのため, 文献 (4)–(6) では角度空間で定義しているシンクロ偏差も, 本稿では作業空間で定義している. 紙面の都合上省略するが, 外乱が存在する場合には外乱抑制問題を考えることで L2 ゲイン
制御性能解析を行うことが可能である 7) .

4

シミュレーション

本稿で提案されているシンクロ視覚フィードバック
制御則とシンクロ要素を考慮しない制御則との性能比
較をシミュレーションにより行う. 本稿では 3 次元の視覚フィードバック制御則を提案したが, シミュレー
ションでは, 簡単のために水平 2 自由度マニピュレータ
である SICE-DD アームを考える. 両制御則を比較するためゲイン K に対しては同じゲインを設定したが,カップリングゲインのみシンクロ制御の場合は kp12 =kR12 = kp21 = kR21 = 0.75, 同調を考慮しない制御の場合には kp12 = kR12 = kp21 = kR21 = 0 とした.

0

0.2

0.4

0.6

0

0.2

0.4

0.6

0

0.2

0.4

0.6

0

-0.4

0.4

0

0.2

0.4

0.6

0

0.2

0.4

0.6

0

0.2

0.4

0.6

0

-0.4
1

N^G elz [rad]

0

0

-1

-0.3
pely [m]0.41ν := N Ks x⎡⎤
I 0 0 00
0
0
⎢0 I 0 0
0
0
0⎥
⎢
⎥
T
⎢0 0 −I 0 −AdT −1
⎥
Ad
0
−1
)
(gh o ·gh1 d1 )
(geh
N =⎢
⎥
2 2
1
⎢
⎥
⎣0 0 0 −I
AdT(g−1 ) −AdT(g−1 ·gh d ) 0 ⎦pelx [m]pelx [m]-0.3N^G elz [rad]補題 1 外乱がない (すなわち w = 0) とする. このとき, 出力を

0.1
0

0.1
0

pely [m]ただし, E(gab ) := 12 pab 2 + φ(eξ̂θab ) であり, φ(eξ̂θab )は回転行列に対する誤差関数を表す正定関数である.

time [s]-1time [s]Fig. 3: Hand control error eh1 (Dashed) and eh2 (Dotted),
and the synchronization error es21 (Solid) (Left: proposed
method, Right: non-synchronized method)
Fig. 3 に二つの制御則を比較したシミュレーション
結果を示す. Fig. 3 は 3 次元空間中での x, y 軸の位置
偏差と z 軸の回転偏差を示す. 破線と点線が制御偏差
eh1 , eh2 を, 実線がシンクロ偏差 es21 をそれぞれ表しており, 左図が本稿で提案した同調制御則, 破線が同調を考慮しない制御則の場合である.
最初の偏差の減り方からわかるように, 提案した同調制御の方は eh1 , eh2 よりもシンクロ偏差 es21 の方が早く偏差が収束していることがわかる.
一方, 本シミュレーションでは 2 秒時に 1 台目のマ
ニピュレータにだけ外乱トルク τ1d を加えている. 2 秒
以降の結果より, 外乱が入った場合, 偏差の大きさの最大値は同調制御を考えている方が小さくなっていることがわかる. これは同調制御の利点として考えられる.

5

おわりに

本稿では視覚フィードバックシステムに対する同調制御として, 二台のマニピュレータからなる受動性に基づくシンクロ視覚フィードバック制御則を提案した.
提案した制御則に対して安定性解析を行い, シミュレー
ションにて制御則の有効性を示した.

参考文献
1) F. Chaumette and S. Hutchinson: “Visual Servoing
and Visual Tracking,” B. Siciliano and O. Khatib(Eds.), Springer Handbook of Robotics, SpringerVerlag, 563/583 (2008)
2) V. Lippiello, B. Siciliano and L. Villani: “Positionbased Visual Servoing in Industrial Multirobot Cells
using a Hybrid Camera Configuration,” IEEE Trans.
Robotics, 23-1, 73/86 (2007)
3) T. Murao, H. Kawai and M. Fujita: “Predictive Visual Feedback Control with Eye-in/to-Hand Configuration via Stabilizing Receding Horizon Approach,”Proc. of the 17th IFAC World Congress on AutomaticControl, 5341/5346 (2008)
4) D. Sun and J. K. Mills: “Adaptive Synchronized Control for Coordination of Multirobot Assembly Tasks,”
IEEE Trans. Robotics and Automation, 18-4, 498/510(2002)
5) A. Rodriguez-Angeles and H. Nijmeijer: “Mutual
Synchronization of Robots via Estimated State Feedback: A Cooperative Approach,” IEEE Trans. Control Systems Technology, 12-4, 542/554 (2004)
6) S.-J. Chung and J.-J. E. Slotine: “Cooperative Robot
Control and Synchronization of Lagrangian Systems,”
Proc. of the 46th IEEE Conference on Decision andControl, 2504/2509 (2007)7) M. Fujita, H. Kawai and M. W. Spong: “Passivitybased Dynamic Visual Feedback Control for Three
Dimensional Target Tracking: Stability and L2 -gain
Performance Analysis,” IEEE Trans. Control Systems
Technology, 15-1, 40/52 (2007)8) R. Murray, Z. Li and S. S. Sastry: A Mathematical Introduction to Robotic Manipulation, CRC Press(1994)