受動性に基づく動的可動カメラを用いた視覚フィードバック制御○村 尾 俊 幸 ∗ · 河 合 宏 之 ∗∗ · 藤 田 政 之 ∗∗ 金 沢 大 学, ∗∗ 法 政 大 学

Visual Feedback Control with a Dynamic Movable Camera based on Passivity Approach○ Toshiyuki Murao∗ , Hiroyuki Kawai∗∗ and Masayuki Fujita∗∗ Kanazawa University,∗∗Hosei University

This paper deals with control of visual feedback system with a dynamic movable camera. This system consistsof a robot manipulator and a camera that is attached to the end-eﬀector of another robot manipulator. Firstly thebrief summary of the dynamic visual feedback system with an eye-in-hand conﬁguration is given. Next we construct
the visual feedback system with a dynamic movable camera by combining the hand control error system. Based on
the passivity, stability and L2 -gain performance analysis are discussed.

1

Cameraはじめに
CameraManipulator

ロボットやメカニカルシステムに自律的な振る舞いをさせる

Work
Manipulator

5cgchには, 多くの情報が必要となる. 特に視覚情報は未知環境下にお

5h
gzh

gwc

いて周囲の状況を認識させるために有用であり, この視覚情報をダイナミカルシステムのフィードバックループに組み込んだ制御のことを視覚フィードバック制御という 1) .

5w

ロボットの視覚フィードバック制御には二つの制御方法と二World Frame

つのカメラ構造に大別できることが知られている. 二つの制御方

gwh
gwz
gwo

gco

gho
5z
5o

Target Object

法とは, 位置ベース法と特徴ベース法であり, 二つのカメラ構造とは, Eye-in-Hand 構造と固定カメラ構造である 1) . 二つの制

Fig. 1:視覚フィードバックシステム

御方法については, カメラ視野の問題に対応するために多くの議論がなされている. Malis ら 2) はキャリブレーション偏差に対してもロバスト性を保証するために, 位置ベース法と特徴ベース

んでいる. 本稿では動的視覚フィードバックシステムを構成し,法の利点を組み合わせた制御法を提案している. Corke ら 3) の閉ループ系の漸近安定性と L2 ゲイン制御性能解析をおこなう.

方法は z 軸の並進と回転の制御方法を分けることによって, カメラ画像内に全ての特徴点を維持しておくことを可能にしている.
また文献 4) では位置ベース法と後方への動きを切り替えることによってカメラ視野の問題を解決している. このように制御方法に関しては様々な制御則が提案されている. しかしその一方で,カメラ構造に対して議論されている論文は少ない.

2

Eye-in-Hand 構造の動的視覚フィードバックシステム

2.1

視覚フィードバックシステムにおける剛体運動の表現

本稿では, Fig. 1 に示すような二台のマニピュレータを有し,五つの座標系によって表される視覚フィードバックシステムにそこで本稿では Fig. 1 に示すような動的可動カメラを用いたついて考える. カメラマニピュレータの台座に基準座標系 Σw ,視覚フィードバックシステムについて考える. 本システムは二

手先にカメラ座標系 Σc , ワークマニピュレータの台座に台座座

台のマニピュレータによって構成され, カメラを有しているマニ標系 Σz , 手先に手先座標系 Σh また観測対象に観測対象座標系

ピュレータをカメラマニピュレータ, もう一方をワークマニピュ

Σo を五つの座標系として定義する. このとき, 基準座標系から

レータとそれぞれ定義する. カメラマニピュレータとワークマニ

みたカメラの位置姿勢, 台座の位置姿勢, 手先の位置姿勢およびピュレータをそれぞれ “目” と “手” と捉えることによって, こ

観測対象の位置姿勢をそれぞれ gwc , gwz , gwh , gwo で表す. 同の動的可動カメラを用いた視覚フィードバックシステムは Fig. 2様に, カメラ座標系からみた手先の位置姿勢と観測対象の位置姿に示すような人間の上半身としてみなすことが可能であると考え

勢をそれぞれ gch , gco とし, 台座座標系からみた手先の位置姿勢

る. またカメラ構造の点においては, 本システムは Eye-in-Hand

を gzh , 手先座標系からみた観測対象の位置姿勢を gho と表す.

構造

5)と固定カメラ構造

6)の両方の構造を特殊な場合として含本システムの視覚フィードバックシステムの制御目的は, カメラ

と定義する. このとき, 推定偏差ベクトル ee を状態とした推定
偏差システムは次式のように表される.

5c

gwc

World
Frame

5w

gwo

gwz
5z

gwh
gzh

b
b
Vee
= −Ad(gee−1 ue + Vwo
)

gco

(4)

同様に, カメラ座標系からみた観測対象の相対位置姿勢の推

gch
gho

5o
Target Object

定値と目標相対位置姿勢の偏差をカメラ制御偏差とし, gec :=−1
gcdḡco と定義する. カメラ制御偏差に対するカメラ制御偏差ベ
ξ̂θec TT)] と定義するとき, カメラ制御偏クトルを ec := [pT
ec eR (e5h差ベクトル ec を状態にもつカメラ制御偏差システムは
b
b
b
Vec
= −Ad(ḡ−1 ) Vwc+ ue − Ad(g−1 ) Vcd
co

ec

(5)

b
として表される. ここで Vcd
はカメラ座標系からみた観測対象

Fig. 2:視覚フィードバックシステムの相対位置姿勢 gco の目標速度である.
座標系からみた観測対象の相対位置姿勢 gco と手先座標系からみた観測対象の相対位置姿勢 gho を, それぞれの目標相対位置
姿勢である gcd と ghd に常に一致させておくことである. ここで, 同次表現 gab や 演算子 ∧(wedge) の詳しい説明については
文献

7)を参照していただきたい.

2.3

Eye-in-Hand 構造の動的視覚フィードバックシステム

nc 自由度を有するカメラマニピュレータのマニピュレータダ
イナミクスを次式のように表すこととする.

Mc (qc )q̈c + Cc (qc , q̇c )q̇c + gc (qc ) = τc + τcd(6)

qc , q̇c , q̈c ∈ Rnc はそれぞれの各関節の角度, 角速度, 角加速度

はじめに視野をより広く得るために Eye-in-Hand 構造の動を表し, τc ∈ Rnc は入力トルク, τcd ∈ Rnc はトルク外乱,

的視覚フィードバックシステムについて議論する. Eye-in-Hand

Mc (qc ) ∈ Rnc ×nc は正定な慣性行列, Cc (qc , q̇c )q̇c ∈ Rnc は遠構造の動的視覚フィードバックシステムはカメラマニピュレータ

心力・コリオリ力項, gc (qc ) ∈ Rnc は重力項を表す.

と Σw , Σc , Σo の三つの座標系のみが用いられ, 制御目的は gco
を gcd に常に一致させておくこととなる. 視覚フィードバック

また, カメラの速度はマニピュレータヤコビアン Jcb (qc ) を用いることでつぎのように表される.

システムにおける三つの座標系間に成り立つ剛体運動の表現をb
Vwc= Jcb (qc )q̇c導出する. まず, カメラ座標系からみた観測対象の相対位置姿勢(7)

一方, カメラの速度の目標値を ucd とし, カメラマニピュレータ

gco は
−1
gco = gwc
gwoの関節角速度の目標値を q̇cd で表すとすると, (7) 式と同様にし(1)てマニピュレータヤコビアンを用いることで ucd = Jcb (qc )q̇cdbbで表される. 一方, カメラの速度を Vwc, 観測対象の速度を Vwoと表すことができる. カメラマニピュレータの関節角速度に関すと定義すると, カメラ座標系からみた観測対象の相対的な位置姿る偏差を ξc := q̇c − q̇cd と定義する.

bは, (1) 式を時間微分することで, gco の基勢に対する速度 Vco

本式として

τc = Mc (qc )q̈cd + Cc (qc , q̇c )q̇cd + gc (qc )
b
b
b
Vco
= −Ad(gco−1 Vwc + Vwo
)

(8)

cd

を考える. q̇cd , q̈cd はそれぞれ関節角速度と関節角加速度の目標値である. また, このとき, (4)–(8) 式を用いることで, Eye-in-Hand

.

2.2

T
(qc )AdT(g−1 ) ec + uξc
+Jcb

(2)

と導かれる. ここで Ad(gab ) は同次表現 gab の随伴写像である
7)

このとき, カメラマニピュレータへの入力トルクとして推定偏差システムとカメラ制御偏差システム

本節は推定偏差システムとカメラ制御偏差システムについて述べる 5) . カメラから得られる視覚情報 f は, 相対位置姿勢 gcoを直接得ることができないため, その推定値 ḡco を得るためにオブザーバを構成する. (2) 式の基本式に基づき, 推定値 ḡco の運動モデルを

構造の動的視覚フィードバックシステムは
3
2
3 2
T
AdT(g−1 ) ec
−Mc−1 Cc ξc + Mc−1 Jcb
ξ̇c
cd
7
6
b 5
4 Vec
=4
5
−Ad(ḡco−1 Jcb ξc
)
b
Vee
0
2 −1
3
2 −1 3–Mc00
Mc 0 »
τcd
6 0 −Ad −17I+45ueye +4 0 0 5 b (9)(ḡco )Vwo
0
0
−Ad(g−1 )
0 I
ee

b
b
= −Ad(ḡcoV̄co−1 Vwc + ue)

(3)と構成する. ue は推定偏差の振る舞いを安定にするために加えられる入力である.

で表される. ただし,

2

3
uξc
b 5
ueye := 4 ucd + Ad(gcd ) Vcd
ue

(10)

つぎに, カメラ座標系から観測対象への相対位置姿勢の真値で定義される. 詳細は省略するが Eye-in-Hand 構造の動的視覚−1gco と定義する. こと推定値の偏差を推定偏差とし, gee := ḡco

フィードバックシステム (9) は入力 ueye からある出力 νeye にTξ̂θee Tの推定偏差に対する推定偏差ベクトルを ee := [pT)]ee eR (e対して受動性を有する 5) .

3
3.1

動的可動カメラを用いた視覚フィードバック制御を考える. q̇hd , q̈hd はそれぞれ関節角速度と関節角加速度の目標値である. また, uξ は後に提案する新たな入力である. このと手先制御偏差システム

き, (9)(13)–(16) 式を用いることで, 動的可動カメラを用いた視

本章ではカメラマニピュレータに加えてワークマニピュレー
タも考慮に入れた動的可動カメラ構造の視覚フィードバックシ
ステムに対して議論する.
手先から観測対象への相対位置姿勢の推定値はカメラから観測対象への相対位置姿勢の推定値を用いることにより次式のように表すことができる.
−1ḡcoḡho = gch

(11)

−1
ここで gch = gwc
gwz gzh である. gwc , gzh が各マニピュレータの関節角度から, また gwz がシステムの構造上の特徴から得ることができるために gch も観測可能となる. (11) 式を時間微分

覚フィードバックシステムは
2
3
T
2
3
AdT(g−1 ) ec
−Mc−1 Cc ξc + Mc−1 Jcb
ξ̇c
cd
6
7
T
6 ξ̇h 7 6 −Mh−1 Ch ξh + Mh−1 Jhb
AdT(g−1 ) eh 7
6 b 7 67hd
6 Vec 7 = 6
7−Ad(ḡco−1 Jcb ξc
6
7 6
7
)
7
4 Vb 5 6
ee
0
4
5
b
Veh
−Ad(ḡ−1 ) Jhb ξh
ho
3 2
2 −1
3
0
0
0
0
Mc
Mc−1 0 0
−1
7
6 0 M
−1
0
0
0
h
0 Mh 07
7 6
6
7
7 6
6
I
0
0 −Ad(ḡco
−1
+6 0
7u+6
0
0 07
)
6
7w
7
6 0
0
0
0
−Ad(gee
−1
0 I5
5 4 0
4
)
0
0
0
I
−Ad(ḡ−1 )
0
0 0
ho

(17)

することで手先からみた観測対象の速度の推定値はbb= −Ad(ḡ−1 ) Vwh
+ ue
V̄ho

(12)

ho

で表される. ただし,
2

3
uξc
3
2
7
6
uξh
τcd
7
6
b
7
4 τhd 5u := 6
6 ucd + Ad(gcd ) Vcd 7 , w :=
b
5
4
ue
Vwo
b
uhd + Ad(ghd ) Vhdbと導かれる. ここで Vwh
はワークマニピュレータの手先の速度である.
手先座標系からみた観測対象の相対位置姿勢の推定値と目標−1
相対位置姿勢の偏差を手先制御偏差とし, geh := ghd
ḡho と定(18)

義する. 手先制御偏差に対する手先制御偏差ベクトルを eh :=で定義される. 動的視覚フィードバックシステムにおける制御[pTeh eTR (eξ̂θeh )]T

目的は, カメラから観測対象への相対位置姿勢と手先から観測と定義するとき, 手先制御偏差ベクトル eh を

対象への相対位置姿勢を常に一致させておくことであり, 状態状態にもつ手先制御偏差システムは
b
b
b
Veh
= −Ad(ḡ−1 ) Vwh+ ue − Ad(g−1 ) Vhdhoとして表される.

(13)

eh

b
ここで Vhdは手先座標系からみた観測対象の相対位置姿勢 gho の目標速度である.

3.2

x := [ξcT ξh T eTc eTe eTh ]T を 0 に留めておくことで制御目的が達成される.

3.3

動的視覚フィードバックシステムの入出力間の関係制御目的を達成するための制御則を提案するまえに, 制御則

動的可動カメラを用いた視覚フィードバックシステム

nh 自由度を有するワークマニピュレータのマニピュレータダ
イナミクスを次式のように表すこととする.

Mh (qh )q̈h + Ch (qh , q̇h )q̇h + gh (qh ) = τh + τhdの構成に対して重要な役割を果たす動的視覚フィードバックシ
ステムの有する性質を示す.
補題 1 外乱がない (すなわち w = 0) とする. このとき, 出力を(14)

マニピュレータこそ異なるが, ワークマニピュレータの各記号
は, カメラマニピュレータの各記号と同じ意味をそれぞれ表して
いる.

3
I 0
0
0
0
7
60I
0
0
0
7
6
7
6 0 0 −AdT −1 00)(gν := N x, N :=6
7 (19)
cd
6 0 0 Ad
−I Ad(e−ξ̂θeh ) 754(e−ξ̂θec )T00
00 −Ad(g−1 )2

hd

また, 手先の速度はマニピュレータヤコビアン Jhb (qh ) を用とするとき, 動的視覚フィードバックシステム (17) の入出力間にいることでつぎのように表される.

Z Tb= Jhb (qh )q̇h
Vwh

(15)

一方, 手先の速度の目標値を uhd とし, ワークマニピュレータの
関節角速度の目標値を q̇hd で表すとすると, (15) 式と同様にしてマニピュレータヤコビアンを用いることで uhd = Jhb (qh )q̇hdと表すことができる. ワークマニピュレータの関節角速度に関する偏差を ξh := q̇h − q̇hd と定義する.

0uT νdτ ≥ −β0 , ∀ T > 0(20)が成り立つ. ただし β0 はある非負の定数である.
補題 1 はエネルギー関数

V =

1 T
1
ξc Mc ξc + ξhT Mh ξh + E(gec ) + E(gee ) + E(geh )
2
2
(21)

このとき, ワークマニピュレータへの入力トルクとしてを用いて, Ṁc − 2Cc , Ṁh − 2Ch の歪対称性と p̂ec , p̂ee と p̂eh のτh = Mh (qh )q̈hd + Ch (qh , q̇h )q̇hd + gh (qh )T(qh )AdT(g−1 ) eh + uξh .
+Jhbhd

歪対称性を利用することで証明される. ここで E(g) := 12 p2 +(16)φ(eξ̂θ ) であり, 関数 φ(eξ̂θ ) は回転行列に対する誤差関数を表す

正定関数である. この補題は, (17) 式の動的視覚フィードバック

ただし

システムが受動性を有していることを示唆している.

l=

この動的可動カメラを用いた視覚フィードバックシステム (17)bは, カメラ速度 Vwc= 0, カメラ速度の目標値 ucd = 0, 基準座

ke (γ 2 + 1)
γ 2 (2ke − 1) − 1(28)とする. このとき, 制御性能解析に関するつぎの定理が示される.

標系からみた台座の相対位置姿勢 gwz = I, さらにカメラ制御
偏差 ec とカメラマニピュレータを考慮しなかったとき, 文献 6)

定理 2 与えられた γ に対して, (23)–(27) 式を満たすようにゲで示した固定カメラ構造の動的視覚フィードバックシステムとイン Kξh , Kξc , Kh, kc , ke を選ぶとき, (17)(22) 式で構成され同一のものとなる. また 2 章より Eye-in-Hand 構造の動的視

る動的視覚フィードバックシステムは γ 以下の L2 ゲインを有覚フィードバックシステム 5) が含まれていることは明らかであする.

る. したがって Eye-in-hand 構造と固定カメラ構造の動的視覚
フィードバックシステムは, 動的可動カメラを用いた視覚フィー
ドバックシステム (17) の特殊な場合となる.

3.4

定理 2 は (21) 式のエネルギー関数を蓄積関数とみなすことで証明される. この制御性能解析では観測対象の運動を外乱として捉えているために, γ が小さいコントローラであれば観測対象

動的視覚フィードバック制御則と安定性

動的視覚フィードバックシステム (17) に対し, 外乱がない (すなわち w = 0) 場合に平衡点 x = 0 を安定とする制御則としての運動が状態に与える影響がより少ないことを示しており, γ を外乱抑制レベルを示す指標とみなすことができる.

次式を提案する.

4u = −Kν = −KN x, K := diag{Kξc , Kξh , Kc , Ke , Kh }

本稿ではカメラマニピュレータとワークマニピュレータとい

(22)
:=

Kξc

diag{kξc 1 , · · · , kξc nc }diag{kξh 1 , · · · , kξh nh }

と

Kξh

おわりに

う二つのマニピュレータを用いることにより, 従来研究で提案し:=ていた Eye-in-Hand 構造と固定カメラ構造の二つの視覚フィーは カ メ ラ マ ニ ピュレ ー タ と ワ ー

ドバックシステムを特殊な場合として含むような, 動的可動カメ

ク マ ニ ピュレ ー タ の 各 関 節 に 対 す る ゲ イ ン で あ り,ラを用いた視覚フィードバックシステムを構成した. さらに閉

Kc := diag{kc1 , · · · , kc6 }, Ke := diag{ke1 , · · · , ke6 } とループ系の漸近安定性と L2 ゲイン制御性能解析をおこなった.

Kh := diag{kh1 , · · · , kh6 } は x 軸, y 軸, z 軸の並進と回転におけるカメラ制御偏差と推定偏差と手先制御偏差に対するゲイ

参考文献

ンである. ただし, ゲインにおける各要素はすべて正とする. こ

1) S. Hutchinson, G. D. Hager and P. I. Corke, “A Tuto-のとき, 補題 1 で示した動的視覚フィードバックシステムの受

rial on Visual Servo Control,” IEEE Trans. Robotics and

動性に基づくことで, 安定性に関してつぎの定理が導かれる.

Automation, Vol. 12, No. 5, pp. 651–670, 1996.

定理 1 w = 0 のとき, 動的視覚フィードバックシステム (17) と

2) E. Malis, F. Chaumette and S. Boudet, “2-1/2-D Vi-(22) 式の入力で構成される閉ループ系の平衡点 x = 0 は漸近安

sual Servoing,” IEEE Trans. Robotics and Automation,

定である.

Vol. 15, No. 2, pp. 238–250, 1999.

この定理 1 は (21) 式のエネルギー関数をリアプノフ関数候補

Approach to Image-Based Visual Servo Control,” IEEEとすることにより示される.

3.5

3) P. I. Corke and S. A. Hutchinson, “A New Partitioned
Trans. on Robotics and Automation, Vol. 17, No. 4, pp.

L2 ゲイン制御性能解析

507–515, 2001.

本節では外乱が存在する場合について考察する. 特に, 本稿では外乱抑制問題を考えることで L2 ゲイン制御性能解析をおこなう.
ここで, 正のスカラーの定数 kc と ke を用いることによって,

Kc = kc I, Ke = ke I とゲインを定義しなおす. また, ある正の
1
1
I− I>02γ 2
2
11Kξc − 2 I − I > 02γ
2(2kc − 1)l1
I >0
Kh − I −2
2kc − 1 − 2l
1kc − − l > 0211
ke − 2 − > 02γ2“Keeping Features in the Field of View in Eye-In-Hand
Visual Servoing: A Switching Approach,” IEEE Trans.
on Robotics, Vol. 20, No. 5, pp. 908–913, 2004.
5) 河合, 村尾, 藤田, “受動性に基づく動的視覚フィードバック

数 γ を用いてつぎの行列を定義する.

Kξh −4) G. Chesi, K. Hashimoto, D. Prattichizzo and A. Vicino,

(23)
(24)

制御の安定性と L2 ゲイン制御性能解析,” 第 32 回 制御理論シンポジウム 資料, pp. 23–26, 2003.

6) 村尾, 河合, 藤田, “受動性に基づく視覚フィードバック制
御の固定カメラ構造への展開,” 第 4 回 制御部門大会 資料,(25)

pp. 327–330, 2004.

(26)

7) R. Murray, Z. Li and S. S. Sastry, A Mathematical In-(27)

troduction to Robotic Manipulation, CRC Press, 1994.