ナビゲーション関数を用いた視覚フィードバック制御の目標値生成に対する一考察
村尾俊

幸 ∗ ・河合宏之 ∗∗A Study on Path Planning for Visual Feedback Control via Navigation
Function
Toshiyuki Murao∗ and Hiroyuki Kawai∗∗
Abstract
This paper investigates passivity-based pose control via a navigation function for three-dimensional (3D) eye-in-hand visual feedback systems. Firstly, we describe the representation of a relative pose anda camera model. Next, a passivity-based stabilizing pose control law for the 3-D visual motion error
system which the time-varying desired pose can be applied to is proposed. Moreover, a path planner tobe appropriate for the visual motion error system is designed through a navigation function. Finally,
we present simulation results in order to verify the validity of the proposed control scheme.
Keywords: visual feedback control, navigation function, passivity, stability

1. はじめに構造と，固定カメラ構造とも言われるカメラとハンドが別々

Cameraに配置された Eye-to-Hand 構造である [1, 2]．文献 [6] は，
位置ベース法のようにカメラから目標となる対象物 (以下，

5c

目標対象と呼ぶ) への相対位置姿勢をもとに入力を与える制

gco

gwc

御手法となっているが，通常の位置ベース法が各時点における静的な画像情報を用いて相対位置姿勢を推定するのにTargetObject対して，動きのモデルを用いて位置姿勢を推定し，かつ受動

5o

gwo

5w

World Frame

図 1: Visual feedback system with an eye-in-hand configuration.

性を巧みに扱うことにより推定と制御を含めた全体の安定性について議論するという点で新しい制御アプローチとなっている. この受動性に基づく視覚フィードバック制御の手法
は，Eye-in-Hand 構造 [6] だけでなく, Eye-to-Hand 構造[7] に対しても同様のアプローチで制御則を提案できる．
近年，視覚フィードバック制御の研究はますます盛んに行われており，工場だけでなくより複雑なシステムや幅広

本論文では，図 1 で示す Eye-in-Hand システムの受動い分野へも適用されるようになってきている [1, 2]．中でも性に基づく視覚フィードバック制御 [6] に対する目標値生

少子高齢化社会を迎える日本において必要不可欠とされる成手法について議論する．著者らは文献 [8] にて，常に障医療分野では，視覚フィードバック制御による腹腔鏡の制害物が見えているという仮定のもと，カメラから得られる

御 [3] や超音波探触子の制御 [4] などが，また生命科学の分

視覚情報を用いて障害物を回避できるような目標値生成手

野では，細胞注入 [5] などの応用研究が行われている．

法をナビゲーション関数を用いることで提案している．し視覚フィードバック制御則は，従来より二つの制御方法とかし実システムにおいては，全方位カメラ等を用いない限

二つのカメラ構造に大別できることがよく知られている．すり，カメラが障害物を回避した後は障害物が見えなくなる

なわち，制御方法は位置ベース法と画像ベース法，カメラ構ことが多い．そこで本論文では，障害物をかわした後に切造はカメラがハンドの手先に取り付けられた Eye-in-Hand

り替えるための目標値生成手法について提案する．具体的

Received on October 22, 2012.
∗ 産業技術大学院大学, Advanced Institute of Industrial Technology∗∗ 金沢工業大学, Kanazawa Institute of Technology

xiには，障害物を回避した後は再びその障害物には衝突しな

Camera Framexc

いという仮定をおき，カメラがある作業範囲内から外に出ることなく最終目標位置姿勢へ収束することのみを目的とfcoyi

した目標値生成手法を提案する．

zc

l

fcoi
zo

本論文の構成は以下のとおりである．2 節では，従来研究[6] で提案している Eye-in-Hand 構造の 3 次元視覚フィー
ドバック制御について述べる．3 節では，2 節で述べた制御

pcoi

yi
fcoxi

gco

poi

レーション結果を示した後，最後に 5 節でまとめをおこなう．

xo

yo

5o

World Frame

画像特徴量を fcoi := [fcoxi fcoyi ]T と定義すると，特徴

本論文では，図 1 に示すような，三つの座標系によってについて考える．基準とする座標系として基準座標系 Σw ，
カメラにカメラ座標系 Σc ，目標対象に目標対象座標系 Σo5w

図 2: Pinhole camera model.

2.1. 剛体運動の表現とカメラモデル表される Eye-in-Hand 構造の視覚フィードバックシステム

xw
yw

Target Object

2. 受動性に基づく安定化視覚フィードバック制御

zw

Image Plane

Pのための目標値生成手法について議論する．4 節でシミュ

5cyc

量は透視変換を用いることで，
»
–
λ
xcoi
fcoi =
zcoi ycoi(4)を定義する．また，任意の二つの座標系 Σa から Σb の位置で表される． ただし，λ ∈ R はカメラの焦点距離であり，

pab ∈ R3 と姿勢を表す回転行列 eξ̂θab ∈ SO(3) を要素に持pcoi の各要素を pcoi = [xcoi ycoi zcoi ]T として表した．このつ同次表現 gab を以下のように定義する．

»
gab =

eξ̂θab
0

pab
1

–

T
T T
fcoi を縦に並べたベクトル fco = [fco1· · · fco4
] ∈ R2mが実際にカメラから得られる視覚情報となる．
4×4

∈R

(1)

2.2. 視覚フィードバック偏差システム

ただし，演算子 ∧(wedge) は 3 次元ベクトルを 3 × 3 の歪

一般的な位置ベース法の視覚フィードバックシステムの対称行列へ写像する演算子であり，その逆写像は ∨(vee) で制御目的は，カメラ座標系 Σc からみた目標対象座標系 Σo定義される [9]．

の相対位置姿勢 gco を目標位置姿勢 gcd に常に一致させてカメラ座標系からみた目標対象の相対位置姿勢 gco は同が，実システムを考慮に入れ，最終値は一定値とする．制

次表現の合成則から以下のように導かれる．
−1
gco = gwc
gwo

おくことである．ただし，目標位置姿勢 gcd は時変とする
御則を構成するにあたり gco を直接得ることができないた(2)

また，カメラ座標系からみた目標対象の相対速度は，gco をめ，gco の推定値 ḡco を得るために (3) 式に基づいたつぎの
視覚オブザーバを構成する．
bV̄co

時間微分することで，
b
Vco

=

b
b
−Ad(gco−1 Vwc + Vwo
)

(3)

−1
b
のように導かれる [6]．ここで，Vab= (gabġab )∨ ∈ R6 は剛体運動のボディ速度を表しており，Ad(gab ) ∈ R6×6 は同=b−Ad(ḡco−1 Vwc + ue)

(5)ue ∈ R6 は推定偏差の振舞いを安定にするために加えられる入力であり，後に制御則の一部として提案する．
ここで，制御偏差 gec と推定偏差 gee をつぎのように定義する．

次表現 gab の随伴写像である [9]．

gec一方，上述の相対位置姿勢 gco は目標対象の位置姿勢 gwo

=

−1
gcd
gco

(6)

=

−1
ḡco
gco

(7)

が測定不可能であるために未知情報となる．本論文では，目gee

標出力として得られる情報はピンホールカメラ (図 2 参照)

また，rei := sk(eξ̂θei )∨ ∈ R3 ，sk(eξ̂θei ) :=−ξ̂θei1(eξ̂θei −2
T
T T) を用いて，制御偏差ベクトルを ec := [pec rec] ∈から得られる画像特徴量とし，目標対象に取り付けられたem(≥ 4) 点の特徴点の画像面上における x 座標と y 座標を

T T] ∈ R6 と定義
R6 と，推定偏差ベクトルを ee := [pTee ree

考える．このとき，目標対象座標系 Σo の原点から各特徴点する．推定偏差ベクトル ee は視覚情報 fco と推定値 ḡco かまでのベクトルを poi ∈ R3 (i = 1, · · · , m) とすると，カメラからみた相対的な特徴点 pcoi ∈ R3 は pcoi = gco poi で表される．ただし，特徴点 poi はすべて既知であるとし，同らつぎのように得ることができる．

ee = Je† (ḡco )(fco − f¯co )(8)T
次表現でよく用いられる表記法 ([pTab 1] を pab と表記 [9])ここで，f¯co は画像特徴量 fco の推定値を表し，Je (·) は画により表したものである．

像ヤコビアンの一種である [6]．また，目標値 gcd と推定偏gcd

差ベクトル ee を用いると，制御偏差ベクトル ec は計算可能である [7]．
視覚フィードバック偏差システムは (6)(7) 式を時間微分することで下記のように導出することができる．
"
#
» b –
» –
−Ad(gec
0
−1
Vec
I
b
)
=
(9)
u
+
Vwo
b
0
−Ad(g−1 )IVeeee

ただし，入力を

u := [uTc uTe ],bbuc := Ad(g−1 ) Vwc
+ Vcd

(10)

cd

と定義している．目標位置姿勢を時不変としていた文献 [6]の視覚フィードバック偏差システムと本論文の偏差システ
ム (9) は同じ形となっているが，(10) 式にあるように，入力

u の一部に目標位置姿勢の速度 (以下では，目標速度とする)
bが陽に入るところが異なる．Eye-in-Hand 構造の視覚
Vcd
T T
フィードバック偏差システムにおいて，状態 x := [eTc ee ]b
Vcd

fco

Visual Motionb
Vwc

Observer-based
Pose Controllerb
Vwo

VisionCamera

図 3: Block diagram of visual motion observer-basedpose control.

2

定となる．

本節ではカメラ座標系からみた目標対象の相対位置姿勢

gco を目標値 gcd に一致させるための安定化制御則を提案した．紙面の都合上省略するが，外乱が存在する場合には外乱抑制問題を考えることで L2 ゲイン制御性能解析を行うことが可能である [6]. 図 3 に受動性に基づく安定化視覚フィードバック制御のブロック図を示す．

を 0 にとどめておくことで相対位置姿勢 gco が目標位置姿
勢 gcd に一致することになる．この視覚フィードバック偏b= 0) して差システム (9) は目標対象が静止 (すなわち Vwo

いるとき，入力 u から出力 −x に対して受動性を有することを，以下のエネルギー関数を用いることで証明すること

3. ナビゲーション関数を用いた目標位置姿勢生成
前節では，カメラからみた目標対象の相対位置姿勢を目標値に一致させる安定化制御則を提案したが，目標値に対する議論はなかった．本研究の視覚フィードバック制御のができる．

V (x) = E(gec ) + E(gee )制御目的は，ある作業範囲内に留まりつつ，目標位置姿勢(11)の最終値 gcdf = (pcdf , e

ここで E(gab ) := 12 pab 2 +φ(eξ̂θab ) であり，関数 φ(eξ̂θab )標位置姿勢 gcd = (pcd , eは回転行列に対する誤差関数を表す正定関数である [10]．

みた目標対象の相対位置姿勢 gco = (pco , eξ̂θco ) を一致させ

ξ̂θcd

ξ̂θcd

f

) に収束するような時変の目) を生成し，それにカメラからることとなる．

2.3. 安定化視覚フィードバック制御

そこで本節では，目標位置姿勢 gcd がある範囲に留ま

Eye-in-Hand 構造の視覚フィードバック偏差システム (9)に対し，受動性に基づくことで，目標対象が静止している
場合に平衡点 x = 0 を安定とする制御則として次式を提案する．

りながら最終値 gcdf へ収束するような gcd のボディ速度
b
b T
b T T
b
Vcd
= [(vcd) (ωcd) ] を設計する. 目標速度 Vcdが設計されれば，gcd はつぎの微分方程式を解くことで生成できることに注意されたい.

u = −K(−x) = Kx

(12)bġcd = gcd V̂cd

K := diag{Kc , Ke }Kc := diag{kc1 ，· · · , kc6 } と Ke := diag{ke1 ，· · · , ke6 }は x 軸，y 軸，z 軸の並進と回転における制御偏差と推定偏差に対するゲインである．ただし，ゲインにおける各要
素はすべて正とする．このとき安定性に関してつぎの定理(14)

前節で提案した視覚フィードバック制御則 (12) と (10) 式bはで表される偏差システムの入力により，カメラ入力 Vwc

“
”
b
b
Vwc
= Ad(gcd ) Kc ec − Vcd(15)bのように構成されるが，目標速度 Vcdと目標位置姿勢 gcdが導かれる．
b= 0) とき，Eye定理 1. [6] 目標対象が静止している (Vwoin-Hand 構造の視覚フィードバック偏差システム (9) と (12)式の入力で構成される閉ループ系の平衡点 x = 0 は漸近安定である．

を用いることで生成できるようになる．

3.1. 目標軌道生成のための姿勢偏差b
目標速度 Vcdを設計するにあたり，本節では姿勢に対して議論する．作業範囲内の維持は，位置成分を用いて達成さξ̂θcd

証明 : (11) 式のエネルギー関数をリアプノフ関数候補とすせるために，姿勢成分は目標姿勢 eξ̂θcd をその最終値 e

ると，(9)，(12) 式の解軌道に沿った時間微分はに収束させることのみを目的とする．はじめに，つぎの姿V̇ = uT (−x) = −xT Kx(13)と導かれる．したがって，システムの平衡点 x = 0 は漸近安

f

勢偏差を定義する．

eξ̂θed = e−ξ̂θcdfeξ̂θcd(16)

また，偏差ベクトルを red := 12 (eξ̂θed − e−ξ̂θed )∨ として定義する．目標位置姿勢の最終値 gcdf が時不変であることを考慮すると, (16) 式を時間微分することで，姿勢偏差速度の関係式はつぎのように導出することができる．
b
ωed

=

b
ωcd

(17)

3.3. 目標速度の提案と収束解析
ナビゲーション関数および姿勢偏差を用いることで，つぎの目標速度を提案する．
"#−e−ξ̂θcd Kdp ∇ϕ(pcd )b
Vcd =−Kdr e−ξ̂θed red
た だ し ，Kdp

3.2. ナビゲーション関数
本節では，作業範囲内に留まりながら最終目標位置へ収束させるためのナビゲーション関数 ϕ(pcd ) を設計する．
ナビゲーション関数はつぎで定義される特殊人工ポテン
シャル関数である．

:=

(22)

diag{kdp1 , kdp2 , kdp3 } と Kdr:=

diag{kdr1 , kdr2 , kdr3 } は，それぞれ x 軸，y 軸，z 軸の並進と回転における目標速度に対するゲインである．また,
”T
“
cd )
は ϕ(pcd ) の勾配ベクトルを表し，∇ϕ(pcd ) := ∂ϕ(p∂pcd

計算すると次ページ下記に示す (23) 式となる．
提案した目標速度 (22) を用いることで，つぎの定理が成

定義 3..1. [11, 12] 滑らかなモース関数 ϕ(pcd ) : D → [0, 1]
が以下の性質を有するとき，ϕ はナビゲーション関数である．

1. pcdf ∈ D のとき唯一の最小値が存在する．

立する．
b
= 0)
定理 2. pcd (0) ∈ D かつ目標対象が静止している (Vwoとする．このとき，(22) 式によって生成される目標トラジェ

2. D の境界で最大値を持つ．

クトリーは pcd (t) ∈ D であり，かつ目標位置姿勢 gcd (t) はここで，D はカメラが移動可能な領域を中心 p0 ，半径 ρ0

漸近安定な平衡点 gcdf を持つ．

∗の球体で定義した集合である ．

証明 : つぎの正定な関数を考える．

ナビゲーション関数を具体的に設計していく．はじめに

Vn = ϕ(pcd ) + φ(eξ̂θed )

目標位置 pcd とその最終値 pcdf との間の偏差に関するつぎの関数を定義する．

(24)(17)(22) 式を用いると，(24) 式の正定関数を解軌道に沿っ2κ

s(pcd ) = pcd − pcdf (18)

ここで，κ は形成されるポテンシャル場を変化させるためて時間微分することにより，つぎの関係式が得られる．
T ξ̂θed beωedV̇n = (∇ϕ)T ṗcd + redのパラメータである．つぎに，移動可能領域 D の内部にカbT ξ̂θed b= (∇ϕ)T eξ̂θcd vcd
+ red
e
ωcd

メラを維持させるための関数 η をつぎのように定義する．

= −(∇ϕ)T eξ̂θcd e−ξ̂θcd Kdp ∇ϕη(pcd ) = ρ20 − pcd − p0 2(19)

T ξ̂θede
Kdr e−ξ̂θed red−red= −(∇ϕ)T Kdp ∇ϕ − (e−ξ̂θed red )T Kdr e−ξ̂θed red(25)

この関数 (19) は 0 に近づくほど領域の端に近くなることを示している．また，パラメータ κ を用いたモデル空間上のナビゲーション関数 ϕ̃(x) ∈ R → [0, 1] をつぎのように定義する．

„
ϕ̃(x) =

x
μ+x

κ

(20)

(18)–(20) 式の関数を組み合わせることで，ナビゲーション
関数を„s(pcd )μη(pcd ) + s(pcd )D, ∀t > 0 となることがわかる．またナビゲーション
関数では ∇ϕ(pcd ) → 0 のときには pcd (t) → pcdf と«1

ここで，μ は収束速度を調整するためのパラメータである．

sϕ(pcd ) = ϕ̃ ◦ (pcd ) =η

したがって，初期状態 pcd (0) ∈ D に対して，pcd (t) ∈ために，姿勢偏差の定義より eξ̂θcd (t) → eξ̂θedfる．したがって，gcd (t) → gcdf が導かれる.

とな

2

定理 2 はナビゲーション関数をリアプノフ関数候
補の一部とすることで証明している．この定理 2 に«1

κなる [12]．さらに，e−ξ̂θed red → 0 は red → 0 となる(21)

より，提案したナビゲーション関数を用いた軌道生成をおこなうことで，目標位置姿勢 gcd (t) が作業範囲と設計する．ここで，◦ は合成演算子を表している. 紙面

内に留まりつつ最終目標位置姿勢 gcdf に収束するこの都合上証明は省略するが，文献 [12] の Theorem 4 と同とが示された．図 4 にナビゲーション関数を用いた様の手法で，κ を適切に設定した (21) 式の関数がナビゲー

受動性に基づく視覚フィードバック制御のブロック図

ション関数となることは証明できる．

を示す．

∗ 本稿では目標対象は静止しているとしているため，p は目標対象座標系 Σ を基準として考えることができる．たとえば，p = [0 0 0]To00とすると，目標対象を中心に半径 ρ0 の空間がカメラの移動可能領域となり，p0 = pco (0) とするとカメラの初期値が中心の空間となる．

gcdf

gcd
b
Vcd

Function-based
Path Planner

fco

Visual Motion
Observer-based
Pose Controller

b
Vwc

3

b
Vwo

VisionCamerapwcz [m]Navigation

pwc(3)

21

図 4: Block diagram of visual motion observer-basedpose control with navigation function-based path planner.

-0.2

-2
-3

0

1

2

3

-0.4

4
0

1

2

1

2

3

0

1

2

3

1
2
time [s]3-2-4 pwcx [m]いる．Fig. 6 より，目標対象の前へカメラが動いてい内部としたが，ρ20 − pcd − p0 2 の最小値が 23.75 であり，常に正の値を維持していることから作業範囲の外に出ていないことがわかる．したがって，ナビゲー

rerz

perz

0

-0.4

-2 -6

作業範囲を目標対象を中心とした半径 10 m の球体の

-0.2

0.5

0

2

ることが確認できる．また，本シミュレーションでは

0

1.5
1
0

0

0.2
0

3

0

図 6: Trajectory of camera.

0.4
rery

pery

8

0

4
pwcy [m] 20-1

pwc(0)

6

rerx

perx

0

8

ション関数を用いた視覚フィードバック制御において，
0

12time [s]3

カメラが作業範囲内に留まりつつ最終目標位置姿勢に収束することが示された．

図 5: Pose control error.

5. おわりに
4. シミュレーション検証

本論文では 3 次元 Eye-in-Hand 構造の視覚フィー

本節では，提案した制御手法の有効性を確認するた

ドバックシステムに対して，作業範囲内に留まらせつめに静止している目標対象に対するシミュレーション

つ最終目標位置姿勢へ収束させるためのナビゲーショ

検証をおこなう．

ン関数を用いた安定化視覚フィードバック制御手法を

初期設定は pwc = [0 0 1]T m，ξθwc = [π/12 −提案した．制御則への目標位置姿勢生成をナビゲー

π/12 π/12] rad，pwo = [−5 6.5 3] m，ξθwo =[0 0 0]T rad とする．また最終的な目標位置姿勢はション関数を用いて設計し，ナビゲーション関数をリ
アプノフ関数候補の一部とすることで設計する目標位

pcdf = [0 1.5 0]T m，ξθcdf = [0 0 0]T rad とする．さ
らに，本シミュレーションにおけるゲインの設定や各置姿勢が最終的な目標位置姿勢に収束することを示し

パラメータは以下のようにした．Kc = 5I ，Ke = 10I ，

御手法の有効性を示した．

T

Tている．最後にシミュレーションにより，提案した制

Kdp = 50I ，Kdr = 2I ，p0 = [0 0 0]T m, ρ0 = 10 m，
κ = 2，μ = 4.
Fig. 5 に相対位置姿勢 gco (t) と最終的な目標位置姿
勢 gcdf との間の偏差 er を示す．Fig. 5 より，全ての偏差が定常状態において 0 に収束していることが確認できる．また，Fig. 6 にカメラのトラジェクトリー

参考文献[1] F. Chaumette and S. Hutchinson, “Visual
Servoing and Visual Tracking,” B. Sicilianoを示す．pwc (0) と pwc (3) はカメラの初期値と最終値

and O. Khatib (Eds.), Springer Handbookof Robotics, Springer-Verlag, pp. 563–583,

を表しており，また図中の直方体が目標対象を表して

2008.

∇ϕ =



2μ κ(ρ20 − pcd − pc0 2 )(pcd − pcdf ) + pcd − pcdf 2 (pcd − pc0 )1+κκ(μ(ρ20 − pcd − pc0 2 ) + pcd − pcdf 2κ ) κ(23)

[2] 橋本浩一, (講座) ビジュアルサーボ− I〜VI, シ
ステム/制御/情報, Vol. 53, No. 9, pp. 411–416,
No. 11, pp. 476–483, Vol. 54, No. 1, pp. 34–
42, No. 3, pp. 117–123, No. 5, pp. 206–213,
No. 7, pp. 264–273, 2009–2010.
[3] K. Omote et al., “Self-Guided Robotic Cam-[10] F. Bullo and A. D. Lewis, Geometric Control of Mechanical Systems, Springer-Verlag,
2004.
[11] E. Rimon and D. E. Koditschek, “Exact
Robot Navigation using Artificial Potential
Functions”, IEEE Trans. Robotics and Automation, Vol. 8, No. 5, pp. 501–518, 1992.

era Control for Laparoscopic Surgery Compared with Human Camera Control,” The[12] D. E. Koditschek and E. Rimon, “Robot Nav-American Journal of Surgery, Vol. 177,
No. 4, pp. 321–324, 1999.

igation Functions on Manifolds with Boundary”, Advances in Applied Mathematics,
Vol. 11, No. 4, pp. 412–442, 1990.

[4] A. Krupa and F. Chaumette, “Control of an
Ultrasound Probe by Adaptive Visual Servoing,” Proc. of the 2005 IEEE/RSJ International Conference on Intelligent Robots andSystems, pp. 2007–2012, 2005.
[5] S. Yu and B. J. Nelson, “Autonomous injection of biological cells using visual servoing,”
In:D. Rus and S. Singh (Eds.), Experimental
Robotics VII, Springer-Verlag, pp. 169–178,
2001.
[6] 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.
[7] T. Murao, H. Kawai and M. Fujita, “Stabilizing Predictive Visual Feedback Control
for Fixed Camera Systems,” Electronics andCommunications in Japan, Vol. 94, No. 8,
pp. 1-11, 2011.
[8] Y. Tsuruo, T. Murao H. Kawai and M. Fujita,“Visual Motion Observer-based Pose Control
via Obstacle Avoidance Navigation Function
for Eye-in-Hand Systems,” Proc. of the 38th
Annual Conference of the IEEE Industrial
Electronics Society, 2012(to appear).
[9] R. Murray, Z. Li and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994.