受動性に基づく固定カメラシステムの3 次元視触覚フィードバック制御
河合宏之 (金沢工業大学) 村尾俊幸 (産業技術大学院大学) 藤田政之 (東京工業大学)

Visual Force Feedback Control for Fixed Camera Systems
based on Passivity Approach
H. Kawai (Kanazawa Institute of Technology), T. Murao (Advanced Institute of Industrial
Technology) and M. Fujita (Tokyo Institute of Technology)
Abstract– This paper investigates passivity based visual force feedback control for fixed camera systems.
The proposed method can be regarded as an extension of the hybrid position/force control to the hybrid
vision/force control. Both the passivity of the manipulator dynamics and the passivity of the visual feedback
system are preserved in the visual force feedback system. The main contribution of this paper is to showthat the visual force feedback system has the passivity which allows us to prove stability in the sense of
Lyapunov.
Key Words: visual feedback control, force control, passivity, stability

1

はじめに

視覚情報をフィードバックループに組み込んだ制御
は視覚フィードバック制御と呼ばれ, 様々な分野で適用されはじめている [1, 2]. 近年では, 実システムへの応用例として, 工場における組み立て用ロボット以外にもDNA を細胞へ注入するロボット [3] や腹腔鏡を把持す
る手術支援ロボット [4] に視覚フィードバック制御が適用されている. この腹腔鏡把持ロボットにおいては術者の視野を確保するための視覚フィードバック制御のほかに, 鉗子が臓器に触れるため力覚・触覚の情報も必要となってくる. そのため, 視覚と力覚・触覚を併せてひとつのシステムとして捉えることが重要となる.
接触する対象の位置姿勢が既知の場合は, 視覚を用
いずとも例えば文献 [5] のように位置と力のハイブリッ
ド制御による方法が有用である. しかし, 接触する対象の位置姿勢が未知の場合には視覚情報を用いることが有用となり, 文献 [6] や文献 [7] では視覚と力のハイブリッド制御による方法が提案され, 安定性について議論されている. しかしながら, これらの従来研究ではロボットが平面 2 自由度マニピュレータに限定されており, 3 次元の視覚フィードバック制御と力覚・触覚制御を融合し, その安定性まで議論された研究はなされていない. そこで, 本稿では従来研究 [8] で提案している
位置ベース法の 3 次元視覚フィードバック制御を基に,固定カメラ構造の視触覚フィードバック制御について考え, その安定性について議論する.

2

視覚フィードバックシステム

2.1

剛体運動の表現と推定偏差システム

本稿では, Fig. 1 に示すような四つの座標系によって表される視触覚フィードバックシステムについて考える. 図中の四つの座標系をそれぞれ基準座標系 Σw , 手
先座標系 Σh , カメラ座標系 Σc および観測対象座標系
Σo と定義する. このとき, 基準座標系からみた手先の位置姿勢, カメラの位置姿勢および観測対象の位置姿
勢をそれぞれ gwh = (pwh , eξ̂θwh ), gwc = (pwc, eξ̂θwc ),gwo = (pwo , eξ̂θwo ) で表す. ここで, 任意の二つの座標

Image
Camera

gch

5c
gco

5h

Force
Sensor

gwh

gwc
gho
Contact Surface

5o

gwo

5w

World Frame

Target Object

Fig. 1: Visual force feedback system with a ﬁxed camera conﬁguration.
系を ΣA , ΣB としたとき, pab ∈ R3 は ΣA の原点から
ΣB の原点への位置ベクトルを表し, eξ̂θab ∈ SO(3) (ただし, ξab ∈ R3 , θab ∈ R であり, 簡単化のため混乱がな
い限り添え字は θ のみにつけて表記する) は ΣA を基準とした ΣB の姿勢を意味する [9]. 演算子 ∧(wedge)
は 3 次元ベクトルを 3 × 3 の歪対称行列へ写像する演算子であり, その逆写像は ∨(vee) で定義される. gabは混同しない限り, よく知られた 4 × 4 行列の同次表現eξ̂θab pab
gab =
(1)
0
1
として用いる [9]. 基準座標系からみたそれぞれの位
置姿勢と同様に, カメラ座標系からみた手先の位置姿
勢と観測対象の位置姿勢を gch = (pch , eξ̂θch ), gco =(pco , eξ̂θco ) とし, 手先座標系からみた観測対象の位置
姿勢を gho = (pho , eξ̂θho ) と表す.
これらの座標系から, 視覚フィードバックシステムにおける三つの座標系間に成り立つ剛体運動の表現を導出する. まず, カメラ座標系からみた観測対象の相対位
置姿勢 gco は−1
gco = gwc
gwo(2)で表される. また, カメラ座標系からみた観測対象の相対速度は, gco を時間微分することで,
b
b
b
Vco
= −Ad(gco−1 Vwc + Vwo
)

(3)

−1
b
のように導かれる [8]. ここで, Vab= (gabġab )∨ ∈ R6
は剛体運動のボディ速度を表しており, Ad(gab ) ∈ R6×6
は同次表現 gab の随伴写像である [9].
一方, カメラから得られる視覚情報 f(gco ) は, 相対
位置姿勢 gco により変化するためその情報を含んではいるが, 二次元情報であるため直接 gco を得ることはできない. そこで, オブザーバを構成することで相対位
ˆ

置姿勢の推定値 ḡco = (p̄co , eξ̄θ̄co ) を得ることを考える.
本稿では固定カメラ構造の視覚フィードバックシステbム (すなわち Vwc= 0) を考えるため, gco の基本式は(3) 式よりR6 として, ec := [pTec eTR (eξ̂θec )]T と定義する. (10) 式
より, 制御偏差 gec は推定偏差 gee を用いて以下のよ
うに表すことができる.
−1
gho = gd−1 ḡho gee (11)gec = gd−1 gho = gd−1 ḡho ḡho(11) 式において, 目標値 gd と推定値 ḡho は既知情報であるため, 推定偏差 gee が得られれば, 相対位置姿勢
gho を直接得なくとも制御偏差 gec は求まることがわ
かる. このとき, − π2 ≤ θee ≤ π2 の範囲内で, 以下の関係式が成り立つ [10].
ξθee =

arcsin eR (eξ̂θee )
eR (eξ̂θee )

eR (eξ̂θee )(12)となる. そこで (4) 式から, 推定値 ḡco の運動モデルをしたがって, 推定偏差ベクトル ee から推定偏差 gee が求められ, (11) 式より制御偏差 gec を導出することが可能となる.
(10) 式で定義した制御偏差 gec を時間微分し, (4) 式を用いることで, 制御偏差システムは

b
= ue
V̄co

b
b
b
−1 Ad −1 V
Vec
= −Ad(gec)(g ) wh + Vwo

b
b
Vco
= Vwo

(4)

(5)

と構成する. ue ∈ R6 は推定偏差の振る舞いを安定にするために加えられる入力である.
推定偏差システムを (4)(5) 式から構成する. まず, カ
メラ座標系から観測対象への相対位置姿勢の真値と推定値の偏差 gee = (pee , eξ̂θee ) を−1
gee := ḡco
gco

(6)

と定義する. この推定偏差に対する推定偏差ベクトルを ee ∈ R6 として, ee := [pTee eTR (eξ̂θee )]T と定義する.
ただし eR (eξ̂θ ) := sk(eξ̂θ )∨ , sk(eξ̂θ ) := 12 (eξ̂θ − e−ξ̂θ )である. この推定偏差ベクトル ee はカメラから得られる視覚情報 f と推定モデルから得られる視覚情報 f¯ お
よび画像ヤコビアンの擬似逆行列 J † (ḡco ) を用いて

ee = J † (ḡco )(f − f¯)(7)と導出される [8]. このとき, 推定偏差ベクトル ee を状態とした推定偏差システムは, (6) 式を時間微分し,(4)(5) 式を代入することでbb
Vee= −Ad(gee−1 ue + Vwo)

2.2 制御偏差システム
視覚フィードバックシステムにおける制御目的を達成するために, 制御偏差システムについて議論する. カ
メラ座標系からみた手先の位置姿勢 gch は同定により得られるとすると, 手先座標系からみた観測対象の位置
姿勢の推定値は−1ḡho := gch
ḡco

(9)
ξ̂θec

となる. ここで, 制御偏差 gec = (pec, e

gec := gd−1 ghoと導出される.

2.3 視覚フィードバックシステムの受動性
推定偏差システム (8) と制御偏差システム (13) を併せることで, 固定カメラ構造における視覚フィードバッ
クシステムをつぎのように構成する.
 b  −1
−Ad(gec
0
I
Vec
)
b
=
Vwo
uce +
b
−1
0
−Ad(gee
Vee
I
)
(14)
b T
ただし, uce := [(Ad(g−1 ) Vwh) uTe ]T を入力として定d義する. ここでは, マニピュレータダイナミクスを考慮b
せず, 手先速度 Vwh
を直接決められると仮定する (次節以降はこの仮定は不要). 制御偏差ベクトル ec と推定偏差ベクトル ee を視覚フィードバックシステムの状
態とし e := [eTc eTe ]T と定義する. このとき, つぎの補
題が成り立つ.
b
補題 1[8] 観測対象が静止している (すなわち Vwo= 0)とする. 出力を(8)と導出される.

(13)dνce := −e(15)とするとき, (14) 式の視覚フィードバックシステムの
入出力間に
 T
uTce νcedt ≥ −βce , ∀T > 0(16)0が成り立つ. ただし, βce はある非負の定数である.
証明は以下のエネルギー関数を用いて行われる.

Vce = E(gec ) + E(gee )(17)

ただし, E(gab ) := 12 pab 2 + φ(eξ̂θab ) であり, φ(eξ̂θab )

)を
(10)

で定義する. このとき gd は相対位置姿勢の目標値である. また, 制御偏差に対する制御偏差ベクトルを ec ∈:= 12 tr(I − eξ̂θab ) は回転行列に対するエネルギー関数を表す正定関数である [11]. この補題は, 視覚フィード
バックシステムにおいて入力を uce , 出力を νce とする
とき, その間に受動性が成り立つことを意味している.

3

T

Ad(gd-1)ec

視触覚フィードバック制御

3.1 視触覚フィードバックシステム
本節では, 前節で述べた固定カメラ構造の視覚フィー
ドバックシステムに接触力を考慮したマニピュレータ
ダイナミクスを考えることで, 固定カメラ構造に対する
視触覚フィードバックシステムを構成する.
接触力を考慮した n 自由度のマニピュレータダイナ
ミクスは次式で表される [5].
M (q)q̈ + C(q, q̇)q̇ + g(q) = τ + Jϕ (q)λ(18)

q, q̇, q̈ ∈ R はそれぞれ各関節の角度, 角速度, 角加速度を表し, τ ∈ Rn は入力トルク, λ ∈ R は接触力,
M (q) ∈ Rn×n は正定な慣性行列, C(q, q̇)q̇ ∈ Rn は遠心力・コリオリ力項, g(q) ∈ Rn は重力項を表す. Jϕ (q)は接触点における手先拘束がnϕ(q) = 0,

ϕ(q) ∈ R

JϕT (q)q̇ = 0,∂ϕ(q)Jϕ (q) =
∂q

T
∈ Rn

(20)

τ = M (q)q̈r + C(q, q̇)q̇r + g(q)+JbT (q)AdT(g−1 ) ec − Jϕ λd + us + Jϕ uF (21)dを考える. Jb (q) はボディマニピュレータヤコビアン,
λd は接触力の目標値であり, us と uF は後で定める角速度および接触力に関する新しい入力である. また, q̇rは

q̇r := Qϕ (q)q̇d + αJϕ (q)Fe(22)

で定義される新たな変数であり, α は正の定数とする.
Qϕ (q) は次式で定義される射影行列である [5].

Qϕ (q) = I − Jϕ (q)JϕT (q)また, Fe は接触力に関する偏差として
 t
 t


Fe :=
eλ dτ ∈ R
λ − λd dτ =

(23)

(24)

0

として定義する. ただし, eλ := λ − λd である. このときḞe = eλue

VisualFeedback
Systemec
eeFig. 2: Block diagram of the dynamic visual force
feedback system.
uc は後ほど視覚情報から構成する新たな入力である. ここで, マニピュレータの関節角速度に関する偏差 s ∈ Rn
を
s := q̇ − q̇r ∈ Rn

(28)と定義する. このとき, 関節角速度の偏差 s と接触力の偏差 Fe の間にJϕT s = −αFe(29)なる関係がある [5].
固定カメラ構造の視触覚フィードバックシステムは(14)(18)(21)(25) 式より
“
” 3
2
ṡ 3
−M −1 Cs − JbT AdT(g−1 ) ec − Jϕ eλd
76 Ḟe 7 6eλ
7
4 Vb 5=645−Ad(g−1 ) Jb s
ec
b
ho
Vee
0
3
2
2
3
−1
−1
M
M Jϕ
0
0
0
0
0
0
7
6 0 7
6 0
+4 0
5u +4 I 5w
00 −Ad(gec
−1
)
I
0
0
0
−Ad(g−1 )ee(30)と構成される. ここで, システムの状態, 入力および外乱をそれぞれ
⎤
⎡
⎤
⎡
us
s
⎢
⎥
⎢ Fe ⎥uFb⎥⎢⎥
x := ⎢⎣ ec ⎦ , u := ⎣ Ad(g−1 ) uc ⎦ , w := Vwodeeueと定義する. Fig. 2 に視触覚フィードバックシステム(30) のブロック図を示す.
視触覚フィードバックシステムにおける制御目的は未知な位置姿勢の観測対象に, 適切な接触力でロボットの手先を接触させることである. そこで, 本稿で考える問題をあらためて示すと, つぎのようになる.
問題設定 固定カメラ構造の視触覚フィードバックシス
テム (30) において, 観測対象が静止しているとき,(25)
lim s = 0, lim Fe = 0, lim ec = 0 and lim ee = 0が成り立つ.
一方, マニピュレータの手先の速度はボディマニピュ
レータヤコビアン Jb (q) を用いることでつぎのように表される [9].
bVwh= Jb (q)q̇

s
Fe

w

2

で定義される.
ここで, マニピュレータへの入力トルクとして

0

Manipulator
Dynamics& Auxiliary
Controller(19)で表されたとき, その時間微分により



uc
us
uF

b
(=Jb(q)q)
_
Vwh

(26)

t→∞

t→∞

t→∞

t→∞

を満たすように制御入力 u を決定せよ.

3.2 視触覚フィードバックシステムの受動性制御入力 u を決定するために, 視触覚フィードバッ
クシステムの性質としてつぎの補題を示す.

さらに, 関節角速度の目標値をつぎのように設定する.

補題 2 外乱が存在しない (すなわち w = 0) とする. 出力を

q̇d := Q†ϕ (q) Jb† (q)uc − αJϕ(q)Feν := N x, N := diag{In , −α, −I6 , −I6 }(27)とするとき, (30) 式の視触覚フィードバックシステムの入出力間に T
uT ν ≥ −β, ∀T > 0(31)(証明) 補題 2 の証明における (32) 式のエネルギー関
数 V をリアプノフ関数候補とすると, (30)(35) 式の閉ループ系の解軌道に沿った時間微分は (33) 式よりが成り立つ. ただし, β はある非負の定数である.

と導かれる. ゲイン Ks , kF , Kc , Ke の正定性より行列 K が正定であり, N が正則であるため, システムの平衡点 x = 0 は漸近安定となる.
（Q.E.D.）
固定カメラ構造の 3 次元視触覚フィードバックシス
テムに対し, リアプノフの安定定理によりその安定性を示した. この安定性解析は補題 2 で示した受動性に基づいておこなわれている.

V̇ = −xT N T KN x0(証明) エネルギー関数として
V =

1 T
1
s M s + αFe2 + E(gec) + E(gee )
2
2

(32)

を考える. このエネルギー関数の解軌道に沿った時間微分は Ṁ −2C, p̂ec と p̂ee の歪対称性 (すなわち sT (Ṁ −2C)s = 0, pTecp̂ec e−ξ̂θd ωud = −pTec (e−ξ̂θd ωud )∧ pec = 0,
pTee p̂ee ωue = −pTee ω̂ue pee = 0) を用いるとV̇ = −sT C ṡ + sT JbT AdT(g−1 ) ec + sT Jϕ eλ + αFe eλd1−eTc Ad(g−1 ) Jb s + sT Ṁ s + xT N T u
ho

2
1 T
= s (Ṁ − 2C)s + eTλ (Jϕ s + αFe ) + xT N T u
2(33)= xT N T uとなる. 両辺を積分することで TuT νdτ = V (T ) − V (0) ≥ −V (0) = −β (34)0が成り立つ. ただし, β は s, Fe , ec および ee の初期状
態のみに依存する, ある非負の定数である. （Q.E.D.）
補題 1 で示したように視覚フィードバックシステム(14) は受動性を有している. また, 文献 [5] で示されているように, マニピュレータダイナミクス (18) を含む
力制御における偏差システムも受動性を有している. 補
題 2 は, 固定カメラ構造の視触覚フィードバックシステ
ム (30) の入力 u = [uTs uTF (Ad(g−1 ) uc )T uTe ]T から出d

力 ν = [sT − αFeT − eTc − eTe ]T の間に受動性が成り立つことを示しており, 上記のふたつの受動性を保存していると考えることができる.

3.3 受動性に基づく制御則と安定性解析
前述の補題を用いて, 3 次元視触覚フィードバックシ
ステムの入力として, つぎの受動性に基づく制御則を提案する.
u = −Kν = −KN x(35)

K := diag{Ks , kF , Kc , Ke } ∈ R

n+13

ただし Ks := diag{ks1, · · · , ksn} と kF は各関節および接触力の偏差に関するゲインであり, Kc :=diag{kc1 , · · · , kc6 } と Ke := diag{ke1 , · · · , ke6 } は x
軸, y 軸, z 軸の並進と回転における制御偏差と推定偏差に対するゲインである. ただし, ゲインにおける各要
素はすべて正とする.
このとき, 補題 2 で示した視触覚フィードバックシス
テムの受動性に基づくことで, 安定性に関してつぎの定理が導かれる.
定理 1 w = 0 のとき, 固定カメラ構造の 3 次元視触覚
フィードバックシステム (30) と (35) 式の入力で構成される閉ループ系の平衡点 x = 0 は漸近安定である.

4

(36)

おわりに

本稿では, 受動性に基づく視覚フィードバック制御と力制御を融合することで, 固定カメラシステムに対する受動性に基づく視触覚フィードバック制御について考えた. 提案する手法は力制御の従来研究で示されていた位置と力のハイブリッド制御法を, 視覚と力のハイブリッド制御法へと拡張したものだと捉えることができる. 構成した 3 次元視触覚フィードバックシステムが受動性を有することを示し, そのエネルギー関数をリア
プノフ関数とみなすことで安定性解析をおこなった.

参考文献[1] F. Chaumette and S. Hutchinson, “Visual Servo Control, Part I: Basic Approaches,” IEEE Robotics & Automation Magazine, Vol. 13, No. 4, pp. 82–90, 2006.
[2] F. Chaumette and S. Hutchinson, “Visual Servo Control, Part II: Advanced Approaches,” IEEE Robotics& Automation Magazine, Vol. 14, No. 1, pp. 109–117,
2007.
[3] 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.
[4] K. Omote et al., “Self-Guided Robotic Camera Control for Laparoscopic Surgery Compared with Human
Camera Control,” The American Journal of Surgery,Vol. 177, No. 4, pp. 321–324, 1999.
[5] Y. Liu , K. Kitagaki, T. Ogasawara and S. Arimoto,“Model-based Adaptive Hybrid Control for Manupilators Under Multiple Geometric Constraints,” IEEE
Trans. on Control Systems Technology, Vol. 7, No. 1,
pp. 97–109, 1999.
[6] E.C. Dean-León, V. Parra-Vega and A. EspinosaRomero, “Visual Servoing for Constrained Planar
Robots Subject to Complex Friction,” IEEE/ASME
Trans. on Mechatronics, Vol. 11, No. 4, pp. 389–400,
2006.
[7] H. Kawai, T. Murao and M. Fujita, “Passivity-based
Visual Force Feedback Control for Planar Manipulators with Eye-in-Hand Configuration,” Proc. of 16th
IEEE International Conference on Control Applications pp. 1480-1485, 2007.
[8] M. Fujita, H. Kawai and M. Spong, “Passivity-based
Dynamic Visual Feedback Control for Three Dimensional Target Tracking: Stability and L2 -gain Performance Analysis,” IEEE Transactions on Control
Systems Technology, Vol. 15, No. 1, pp. 40–52, 2007.
[9] R. Murray, Z. Li and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press,
1994.
[10] 村尾, 河合, 藤田, “受動性に基づく視覚フィードバック
における推定と制御の内部構造,” 第 36 回 SICE 制御理論シンポジウム 資料, pp. 183-186, 2007.
[11] F. Bullo and A.D. Lewis, Geometric Control of Mechanical Systems, Springer–Verlag, 2004.