論 文

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

村尾

俊幸∗

非会員

藤田

政之∗∗∗

正

非会員

河合

宏之∗∗

Passivity-based Control of Dynamic Visual Feedback Systems
with Movable Camera Configuration
Toshiyuki Murao∗ , Member, Hiroyuki Kawai∗∗, Non-member, Masayuki Fujita∗∗∗ , Non-member
This paper deals with control of dynamic visual feedback systems with a movable camera conﬁguration.
This conﬁguration consists of a robot manipulator and a camera that is attached to the end-eﬀector of another robot manipulator. This system which can be interpreted the dynamic visual feedback system withan eye-in-hand conﬁguration and a ﬁxed camera one as a special case, can enlarge the ﬁeld of view. Firstly
the dynamic visual feedback system with an eye-to-hand conﬁguration is given with the fundamental representation of a relative rigid body motion. Secondly we construct the dynamic visual feedback system with a
movable camera conﬁguration by combining the camera control error system. Next, we derive the passivity ofthe dynamic visual feedback system. Based on the passivity, stability and L2 -gain performance analysis are
discussed. Finally the validity of the proposed control law can be conﬁrmed by comparing the experimental
results.
キーワード：視覚フィードバック制御，マニピュレータダイナミクス，受動性，リアプノフ安定，L2 ゲイン性能解析

Keywords: Visual feedback control, Manipulator dynamics, Passivity, Lyapunov stability, L2 -gain performance analysis

ンドの手先に取り付けられた Eye-in-Hand 構造と，カメラ

1. はじめにとハンドが別々に配置された固定カメラ構造である (1) (2) 。

近年，視覚フィードバック制御は，工場だけでなく医療

さらに，Eye-in-Hand 構造では作業スペースを全て観察

分野などへも応用され始めており，より複雑なシステムやするにはカメラ視野が限定され，固定カメラ構造では作業

幅広い分野へ適用されるようになってきている。この視覚スペースを探索するには不十分であるという理由から，二フィードバック制御は，よく知られているように二つの制御

つのカメラ構造を発展させたシステムが研究され始めてい方法と二つのカメラ構造に大別できる。すなわち，制御方法る。その中の一つが，Eye-in-Hand 構造と固定カメラ構造

は位置ベース法と画像ベース法，カメラ構造はカメラがハを組み合わせた複数のカメラを用いる構造である。例えば，

Eye-in-Hand カメラと固定カメラに回転方向と並進方向に関するタスクを分業させる手法 (3) ，Eye-in-Hand カメラの

∗ 産業技術大学院大学 産業技術研究科

位置を固定カメラで測る手法 (4) ，両構造のカメラを複数用〒 140–0011 東京都品川区東大井 1–10–40
School of Industrial Technology, Advanced Institute of Industrial Technology
1–10–40 Higashi-Ohi Shinagawa-ku, Tokyo 140–0011∗∗ 金沢工業大学 工学部〒 921–8501 石川県石川郡野々市町扇が丘 7–1
College of Engineering, Kanazawa Institute of Technology
7–1 Ohgigaoka Nonoichi, Ishikawa 921–8501∗∗∗ 東京工業大学 大学院理工学研究科〒 152–8550 東京都目黒区大岡山 2–12–1
Graduate School of Science and Engineering, Tokyo Instituteof Technology
2–12–1 Ookayama Meguro-ku, Tokyo 152–8550
電学論 C，128 巻 2 号，2008 年

いることで位置姿勢の推定偏差を減少させる手法 (5) などが提案されている。
また複数のカメラを用いないものとしては，固定カメラ
構造のようにカメラとハンドが別々に配置されているが，カメラが固定されているとは限らない Eye-to-Hand 構造が挙げられる (固定カメラ構造はこの特殊ケースとなる)。実際，工場では溶接作業や車の塗装のような散乱した場所における繰り返しの多い作業が頻繁に行われており，可動カ
メラを用いた Eye-to-Hand 構造により積極的に視野を得15z
CameraManipulatorgwz

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

5h

gzc g
ch
5c
Camera先座標系 Σh ，カメラマニピュレータの台座に台座座標系

Work
Manipulator

系 Σo を定義する。ここで，gij は同次表現と呼ばれ，任意の二つの座標系 Σi から Σj の位置 pij ∈ R3 と姿勢を

gwh

表す回転行列 eξ̂θij ∈ SO(3) から構成され，4 × 4 行列として以下のように定義される。

gho

gco
Target Object



gwc

5o

gwo

gij =

5w
World Frameeξ̂θij0pij
1· · · · · · · · · · · · · · · · · · · · · · · · · (1)ただし，演算子 ∧(wedge) は 3 次元ベクトルを 3 × 3 の歪対称行列へ写像する演算子であり，その逆写像は ∨(vee)Fig. 1. Visual feedback system with a
movable camera conﬁgurationで定義される (13) 。例えば，基準座標系 Σw からみた手先
座標系 Σh の位置姿勢は gwh となる。

。また，医療分野では穿刺手術マ

本システムにおいて順運動学により各マニピュレータのニピュレータなどのメディカルロボットの開発が近年特に関節角度から gwh ，gzc が得られ，また各マニピュレータ

望まれており，作業するマニピュレータの位置制御を行いの台座は動かないことから gwz が同定可能である。さらながら，視野を確保するために腹腔鏡などを用いて広範囲に，同次表現の合成則から gwc ，gch を求めることが可能

る手法が効果的となる(6)をモニタリングする必要が出てくる(7)であり† ，以上の五つの位置姿勢が既知情報となる。逆に，。

観測対象が未知な運動をするために，gwo ，gho ，gco は未このような可動カメラを用いた研究としては，可視度を知情報となる。

用いた可動カメラの位置決め (6) ，カメラ視野などの指標をな

本節では準備として Eye-to-Hand 構造の視覚フィード

どが挙げられる。しかし，これらの論文はマニピュレータ

バックシステムについて議論する。Eye-to-Hand 構造の制ダイナミクスを無視できると仮定し，さらに安定性や観測

御目的は手先座標系からみた観測対象の相対位置姿勢 gho

用いた制御

(8)

，リアルタイム性を強く意識した手法(9)

対象が運動する場合における議論も十分になされていない。

をその目標位置姿勢 ghd に常に一致させておくことである。

そこで本研究では，Fig. 1 に示すようなマニピュレータ

しかし前述したように，手先座標系からみた観測対象の相ダイナミクスを考慮した可動カメラ構造の動的視覚フィー

対位置姿勢 gho を直接観測することはできない。そこでまドバックシステムに対して，手先にカメラを取り付けたマ

ずはカメラからみた観測対象 gco の運動について考える。

ニピュレータ (カメラマニピュレータ) と，作業を行うマニ

カメラ座標系からみた観測対象の相対位置姿勢 gco はピュレータ (作業マニピュレータ) を一つのシステムとして−1
gco = gwc
gwo · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (2)捉え，安定性や制御性能解析について議論する。本論文でで表される。また，カメラ座標系からみた観測対象の相対提案するシステムは，我々が提案している Eye-in-Hand 構

速度は (2) 式を時間微分することで，gco の基本式として

造 (10) (11) と固定カメラ構造 (12) の両構造を特殊な場合とし

b
b
b
−1 V
Vco
= −Ad(gco) wc + Vwo · · · · · · · · · · · · · · · · · · (3)て含んだシステムとなっている。
本論文の構成は以下のとおりである。2 節では固定カメのように導かれる。ここで，Vijb ∈ R6 は剛体運動のボディ

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

速度を表しており，Ad(gij ) ∈ R6×6 は同次表現 gij の随伴

バックシステムについて述べる。3 節では，カメラ制御偏写像である (13) 。この (3) 式は，カメラ座標系からみた観測差システムを組み合わせることにより可動カメラ構造の動bb
対象の相対速度 Vco
がカメラの速度 Vwcと観測対象の速的視覚フィードバックシステムを構成した後に，制御則をb
度 Vwoの差として表されていると解釈できる。(3) 式にお提案し安定性と L2 ゲイン制御性能解析を行う。4 節で 2b
いて，Vwc= 0 と考えると固定カメラ構造となる。導出の自由度マニピュレータによる実験結果を示し，最後に 5 節

詳細は文献 (12) を参照。

でまとめを行う。

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

2. Eye-to-Hand 構造の動的視覚フィードバック
システムのピンホールカメラから画像特徴量を得ることを考えている。しかし，カメラから得られる視覚情報 f ∈ R2m は相〈2・1〉 視覚フィードバックシステムにおける剛体運動の表現

本節は推定偏差システムについて述べる。本システムでは一台対位置姿勢 gco を含んではいるが，二次元情報であるため

本論文では，Fig. 1 に示すような，カメラマニ

直接 gco を得ることはできない†† 。そこで，相対位置姿勢

ピュレータと作業マニピュレータの二台のマニピュレータ−1† 三つの座標系間の関係を用いて g
wc = gwz gzc ，gch = gwc gwhとそれぞれ導出される。
†† カメラモデルの詳細は文献 (12) 参照。本論文では画像特徴量として観測対象上に m 点 (m ≥ 4) の特徴点を取り付けている。

を有し，五つの座標系によって表される視覚フィードバッ
クシステムについて考える。図中の五つの座標系をそれぞれ作業マニピュレータの台座に基準座標系 Σw ，手先に手2

IEEJ Trans. EIS, Vol.128, No.2, 2008

受動性に基づく可動カメラ構造の 3 次元動的視覚フィードバック制御の推定値 ḡco を得るために，非線形オブザーバを構成する。

で定義する。また，推定偏差ベクトルと同様に，手先制御(3) 式に基づき，推定値 ḡco の運動モデルをつぎのようにT ξ̂θeh T
偏差ベクトルを eh := [pT)] と定義する。
eh eR (e

導出した相対位置姿勢 ḡho の運動モデルおよび相対位置構成する。

姿勢の目標値を用いて，手先制御偏差システムを構成する。

b
b
−1 V
V̄co
= −Ad(ḡco) wc + ue · · · · · · · · · · · · · · · · · · · · (4)(7) 式の推定偏差システムの導出と同様に， (10) 式を時間

ue ∈ R6 は推定偏差の振舞いを安定にするために加えられ微分することで，手先制御偏差システムは
b
b
b
Veh
= −Ad(ḡ−1 ) Vwh+ ue − Ad(g−1 ) Vhd· · · · · (11)

る入力であり，後に制御則の一部として提案する。

ho

ここで，相対位置姿勢の真値と推定値の偏差 gee をehbと導出される。ここで Vhd
は手先座標系からみた観測対象−1
gee := ḡcogco · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (5)の相対位置姿勢 gho の目標速度である。
〈2・4〉 Eye-to-Hand 構造の動的視覚フィードバックと定義する。さらに，この推定偏差に対する推定偏差ベクシステム

T ξ̂θee T
トルを ee := [pT)] ∈ R6 と定義する。ただしee eR (e

本節では，作業マニピュレータのダイナミクスを考慮することで，Eye-to-Hand 構造の動的視覚フィーeR (eξ̂θ ) := sk(eξ̂θ )∨ ∈ R3 ，sk(eξ̂θ ) := 12 (eξ̂θ −e−ξ̂θ ) であり，sk(eξ̂θ ) は回転行列に対して歪対称成分を抽出する関数となっている。この推定偏差ベクトル ee は，カメラから得られる視覚情報 f と推定モデルにより得られる視覚情報 f¯，
さらには画像ヤコビアンの擬似逆行列 J † (ḡco ) ∈ R6×2m

ドバックシステムを構成する。
作業マニピュレータは nh 自由度を有するものとし，マニピュレータダイナミクスを次式で表す (15) 。

Mh (qh )q̈h + Ch (qh , q̇h )q̇h + gh (qh ) = τh + τhd · (12)qh , q̇h , q̈h ∈ Rnh はそれぞれの各関節の角度，角速度，
角加速度を表し，τh ∈ Rnh は入力トルク，τhd ∈ Rnh
はトルク外乱，Mh (qh ) ∈ Rnh ×nh は正定な慣性行列，
Ch (qh , q̇h )q̇h ∈ Rnh は遠心力・コリオリ力項，gh (qh ) ∈Rnh は重力項を表す。

を用いることで

ee = J † (ḡco )(f − f¯)· · · · · · · · · · · · · · · · · · · · · · · · · (6)として導かれる (12) 。この推定偏差ベクトル ee が 0 になれ
ば，相対位置姿勢の真値と推定値が一致することとなる。
剛体運動の基本式 (3) と相対位置姿勢の推定モデル (4)また，マニピュレータの手先の速度はボディマニピュレーを用いて，推定偏差システムを構成する。(5) 式を時間微タヤコビアン Jhb (qh ) ∈ R6×nh を用いることでつぎのよ

分し (3)，(4) 式を用いることで，推定偏差システムとしてうに表される (13) 。
b
Vwh= Jhb (qh )q̇h · · · · · · · · · · · · · · · · · · · · · · · · · · · (13)bb
−1 ue + V
Vee
= −Ad(geewo · · · · · · · · · · · · · · · · · · · · (7))一方，手先の速度の目標値を uhd とし，マニピュレータが導出される。
〈2・3〉 手先制御偏差システムの関節角速度の目標値を q̇hd で表すと，(13) 式と同様

つぎに実際に制御目にしてボディマニピュレータヤコビアンを用いることで的を達成するための手先制御偏差について述べる。手先座

uhd = Jhb (qh )q̇hd と表すことができる。ここで，作業マニピ
ュレータの関節角速度に関する偏差を rh := q̇h − q̇hd ∈ Rnhと定義する。この関節角速度偏差 rh が 0 になるとマニピュ標系からみた観測対象の相対位置姿勢 gho も，gco が直接求められないために得ることができない。したがって，その推定値 ḡho を求めることを考え，ḡco を用いてつぎのよ

レータの手先の速度がその目標値に一致することとなる。

うに表す† 。

このとき，作業マニピュレータへの入力トルクとして−1ḡho = gchḡco · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (8)τh = Mh (qh )q̈hd + Ch (qh , q̇h )q̇hd + gh (qh )T+Jhb(qh )AdT(g−1 ) eh + urh (14)b
相対位置姿勢 ḡho の基本式は，手先の速度を Vwhとすhd

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

を考える。q̇hd ，q̈hd はそれぞれ関節角速度と関節角加速

b
b
V̄ho
= −Ad(ḡ−1 ) Vwh+ ue · · · · · · · · · · · · · · · · · · · (9)度の目標値である。urh は関節角速度偏差をなくすための

ここで，相対位置姿勢の推定値 ḡho と手先座標系からみ(12)，(14) 式を用いることで Eye-to-Hand 構造の動的視

新たな入力であり，後に提案する。このとき，(7)，(11)，ho

た観測対象の相対位置姿勢の目標値 ghd との偏差を手先制

覚フィードバックシステムは⎤⎤ ⎡T
AdT(g−1 ) eh
−Mh−1 Ch rh + Mh−1 Jhb
ṙhhd⎥⎢ b ⎥ ⎢
−Ad(ḡ−1 ) Jhb rh⎦⎣ Veh ⎦ = ⎣
ho
b
Vee
0
⎤
⎡ −1 ⎤
⎡ −1

0
0
Mh
Mh 0 
τhd⎥⎥⎢⎢ 0 −Ad −1
I+⎣⎦uf ix +⎣ 0 0 ⎦ b(geh )
Vwo
0 I
−1
0
0
−Ad(gee)⎡

御偏差 geh として−1
geh := ghd
ḡho · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (10)
† 本論文ではカメラキャリブレーションが正確に行われていると仮−1 −1
gwz gwh が正確に計算できるとする。またマニピュ
定し，gch = gzc
レータの手先に特徴点をつけ，手先の位置姿勢も視覚情報を用いて別途推定することにより，外部パラメータに対する不確かさを軽減させることができる (14) 。

電学論 C，128 巻 2 号，2008 年

3

〈3・2〉 可動カメラ構造の動的視覚フィードバックシス· · · · · · · · · · · · · · · · · · · · · · · · · (15)で表される。ただし，

⎡

テム

⎤

本節では，作業マニピュレータ同様，カメラマニ

ピュレータにもダイナミクスを考慮する。nc 自由度を有す

urh
⎢
b ⎥
−1
Ad
uf ix := ⎣(ghd ) uhd + Vhd ⎦ · · · · · · · · · · · · · (16)ue

るカメラマニピュレータのダイナミクスは次式で表される。

Mc (qc )q̈c + Cc (qc , q̇c )q̇c + gc (qc ) = τc + τcd · · (19)で定義される。Eye-to-Hand 構造の動的視覚フィードバッ

ただし，カメラマニピュレータの各記号は (12) 式の作業マ

クシステムにおける制御目的は，運動する観測対象に作業

ニピュレータの各記号と同じ意味を表している。

マニピュレータの手先を追従させることであり，状態 rh ，また，カメラ速度はボディマニピュレータヤコビアン

eh および ee を 0 にとどめておくことで制御目的が達成さ

Jcb (qc ) ∈ R6×nc を用いることでつぎのように表される。

れる。

b
Vwc
= Jcb (qc )q̇c · · · · · · · · · · · · · · · · · · · · · · · · · · · · (20)(注意) 本節ではカメラの動きについては議論していない。
カメラの動きを考えない場合，Eye-to-Hand 構造の動的視覚フィードバックシステム (15) は，文献 (12) の固定カメ

一方，カメラ速度の目標値を ucd とし，マニピュレータの関節角速度の目標値を q̇cd で表すと，両者の間には
ucd = Jcb (qc )q̇cd が成り立つ。カメラマニピュレータの
関節角速度に関する偏差を rc := q̇c − q̇cd ∈ Rnc と定義

ラ構造の動的視覚フィードバックシステムと一致する。

3. 可動カメラ構造の動的視覚フィードバック制御する。このとき，カメラマニピュレータへの入力トルクと

前節では Eye-to-Hand 構造の視覚フィードバック制御

してについて述べてきた。しかし前節のシステムでは，カメラ

τc = Mc (qc )q̈cd + Cc (qc , q̇c )q̇cd + gc (qc )b
速度 Vwcを入力として考慮していなかったため，観測対象

T+Jcb(qc )AdT(g−1 ) ec + urc (21)が視野から外れてしまう可能性がある。例えば文献 (12) で

cd

b
は Vwc
= 0 とした Eye-to-Hand 構造の特殊ケースであるを考える。q̇cd ，q̈cd はそれぞれ関節角速度と関節角加速度

固定カメラ構造に対して議論した。一般的に固定カメラ構の目標値である。このとき，(15)，(18)，(19)，(21) 式を造は Eye-in-Hand 構造に比べて大域的な視野を得ること

用いることで可動カメラ構造の動的視覚フィードバックシができるとされているものの，観測対象が大きく動くようステムはなシステムにおいてはより広いカメラ視野が必要となる。

⎡

⎤
⎤ ⎡
T
−Mh−1 Ch rh + Mh−1 Jhb
AdT(g−1 ) eh
ṙh
hd
⎥
⎢
⎥ ⎢
T
−Mc−1 Cc rc + Mc−1 Jcb
AdT(g−1 ) ec ⎥⎢ ṙc ⎥ ⎢
⎢
⎥
cd
⎢
⎥
⎥
⎢ Vb ⎥=⎢
⎢
⎥
−1
−Ad
J
r
⎢ eh ⎥ ⎢(ḡho ) hb h⎥
⎢ b ⎥ ⎢⎥⎣ Vec ⎦ ⎣−1 Jcb rc−Ad(ḡco
⎦
)
b
Vee
0
⎡ −1
⎤
Mh
0
0
0
0
⎢
⎥
0
0
0
⎢ 0 Mc−1⎥⎢⎥⎢⎥u0 −Ad(g−1 )
0
I
+⎢ 0
⎥
eh
⎢
⎥
−1
0
0
−Ad(gec
I
⎣ 0
⎦
)
−1
0
0
0
0
−Ad(gee
)
⎤
⎡
−1
M
0 0
⎥
⎢ h
⎢ 0 Mc−1 0⎥
⎥
⎢
+⎢
0 0⎥
⎥ w · · · · · · · · · · · · · · · · · · · · · · (22)
⎢ 0
⎥
⎢
0 I⎦
⎣ 0
0
0 0

b
本節では可動カメラの利点を活かし，カメラ速度 Vwc
を決め，システムの入力とすることでカメラが観測対象を視野から外さないようにする。そこで Fig. 1 に示すように，カメラをマニピュレータの手先に取り付け制御することを考える。なお，可動カメラ構造と固定カメラ構造の性能比
較については 4 節の実験検証にて詳しく行う。
〈3・1〉 カメラ制御偏差システム

本論文の制御目的は前述の gho を ghd に常に一致させておくことに加え，カメラが観測対象を外さないように，カメラ座標系からみた観測対象の相対位置姿勢 gco をその目標位置姿勢 gcd に常に一致させておくことである。しかし，前節で述べたように gco を直接得ることはできないため，制御目的を達成するためにその推定値 ḡco を用いる。相対位置姿勢の推定値
ḡco とカメラ座標系からみた観測対象の相対位置姿勢の目
標値 gcd との偏差をカメラ制御偏差 gec として−1
gec := gcd
ḡco · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (17)で表される。ただし，で定義する。また，カメラ制御偏差ベクトルを ec :=

⎤
urh
⎡
⎤
⎥
⎢
urc
⎥
⎢
τhd
⎥
⎢
⎢
⎥
b ⎥
−1 uhd + V
u := ⎢
hd ⎥ , w := ⎣ τcd ⎦ (23))⎢ Ad(ghd⎢b ⎥b−1 ucd + V
Vwo⎣ Ad(gcd
cd ⎦
)
ue
⎡

[pTec eTR (eξ̂θec )]T と定義する。
(11) 式の手先制御偏差システムの導出と同様に，(17) 式
を時間微分することで，カメラ制御偏差システムは
b
b
b
−1 V
−1 V
Vec
= −Ad(ḡco) wc + ue − Ad(gec) cd · · · · · · (18)bと導出される。ここで Vcd
はカメラ座標系からみた観測対

象の相対位置姿勢 gco の目標速度である。

で定義される。可動カメラ構造の動的視覚フィードバック
4

IEEJ Trans. EIS, Vol.128, No.2, 2008

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

Jhd
uhd
urh

Manipulator Dynamics&
Auxiliary ControllerWork ManipulatorAd(ghd-1 )eh
b
Vwh

rh

Jcd
ucd
urc

Manipulator Dynamics&
Auxiliary Controller
Camera Manipulator

ue

Vwcb
rc
T

Ad(gcd-1 )ecb
Vwo
Hand Control
Error System
Estimation
Error System
Camera Control
Error System

1
1
V = rhT Mh rh + rcT Mc rc +E(geh )+E(gec )+E(gee )22· · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (26)ehを考える。ここで E(gij ) := 12 pij 2 + φ(eξ̂θij ) であり，関ee

数 φ(eξ̂θij ) は回転行列に対する誤差関数を表す正定関数である (16) 。視覚フィードバックシステムとマニピュレータダ

ec

イナミクスのもつ歪対称性から，エネルギー関数 V の時
間微分は

Visual Feedback
System1
1
V̇ = rhT (Ṁh −2Ch )rh + rcT (Ṁc −2Cc )rc +xT N T u
2
2= uT ν · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (27)Fig. 2. Block diagram of dynamic visualfeedback system with a movable camera
conﬁgurationとなる。(27) 式の両辺を積分することでTuT νdτ = V (T ) − V (0) ≥ −V (0) := −β0 (28)0システムにおける制御目的は，カメラから観測対象への相が成り立つ。β0 は初期状態のみに依存するある非負の定数

対位置姿勢と手先から観測対象への相対位置姿勢を常にそ

この補題は，(22) 式の動的視覚フィードバックシステム

x := [rhT rcT eTh eTc eTe ]T を 0 に留めておくことで制御目が受動性を有していることを示唆している。この可動カメ

的が達成される。

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

本システムはカメラが可動式であるために，固定カメラ

ニピュレータを考慮しないとき，Eye-in-Hand 構造の動的構造に比べてカメラ視野を広くすることが可能であり，そ視覚フィードバックシステムとみなすことができる。具体

れによりロボットの作業スペースをより広げることができb
的には，手先速度 Vwh= 0，手先速度の目標値 uhd = 0，

る。従来研究 (12) では広域を見渡せる位置にカメラを固定

基準座標系からみた台座の相対位置姿勢 gwz = I ，さらにさせなければ作業スペースを広げることができなかったの

手先制御偏差 eh と作業マニピュレータを考慮しない特別に対して，本システムでは近い距離を保ちながら可動カメ

な場合が，文献 (10) (11) で示した Eye-in-Hand 構造の動ラを観測対象に追従させることでそれが可能となり，不必的視覚フィードバックシステムとなる。また 2 節より固定

要に広域を見る必要がなくなる。その結果，観測対象以外

カメラ構造の動的視覚フィードバックシステム (12) が含まの不適切な特徴点の抽出を避ける効果もあると考えられる。

れていることは明らかである。したがって可動カメラ構造

また (7)，(11)，(18) 式のみの，マニピュレータダイナミの動的視覚フィードバックシステム (22) は Eye-in-Hand

クスを考慮しない可動カメラ構造の視覚フィードバックシ

構造と固定カメラ構造という一般的によく知られている二ステムも構成でき，モバイルロボットなどの制御システム

つのカメラ構造の動的視覚フィードバックシステムを特殊に対しても適用可能となる。Fig. 2 に可動カメラ構造の動な場合として含んだ形となる。

的視覚フィードバックシステムのブロック図を示す。

〈3・4〉 動的視覚フィードバック制御則と安定性〈3・3〉 動的視覚フィードバックシステムの入出力間の関係制御則を提案するために，動的視覚フィードバック

なわち w = 0) 場合に平衡点 x = 0 を安定とする制御則として次式を提案する。

〔補題 1〕 外乱がない (すなわち w = 0) とする。このと

u = −Kν = −KN x，· · · · · · · · · · · · · · · · · · · · (29)き，出力を

ν = N x · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (24)
⎤
⎡
I 0
0
0
0
⎥
⎢
0
0
0 ⎥
⎢0I
⎥
⎢
−I
0
0 ⎥
N := ⎢
⎥
⎢00
⎢00
0
−I
0 ⎥
⎦
⎣
0 0 Ad(e−ξ̂θeh ) Ad(e−ξ̂θec ) −Iとするとき，(22) 式の動的視覚フィードバックシステム

K : = diag{Krh , Krc , Kh , Kc , Ke }Krh := diag{krh1 , · · · , krhnh } と Krc := diag{krc1 , · · · ,krcnc } は作業マニピュレータとカメラマニピュレータの各関節に対するゲインであり，Kh := diag{kh1 , · · · , kh6 }，Kc := diag{kc1 ，· · · , kc6 } と Ke := diag{ke1 ，· · · , ke6 }は x 軸，y 軸，z 軸の並進と回転における手先制御偏差とカメラ制御偏差と推定偏差に対するゲインである。ただし，の入出力間にゲインにおける各要素はすべて正とする。

uT νdτ ≥ −β0 ，∀ T > 0· · · · · · · · · · · · · · · (25)

このとき，補題 1 で示した動的視覚フィードバックシス0が成り立つ。ただし β0 はある非負の定数である。

テムの受動性に基づくことで，安定性に関してつぎの定理(証明) エネルギー関数として電学論 C，128 巻 2 号，2008 年

動的

視覚フィードバックシステム (22) に対し，外乱がない (すシステムの有する性質を示す。

T

2

である。

れぞれの目標位置姿勢に一致させておくことであり，状態が導かれる。
5

⎡

⎤ 2
I 000
0
1 ⎢
⎥
0
+ 2 ⎣0I 00⎦x2γ
0 0 0 0 Ad(e−ξ̂θee )1+xT N T u + x2
2
1
1
≤ 2 W x2 + xT N T u + x2 · · · · · · · (37)2γ2

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

(証明) (26) 式のエネルギー関数をリアプノフ関数候補とすると，(22)，(29) 式の解軌道に沿った時間微分は (27) 式
よりV̇ = uT ν = −xT N T KN x· · · · · · · · · · · · · · · · · · (30)という関係式が成り立つ。ここで W := diag{I, I, 0, 0, I}である。(29) 式の制御則を代入するとと導かれる。ゲイン Krh ，Krc ，Kh ，Kc ，Ke の正定性より行列 K が正定であり，N が正則であるため，システム

1γ2
x2 − w2
2
2
1
1
≤ −xT N T KN x + 2 W x2 + x22γ
2
= −xT P x · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (38)2の平衡点 x = 0 は漸近安定となる。

V̇ +定理 1 では，カメラマニピュレータと作業マニピュレー
タを一つのシステムとして含めた可動カメラ構造の動的視覚フィードバックシステムに対し，リアプノフの安定定理
によりその安定性を示した。この安定性解析は補題 1 で示となる。ただしした受動性に基づいて行われている。
〈3・5〉 L2 ゲイン制御性能解析

本節では外乱が存在

P := N T KN −する場合について考察する。特に，本論文では外乱抑制問
題を考えることで L2 ゲイン制御性能解析を行う。

1
1
W − I · · · · · · · · · · · · · · · · (39)2γ 22とした。P ≥ 0 とし，両辺を積分するとここで，正のスカラーの定数 kc と ke を用いることにT

よって，Kc = kc I ，Ke = ke I とゲインを定義しなおす。

(γ 2 w2 − x2 )dτ ≥ 2V (T ) − 2V (0)また，ある正の数 γ を用いてつぎの条件を与える。

0≥ −2V (0) := −β · · · (40)11
Krh − 2 I − I > 0 · · · · · · · · · · · · · · · · · · · · · (31)2γ211Krc − 2 I − I > 0 · · · · · · · · · · · · · · · · · · · · · (32)2γ2(2kc − 1)l1
I > 0 · · · · · · · · · · · · · · (33)
Kh − I −2
2kc − 1 − 2l
1kc − − l > 0 · · · · · · · · · · · · · · · · · · · · · · · · · · · · (34)211ke − 2 − > 0 · · · · · · · · · · · · · · · · · · · · · · · · · (35)2γ2が成り立つ。β は初期状態にのみ依存する非負の定数である。
ここで P は

⎡
⎢
⎢
⎢
P =⎢
⎢
⎢
⎣

0
0
Krh − 2γ1 2 I − 12 I00
Krc − 2γ1 2 I − 12 I
00
Kh + ke I − 12 I00 ke Ad(eξ̂θec ) Ad(e−ξ̂θeh )
0

ただし

0

−ke Ad(e−ξ̂θeh )⎤ke (γ 2 + 1)
· · · · · · · · · · · · · · · · · · · · · · · · (36)l= 2γ (2ke − 1) − 1

00ke Ad(eξ̂θeh ) Ad(e−ξ̂θec )00−ke Ad(eξ̂θeh )とする。このとき，制御性能解析に関するつぎの定理が示

kc + ke − 12 I

される。

−ke Ad(e−ξ̂θec )−ke Ad(eξ̂θec )
ke − 2γ1 2 − 12 I⎥⎥⎥⎥
⎥ · (41)⎥⎥⎦〔定理 2〕 与えられた γ に対して，(31)〜(35) 式を満たす
ようにゲイン Krh ，Krc ，Kh ，kc ，ke を選ぶとき，(22)，と変形できるので，シューア補元を計算することで P ≥ 0(29) 式で構成される動的視覚フィードバックシステムは
γ 以下の L2 ゲインを有する。
(証明) 蓄積関数として (26) 式のエネルギー関数を考える。
(22)，(29) 式の閉ループ系の解軌道に沿って時間微分し，となる十分条件は (31)〜(35) 式を満たすこととなる†。 2
この制御性能解析では観測対象の運動を外乱としてとらえているために，γ が小さいコントローラであれば観測対象の運動が状態に与える影響がより少ないことを示している。本論文では，H∞ 制御における最も基本となる外乱抑平方完成を用いると制問題を考えた。本論文で提案した手法は，適切な一般化

1γ2
x2 − w2
2
2 ⎡
⎤ 2
I 000
0
2
γ
1 ⎢
⎥
0
=−
w− 2⎣ 0 I 00⎦x2
γ
0 0 0 0 Ad(e−ξ̂θee )V̇ +プラントを構成することで，外乱抑制問題以外にも拡張可能である。
† これは P > 0 となる条件式であり，最悪外乱も考えていることから，この条件式は十分条件でしかない。

6

IEEJ Trans. EIS, Vol.128, No.2, 2008

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

yc

Base Frame

zz

xz

5z
yz

5c

xh

Object Frame

zw
5z
yw

0.01

0.01

0

0

-0.01

Hand Frame

0.03

5o

pehy [ m]

xo

xw

yh 5h

Camera Frame

zo

World Framezhpehx [ m]

zc

xc

yo

Fig. 3. Experimental arm (Left: camera
manipulator, Right: work manipulator)

0

-0.03
NGehz [ rad]

0

0.1

0.2

0.1

0.3

0.45
xo [m]( 0 < t < 4 [s] )

0.2

6

8 9.6

-0.01
0.03

-0.1

0

2

4

6

8 9.6

0

2

4

6

8 9.6

0

2

4
6
time [s]

8 9.6

0
0

2

4

6

8 9.6

-0.03
0.1
0

0

2

4
6
time [s]

8 9.6

-0.1

Fig. 5. Hand control error (Left: Gain A,
Right: Gain B)

0.6

0.45
xo [m]を示す。それぞれの初期設定は pwh = [0.4732 0.1 0]T ，

0.3

ξθwh = [0 0 0]T ，pwz = [0 0 − 1.16]T ，ξθwz = [0 0 0]T ，
pwc = [0.4732 0.1 − 1.16]T ，ξθwc = [0 0 0]T とする。

( 4 < t < 9.6 [s] )また本実験ではマニピュレータの追従問題を考え，目標相Fig. 4. Trajectory of the target object

対位置姿勢は時不変とし phd = [0 0 − 2]T ，eξ̂θhd = I ，pcd = [0 0 − 0.84]T ，eξ̂θcd = I とした。初期状態では偏差は 0 とする。
本〈4・2〉 L2 ゲイン制御性能解析に対する実験結果

4. 検証実験〈4・1〉 実験設定

4

0

0.1

0.6

2

0

yo [m]-0.1

yo [m]-0.1

0

本稿では 3 次元の動的視覚フィー

実験の外乱抑制問題に対するゲインは以下の手順で決定

ドバック制御則を提案したが，検証実験では，簡単のためにする。

水平 2 自由度マニピュレータである SICE-DD アーム (15)

Step 1:を 2 台用いて実験を行った。

Fig. 3 にマニピュレータと座標系を示す。手先にカメラがStep 2:

取り付けられた左のマニピュレータをカメラマニピュレー

推定偏差に対するゲイン ke を (33)〜(35) 式が満足するように設定する。

タ，右のマニピュレータを作業マニピュレータとし，それStep 3:
それぞれの角速度偏差に対するゲイン Krh とKrc が与えられた γ と (31)，(32) 式を満足するように

ぞれ五つの座標系を定める。両マニピュレータは台座に近いリンク 1 (0.2[m]) とリンク 2 (0.3[m]) によって構成さ決める。

れている。これらのマニピュレータはディジタル制御装置

実際に設計したゲインを以下に示す。

として dSPACE 社の DS1005(PowerPC 750 processor，Gain A : γ = 0.388, kc = 10, ke = 20

480MHz) を使用し制御する。カメラは CCD カメラとして SONY 社の XC-HR57(60[fps]) を使用し，カメラから画像を入力するボードとして，PCI バスモノクロ画像入力
ボードの PicPort-Stereo-H4D(60[fps]) を使用する。また，
画像処理は画像処理用ソフトウェア HALCON を使用して，観測対象上に取り付けた 4 点の中心点をそれぞれ算出

Kh = diag{20, 20, 10, 10, 10, 20},
Krh = diag{5, 5}, Krc = diag{5, 5}Gain B : γ = 0.241, kc = 30, ke = 30
Kh = diag{50, 50, 25, 25, 25, 50},
Krh = diag{10, 10}, Krc = diag{10, 10}する。
本論文では紙面の都合上，定理 1 を検証するための外乱

Figs. 5〜7 にこれら二つのゲインを比較した実験結果を示す。Figs. 5，6 は 3 次元空間での x，y 軸の位置偏差とz 軸の回転偏差を示す。Fig. 5 が手先制御偏差，Fig. 6 がカメラ制御偏差である。また，Fig. 7 には状態 x のユー
クリッドノルム x を示す。Figs. 5，6 において，左図がGain A の場合を，右図が Gain B の場合を示している。
Fig. 7 では上図が Gain A の場合を，下図が Gain B の場
合を示している。Figs. 5〜7 からも明らかなように，外乱が存在しない場合の実験を省略し，観測対象を動かした場合の検証実験のみを行い，異なる γ のゲインを用いること
で，L2 ゲイン制御性能解析に対する検証実験を行う。さらに同じ条件の下，従来研究である固定カメラ構造の視覚
フィードバック制御 (12) との視野に関する比較を行う。
観測対象は 4 秒間直線運動をさせた後，5.6 秒間は 8の字運動をさせる。Fig. 4 に観測対象のトラジェクトリー
電学論 C，128 巻 2 号，2008 年

手先制御偏差に対するゲイン Kh とカメラ制

御偏差に対するゲイン kc を設定する。

7

0

0

-0.02

-0.02

2

4

6

8 9.6

0.04

0

-0.04
NGecz [rad]

0

0.15

20

0

2

4

6

8 9.6

1

0
0

2

4

6

8 9.6

-0.04
0.15

0

0

-0.15

-0.15

0

2

4
6
time [s]

8 9.6

0

2

4

6

fov (Movable)

pecy [m]0.04

40
fov (Fixed)

0.02

pecx [m]

0.02

8 9.6

0

2

4
6
time [s]|| x || (C = 0.388)

2

4

0

2

4

8 9.6

6

8

9.6

time [s]

6

8

9.6

Fig. 9. Value of the feild of view measure(Top: ﬁxed camera, Bottom: movable camera(Dashed: Gain A, Solid: Gain B))

0.25|| x || (C = 0.241)

0

2

1

Fig. 6. Camera control error (Left: Gain A,
Right: Gain B)〈4・3〉 カメラ構造の違いに対する実験結果

つぎに本

0.2

研究の可動カメラ構造と従来研究の固定カメラ構造とで得0.15られる視野に関する比較を行う。
〈3・3〉
節で述べたように，

0.1

本論文で提案している可動カメラ構造の動的視覚フィード

0.05
0
0

バックシステムは，我々の従来研究である固定カメラ構造の
2

4

6

8

9.6

動的視覚フィードバックシステムを特殊ケースとして理論0.25的に含んだシステムとなっている。しかし本論文では，可動0.2

カメラのシステムが従来研究の固定カメラシステムに対し0.15てどのように優れているかを定量的には述べてこなかった。

0.1

そこで本節では，実験を通して可動カメラと固定カメラで

0.05
0
0

得られる視野を定量的に示すことで比較を行う。本節の実
2

4time [s]68験結果は，可動カメラシステムに関するゲインは Gain A，

9.6

Gain B を用い，固定カメラシステムに関するゲイン (12) はGain B の ke , Kh , Krh を用いる。
Fig. 8 に観測対象の特徴点 4 点の実測値のトラジェクトリーを示す。左図が Gain B を用いたときの可動カメラ，

Fig. 7. Norm of the state (Top: Gain A,
Bottom: Gain B)
300

300
200

200

100

100

fy [pixel]

3

0

0

-100

-100

-200

-200

-300

-300

-200

-100

100
0
fx [pixel]200

右図が固定カメラの場合の実験結果である。紙面の都合上，Gain A を用いたときの可動カメラの実験結果については省略する。両図で示されている fx と fy の範囲が，実験で用いたカメラが実際に観測できる範囲に相当する。両者を比較すると，固定カメラでは観測対象が動いた際に特徴点が視野の境界まで近づくのに対し，可動カメラでは特徴点の値がカメラの中央に留まっているのがわかる。これは，観測対象をカメラの中心で捉えるようにカメラが動いていることを意味している。
-200

-100

0
100
fx [pixel]

また，カメラ視野を測る指標としては Nelson ら (17) が200以下の指標を提案している。

Fig. 8. Trajectory of the feature points (Left:movable camera (Gain B), Right: ﬁxed
camera)f ov = 

1
m
i=1

f2
1 − f 2xi
xM



f2

1 − f 2yi

 · · · · · · · · · (42)yM

ここで，[fxi fyi ] はカメラから得られる画像面での特徴点がある場合，γ の小さい Gain B のほうが偏差が少なく追の座標であり，[fxM fyM ] は画像面でのそれらの最大値を従性能が高いことが示されている。したがって，提案する表す。この指標の最小値は 1 であり，1 に近いほど観測対制御則の有効性が検証実験によって示された。

象が画像中心に存在していることに相当する。すなわち，カ8

IEEJ Trans. EIS, Vol.128, No.2, 2008

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

メラ視野に対して観測対象を外しにくい位置にカメラが存

namic Visual Feedback Control of Robotic Systems”, Trans.
the Institute of Systems, Control and Information Engineers, Vol. 17, No. 1, pp. 39–47 (2004) (in Japanese)
河合宏之, 東 剛人, 藤田政之: “マニピュレータダイナミクスを考慮したオブザーバ型動的視覚フィードバック制御”, システム制御情報学会論文誌, Vol. 17, No. 1, pp. 39–47 (2004)（11） 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)（12） H. Kawai, T. Azuma and M. Fujita: “Passivity-based 3D
Dynamic Visual Feedback Control with a Fixed Camera”,
Trans. the Institute of Systems, Control and Information
Engineers, Vol. 18, No. 4, pp. 127–135 (2005) (in Japanese)
河合宏之, 村尾俊幸, 東 剛人, 藤田政之: “受動性に基づく 3 次元動的視覚フィードバック制御の固定カメラ構造への展開”, システム制
御情報学会論文誌, Vol. 18, No. 4, pp. 127–135 (2005)（13） R. Murray, Z. Li and S. S. Sastry: A Mathematical Introduction to Robotic Manipulation, CRC Press (1994)（14） H. Kawai, T. Murao and M. Fujita: “Passivity-based Dynamic Visual Feedback Control with Uncertainty of Camera
Coordinate Frame”, Proc. of the 2005 American Control
Conference, pp. 3701–3706 (2005)（15） H. Kobayashi et al.: Practical Control of Robot Manipulators, Corona Publishing Co., Ltd. (1997) (in Japanese)
小林尚登 ほか：ロボット制御の実際, コロナ社 (1997)（16） F. Bullo and A. D. Lewis: Geometric Control of Mechanical
Systems, Springer-Verlag (2004)（17） B. J. Nelson and P. K. Khosla: “Integrating Sensor Placement and Visual Tracking Strategies”, Proc. of the 1994
IEEE International Conference on Robotics and Automation, pp. 1351–1356 (1994)

在していることになる。
本実験におけるこの指標の時間変化の図を Fig. 9 に示す。
上図が固定カメラ，下図が可動カメラの場合であり，下図の中の破線が Gain A，実線が Gain B の場合の実験結果を示している。上図と下図では，縦軸の値が異なることに注意されたい。Fig. 9 において，可動カメラのほうが値が小さく，観測対象を視野から外さないようにカメラが動いていたのがわかる。したがって，可動カメラ構造の動的視覚フィードバック制御は固定カメラ構造より得ることのできるカメラ視野が広がっていると言える。

5. おわりに
本論文では，二台のマニピュレータを用いた可動カメラ
構造の 3 次元動的視覚フィードバックシステムを構成した。
ロボットの作業スペースをより広げるために，カメラを固定させずに可動カメラを用いた本システムは，Eye-in-Hand
構造と固定カメラ構造の両構造を特殊な場合として含んでいる。動的視覚フィードバックシステムが有する受動性に基づいて制御則を提案し，リアプノフの安定定理により安定性解析を行った。また，観測対象が運動する場合に外乱抑制問題を考えることで，L2 ゲイン制御性能解析を行った。最後に，検証実験により L2 ゲイン制御性能解析と可動カメラ構造の有効性をそれぞれ示した。

村（平成 19 年 3 月 5 日受付，平成 19 年 10 月 15 日再受付）

尾俊

幸 （正員） 2003 年金沢大学工学部電気・情報工学科卒業。2005 年金沢大学大学院博士前期課程修了，同年金沢大学大学院博士後期課程入学，同年

文献

東京工業大学大学院博士後期課程転入学，2006 年
産業技術大学院大学研究員，2007 年同助教。視（ 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)（ 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)（ 3 ） G. Flandin, F. Chaumette and E. Marchand: “Eye-inHand/Eye-to-Hand Cooperation for Visual Servoing”, Proc.
of the 2000 IEEE International Conference on Robotics andAutomation, pp. 2741–2746 (2000)（ 4 ） M. Elena, M. Cristiano, F. Damiano and M. Bonfe: “Variable
Structure PID Controller for Cooperative Eye-in-Hand/Eyeto-Hand Visual Servoing”, Proc. of the 2003 IEEE International Conference on Control Applications, pp. 989–994(2003)（ 5 ） V. Lippiello, B. Siciliano and L. Villani: “Eye-in-Hand/Eyeto-Hand Multi-Camera Visual Servoing”, Proc. of the 44th
IEEE Conference on Decision and Control and European
Control Conference, pp. 5354–5359 (2005)（ 6 ） R. Sharma and S. Hutchinson: “Motion Perceptibility andits Application to Active Vision-based Servo Control”, IEEE
Trans. Robotics and Automation, Vol. 13, No. 4, pp. 607–617(1997)（ 7 ） 下山 勲 ほか：ロボットフロンティア, 岩波書店 (2005)（ 8 ） B. J. Nelson and P. K. Khosla: “Visually Guided Manipulation using Active Camera-Lens Systems”, Proc. of the 1997
IEEE International Conference on Robotics and Automation, pp. 437–442 (1997)（ 9 ） A. Muis and K. Ohnishi: “Eye-to-Hand Approach on Eyein-Hand Configuration within Real-Time Visual Servoing”,
IEEE/ASME Trans. Mechatronics, Vol. 10, No. 4, pp. 404–
410 (2005)（10） H. Kawai, T. Azuma and M. Fujita: “Observer-based Dy-

電学論 C，128 巻 2 号，2008 年

覚フィードバック制御に関する研究に従事。計測
自動制御学会，システム制御情報学会，IEEE の
会員。

河

合

宏

之 （非会員） 1999 年金沢大学工学部電気・情報工学科卒業。2001 年金沢大学大学院博士前期課程修了，2004 年金沢大学大学院博士後期課程修了，同年法政大学情報技術 (IT) 研究センターポストド
クター，2005 年金沢工業大学講師。視覚フィード
バック制御に関する研究に従事。博士（工学）。計
測自動制御学会，システム制御情報学会， IEEE
の会員。

藤

田

政

之 （非会員） 1984 年早稲田大学大学院理工学研究科博士前期課程修了（電気工学専攻），1985 年同
博士後期課程中退。同年金沢大学工学部助手。同講師，助教授を経て，1992 年北陸先端科学技術
大学院大学情報科学研究科助教授。1999 年金沢大学工学部教授。2005 年東京工業大学大学院理工学研究科教授。ロバスト制御とその応用に関する研究に従事。工学博士。計測自動制御学会，システム制御情報学会，IEEE などの会員。

9