産業技術大学院大学紀要

No.1, pp.141－6 (2007)〔論文〕

受動性に基づく可動カメラ構造の視覚フィードバック制御
村 尾 俊 幸* ・河

合 宏

之**・藤

田 政 之***

Passivity-based Control of Visual Feedback Systems
with Movable Camera Configuration
Toshiyuki Murao*, Hiroyuki Kawai** and Masayuki Fujita***

Abstract

This paper deals with control of visual feedback systems with a movable camera configuration. Thisconfiguration consists of a robot manipulator and a camera that is attached to the end-effector ofanother robot manipulator. This system which can be interpreted the visual feedback system with aneye-in-hand configuration and a fixed camera one as a special case, can enlarge the field of view.
Firstly the visual feedback system with an eye-to-hand configuration is given with the fundamental
representation of a relative rigid body motion. Next, we construct the visual feedback system with a
movable camera configuration by combining the camera control error system. Finally, we derive thepassivity of the visual feedback system. Based on the passivity, stability and L2 gain performance
analysis are discussed.
Keywords: Visual feedback control, Robot control, Passivity, Lyapunov stability, L2 gain performance

1. はじめに
近年，視覚フィードバック制御は，工場だけでなく医療分野などへも応用され始めており，より複雑なシステ
ムや幅広い分野へ適用されるようになってきている．この視覚フィードバック制御は，よく知られているように二つの制御方法と二つのカメラ構造に大別できる．すな
わち，制御方法は位置ベース法と画像ベース法，カメラ
構造はカメラがハンドの手先に取り付けられた

図 1 可動カメラ構造の視覚フィードバックシステム

Eye-in-Hand 構造と，カメラとハンドが別々に配置された固定カメラ構造である[1][2]．

メラを用いた Eye-to-Hand 構造により積極的に視野をさらに，Eye-in-Hand 構造では作業スペースを全て観得る手法が効果的となる[3]．また，医療分野では穿刺手察するにはカメラ視野が限定され，固定カメラ構造では術マニピュレータなどのメディカルロボットの開発が近作業スペースを探索するには不十分であるという理由か

年特に望まれており，作業するマニピュレータの位置制

ら，二つのカメラ構造を発展させたシステムが研究され御を行いながら，視野を確保するために腹腔鏡などを用始めている．その中の一つに，固定カメラ構造のようにいて広範囲をモニタリングする必要が出てくる[4]．このカメラとハンドが別々に配置されているが，カメラが固ような可動カメラを用いた研究としては，可視度を用い定されているとは限らない Eye-to-Hand 構造が挙げらた可動カメラの位置決め[3]，カメラ視野などの指標を用れる（固定カメラ構造はこの特殊ケースとなる）．実際，いた制御[5]，リアルタイム性を強く意識した手法[6]など

工場では溶接作業や車の塗装のような散乱した場所におが挙げられる．しかし，これらの論文は安定性や観測対

ける繰り返しの多い作業が頻繁に行われており，可動カ――――――――――――――――――――――――――――――――――――――――――――――――――――
Received on May 10, 2007
*産業技術研究科，School of Industrial Technology, AIIT**金沢工業大学，工学部，College of Engineering, Kanazawa Institute of Technology***東京工業大学，理工学研究科，Graduate School of Science and Engineering, Tokyo Institute of Technology

142

村尾俊幸，河合宏之，藤田政之

象が運動する場合における議論も十分になされていない．
そこで本研究では，図 1 に示すような可動カメラ構造に，観測対象が未知な運動をするために，g wo ，g ho ，g co
は未知情報となる．

の視覚フィードバックシステムに対して，手先にカメラ

本節では準備として Eye-to-Hand 構造の視覚フィーを取り付けたマニピュレータ（カメラマニピュレータ）ドバックシステムについて議論する．Eye-to-Hand 構造と，作業を行うマニピュレータ（作業マニピュレータ）の制御目的は手先座標系からみた観測対象の相対位置姿を一つのシステムとして捉え，我々が提案している

勢 g ho をその目標位置姿勢 g hd に常に一致させておくこ

Eye-in-Hand 構造[7]と固定カメラ構造[8]の視覚フィーとである．しかし前述したように，手先座標系からみたドバックシステム同様，受動性に基づくことで安定性や観測対象の相対位置姿勢 g ho を直接観測することはでき

制御性能解析について議論する．

ない．そこでまずはカメラからみた観測対象 g co の運動

本論文の構成は以下のとおりである．2 節では固定カについて考える．

メラ構造も含まれる Eye-to-Hand 構造の視覚フィード

カメラ座標系からみた観測対象の相対位置姿勢 g co はバックシステムについて述べる．3 節では，カメラ制御−1g co = g wc
g wo (2)

偏差システムを組み合わせることにより可動カメラ構造の視覚フィードバックシステムを構成した後に，制御則を提案し安定性と L2 ゲイン制御性能解析を行う．最後で表される．また，カメラ座標系からみた観測対象の相対速度は(2)式を時間微分することで， g co の基本式としてに 4 節でまとめを行う．

b(3)
Vcob = − Ad ( g −1 )Vwcb + Vwoco

2. Eye-to-Hand 構造の視覚フィードバックシステムのように導かれる．ここで， Vabb は剛体運動のボディ速度を表しており， Ad ( g ) は同次表現 g ab の随伴写像であab

2.1

視覚フィードバックシステムにおける剛体運動の表現
本論文では，図 1 に示すような，カメラマニピュレー
タと作業マニピュレータの二台のマニピュレータを有し，五つの座標系によって表される視覚フィードバックシス

る[9]．この(3)式は厳密ではないが，カメラ座標系からみb
た観測対象の相対速度 Vcob がカメラの速度 Vwcと観測対bの差で表されることを意味している．(3)式
象の速度 Vwobにおいて，Vwc= 0 と考えると固定カメラ構造となる．導

出の詳細は文献[8]を参照．

テムについて考える．図中の五つの座標系をそれぞれ作
業マニピュレータの台座に基準座標系 Σ w ，手先に手先座

2.2

非線形オブザーバと推定偏差システム

標系 Σ h ，カメラマニピュレータの台座に台座座標系 Σ z ，

本節は推定偏差システムについて述べる．本システム

手先にカメラ座標系 Σ c また観測対象に観測対象座標系

では一台のピンホールカメラから画像特徴量を得ること

Σ o を定義する．ここで， g ab は同次表現と呼ばれ，任意を考えている．しかし，カメラから得られる視覚情報 fの二つの座標系 Σ a から Σb の位置 pab と姿勢を表す回転は相対位置姿勢 g co を含んではいるが，二次元情報であ行列eξˆθ abから構成され，4×4 行列として以下のように定義される．

⎡eg ab = ⎢⎣⎢ 0ξˆθ abpab ⎤ (1)⎥1 ⎦⎥例えば，基準座標系 Σ w からみた手先座標系 Σ h の位置
姿勢は g wh となる．
本システムにおいて， g wh ， g zc は各マニピュレータの関節角度などから得られるとする．また各マニピュレー
タの台座は動かないことから g wz が同定可能である．さらに，同次表現の合成則から g wc ，g ch を求めることが可能であり，以上の五つの位置姿勢が既知情報となる．逆るため直接 g co を得ることはできない．そこで，相対位置姿勢の推定値 g co を得るために，非線形オブザーバを構成する．(3)式に基づき，推定値 g co の運動モデルをつぎのように構成する．

Vcob = − Ad ( g −1 )Vwcb + ue (4)coue は推定偏差の振舞いを安定にするために加えられる入力であり，後に制御則の一部として提案する．
ここで，相対位置姿勢の真値と推定値の偏差 g ee を

g ee := g co−1 g co (5)と定義する．さらに，この推定偏差に対する推定偏差ベ

受動性に基づく可動カメラ構造の視覚フィードバック制御
ˆeRT (eξθ ee )]T と 定 義 す る ． た だ しˆˆ1eR (eξθ ) := sk (eξθ )∨ ， sk (eξˆθ ) := (eξˆθ − e −ξˆθ ) である．こ2の推定偏差ベクトル ee は，カメラから得られる視覚情報

T
ク ト ル を ee := [ peef と推定モデルにより得られる視覚情報 f ，さらには画像ヤコビアンの擬似逆行列 J†( g co ) を用いることで

ee = J†( g co )( f − f ) (6)143

象の相対位置姿勢 g ho の目標速度である．
2.4

Eye-to-Hand 構造の視覚フィードバックシステム(7)式の推定偏差システムと(11)式の手先制御偏差シス
テムを組み合わせることで，Eye-to-Hand 構造の視覚フィードバックシステムは次のように表すことができる．

⎡Vehb ⎤ ⎡− Ad ( geh−1 )⎢ b⎥ = ⎢ 0⎣Vee ⎦ ⎣⎢として導かれる[8]．この推定偏差ベクトル ee が 0 になれば，相対位置姿勢の真値と推定値が一致することとな

I
⎤
⎡0 ⎤
u + V b (12)− Ad ( g −1 ) ⎥⎥ he ⎢⎣ I ⎥⎦ woee ⎦で表される．ただし，る．

を用いて，推定偏差システムを構成する．(5)式を時間微⎡ Ad −1 V b + Vhdb ⎤ (13)uhe := ⎢ ( ghd ) wh
⎥
ue
⎣
⎦

分し (3)，(4)式を用いることで，推定偏差システムとしで定義される．Eye-to-Hand 構造の動的視覚フィードバてックシステムにおける制御目的は，運動する観測対象に剛体運動の基本式(3)と相対位置姿勢の推定モデル(4)Veeb = − Ad ( g −1 )ue + Vwob (7)ee

作業マニピュレータの手先を追従させることであり，状態 eh および ee を 0 にとどめておくことで制御目的が達が導出される．

成される．
2.3

手先制御偏差システム(注意) 本節ではカメラの動きについては議論していつぎに実際に制御目的を達成するための手先制御偏ない． カメラの動きを考えない場合， Eye-to-Hand 構

差について述べる．手先座標系からみた観測対象の相対造の視覚フィードバックシステム(12)は，文献[8]の固定

位置姿勢 g ho も，g co が直接求められないために得ること

カメラ構造の視覚フィードバックシステムと一致する．

ができない．したがって，その推定値 g ho を求めることを考え， g co を用いてつぎのように表す．
−1chg ho := g g co (8)3. 可動カメラ構造の視覚フィードバック制御
前節では Eye-to-Hand 構造の視覚フィードバック制b
相対位置姿勢 g ho の基本式は，手先の速度を Vwhとす

ると，(8)式より以下の式のように導かれる．

Vhob = − Ad ( g −1 )Vwhb + ue (9)ho

ここで，相対位置姿勢の推定値 g ho と手先座標系から御について述べてきた．しかし前節のシステムでは，カbを入力として考慮していなかったため，観メラ速度 Vwc

測対象が視野から外れてしまう可能性がある．例えば文b
献[8]では Vwc= 0 とした Eye-to-Hand 構造の特殊ケー

みた観測対象の相対位置姿勢の目標値 g hd との偏差を手スである固定カメラ構造に対して議論した．一般的に固先制御偏差 g eh として

定カメラ構造は Eye-in-Hand 構造に比べて大域的な視野を得ることができるとされているものの，観測対象が−1
g eh := g hd
g ho (10)大きく動くようなシステムにおいてはより広いカメラ視で定義する．また，推定偏差ベクトルと同様に，手先制T
御偏差ベクトルを eh := [ pehˆeRT (eξθeh )]T と定義する．

導出した相対位置姿勢 g ho の運動モデルおよび相対位
置姿勢の目標値を用いて，手先制御偏差システムを構成する．(7)式の推定偏差システムの導出と同様に，(10)式を時間微分することで， 手先制御偏差システムは野が必要となる．
b
本節では可動カメラの利点を活かし，カメラ速度 Vwc

を決め，システムの入力とすることでカメラが観測対象を視野から外さないようにする．そこで図 1 に示すように，カメラをマニピュレータの手先に取り付け制御することを考える．

Vehb = − Ad ( g −1 )Vwhb + ue − Ad ( g −1 )Vhdb (11)
ho

eh

b
は手先座標系からみた観測対
hdと導出される．ここで V

3.1

カメラ制御偏差システム

本論文の制御目的は前述の g ho を g hd に常に一致させ

144

村尾俊幸，河合宏之，藤田政之ておくことに加え，カメラが観測対象を外さないように，できる．図 2 に可動カメラ構造の視覚フィードバックシ

カメラ座標系からみた観測対象の相対位置姿勢 g co をそ

ステムのブロック図を示す．

の目標位置姿勢 g cd に常に一致させておくことである．
しかし，前節で述べたように g co を直接得ることはできないため，制御目的を達成するためにその推定値 g co を用いる．相対位置姿勢の推定値 g co とカメラ座標系からみた観測対象の相対位置姿勢の目標値 g cd との偏差をカ
メラ制御偏差 g ec として

g ec := g cd−1 g co (14)で定義する．また，カメラ制御偏差ベクトルを

ec := [ pecT

図 2 可動カメラ構造の視覚フィードバックシステム

ˆeRT (eξθec )]T と定義する．(11)式の手先制御偏のブロック図差システムの導出と同様に，(14)式を時間微分することで， カメラ制御偏差システムは
3.3

Vecb = − Ad ( g −1 )Vwcb + u e − Ad ( g −1 )Vcdb (15)co視覚フィードバックシステムの入出力間の関係

制御則を提案するために，視覚フィードバックシステ

ecb
はカメラ座標系からみた観測
cdと導出される．ここで Vムの有する性質を示す．

対象の相対位置姿勢 g co の目標速度である．

b
補題 1 観測対象が静止している（すなわち Vwo= 0 ）とする．このとき，出力を3.2

可動カメラ構造の視覚フィードバックシステム

ν hce := N hce e (18)(12)式の Eye-to-Hand 構造の視覚フィードバックシ⎡ −I⎢
N hce := ⎢ 0⎢ Ad⎢⎣ ( e −ξˆθ eh )ステムと(15)式のカメラ制御偏差システムを組み合わせることで，可動カメラ構造の視覚フィードバックシステ
ムは次のように表すことができる．

⎡Vehb ⎤ ⎡− Ad ( g −1 )eh⎢ b⎥ ⎢
0⎢Vec ⎥ = ⎢⎢Veeb ⎥ ⎢
0
⎣ ⎦ ⎣

0
− Ad ( g −1 )
0

⎤
⎡0 ⎤
⎥
⎢ ⎥ b
+
I
u
⎥ hce ⎢0⎥Vwo⎢⎣ I ⎥⎦− Ad ( g −1 ) ⎥
ee ⎦
I

ec

⎡ Ad ( g −1 )Vwhb + Vhdb ⎤hd⎥⎢u hce := ⎢ Ad ( g −1 )Vwcb + Vcdb ⎥ (17)
cd
⎥
⎢
ue
⎦
⎣
で定義される．可動カメラ構造の視覚フィードバックシ
ステムにおける制御目的は，カメラから観測対象への相対位置姿勢と手先から観測対象への相対位置姿勢を常にそれぞれの目標位置姿勢に一致させておくことであり，状態 e := [ehT

ecT

本システムはカメラが可動式であるために，固定カメ
ラ構造に比べてカメラ視野を広くすることが可能であり，それによりロボットの作業スペースをより広げることがAd ( e −ξˆθ ec )出力間にT

∫ u ν dτ ≥ −β , ∀T > 0 (20)
T
hce

0

hce

hce

が成り立つ．ただし β hce はある非負の定数である．
(証明) エネルギー関数として

Vhce = E ( g eh ) + E ( g ec ) + E ( g ee ) (21)を考える．ここで E ( g ab ) := 1 p ab 2 + φ (e ξˆθ ab ) であり，

2関数 φ (eξˆθ ab) は回転行列に対する誤差関数を表す正定関数である．視覚フィードバックシステムのもつ歪対称性から，エネルギー関数 Vhce の時間微分は
T
T
V&hce = e T N hce
u hce = u hce
ν hce (22)eeT ]T を 0 に留めておくことで制御目的が達成される．

−I

0⎤⎥0 ⎥ (19)− I ⎥⎥⎦とするとき，(16)式の視覚フィードバックシステムの入(16)で表される．ただし，0となる．(22)式の両辺を積分することで
T

∫u ν
0

T
hce

hce

dτ = Vhce (T ) − Vhce (0) ≥ −Vhce (0) := − β hce (23)が成り立つ． β hce は初期状態のみに依存するある非負の定数である．

□

受動性に基づく可動カメラ構造の視覚フィードバック制御

145

この補題は，(16)式の視覚フィードバックシステムがータを一つのシステムとして含めた可動カメラ構造の視受動性を有していることを示唆している．この可動カメ

覚フィードバックシステムに対し，Lyapunov の安定定

ラ構造の視覚フィードバックシステム(16)は，作業マニ

理によりその安定性を示した．この安定性解析は補題 1

ピュレータを考慮しないとき，Eye-in-Hand 構造の視覚で示した受動性に基づいて行われている．

フィードバックシステムとみなすことができる．具体的bには，手先速度 Vwh= 0 ，基準座標系からみた台座の相対位置姿勢 g wz = I ，さらに手先制御偏差 eh と作業マニ
ピュレータを考慮しない特別な場合が，文献[7]で示した
Eye-in-Hand 構造の視覚フィードバックシステムとな

3.5

L2 ゲイン制御性能解析

本節では観測対象が運動している場合に，観測対象の動きを外乱とみなし，外乱抑制問題を考えることで L2ゲイン制御性能解析を行う．

る．また 2 節より固定カメラ構造の視覚フィードバッ

ここで，正のスカラーの定数 k c と k e を用いることによクシステム[8]が含まれていることは明らかである．したって， K c = k c I ， K e = k e I とゲインを定義しなおす．

がって可動カメラ構造の視覚フィードバックシステム

また，ある正の数 γ を用いてつぎの条件を与える．

(2k c − 1)l1
Kh − I −I > 0 (26)2
2k c − 1 − 2l(16)は Eye-in-Hand 構造と固定カメラ構造という一般
的によく知られている二つのカメラ構造の視覚フィード
バックシステムを特殊な場合として含んだ形となる．
3.4

kc −

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

ke −視覚フィードバックシステム(16)に対し，観測対象がb
静止している(すなわち Vwo= 0 )場合に平衡点 e = 0 を安l=u hce = − K hceν hce ， K hce := diag{K h , K c , K e } (24)K c := diag{k c1 ,L k c 6 }と

1
1− > 0 (28)2γ 2 2

ただし定とする制御則として次式を提案する．

K h := diag{k h1 ,L k h 6 } ，

1− l > 0 (27)2k e (γ 2 + 1)(29)
γ 2 (2k e − 1) − 1とする．このとき，制御性能解析に関するつぎの定理がK e := diag{k e1 ,L k e 6 } は x 軸， y 軸， z 軸の並進と回転示される．

における手先制御偏差とカメラ制御偏差と推定偏差に対定理 2 与えられた γ に対して，(26)～(28)式を満たすよするゲインである．ただし，ゲインにおける各要素はすうにゲイン K h ， k c ， k e を選ぶとき，(16)，(24)式で構

べて正とする．

成される視覚フィードバックシステムは γ 以下の L2 ゲ

このとき，補題 1 で示した視覚フィードバックシステ
ムの受動性に基づくことで，安定性に関してつぎの定理

インを有する．
紙面の都合上証明は省略するが，(21)式のエネルギーが導かれる．

関数を蓄積関数としてみなすことで，定理 2 は証明されb定理 1 観測対象が静止している（すなわち Vwo= 0 ）とる．

き視覚フィードバックシステム(16)と(24)式の入力で構

この制御性能解析では観測対象の運動を外乱として成される閉ループ系の平衡点 e = 0 は漸近安定である．

とらえているために， γ が小さいコントローラであれば(証明) (21)式のエネルギー関数を Lyapunov 関数候補

観測対象の運動が状態に与える影響がより少ないことをとすると，(16)，(24)式の解軌道に沿った時間微分は(22)示している．本論文では， H ∞ 制御における最も基本と式よりなる外乱抑制問題を考えたが，適切な一般化プラントを構成することで，外乱抑制問題以外にも拡張可能である．

T
T
V&hce = u hce
ν hce = −e T N hce
K hce N hce e (25)と導かれる．ゲイン K h ，K c ，K e の正定性より行列 K hce
が正定であり， N hce が正則であるため，システムの平衡点 e = 0 は漸近安定となる．

4. おわりに□

本論文では，二台のマニピュレータを用いた可動カメ

定理 1 では，カメラマニピュレータと作業マニピュレラ構造の 3 次元視覚フィードバックシステムを構成し

146

村尾俊幸，河合宏之，藤田政之

た．ロボットの作業スペースをより広げるために，カメラを固定させずに可動カメラを用いた本システムは，
Eye-in-Hand 構造と固定カメラ構造の両構造を特殊な場合として含んでいる．視覚フィードバックシステムが有する受動性に基づいて制御則を提案し，Lyapunov の安定定理により安定性解析を行った．また，観測対象が運動する場合に外乱抑制問題を考えることで，L2 ゲイン
制御性能解析を行った．

参考文献[1] S. Hutchinson, G. D. Hager and P. I. Corke, “A
Tutorial on Visual Servo Control,” IEEE Trans.
Robotics and Automation, Vol. 12, No. 5, pp.
651-670, 1996 (10).
[2] F. Chaumette and S. Hutchinson, “Visual Servo
Control, Part I: Basic Approaches,” IEEE
Robotics & Automation Magazine, Vol. 13, No. 4,
pp. 82-90, 2006 (12).
[3] R. Sharma and S. Hutchinson, “Motion
Perceptibility and its Application to Active
Vision-based Servo Control,” IEEE Trans.
Robotics and Automation, Vol. 12, No. 5, pp.
651-670, 1996 (10).
[4] 下山 勲ら, “ロボットフロンティア,” 岩波書店,
2005 .
[5] B. J. Nelson and P. K. Khosla, “Visually Guided
Manipulation
using
ActiveCamera-Lens
Systems,” Proc. of the 1997 IEEE International
Conference on Robotics and Automation, pp.
437-442, 1997 (4).
[6] A. Muis and K. Ohnishi, “Eye-to-Hand Approachon Eye-in-Hand Configuration within Real-Time
Visual Servoing,” IEEE/ASME Trans. Mechatronics, Vol. 10, No. 4, pp. 404-410, 2005 (12).
[7] M. Fujita, H. Kawai and M. W. Spong,“Passivity-based Dynamic Visual Feedback
Control for Three Dimensional Target Tracking:Stability and L2 gain Performance Analysis,”IEEE Trans. Control Systems Technology, Vol.
15, No. 1, pp. 40-52, 2007 (1).
[8] 河合宏之, 村尾俊幸, 東 剛人, 藤田政之, “受動性に基づく 3 次元動的視覚フィードバック制御の固定カメラ構造への展開,” システム制御情報学会
論文誌, Vol. 18, No. 4, pp. 127-135, 2005 (4).
[9] R. Murray, Z. Li and S. S. Sastry, “AMathematical Introduction to Robotic Manipulation,” CRC Press, 1994.