受動性に基づく視覚フィードバックにおける推定と制御の内部構造○村尾俊幸 （産業技術大学院大学） 河合宏之 （金沢工業大学） 藤田政之 （東京工業大学）

An Internal Structure of Estimation and Control
for Passivity-based Visual Feedback∗T. Murao (Advanced Institute of Industrial Technology), H. Kawai (Kanazawa Institute ofTechnology) and M. Fujita (Tokyo Institute of Technology)
Abstract– This paper investigates an internal structure of estimation and control for passivity based visualfeedback. Firstly we derive a basic representation for the relative rigid body motion and the estimation error
system. Secondly we construct the control error system in order to establish the visual feedback system. The
control error between the real value and the reference of the relative rigid body motion, can be made by theestimation error and the estimated value. Next, we show passivity of the dynamic visual feedback system.
Based on the passivity, stability and L2 -gain performance analysis are discussed. Furthermore, simulation
results are assessed with respect to the performance.
Key Words: Visual feedback control, Passivity, Lyapunov stability, L2 -gain performance analysis

1

はじめに

近年, 視覚フィードバック制御は, 工場だけでなく医療分野などへも応用され始めており, より複雑なシステム
や幅広い分野へ適用されるようになってきている. この視覚フィードバック制御は二つの制御方法 (位置ベース
法と画像ベース法) と二つのカメラ構造 (Eye-in-Hand
構造と固定カメラ構造) に大別できることがよく知られており, それらを発展させた手法も次々と研究されている 1, 2) . 例えば, Occlusion 回避のために Eye-in-Handと固定カメラを組み合わせた複数のカメラを用いる手法 3) や, それぞれの制御方法の固有の欠点を補うために Lyapunov 関数を用いて位置ベース法と画像ベース
法を切り替える手法 4) など, 最近では二つの制御方法
やカメラ構造を組み合わせる手法が提案されている.
一方, 著者らも文献 5) において受動性に基づく位置ベース法の視覚フィードバック制御則を Eye-in-Hand
構造で提案し, さらに文献 6) では Eye-in-Hand と固定カメラの両構造を含むシステムに対して制御則を提案している. しかし, 一般的な位置ベース法で扱う, 目標
値に一致させる制御問題と位置姿勢を推定する推定問題において, 従来研究 5, 6) で提案した制御則では制御偏差が推定入力にもフィードバックされているために,制御偏差が存在しているときにはそれが推定部分に直接影響を与えていた.
そこで, 本稿では推定と制御の内部構造を見直し, 推
定入力が制御偏差に対して非干渉となるような, 受動性に基づく視覚フィードバック制御則を提案する. 提案
した制御則に対して安定性と L2 ゲイン制御性能解析を行い, シミュレーションにて制御則の検証を行う.

2
2.1

Eye-in-Hand 構造の 3 次元視覚フィー
ドバックシステム
剛体運動の表現と推定偏差システム

本稿では, Fig. 1 に示すような, 三つの座標系によって表される Eye-in-Hand 構造の視覚フィードバックシ
ステムについて考える. 図中の座標系をマニピュレー
タの台座に基準座標系 Σw , カメラにカメラ座標系 Σc ,World 5w
Frame

Manipulator

gwc
5c

gwo

Cameragco
Target Object

5oFig. 1: Eye-in-hand visual feedback system
観測対象に観測対象座標系 Σo として定義する.
本システムの制御目的は運動する観測対象にカメラを追従させることである. 言い換えると, カメラ座標系 Σc からみた観測対象座標系 Σo の相対位置姿勢 gco
を目標位置姿勢 gd (本稿では一定値であるとする) に常に一致させておくこととなる. ここで, gab は同次表
現と呼ばれ, 任意の二つの座標系 Σa から Σb の位置
pab ∈ R3 と姿勢を表す回転行列 eξ̂θab ∈ SO(3) から構成され, 4 × 4 行列として以下のように定義される.
eξ̂θab pab
gab =
(1)
0
1
ただし, 演算子 ∧(wedge) は 3 次元ベクトルを 3 × 3の歪対称行列へ写像する演算子であり, その逆写像は∨(vee) で定義される 7) .
この相対位置姿勢 gco と目標値 gd の偏差を, 制御偏差 ger としてつぎのように定義する.

ger := gd−1 gco(2)さらに, この制御偏差に対する制御偏差ベクトルを er :=[pTer eTR (eξ̂θer )]T ∈ R6 と定義する. ただし eR (eξ̂θ ) :=sk(eξ̂θ )∨ ∈ R3 , sk(eξ̂θ ) := 12 (eξ̂θ − e−ξ̂θ ) である.
制御目的を達成するために gco の運動について考える. gco は Fig. 1 における同次表現の合成則から−1
gco = gwc
gwo(3)で表される. また, カメラ座標系からみた観測対象の相対速度は, gco を時間微分することで,
b
b
b
−1 V
= −Ad(gco
Vco) wc + Vwo

(4)

−1
b
のように導かれる 5) . ここで, Vab= (gabġab )∨ ∈ R6
は剛体運動のボディ速度を表しており, Ad(gab ) ∈ R6×6
は同次表現 gab の随伴写像である 7) .
上述の相対位置姿勢 gco は観測対象の位置姿勢 gwo
が測定不可能であるために未知情報となる. そこで, 相対位置姿勢 gco を含んでいる視覚情報 f ∈ R2m を利用し, 非線形オブザーバを構成することで, 相対位置姿
勢の推定値 ḡco を得ることを考える. (4) 式に基づき,推定値 ḡco の運動モデルをつぎのように構成する.
b
b
−1 V
V̄co
= −Ad(ḡco) wc + ue(5)6ue ∈ R は推定偏差の振舞いを安定にするために加えられる入力であり, 後に制御則の一部として提案する.
ここで, 相対位置姿勢の真値 gco と推定値 ḡco の偏差を, 推定偏差 gee として−1
gee := ḡco
gco

(6)

と定義する. さらに, 推定偏差ベクトルを ee :=[pTee eTR (eξ̂θee )]T ∈ R6 と定義する. 姿勢推定偏差の回転量 θee を十分に小さいとし, さらにカメラの画像特徴量をその推定値の周りで 1 次テーラー展開することで, この推定偏差ベクトル ee は, カメラから得られる
視覚情報 f と推定モデルにより得られる視覚情報 f¯, さらには画像ヤコビアンの擬似逆行列 J † (ḡco ) ∈ R6×2m
を用いることでつぎのように導かれる 5) .
†ee = J (ḡco )(f − f¯)(7)

この推定偏差ベクトル ee が 0 になれば, 相対位置姿勢の真値と推定値が一致することとなる.
gee を時間微分し, (4), (5) 式を用いることで, 推定偏差システムは以下のように導出される.
bb−1 ue + V
Vee
= −Ad(gee
wo
)

(8)

この推定偏差システム (8) に対して, 観測対象が静b
止している (すなわち Vwo= 0) とき, 入力 ue から出
力 −ee の間に受動性が成立する. これは以下のエネル
ギー関数を用いて証明される 5) .
1
(9)Ve = pee 2 + φ(eξ̂θee ) := E(gee )2
ただし, φ(eξ̂θab ) は回転行列に対する誤差関数を表す正定関数である.

2.2 制御偏差システム
つぎに制御目的を達成するための制御偏差システムについて議論する. 従来研究 5) では gec := gd−1 ḡco を制御偏差として定義し, この偏差 gec に対して制御偏差システムを構成していた. それは, (2) 式で定義した制
御偏差 ger が, 未知情報 gco を用いて定義しているために, 直接得ることができないためである. しかし gec
は推定値 ḡco との偏差をとっているために, 制御偏差システムを構成したときに推定入力が制御偏差システ
ムに直接影響していた. それに対して本稿ではその影

ee

pee
gee
Eq.(11) NG ee Eq.(1)Eq.(10)gco

ger

Fig. 2: Relation between ee and ger
響を取り除くために, ger による制御偏差システムを用いて, 視覚フィードバックシステムを再構成することを試みる. その際に必要となる ger は, 未知情報 gco を直接用いずに, 推定偏差ベクトル ee と推定値 ḡco を用いて導出する手法を提案する.
はじめに制御偏差 ger の導出過程から述べる. (2) 式を再考すると, 推定偏差 gee を用いることで ger は以下のように表すことができる.
−1
gco = gd−1 ḡco gee (10)ger = gd−1 gco = gd−1 ḡco ḡco(10) 式において, 目標値 gd と推定値 ḡco は既知情報であるため, 推定偏差 gee が得られれば制御偏差 ger は求まることがわかる. 但し, gee も gco を用いて定義されているため直接求めることは不可能であるが, 推定偏差ベクトル ee は (7) 式より, 既知情報より生成可能な情報となる. したがって, つぎに ee から gee を導出す
ることを考える.
推定偏差ベクトル ee の定義より, 位置偏差 pee はそのまま得られる. 姿勢偏差 eξ̂θee に関しては ee から得られるのは eR (eξ̂θee ) であるが, − π2 ≤ θee ≤ π2 の範囲
内では, 以下の関係式から ξθee を求めることができる.
ξθee =

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

eR (eξ̂θee )(11)

したがって, 推定偏差ベクトル ee から ξθee を介して, 推定偏差 gee を求めることができる. ここで, 仮定− π2 ≤ θee ≤ π2 は, ξθee  = sin−1 eR (eξ̂θee ) を一意に決めるための仮定であるが, 既に 2.1 節で姿勢推定
偏差 θee を十分に小さいとしているため新たな条件とはならない.
このように推定偏差 gee が求まるので, さらに ḡco を用いて, (10) 式より制御偏差 ger を導出することが可能となる. Fig. 2 に ee と ger の関係を示す.
(2) 式で定義した制御偏差 ger を時間微分し, (4) 式を用いることで, 制御偏差システムは以下のように導出
される.


b
b
b
−1
Ver
= −Ad(ger(12)) Ad(g−1 ) Vwc + Vwod

この制御偏差システム (12) に対しても, 観測対象が静止bb
している (すなわち Vwo= 0) とき, 入力 Ad(g−1 ) Vwc
かd
ら出力 −er の間に受動性が成立し, 以下のエネルギー関数を用いることで証明される.
1(13)
Vr = per 2 + φ(eξ̂θer ) := E(ger )2

2.3 視覚フィードバックシステム
制御偏差システム (12) と推定偏差システム (8) より
視覚フィードバックシステムをつぎのように構成する.
 b  −10−Ad(ger
Ver
I b
)
V
(14)
uce +
b =
I wo
0
−Ad(gee
Vee
−1
)

b T
ただし, uce := [(Ad(g−1 ) Vwc) uTe ]T とした. また偏差dに対しても, e := [eTr eTe ]T と定義する. この視覚フィー
ドバックシステム (14) に対して受動性が成立する.
補題 1とする. このとき, 出力を νce := −e とするとき, (14)式の視覚フィードバックシステムの入出力間に T
uTceνce dτ ≥ −βce , ∀ T > 0(15)b
観測対象が静止している (すなわち Vwo
= 0)

N

KN

er

Kc

Ad(gd)

uN

N

ud

ec

ue

ee

uN

KN
Kc

Ad(gd)ud

Ad(e^ )
NGec

ee

Ke

ue

Ke

Fig. 3: Block diagram of the control law (Left: proposed
method, Right: another one5) )0が成り立つ. ただし βce はある非負の定数である.
これは (13) 式と (9) 式のエネルギー関数を併せた以下の式を視覚フィードバックシステム (14) のエネルギー関数とすることで証明される.

Ver = E(ger ) + E(gee )

3

(16)

J
f

ee
gco

Jd

Observer

q

b
Vwo
q

Eye-in-Hand
Manipulator

f

Jd

q
gd
gco

Controller

J

b
Vwo
q

Eye-in-Hand
Manipulator

f

gd
gco

Observerfq

Fig. 4: Block diagram of the dynamic visual feedback
control (Left: proposed method, Right: another one5) )3 次元動的視覚フィードバック制御

3.1 動的視覚フィードバックシステムと受動性
本節では, 前節で構成したシステムに対して, つぎのマニピュレータダイナミクスを考慮する.
M (q)q̈ + C(q, q̇)q̇ + g(q) = τ + τdqgd
gco Controller
ee

(17)

q, q̇, q̈ ∈ Rn はそれぞれの各関節の角度, 角速度, 角加速度を表し, τ ∈ Rn は入力トルク, τd ∈ Rn はトルク
外乱, M (q) ∈ Rn×n は正定な慣性行列, C(q, q̇)q̇ ∈ Rn
は遠心力・コリオリ力項, g(q) ∈ Rn は重力項を表す.
また, カメラの速度はボディマニピュレータヤコビアb
ン Jb (q) ∈ R6×n を用いることで Vwc= Jb (q)q̇ として表される 7) . 一方, カメラの速度の目標値を ud とし,マニピュレータの関節角速度の目標値を q̇d で表すと,
ud = Jb (q)q̇d と表すことができる. ここで, 関節角速度に関する偏差を ξ := q̇ − q̇d ∈ Rn と定義する. この
関節角速度偏差 ξ が 0 になるとカメラの速度がその目標値に一致することとなる.
このとき, マニピュレータへの入力トルクとして
τ = M (q)q̈d + C(q, q̇)q̇d + g(q)この動的視覚フィードバックシステム (19) に対してもつぎのエネルギー関数を用いることで下記に示す受動性が成立する.
1
V = ξ T M ξ + E(ger ) + E(gee )(20)2
補題 2 外乱がない (すなわち w = 0) とする. このとき, 出力を ν := N x, N := diag{I, −I, −I} とする
とき, 動的視覚フィードバックシステム (19) の入出力
間に
 TuT νdτ ≥ −β0 , ∀ T > 0(21)0が成り立つ. ただし β0 はある非負の定数である.

3.2 動的視覚フィードバック制御則と安定性
動的視覚フィードバックシステム (19) に対し, 補題
2 の受動性に基づくことで, 外乱がない場合に平衡点
x = 0 を安定とする制御則として次式を提案する.
u = −Kν = −KN x, K := diag{Kξ , Kc , Ke }(22)+JbT (q)AdT(g−1 ) er + uξ (18)ただし, ゲインにおける各要素はすべて正とする. このとき安定性に関してつぎの定理が導かれる.

を考える. q̇d , q̈d はそれぞれ関節角速度と関節角加速度の目標値である. uξ は関節角速度偏差をなくすための新たな入力であり, 後に提案する. このとき,(14),(17),(18) 式を用いることで Eye-in-Hand 構造の3 次元動的視覚フィードバックシステムは⎤⎡⎤ ⎡−M −1 Cξ + M −1 JbT AdT(g−1 ) er
ξ˙
d
⎥
b ⎦=⎢
⎣ Ver
−Ad(gco−1 Jb ξ
⎦
⎣
)
b
Vee
0
⎤ ⎡ −1 ⎤
⎡ −1
M
0
0
0
M
⎦u+⎣ 0 I ⎦w(19)−10+⎣ 0 −Ad(ger)−100−Ad(gee0 I)

定理 1 w = 0 のとき, 動的視覚フィードバックシステ
ム (19) と (22) 式の入力で構成される閉ループ系の平衡点 x = 0 は漸近安定である.

dで表される. ただし, 入力, 外乱, 状態をそれぞれ
b T T
u := [uTξ (Ad(g−1 ) ud )T uTe ]T , w := [τdT (Vwo) ] ,dx := [ξ T eTr eTe ]T と定義している. Eye-in-Hand 構
造の動的視覚フィードバックシステムにおいて, 状態 x
を 0 にとどめておくことで制御目的が達成される.

定理 1 は, (20) 式のエネルギー関数を Lyapunov 関
数候補とすることで証明される.
Fig. 3 の左図に本稿で提案した制御則, 右図に従来研
究 5) で提案した制御則のブロック図を示す. 文献 5) の推定偏差に対する制御則 ue = Ke (ee − AdT(eξ̂θec ) ec ) は,ec で定義された制御偏差が推定入力に干渉してしまう
ために, 制御偏差 ec が存在する場合, それが推定入力 ueに影響を与えていた. それに対して本稿で提案した制御
則 (22) uξ = −Kξ ξ, ud = Ad(gd ) Kc er , ue = Ke ee は,それぞれの偏差がそれぞれの入力に対してのみフィードバックされている. したがって, 本稿の制御則は上記の影響を回避することができる. このことは動的視覚
フィードバックシステムと制御則で構成される閉ループ系のブロック図からも読み取れる. Fig. 4 の左図に本

2

3

4

5

6

0

-0.05
0.2

0

2

3

4

5

6

0

2

3
time [s]

4

5

6

0

-0.2

-0.02
0.04

0

3

2

4

5

6

0

-0.04
0.2

0

2

3

4

5

6

0

2

3
time [s]

4

5

6

0.47

0.475

0

-0.1

5)

(Solid: proposed method, Dashed: another one )稿の閉ループ系, 右図に従来研究 5) の閉ループ系のブロック図を示す. 従来研究のオブザーバと異なり, 本稿のオブザーバには目標相対位置姿勢 gd が影響せず, 推
定部分に制御部分が直接影響していないことがわかる.
このように制御則を提案できるのは, 動的視覚フィー
ドバックシステム (19) の入力に対する行列が対角行列になっているためであり, 構成した動的視覚フィード
バックシステムの偏差系において, 推定と制御の構造を分離していると解釈できる. そのために, 制御偏差シス
テムや推定偏差システムで制御則を個別に設計したとしても, 全体のシステムを安定化させる制御則となる.

3.3 L2 ゲイン制御性能解析
本節では外乱が存在する場合に, 外乱抑制問題を考えることで L2 ゲイン制御性能解析を行う.
ここで, 正のスカラーの定数 ke を用いることによって, Ke =: ke I とゲインを定義しなおす. また, ある正の数 γ を用いてつぎの条件を与える.
γ2 + 1I
2γ 2(γ 2 + 1)(2ke − 1) − 1
IKc >2γ 2 (2ke − 1) − 2
γ2 + 1
ke >
2γ 2

(23)
(24)
(25)

このとき, 制御性能解析に関するつぎの定理が示される.
定理 2 与えられた γ に対して, (23)〜(25) 式を満たす
ようにゲイン Kξ , Kc , ke を選ぶとき, (19), (22) 式で構成される動的視覚フィードバックシステムは γ 以下の L2 ゲインを有する.
定理 2 は, (20) 式のエネルギー関数を蓄積関数とみなすことにより証明できる. この制御性能解析では観測対象の運動を外乱として捉えているために, γ が小さいコントローラであれば観測対象の運動が状態に与える影響がより少ないことを示している.

4

-0.05

0.1

-0.05

0.1

0.47

0.475

0

0

-0.2

Fig. 5: Control error (Left) and estimetion one (Right)

Kξ >

0.

0.

ywo [m]peex [m]
0

0.

0.

0

ywo [m]pery [m]0.05

peey [m]-0.04N^G erz [rad]

0.02

0

N^G eez [rad]perx [m]0.04

シミュレーション

本稿と文献 5) で提案されている視覚フィードバッ
ク制御則の性能比較をシミュレーションにより行う.
本稿では 3 次元の動的視覚フィードバック制御則を提案したが, シミュレーションでは, 簡単のために水平 2 自由度マニピュレータである SICE-DD アーム
を考える. 両制御則を比較するため, ゲインは両制御則に対して同じゲインを設定し, Kξ = 10I2 , Kc =diag{30, 30, 15, 15, 15, 30}, ke = 20 とする.
Figs. 5〜6 に二つの制御則を比較したシミュレーショ
ン結果を示す. Fig. 5 は 3 次元空間中での x, y 軸の位

0.4

0.42

0.44
xwo [m]

0.46

0.48

-0.1

0.4

0.42

0.44
xwo [m]

0.46

0.48

Fig. 6: Estimated value of the target object(Solid)and realone(Dashed)(Left:proposed method, Right:another one5) )置偏差と z 軸の回転偏差を示す. 左図が制御偏差, 右図が推定偏差であり, 実線が本稿で提案する制御則, 破線が従来研究 5) の制御則の場合である. Fig. 6 は観測対象の運動を推定した値を実線で, 実際のトラジェクト
リーを破線で示す. 左図が提案する制御則, 右図が従来
研究 5) の制御則の場合である. また, Fig. 6 の上部にある図は直線運動部分の拡大図を示したものである.
Fig. 5 の左図の実験結果より, 本稿で提案した制御則のほうが, 同じゲインに対して制御偏差が小さく, 性能が良いことがわかる. それは Fig. 5 の右図や Fig. 6 からわかるように, 従来研究 5) の制御則に比べて本稿で提案した制御則の方が, 観測対象が運動した場合でも推定がより正確に行われているためであると考えられる.

5

おわりに

本稿では文献 5) の Eye-in-Hand 構造の視覚フィー
ドバックシステムに対して, 推定と制御の内部構造を見直し, 視覚フィードバックシステムを再構成した. 制御
偏差として真値と目標値との偏差を取ることで, 推定入力が制御偏差に対して非干渉となるような制御則を提案した. そのために, カメラから得られる推定偏差と非線形オブザーバから得られる推定値から, 制御偏差が得られることを示した. 最後に, 従来研究に対して, 本研
究の制御則がより有効であることをシミュレーションにより示した. 本稿では Eye-in-Hand 構造に限定して議論してきたが, 文献 6) の可動カメラ構造に対しても同様の議論が展開可能である.

参考文献
1) F. Chaumette and S. Hutchinson: “Visual Servo Control, Part I: Basic Approaches”, IEEE Robotics & Automation Magazine, 13-4, 82/90 (2006)
2) F. Chaumette and S. Hutchinson: “Visual Servo Control, Part II: Advanced Approaches”, IEEE Robotics& Automation Magazine, 14-1, 109/117 (2007)3) 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)
4) N. R. Gans and S. A. Hutchinson: “Stable Visual
Servoing through Hybrid Switched-System Control”,
IEEE Trans. Robotics, 23-3, 530/540 (2007)5) 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)
6) T. Murao, H. Kawai and M. Fujita: “Passivity-based
Control of Visual Feedback Systems with Dynamic
Movable Camera Configuration”, Proc. of the 44th
IEEE Conference on Decision and Control and European Control Conference, 5360/5365 (2005)
7) R. Murray, Z. Li and S. S. Sastry: A Mathematical Introduction to Robotic Manipulation, CRC Press(1994)