1

システム制御情報学会論文誌，Vol. 0, No. 0, pp. 1–9, 0000

論

文

受動性に基づく 3 次元動的視覚フィードバック制御の固定カメラ構造への展開*河合宏之 †・村尾 俊幸 ‡ ・東剛人 ‡ ・藤田政之 ‡Passivity-based 3D Dynamic Visual Feedback Control
with a Fixed Camera*
Hiroyuki Kawai†, Toshiyuki Murao‡ , Takehito Azuma‡ and Masayuki Fujita‡
In this paper, we investigate the control and the estimation of dynamic visual feedback systems
with a fixed camera. Firstly the fundamental representation of the visual feedback system with four
coordinate frames is established by using the homogeneous representation and adjoint transformation. Secondly we consider the observer which is reproduced from the fundamental representationof relative rigid body motion just as Luenberger observer for linear systems. Then, the relationshipbetween the estimation error in the 3D workspace and in the image plane is established. Next, wederive the passivity of the dynamic visual feedback system by combining the passivity of both thevisual feedback system and the manipulator dynamics. The stability via Lyapunov method for thefull 3D dynamic visual feedback system is discussed based on the passivity. The L2 -gain performance
analysis for the disturbance attenuation problem is considered via the dissipative systems theory.
Finally simulation results are shown to verify the stability and L2 -gain performance of the dynamic
visual feedback system.

1.

はじめに

バックシステムについて深い議論がなされているのに対し, ロボットは単なる位置決めをするための装置ととらロボットやメカニカルシステムに自律的な振舞いをさえられている. 一方, マニピュレータダイナミクスまでせるには多くの情報が必要となる. 特に視覚情報は未知考慮した研究としては文献 [4–6] などが挙げられる. こ

環境下において周囲の状況を認識させるために有用であれらの研究は, 平面マニピュレータに限定されてはいる

り, 視覚情報をダイナミカルシステムのフィードバック

が, Lyapunov の安定定理に基づき安定性を保証する制

ループに組み込んだ制御は視覚フィードバック制御とよ御則が提案されている. Kelly ら [7] は, 観測対象までばれる [1].

の距離が既知であるとの仮定が必要であるが, 平面マニ

ロボットの視覚フィードバックシステムの制御目的のピュレータに限定しない制御則を提案しており, Cowan

ひとつは, 移動する観測対象にロボットの手先やカメラ

ら [8] は Navigation 関数を用いてカメラ視野を考慮したを追従させることである. この制御目的を達成させる

制御則について議論している. いずれの研究も, 観測対ために, 安定性の大域化 [2] や軌道計画問題 [3] が研究

象が静止している場合の安定性解析まではおこなっていされている. これら多くの従来研究では, 視覚フィード∗†

るが, 観測対象が運動する場合や制御性能解析については十分な議論がなされていない. 筆者らは文献 [9,10] で,原稿受付 2004 年 4 月 22 日
法政 大 学 情 報技 術 (IT) 研 究セ ン ター

Information
Technology Research Center, Hosei University; 2-17-1
Fujimi, Chiyoda-ku, Tokyo 102-8160, JAPAN‡金沢 大 学 大学 院 自 然 科学 研 究科 Graduate School of
Natural Science and Technology, Kanazawa University;
2-40-20 Kodatsuno, Kanazawa city, Ishikawa 920-8667,
JAPAN
Key Words: visual feedback control, manipulator dynamics, passivity, Lyapunov stability, L2 -gain performance
analysis.

カメラがロボットの手先に取り付けられた Eye-in-Hand
構造の視覚フィードバックシステムに対して, 安定性と制御性能解析を議論している. しかし, 実システムでは必ずしもカメラとロボットの手先が一致しているとは限らない. たとえば, 文献 [11] でみられるような DNA 細胞に核を自動注入するシステムは固定カメラ構造の視覚
フィードバックシステムとして表されるため, 文献 [9,10]の議論だけでは十分ではない.

–1–

2

システム制御情報学会論文誌

第0巻

Image

CameraΣB の姿勢を意味する [12]. 演算子 ∧(wedge) は 3 次元ベクトルを 3 ×3 の歪対称行列へ写像する演算子であり,その逆写像は ∨(vee) で定義される. gab は混同しない限

gch
Sh

gwc
gco

り, よく知られた 4 × 4 行列の同次表現gwheξ̂θab pab
gab =
0
1

gho
gwo

Sw
World Frame(1)として用いる [12]. 同様に, カメラ座標系からみた手

So
Target Object

Fig. 1(0000)

添え字は θ のみにつけて表記する) は ΣA を基準としたRobot
Manipulator

Sc

第0号

先の位置姿勢と観測対象の位置姿勢をそれぞれ gch =(pch ,eξ̂θch ), gco = (pco ,eξ̂θco ) とし, 手先座標系からみた観測対象の位置姿勢を gho = (pho ,eξ̂θho ) と表す.
これらの座標系から, 視覚フィードバックシステムにVisual feedback system with a ﬁxed camera

本論文では, Fig. 1 に示すような四つの座標系を有するおける三つの座標系間に成り立つ剛体運動の表現を導出システムとして固定カメラ構造の動的視覚フィードバッ

する1 . まず, カメラ座標系からみた観測対象の相対位置

クシステムを構成する. そして, マニピュレータダイナ

姿勢 gco は

ミクスを考慮したうえで Lyapunov の安定定理に基づき−1
gco = gwc
gwo

安定性解析をおこなう. さらに, 観測対象が運動する場(2)

合に外乱抑制問題を考えることで, L2 ゲイン制御性能解で表される. ここで, 速度表現のひとつについて述べ

析をおこなう. 文献 [9,10] では, 三つの座標系を有するb
T
T T= [vabωab] ∈ R6 (たる. 剛体運動のボディ速度を Vabだし, vab ∈ R3 は 並進速度, ωab ∈ R3 は角速度 を表

Eye-in-Hand 構造の視覚フィードバックシステムに対して, 安定性と制御性能解析を議論している. 本論文では,座標系の数を三つから四つへ拡張することで, 固定カメ

す) と定義するとき, カメラの速度と観測対象の速度は
b
T
T T
b
T
T T
Vwc
= [vwcωwc] ∈ R6 と Vwo= [vwoωwo] ∈ R6 で定義される.
このとき (2) 式を微分することで, gco の基本式として

ラ構造における動的視覚フィードバックシステムの構成
法を, 従来研究の手法に基づいて提案している.
本論文の構成は以下のとおりである. 2. で視覚フィー

b
b
b
Vco
= −Ad(gco−1 V) wc + Vwo

ドバックシステムにおける剛体運動の基本となる式を導出したあと, 非線形オブザーバを構成する. そして, 四つb
TT Tが導かれる2 . ただし Vco= [vcoωco] ∈ R6 であり, 導の座標系を有する視覚フィードバックシステムの入出力

出の詳細は付録 1. を参照.

間の関係を示す. 3. では, マニピュレータダイナミクス

2.2を考慮した動的視覚フィードバック制御則を提案し, 安

カメラモデル

つぎに, 視覚情報を得るために, Fig. 2 に示すような定性と L2 ゲイン制御性能解析をおこなう. 4. で 2 自

ピンホールカメラのモデルを示す. 画像特徴量として観由度マニピュレータによるシミュレーション結果を示し,

xi

最後に 5. でまとめをおこなう.

2.

(3)

受動性に基づく視覚フィードバックシ

fyi

Camera Frame
xc
zc

l

fi

ステム

zo

2.1

視覚フィードバックシステムにおける剛体運動の表現
本論文では, Fig. 1 に示すような四つの座標系によっ

yi

pci
pco

P
poi

So

て表される視覚フィードバックシステムについて考え

xo

る. 図中の四つの座標系をそれぞれ基準座標系 Σw , 手

Target Object

先座標系 Σh , カメラ座標系 Σc および観測対象座標系

Fig. 2

Σo と定義する. このとき, 基準座標系からみた手先の位置姿勢, カメラの位置姿勢および観測対象の位置
姿勢をそれぞれ gwh = (pwh ,eξ̂θwh ), gwc = (pwc ,eξ̂θwc ),gwo = (pwo ,eξ̂θwo ) で表す. ここで, 任意の二つの座標系
を ΣA , ΣB としたとき, pab ∈ R3 は ΣA の原点から ΣBの原点への位置ベクトルを表し, eξ̂θab ∈ SO(3) (ただし,ξab ∈ R3 , θab ∈ R であり, 簡単化のため混乱がない限り

1

Sc
yc

yo

fxi
zw

Image Plane
xw
yw

Sw

World Frame

Pinhole camera

ここでは, 基準座標系, 観測対象座標系およびカメラ座
標系の関係を示すが, 他の座標系間においても同様の関係が導出可能である.
2
三つの座標間の関係 (2) と基本式 (3) は, 文献 [9] の(3)(4) 式と (11) 式 (および文献 [10] の (1) 式と (2) 式)と表現は異なるが同一の式を表している.

–2–

3

河合・村尾・東・藤田：受動性に基づく 3 次元動的視覚フィードバック制御の固定カメラ構造への展開

T
T ξ̂θee
ee := pT)ee eR (e

測対象に取り付けられた m 点 (m ≥ 4) の特徴点の画像
面上での x 座標と y 座標を考える [13]. このとき, 観
測対象座標系 Σo の原点から各特徴点までのベクトルを

(11)
1
2

ただし eR (eξ̂θ ) := sk(eξ̂θ )∨ , sk(eξ̂θ ) := (eξ̂θ −e−ξ̂θ ) で3poi ∈ R (i = 1,···,m) とすると, カメラからみた相対的な特徴点 pci ∈ R3 (i = 1,···,m) はある.

つぎに, カメラから得られる視覚情報と推定モデルに(4)より得られる視覚情報との関係について述べる. 特徴量で表される. ただし, 特徴点 poi はすべて既知であると

fi , f¯i (i = 1,···,m) をそれぞれ順に並べたベクトルを f, f¯とすると, f と f¯ の間につぎの関係が成り立つ [9].

pci = gco poi

し, (4) 式は同次表現でよく用いられる表記法 ([pT 1]Tf − f¯ = J(ḡco )ee

を p と表記) により表したものである. pci の各要素を

(12)

T

pci = [xci ,yci ,zci ] , カメラの焦点距離を λ と表す. 三次
元情報である pci はカメラから直接得られないため, カメラから得られる画像特徴量 f について考える. 画像面
上での特徴点の座標を [Xi Yi ]T (i = 1,···,m) として特徴
量を fi := [Xi Yi ]T (i = 1,···,m) とすると, 特徴量は透視

ただし

T

T
J(ḡco ) : = J1T (ḡco ) ··· Jm(ḡco )
⎤
⎡
λ
λx̄ci
0
−


2 ⎥ ˆ
⎢ z̄ciz̄ci⎥ eξ̄ θ̄co I −p̂oi
Ji (ḡco ) : = ⎢
⎦
⎣
λ
λȳci
0
− 2
z̄ci
z̄ci
i = 1,···,m変換を用いることで

λ
fi =
zci



xci
yci



(5)

である. J(ḡco ) が列フルランクを満たすように 4 点以上の特徴点をとり, その擬似逆行列 J † (ḡco ) を用いるこで表される.

2.3

視覚フィードバックシステムにおける非線形オブザーバカメラから得られる視覚情報は, (4), (5) 式より相対位
置姿勢 gco を含んではいるが, 二次元情報であるため直接 gco を得ることはできない. そこで, 相対位置姿勢のˆ
推定値 ḡco = (p̄co ,eξ̄θ̄co ) を得るために, オブザーバを構成する. 本論文では固定カメラ構造の視覚フィードバッ
b= 0) を考えるため, (3) 式で表
クシステム (すなわち Vwc
された gco の基本式は
b
b
Vco
= Vwo

とで

ee = J † (ḡco )(f − f¯)(13)が導かれる. したがって視覚フィードバックシステムの非線形オブザーバは, (7)∼(9) 式と, 後に ee とオブザー
バゲインを用いて提案する入力 ue によって構成される.

2.4

推定偏差システム

剛体運動の基本式とカメラモデルおよび相対位置姿
勢の推定モデルを用いて, 推定偏差システムを構成する.

(3) 式の導出と同様にして, (10) 式を時間微分し (6), (7)式を代入することで, 推定偏差システムは(6)となる. そこで, (6) 式に基づき, 推定値 ḡco の運動モデbb−1 ue + V
Vee
= −Ad(geewo)

ルをつぎのように構成する.
b
= ue
V̄co

b
T
T T
と導出される. ただし Vee= [veeωee] ∈ R6 である.

(7)

2.5

ue ∈ R6 は推定偏差の振舞いを安定にするために加えられる入力であり, 後に制御則の一部として提案する. また, (4), (5) 式と同様にして推定値 ḡco を用いた特徴量をp̄ci = ḡco poi
λ x̄ci¯fi =z̄ci ȳci

制御偏差システム

手先座標系からみた観測対象の相対位置姿勢 gho は−1
gho = gch
gco

(8)

(15)

で表される. しかしながら, gco は直接求められないた
め, その推定値 ḡco を用いたときの手先座標系からみた(9)

観測対象の相対位置姿勢 ḡho を−1
ḡho = gch
ḡco

とする.
ここで, 相対的な位置姿勢の真値と推定値の偏差 gee =(pee ,eξ̂θee ) を−1
gee := ḡco
gco

(14)

(16)

と表す. 相対位置姿勢 ḡho の基本式は, 手先の速度を
b
T
T T
Vwh
= [vwhωwh] ∈ R6 とすると, (3) 式の導出と同様にして, (16) 式より

(10)

b
b
b
V̄ho
= −Ad(ḡ−1 ) Vch+ V̄coと定義する. この推定偏差に対する推定偏差ベクトルをhoee としてつぎのように定義する.
–3–

4

システム制御情報学会論文誌b= −Ad(ḡ−1 ) Vwh
+ ue

第0巻

第0号



(17)

νce :=

hoと導かれる. ただし,
−1gwh
gch = gwc



−AdT
0
(g−1 )edAd(e−ξ̂θec ) −I(23)とするとき, (22) 式の視覚フィードバックシステムの入

(18)

出力間に

b
b
より導かれる Vch= Vwhの関係1 と (7) 式を用いてい

T

る2 .

0

ここで, 手先座標系からみた観測対象の相対位置姿勢∀
uTce νce dτ ≥− βce , T > 0(24)が成り立つ. ただし βce はある非負の定数である.

の目標値を gd = (pd ,eξ̂θd ) で表し一定値であるとする.

証明は以下のエネルギー関数を用いておこなわれる.

このとき相対位置姿勢の推定値と目標値との偏差を制御

1
1
Vce = pec2 + φ(eξ̂θec ) + pee 2 + φ(eξ̂θee ) (25)22

偏差 gec とよび gec = (pec,eξ̂θec ) を

gec := gd−1 ḡho(0000)(19)

ここで, 関数 φ(eξ̂θ ) は回転行列に対する誤差関数を表す
正定関数である [14]. 証明の詳細は付録 2. を参照.

で定義する. また, 推定偏差ベクトルと同様に, 制御偏差ベクトル ec を

この補題は, (22) 式の視覚フィードバックシステムがTT ξ̂θec
ec := pT)ec eR (e

受動性を有していることを示唆しており, 出力 νce をネ(20)

ガティブフィードバックしたものを入力 uce として用い
れば, 視覚フィードバックシステムが安定となることがと定義する.
導出した推定相対位置姿勢モデルおよび相対位置姿勢示される [15]. Fig. 3 に視覚フィードバックシステムのブの目標値を用いて, 制御偏差システムを構成する. (14)ロック図を示す. 図中の OMFC と Estimated OMFC

式の推定偏差システムの導出と同様に, (19) 式を時間微はそれぞれカメラからみた観測対象の運動と推定モデル

分し (17) 式を代入することで, 制御偏差システムはを表しており, HMFC はカメラからみた手先の運動を表
している.

b
b
Vec
= −Ad(ḡ−1 ) Vwh
+ ue

(21)

b
Vwo

ho

b
T
T T
と導出される. ただし Vec= [vecωec] ∈ R6 である.

OMFC

gco

2.6

視覚フィードバックシステムの入出力間の関係
剛体運動の視覚フィードバックシステムを (14), (21)
ue

式より



Visual Feedback System

 
 

b
−Ad(ḡ−1 )
I
0
Vec
ho
V b (22)
=
uce +
b
Vee
I wo
0
−Ad(g−1 )fgco , fEstimated
OMFC

gco

+

b
Vwh

ee

gch

ee

+

^ ec)ec
Ad(e xq

gco

uce
HMFC

Image Jacobian
JO

gho

nce

DesiregdgchTAd(gd-1)ec

b T T Tと構成する. ただし, uce := [(Vwh) ue ] であり, シス

T T
テムの状態を e := [eTc ee ] と定義しておく. このとき,Fig. 3Block diagram of visual feedback system

視覚フィードバックシステムに対して以下の補題が成り
立つ.

3.

b
【補題 1】 観測対象が運動していない (すなわち Vwo=

3.1

動的視覚フィードバックシステム
本節では, 剛体運動の視覚フィードバックシステムにマニピュレータダイナミクスを加えることで, 動的視覚
フィードバックシステムを構成する.
n 自由度のマニピュレータダイナミクスは次式で表される [16].

0) とする. 出力を1

動的視覚フィードバック制御

ここで gch = I とすると, カメラ座標系が手先座標系と一致していることになり, さらにカメラの速度
b
b
= Vwh
とすれば,
と手先の速度を一致, すなわち Vwc
Eye-in-Hand 構造での議論が可能となる.
2
この関係はカメラキャリブレーションが正確に行われているという仮定のもとで成り立つ. カメラキャリブ
レーションの問題はそれ自体がひとつの研究課題であり, いくつも解が提案されているため (たとえば, 文
献 [5,6]), 本論文ではキャリブレーション誤差はないとして議論を進める.

M (q)q̈ + C(q, q̇)q̇ + g(q) = τ + τd

(26)q, q̇, q̈ ∈ Rn はそれぞれの各関節の角度, 角速度, 角加速度を表し, τ ∈ Rn は入力トルク, τd ∈ Rn はトルク外乱,
M (q) ∈ Rn×n は正定な慣性行列, C(q, q̇)q̇ ∈ Rn は遠心力・コリオリ力項, g(q) ∈ Rn は重力項を表す.
また, マニピュレータの手先の速度はマニピュレータ–4–5

河合・村尾・東・藤田：受動性に基づく 3 次元動的視覚フィードバック制御の固定カメラ構造への展開

3.2

(27)

動的視覚フィードバックシステムの入出力間の関係
制御則を提案するまえに, 制御則の構成に対して重要

一方, 手先の速度の目標値を ud とし, マニピュレータのな役割をはたす動的視覚フィードバックシステムの有す

関節角速度の目標値を q̇d で表すと, (27) 式と同様にしてる性質を示す.

ヤコビアン Jb (q) を用いることでつぎのように表される.
b= Jb (q)q̇Vwh

マニピュレータヤコビアンを用いることで ud = Jb (q)q̇d【補題 2】 外乱がない (すなわち w = 0) とする. このと表すことができる. マニピュレータの関節角速度に関とき, 出力をする偏差 ξ ∈ Rn を

ν = Nx⎡ξ := q̇ − q̇dI0⎢ 0 −AdT −1 W
N := ⎣(g ) c(28)と定義する. また, 提案する制御則に設計の自由度を与えるために, つぎのような重み行列 Wc , We ∈ R6×6 を
定義する.

0
0

⎤

⎡

⎤

(31)

ξ
⎥
⎢ ⎥
, x := ⎣ ec ⎦⎦dee
0 Ad(e−ξ̂θec ) Wc −Weとするとき, (30) 式の動的視覚フィードバックシステム







wpc I3 0
wpe I3 0
Wc :=, We :=
0 wrc I3
0 wre I3

の入出力間にT0

ただし wpc , wpe , wrc , wre ∈ R は正の定数とする.

uT νdτ ≥ −β0 , ∀ T > 0(32)が成り立つ. ただし β0 はある非負の定数である.

このとき, マニピュレータへの入力トルクとして（証明） エネルギー関数として

τ = M (q)q̈d + C(q, q̇)q̇d + g(q)W e + uξ+ JbT (q)AdT(g−1 ) c c

11
V = ξ T M (q)ξ + wpc pec2 + wrc φ(eξ̂θec )2
2
1+ wpe pee 2 + wre φ(eξ̂θee ) (33)
2

(29)

d

を考える. q̇d , q̈d はそれぞれ関節角速度と関節角加速度

W e は, 視覚フィードの目標値である1 . JbT (q)AdT(g−1 ) c cを考える. p̂ec と p̂ee の歪対称性と Ṁ − 2C の歪対称性

バックシステムにおける制御偏差 ec を含んだ新たな偏から, エネルギー関数 V の時間微分はd

差のフィードバック項である2 . uξ は後に提案する新たな入力である. このとき, (22), (26), (29) 式を用いることで動的視覚フィードバックシステムは

⎡

⎤

⎡

−1

W e
C(q, q̇)ξ − JbT (q)AdT(gd−1 ) c c⎤−M (q)ξ˙⎢ b ⎥ ⎢⎥
−Ad(ḡ−1 ) Jb (q)ξ⎣ Vec ⎦ = ⎣
⎦
ho
b
Vee
0
⎤
⎡
⎤
⎡
−1
0
0
M (q)
M (q)−1 0⎥⎢⎥⎢I+ ⎣ 0 −Ad(ḡ−1 )00 ⎦w⎦u + ⎣ho00−1−Ad(gee
)

0

I
(30)

1
V̇ = ξ T (Ṁ − 2C)ξ + xT N T u
2= uT ν

(34)となる. (34) 式の両辺を積分することでT
0uT νdτ = V (T ) − V (0) ≥ −V (0) := −β0(35)が成り立つ. β0 は初期状態のみに依存するある非負の定
数である.

□

この補題は, (30) 式の動的視覚フィードバックシステム
が受動性を有していることを示唆している. したがって

T T T
T
b T T
で表される. ただし, u := [uTξ ud ue ] , w := [τd (Vwo ) ]動的視覚フィードバックシステムは, (22) 式の視覚フィーで定義される. 動的視覚フィードバックシステムにおけドバックシステムの受動性を保存しているとみなすこと

る制御目的は, 運動する観測対象にマニピュレータの手ができる. また Ṁ −2C の歪対称性と p̂ec と p̂ee の歪対先を追従させることであり, 状態 ξ, ec および ee を 0 に

称性は, 視覚フィードバックシステムの受動性とマニピュ

とどめておくことで制御目的が達成される.

レータダイナミクスの受動性の類似点のひとつと考えられる. Fig. 4 に示す閉ループ系のブロック図からも, 動的
視覚フィードバックシステムがマニピュレータと Fig. 3

1

手先の速度の目標値の時間微分である u̇d を Kc , gd ,Wc , ḡ˙ ho を用いて得ることにより, 関節角加速度の目
標値 q̈d は得られる.
2
動的視覚フィードバックシステムでは, 関節角度の目
標値 qd は逆運動学問題を解くことで求めなければなで示した視覚フィードバックシステムを結合したシステムであると解釈できる.

3.3

動的視覚フィードバック制御則と安定性
動的視覚フィードバックシステム (30) に対し, 外乱がない (すなわち w = 0) 場合に平衡点 x = 0 を安定とする
制御則として次式を提案する.

らないが, 提案する制御則では JbT (q)AdT
Wc ec を(g −1 )d

偏差 eq := q − qd のフィードバック項の代わりとして用いているため, 逆運動学問題を解く必要はない.

–5–

6

システム制御情報学会論文誌

第0巻

T

Ad(gd-1)Wcecb
Vwh(=Jb(q)q)_ud
ux

Fig. 4

x

ue

V̇ =

nce

Block diagram of dynamic visual feedback system⎡⎤

Kξ 0 0⎥⎢u = −Kν = −KN x, K := ⎣ 0 Kc 0 ⎦0 0 KeT

T

V̇ = u ν = −x N KN x1
1
γ2
w2 − x2 + x222
2
1
−xT N T KN x + 2 xT W x
2γ


 2

0
γ2 
1 I 0


− w − 2x2
γ 0 0 Ad(e−ξ̂θee ) We 

(39)となる. このとき最悪外乱として w は

(36)

1
w= 2
γ

Kξ := diag{kξ1 ,···,kξn } は各関節に対するゲインであり, Kc := diag{kc1 ,···,kc6 } と Ke := diag{ke1 , ···,ke6 }は x 軸, y 軸, z 軸の並進と回転における制御偏差と推定偏差に対するゲインである. ただし, ゲインにおける各要素はすべて正とする.
このとき, 補題 2 で示した動的視覚フィードバックシ
ステムの受動性に基づくことで, 安定性に関してつぎの定理が導かれる.
【定理 1】 w = 0 のとき, 動的視覚フィードバックシ
ステム (30) と (36) 式の入力で構成される閉ループ系の平衡点 x = 0 は漸近安定である.
（証明） (33) 式のエネルギー関数を Lyapunov 関数候補とすると, (30), (36) 式の解軌道に沿った時間微分は(34) 式よりT(0000)

微分し, 平方完成を用いるとVisualFeedback
SystemManipulator
Dynamics

第0号




I 0
0
x
0 0 Ad(e−ξ̂θee ) We

(40)で与えられる. したがって, 任意の w に対して以下の不等式が成り立つ.

γ21V̇ + x2 − w2
2
2
≤ −xT N T KN x += −xT P x

1 T
1x W x + x2
2γ 2
2
(41)

P ≥ 0 とし, 両辺を積分すると
T
0(γ 2 w2 − x2 )dτ ≥ 2V (T ) − 2V (0)≥ −2V (0) := −β(42)が成り立つ. β は初期状態にのみ依存する非負の定数である. したがって, P ≥ 0 のとき動的視覚フィードバック(37)

システムは外乱 w から状態 x に関して γ 以下の L2 ゲと導かれる. ゲイン Kξ , Kc , Ke の正定性より行列 K

インを有する.

が正定であり, N が正則であるため, システムの平衡点

この制御性能解析では観測対象の運動を外乱としてとら

x = 0 は漸近安定となる.

□えているために, γ が小さいコントローラであれば観測マニピュレータダイナミクスを考慮した 3 次元動的視覚

対象の運動が状態に与える影響がより少ないことを示し

フィードバックシステムに対し, Lyapunov の安定定理□ている. 本論文では, H∞ 制御における最も基本となるによりその安定性を示した. この安定性解析は補題 2 で外乱抑制問題を考えた. 本論文で提案した手法は, 適切示した受動性に基づいておこなわれている.

な一般化プラントを構成することで, 外乱抑制問題以外

3.4にも拡張可能である.

L2 ゲイン制御性能解析

本節では外乱が存在する場合について考察する. 特に,

4.

本論文では外乱抑制問題を考えることで L2 ゲイン制御

4.1

シミュレーション設定
本節のシミュレーションでは, マニピュレータとして水平 2 自由度マニピュレータである SICE-DD アーム [16]を考える. しかし, 提案する制御則自体は 3 次元空間を運動する観測対象 (並進 3 自由度, 回転 3 自由度) に追従する 3 次元の動的視覚フィードバック制御則である.
座標系は Fig. 5 に示すように, マニピュレータの台座を基準座標系の原点とし, 四つの座標系を定める. カメラ座標系の原点は [0.47 0.1 0.19]T [m] とする. また, マ
ニピュレータは台座に近いリンク 1 (0.2[m]) とリンク
2 (0.3[m]) によって構成されている. 観測対象は 2 秒
間直線運動をさせた後, 2.8 秒間は 8 の字運動をさせる. Fig. 6 に観測対象のトラジェクトリーを示す. 観測

性能解析をおこなう.
ここで, ある正の数 γ としてつぎの行列を定義する.

P := N T KN −

1
1
W− I
2γ 2
2

シミュレーション(38)

ただし W := diag{I,0,We2 } とする. このとき, 制御性
能解析に関するつぎの定理が示される.
【定理 2】 与えられた γ に対して, P ≥ 0 を満たすよ
うにゲイン Kξ , Kc , Ke および重み Wc , We を選ぶと
き, (30), (36) 式で構成される動的視覚フィードバックシ
ステムは γ 以下の L2 ゲインを有する.
（証明） 蓄積関数として (33) 式のエネルギー関数を考える. (30), (36) 式の閉ループ系の解軌道に沿って時間

–6–

7

河合・村尾・東・藤田：受動性に基づく 3 次元動的視覚フィードバック制御の固定カメラ構造への展開
10
5
0
-5
-10
0
0.04
0.02
0
-0.02
-0.04
0
0.1
0.05
0
-0.05
-0.1
0

xc
5c
Camera Frame

qwh(=q1+q2) gco
yh

gwh

5w
World Frame

gwo

5h
Hand
Frame

xh

gho

zo
qwo

yo

xo

5o Object Frame

Fig. 7
Coordinate frames for dynamic visual feedback
system with two degree of freedom manipulator

0

0

0.1

0.1

0.2

0.2

0.6

0.45
xo [m]( 0 < t < 2 [s] )Fig. 6

0.3

0.6

0.45
xo [m]

norm || x || (Gain B)

yo [m]-0.1

yo [m]

-0.1

4

6

2
4
time [s]

6

-5
0
0.04
0.02
0
-0.02
-0.04
0
0.1
0.05
0
-0.05
-0.1
0

2

4

6

2

4

6

2
4
time [s]6Control errors and estimation errors(Gain A: dashed, Gain B: solid)
norm || x || (Gain A)

Fig. 520peey [m]

q1
xw

zh

-q2

6

pecy [m]

yw

gch

4

e^xqecz [rad]zw2x 10-3e^xqeez [rad]gwc

5pecx [m]ycx 10-3peex [m]zc

0.3( 2 < t < 4.8 [s] )Trajectory of the target object

0.12
0.1
0.08
0.06
0.04
0.02
0
0

1.0

2.0

4.0

5.0

6.0

0.12
0.1
0.08
0.06
0.04
0.02
0
0

3.0
time [s]

1.0

2.0

3.0
time [s]

4.0

5.0

6.0

Fig. 8Euclid norms of x

対象の特徴点は四角形の四つの頂点とし, その中心が観測対象の座標系の原点である. 目標相対位置姿勢は(top: Gain A, bottom: Gain B)

pd = [0 0 − 0.81]T , eξ̂θd = I とし, 初期状態では偏差は0 とする.

とが確認される. したがって, 提案する制御則の有効性

4.2

シミュレーション結果
本節では漸近安定性と L2 ゲイン制御性能解析についてのシミュレーション結果を示す. γ = 0.437 を満たすゲインとして, Gain A を Kc = diag{160,160,80,80,80,160},
Ke = 25I6 , Kξ = 20I2 , Wc = 0.1I6 , We = I6 とした. また, γ = 0.203 を満たすゲインとして Kc = diag{200,200,
100,100,100,200}, Ke = 50I6 という Gain B を定めた.
ただし, Kξ , Wc , We は Gain A と同じ値とした.
Figs. 7, 8 にシミュレーション結果を示す. Fig. 7 は 3次元空間での x, y 軸の位置偏差と z 軸の回転偏差を示す. 図中における左図が制御偏差, 右図が推定偏差である. また, Fig. 8 には状態 x のユークリッドノルム xを示す. Fig. 7 において, 図中の破線が Gain A の場合を, 実線が Gain B の場合を示している. Fig. 8 では上図が Gain A の場合を, 下図が Gain B の場合を示している. Figs. 7, 8 からも明らかなように, 外乱がある場合 (すなわち 0 ≤ t ≤ 4.8) は, Gain B のほうが偏差が少なく追従性能が高いことが示されている. また, 外乱がない場
合 (t > 4.8) には, 状態が平衡点 x = 0 に収束しているこがシミュレーションによって示された. 実機による実験
は今後の課題であるが, 文献 [17] で示した Eye-in-Hand
構造のシステムにおける実験と同様にしておこなうことが可能である.

5.

おわりに

本論文では, マニピュレータダイナミクスを考慮し, 四つの座標系を有する固定カメラ構造の 3 次元動的視覚
フィードバックシステムを構成した. そして, 動的視覚
フィードバックシステムが有する受動性に基づいて制御則を提案し, Lyapunov の安定定理により安定性解析をおこなった. また, 観測対象が運動する場合に外乱抑制問
題を考えることで, L2 ゲイン制御性能解析をおこなった.
この L2 ゲイン制御性能解析は消散性理論に基づいて示されている. 最後に, シミュレーションにより安定性とL2 ゲイン制御性能解析の有効性を検証した. 今後の課題
は, カメラキャリブレーションの考慮や実機による実験などである. また, 四つの座標系を有する視覚フィード–7–

8

システム制御情報学会論文誌

第0巻

バックシステムにおいて, カメラ座標系を固定せず, 従来

第0号(0000)

perspective view of 3 points; Technical Report, INRIA (1993)[14] F. Bullo and R. Murray: Tracking for fully actuatedmechanical systems: a geometric framework; Automatica, Vol. 35, No. 1, pp. 17–34 (1999)[15] A. van der Schaft: L2 -Gain and Passivity Techniques in Nonlinear Control (2nd ed.), Springer–
Verlag (2000)[16] 小林 ほか: ロボット制御の実際, コロナ社 (1997)[17] H. Kawai, T. Murao and M. Fujita: An experimental
study of dynamic visual feedback control on SICEDD arm; Proc. of the 42nd SICE Annual Conference,
pp. 686-691 (2003)

研究ですでに提案した Eye-in-Hand 構造の視覚フィー
ドバックシステムと併せることで, 構造上の特徴を両方有する視覚フィードバックシステムに対して議論可能になると考えられる.

参 考 文 献[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] 橋本, 田中, 則次: 視覚サーボにおけるポテンシャル切り替え制御; 計測自動制御学会論文集, Vol. 36, No. 8,
pp. 660–667 (2000)[3] Y. Mezouar and F. Chaumette: Optimal camera
trajectory with image-based control; Int. Journal of
Robotics Research, Vol. 22, No. 10–11, pp. 781–804(2003)[4] R. Kelly: Robust asymptotically stable visual servoing of planar robots; IEEE Trans. Robotics andAutomation, Vol. 12, No. 5, pp. 759–766 (1996)[5] B. E. Bishop and M. W. Spong: Adaptive calibrationand control of 2D monocular visual servo systems;
Control Engineering Practice, Vol. 7, No. 3, pp. 423–
430 (1999)[6] E. Zergeroglu, D. M. Dawson, M. S. de Queiroz
and A. Behal: Vision-based nonlinear tracking controllers with uncertain robot-camera parameters;
IEEE/ASME Trans. Mechatronics, Vol. 6, No. 3,
pp. 322–337 (2001)[7] R. Kelly, R. Carelli, O. Nasisi, B. Kuchen and
F. Reyes: Stable visual servoing of camera-in-handrobotic systems; IEEE Trans. Mechatronics, Vol. 5,
No. 1, pp. 39–48 (2000)[8] N. J. Cowan, J. D. Weingarten and D. E. Koditschek:
Visual servoing via navigation functions; IEEE
Trans. Robotics and Automation, Vol. 18, No. 4,
pp. 521–533 (2002)[9] A. Maruyama, H. Kawai and M. Fujita: Passivitybased visual feedback control of nonlinear mechanicalsystems –Stability and L2 -gain performance analysis;
システム制御情報学会論文誌, Vol. 15, No. 12, pp. 627–
635 (2002)[10] 河合, 東, 藤田: マニピュレータダイナミクスを考慮した
オブザーバ型動的視覚フィードバック制御; システム制
御情報学会論文誌, Vol. 17, No. 1, pp. 39–47 (2004)[11] S. Yu and B. J. Nelson: Autonomous injection of
biological cells using visual servoing; Experimental
Robotics VII (D. Rus and S. Singh (Eds)), Springer–
Verlag, pp. 169–178 (2001)[12] R. Murray, Z. Li and S. S. Sastry: A Mathematical Introduction to Robotic Manipulation, CRC Press(1994)[13] H. Michel and P. Rives: Singularities in the determination of the situation of a robot eﬀector from the付録

付録 1. (3) 式の導出

(2) 式より gco の基本式を導出する. (2) 式を時間微分
すると
−1
−1
ġco = ġwc
gwo + gwc
ġwo
−1
−1
−1
−1
= −gwcġwcgwc
gwo + gwc
gwo gwo
ġwo

(A1)

−1
が導かれる. ここで, gab gab= I4 の性質を用いている.
b
T
T T
剛体運動のボディ速度を Vab= [vabωab] ∈ R6 (ただし,vab ∈ R3 は並進速度, ωab ∈ R3 は角速度を表す) と定義するとき, 同次表現を用いたボディ速度は


v
ω̂
ab
ab
−1
b
∈ R4×4= gabġab =
(A2)
V̂ab
0 0
−1
で表される [12]. そこで, 上式の両辺に左から gco
を乗じ, 速度の定義と (2) 式に注意するとb−1 b −1
−1 −1
b
V̂co
V̂wc gwc gwo + gco= −gcogwc gwo V̂wo
−1 bb
V̂wc gco + V̂wo
= −gco(A3)が成り立つ. さらにV̂ b = gV̂ b g−1 ⇐⇒ V b = Ad(g) V beξ̂θ p̂eξ̂θAd(g) :=
0 eξ̂θ

(A4)の関係 [12] を用いると, (A3) 式より gco の基本式として
b
b
b
Vco
= −Ad(gco−1 Vwc + Vwo)(A5)が導かれる.
付録 2. 補題 1 の証明（証明） (25) 式のエネルギー関数 Vce の時間微分は,並進速度の定義 (vab = e−ξ̂θab ṗab ) と eξ̂θ e−ξ̂θ = I3 に注意するとξ̂θec −ξ̂θec
ξ̂θec ξ̂θec
eṗec + eT)eωecV̇ce = pTec e
R (eξ̂θee −ξ̂θeeξ̂θee ξ̂θee+pT
eṗee + eT
)e
ωee
ee e
R (e

–8–

河合・村尾・東・藤田：受動性に基づく 3 次元動的視覚フィードバック制御の固定カメラ構造への展開


T

=e


T

=e



Ad(eξ̂θec )00Ad(eξ̂θee )Ad(eξ̂θec )00Ad(eξ̂θee )−Ad(ḡ−1 )
ho

0



b
Vec
b
Vee



I

−Ad(gee
−1
)

むら

お

ゆき

俊 幸 (学生会員)
2003 年金沢大学工学部電気・情報工学科卒業．同年金沢大学大学院博士前期課程
入学，現在に至る．視覚フィードバック制
御に関する研究に従事．計測自動制御学会
の会員．


uce

(A6)

となる. ここで, p̂ec と p̂ee の歪対称性に注意すると

あずま

たけ

東

剛 人 (正会員)
1997 年早稲田大学大学院修士課程修了．
1998 年早稲田大学理工学部助手．2000 年
同大学院博士課程修了．2000 年金沢大学

−ξ̂θd
−ξ̂θd
pT
ωwh = −pTωwh )∧ pec = 0
ec p̂ec e
ec (e
T
pT
ee p̂ee ωue = −pee ω̂ue pee = 0

とし

村 尾

(A7)

ひと

工学部助手．システムバイオロジー，コン
ピュータネットワークの輻輳制御，むだ時が成り立つことから, (A6) 式はV̇ce = eT−Ad(g−1 ) Ad(eξ̂θec )
d

0



−I

間システムの制御およびゲインスケジューリングに関する研究に従事．博士（工学）．計測自動制御学
会，電気学会，IEEE の会員．

uce

= uT
ce νce(A8)

ふじとなる. (A8) 式の両辺を積分することでT
0□

著 者 略 歴
かわ

い

河 合

まさ

ゆき

政 之 (正会員)
1984 年早稲田大学大学院理工学研究科
博士前期課程修了（電気工学専攻），1985

年同博士後期課程中退．同年金沢大学工学部助手．同講師，助教授を経て，1992 年北陸先端科学技術大学院大学情報科学研究科
助教授．1999 年金沢大学工学部教授．ロ
バスト制御とその応用に関する研究に従事．工学博士．計測(A9)が成り立つ. ただし βce は初期状態のみに依存するある
非負の定数である.

た

藤 田

uT
ce νce dτ = Vce (T ) − Vce (0)≥ −Vce (0) := −βce9自動制御学会，IEEE などの会員．

ひろ ゆき

宏 之 (正会員)
1999 年金沢大学工学部電気・情報工学科
卒業．2001 年金沢大学大学院博士前期課
程修了，2004 年金沢大学大学院博士後期課程修了，同年法政大学情報技術 (IT) 研

究センターポストドクター．視覚フィード
バック制御に関する研究に従事．博士（工学）．計測自動制御学会，IEEE の会員．

–9–