受動性に基づく視覚フィードバック制御の固定カメラ構造への展開○村 尾 俊 幸 ∗ · 河 合 宏 之 ∗ · 藤 田 政 之 ∗∗金 沢 大 学

Passivity-based Visual Feedback Control with a Fixed Camera○ Toshiyuki Murao∗, Hiroyuki Kawai∗ and Masayuki Fujita∗∗ Kanazawa University

This paper considers the control and the estimation of dynamic visual feedback systems with a ﬁxed camera.
Firstly the model of the visual feedback system with four coordinate frames is constructed by using the homogeneous
representation and adjoint transformation. Secondly we derive the passivity of the dynamic visual feedback system
by combining the passivity of both the visual feedback system and the manipulator dynamics. Finally the stability
and L2 -gain performance analysis are discussed based on the passivity.

1Camera

はじめにImage

ロボットやメカニカルシステムに自律的な振る舞いをさせるには, 多くの情報が必要となる. 特に視覚情報は未知環境化におgchSc

いて周囲の状況を認識させるために有用であり, この視覚情報をダイナミカルシステムのフィードバックループに組み込んだ制

gwc
gco

御のことを視覚フィードバック制御という 1) .

Sh

gho

ロボットの視覚フィードバック制御の目的のひとつに, 移動すgwo

る観測対象にロボットの手先やカメラを追従させることがあげられる. この制御目的を達成させるために, 安定性の大域化 2) やTarget Object So

軌道計画問題 3) が研究されている. これらの従来研究では, 視覚フィードバックシステムについて深い議論がなされているのgwhSwRobotManipulator

Fig. 1:視覚フィードバックシステムに対し, ロボットは単なる位置決めをするための装置として捉えられている. 一方, マニピュレータダイナミクスまで考慮した研究としては文献 4) 5) などが挙げられる. これらの研究は, 平面2受動性に基づく視覚フィードバックシステムマニピュレータに限定されてはいるが, リアプノフの安定定理に基づき安定性を保証する制御則が提案されている. Kelly ら 6)は, 観測対象までの距離が既知であるとの仮定が必要であるが,2.1

視覚フィードバックシステムにおける剛体運動の表現

本稿では, Fig. 1 に示すような四つの座標系によって表され平面マニピュレータに限定しない制御則を提案しており, いずれる視覚フィードバックシステムについて考える. Fig. 1 中の四つの研究も, 観測対象が静止している場合の安定性解析まではおこの座標系をそれぞれ基準座標系 Σw , 手先座標系 Σh , カメラ座標なっているが, 観測対象が運動する場合や制御性能解析について系 Σc および観測対象座標系 Σo と定義する. このとき, 基準座は十分な議論がなされていない.

標系からみた手先の位置姿勢, カメラの位置姿勢および観測対象7)では, Eye-in-Hand 構造の視覚フィードバックシの位置姿勢をそれぞれ gwh = (pwh , eξ̂θwh ), gwc = (pwc , eξ̂θwc ),ステムに対して, 受動性に基づくアプローチを発展させたロボッgwo = (pwo , eξ̂θwo ) で表す. 同様に, カメラ座標系からみた手先トの動的視覚フィードバック制御について議論した. 本稿では,の位置姿勢と観測対象の位置姿勢をそれぞれ gch = (pch , eξ̂θch ),より一般的な視覚フィードバックシステムを考慮にいれた, 四つ

gco = (pco , eξ̂θco ) とし, 手先座標系からみた観測対象の位置姿の座標系を有する固定カメラ構造の視覚フィードバックシステ

勢を gho = (pho , eξ̂θho ) と表す. ここで, 同次表現 g や 演算子

ムへ展開する. さらにマニピュレータダイナミクスを考慮する∧(wedge) の詳しい説明については文献 8) を参照していただきことで, 動的視覚フィードバックシステムを構成する. そして,たい.

従来研究システムの有する性質を示したあと, 閉ループ系の漸近安定性とL2 ゲイン制御性能解析をおこなう.

これらの座標系から, 視覚フィードバックシステムにおける三つの座標系間に成り立つ剛体運動の表現を導出する. まず, カメ

ラ座標系からみた観測対象の相対位置姿勢 gco はで表される. しかしながら, gco は直接求められないため, その−1
gco = gwc
gwo

(1)

推定値 ḡco を用いたときの手先座標系からみた観測対象の相対位置姿勢 ḡho をで表される.
b
一方, カメラの速度を Vwc
bT
T Tの速度を Vwo = [vwo ωwo ]=T[vwc6T Tωwc]−1ḡho = gchḡco6∈ R , 観測対象

∈ R と定義する. このときカメラ座標系からみた観測対象の相対的な位置姿勢に対する速度bと表す. 相対位置姿勢 ḡho の基本式は, 手先の速度を Vwh=TT T[vwhωwh] ∈ R6 とすると, (9) 式を時間微分することで

bTT TVco= [vcoωco] ∈ R6 は, (1) 式を時間微分することで, gco の

bbb= −Ad(ḡ−1 ) Vch+ V̄coV̄hoho

基本式としてb= −Ad(ḡ−1 ) Vwh+ ue(2)と導かれる. ここで Ad(g) は同次表現 g の随伴写像である

8)ξ̂θcoと導かれる. ただし,.

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

カメラから得られる視覚情報は, gco = (pco , e(10)hobbbVco= −Ad(gco−1 Vwc + Vwo)

2.2(9)−1
gch = gwc
gwh

(11)

bb
より導かれる Vch= Vwhの関係と (4) 式を用いている.

) を含んではいるが, 二次元情報であるため直接 gco を得ることはできなˆい. そこで, 相対位置姿勢の推定値 ḡco = (p̄co , eξ̄θ̄co ) を得るた
めに, オブザーバを構成する. 本稿では固定カメラ構造の視覚b= 0) を考えるため, gco
フィードバックシステム (すなわち Vwc

ここで, 手先座標系からみた観測対象の相対位置姿勢の目標値を gd = (pd , eξ̂θd ) で表し一定値であるとする. このとき相対位置姿勢の推定値と目標値との偏差を制御偏差 gec とよび

gec = (pec , eξ̂θec ) を
gec := gd−1 ḡhoの基本式はbb
Vco= Vwo(3)で定義する. また, 推定偏差ベクトルと同様に, 制御偏差ベクトル ec ∈ R 6 をhとなる. そこで, (3) 式の基本式に基づき, 推定値 ḡco の運動モec :=デルを
b= ueV̄co(4)(12)pTeceTR (eξ̂θec )iT(13)と定義する.
ここで, 導出した推定相対位置姿勢モデルおよび相対位置姿と構成する. ue ∈ R6 は推定偏差の振る舞いを安定にするため勢の目標値を用いて, 制御偏差ベクトル ec を状態とした制御偏に加えられる入力である.

差システムを構成すると, (12) 式を時間微分し, (10) 式を代入す

推定偏差システムを (3)(4) 式から構成する. まず, カメラ座ることで標系から観測対象への相対位置姿勢の真値と推定値の偏差 gee =bb
Vec= −Ad(ḡ−1 ) Vwh+ ue(pee , eξ̂θee ) を(14)ho−1gee := ḡcogco(5)と定義する. この推定偏差に対する推定偏差ベクトルを ee ∈ R6としてつぎのように定義する.
hee := pTeeeTR (eξ̂θee )iT(6)ただし eR (eξ̂θ ) := sk(eξ̂θ )∨ , sk(eξ̂θ ) := 12 (eξ̂θ − e−ξ̂θ ) である.

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

2.4

視覚フィードバックシステムの入出力間の関係

剛体運動の視覚フィードバックシステムを (7) (14) 式より#» b – "»–−Ad(ḡ−1 )IVec0bho=Vwo(15)
uce +bI0−Ad(g−1 )Veeeeb Tと構成する. ただし, uce := [(Vwh) uTe ]T であり, システムのこの推定偏差ベクトル ee はカメラから得られる視覚情報と推定T T状態を e := [eTc ee ] と定義しておく. このとき, 視覚フィードモデルから得られる視覚情報を用いて導出される 7) .

バックシステムに対して以下の補題が成り立つ.

推定偏差ベクトル ee を状態とした推定偏差システムは, (5)式を時間微分し, (3)(4) 式を代入することで
bb
Vee= −Ad(gee−1 ue + Vwo)b
補題 1 観測対象が運動していない (すなわち Vwo= 0) とする.

出力を"(7)
νce :=bTT Tと導出される. ただし Vee= [veeωee] ∈ R6 である.

2.3dAd(e−ξ̂θec )0−I#e(16)とするとき, (15) 式の視覚フィードバックシステムの入出力間にZ TuTce νce dτ ≥ − βce , ∀ T > 0(17)制御偏差システム

手先座標系からみた観測対象の相対位置姿勢 gho は−1gho = gch
gco−AdT(g−1 )0(8)が成り立つ. ただし βce はある非負の定数である.

b
Vwo

Visual Feedback Systemと定義する. また, 提案する制御則に設計の自由度を与えるために, つぎのような重み行列 Wc , We ∈ R6×6 を定義する.
––»»
00
wpcI3
wpe I3, We :=Wc :=0wrc I30
wre I3

OMFC

gcouefgco , f

EstimatedOMFCgco+gchbVwhee+^ ec)ecAd(e xqgcouce
HMFC

Image JacobianJOgho

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

nce

このとき, マニピュレータへの入力トルクとしてDesiregdgchτ = M (q)q̈d + C(q, q̇)q̇d + g(q)TAd(gd-1)ec+ JbT (q)AdT(g−1 ) Wc ec + uξ(22)dを考える. q̇d , q̈d はそれぞれ関節角速度と関節角加速度の目標値である. また, uξ は後に提案する新たな入力である. このとFig. 2:視覚フィードバックシステムのブロック図き, (15)(19)(22) 式を用いることで動的視覚フィードバックシス

Vce =11pec 2 + φ(eξ̂θec ) + pee 2 + φ(eξ̂θee )22

テムは`´ 33 2−M (q)−1 C(q, q̇)ξ + JbT (q)AdT(g−1 ) Wc ecξ̇d76b 54 Vec=4−Ad −1 Jb (q)ξ52

証明は以下のエネルギー関数(18)を用いることで, 文献 7) と同様に可能である. ここで, 関数ξ̂θ

φ(e ) は回転行列に対するエネルギー関数を表す正定関数である 9) .

(ḡho )

bVee0
322−100M (q)M (q)−176 0 −Ad −14I+405u+(ḡho )000−Ad(gee−1)

300 5wI

この補題は, (15) 式の視覚フィードバックシステムが受動性を有していることを示唆しており, 出力 νce をネガティブフィードバックしたものを入力 uce として用いると, 視覚フィードバックシステムが安定となることが示される. Fig. 2 に視覚フィードバックシステムのブロック図を示す. 図中の OMFC と EstimatedOMFC はそれぞれカメラからみた観測対象の運動とその推定モデルを表しており, HMFC はカメラからみた手先の運動を表し(23)
TT TTb T Tで表される. ただし, u := [uTξ ud ue ] , w := [τd (Vwo ) ] で定義される. 動的視覚フィードバックシステムにおける制御目的は, 運動する観測対象にマニピュレータの手先を追従させることであり, 状態 ξ, ec および ee を 0 に留めておくことで制御目的が達成される.

3.2ている.

動的視覚フィードバックシステムの入出力間の関係制御目的を達成するための制御則を提案するまえに, 制御則

33.1

動的視覚フィードバック制御の構成に対して重要な役割をはたす動的視覚フィードバックシステムの有する性質を示す.

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

本節では, 剛体運動の視覚フィードバックシステムにマニピュレータダイナミクスを加えることで, 動的視覚フィードバックシステムを構成する.

n 自由度のマニピュレータダイナミクスは次式で表される.
M (q)q̈ + C(q, q̇)q̇ + g(q) = τ + τd(19)q, q̇, q̈ ∈ Rn はそれぞれの各関節の角度, 角速度, 角加速度を表し,τ ∈ R は入力トルク, τd ∈ R はトルク外乱, M (q) ∈ R
nnn×nは正定な慣性行列, C(q, q̇)q̇ ∈ Rn は遠心力・コリオリ力項,g(q) ∈ Rn は重力項を表す.

補題 2 外乱がない (すなわち w = 0) とする. このとき, 出力をν = Nx2I
6
N := 4 000−AdT(g−1 ) Wc
dAd(e−ξ̂θec ) Wc

323(24)0ξ0 75 , x := 4 ec 5ee−Weとするとき, 動的視覚フィードバックシステム (23) の入出力間にZ T
uT νdτ ≥ −β0 , ∀ T > 0(25)0が成り立つ. ただし β0 はある非負の定数である.

また, マニピュレータの手先の速度はマニピュレータヤコビア
補題 2 はエネルギー関数ン Jb (q) を用いることでつぎのように表される.
b= Jb (q)q̇
Vwh(20)

一方, 手先の速度の目標値を ud とし, マニピュレータの関節角速度の目標値を q̇d で表すとすると, (20) 式と同様にしてマニピュレータヤコビアンを用いることで ud = Jb (q)q̇d と表すことができる. マニピュレータの関節角速度に関する偏差 ξ ∈ Rn をξ := q̇ − q̇d(21)V =1 T1
ξ M (q)ξ + wpcpec 2 + wrc φ(eξ̂θec )221+ wpe pee 2 + wre φ(eξ̂θee )2(26)を用いて, Ṁ − 2C の歪対称性と p̂ec と p̂ee の歪対称性を利用することで証明される.
この補題は, (23) 式の動的視覚フィードバックシステムが受動性を有していることを示唆している. したがって動的視覚フィー

TAd(gd-1)Wcec

バックシステムは γ 以下の L2 ゲインを有する.

bVwh(=Jb(q)q)_ud

VisualFeedback
SystemManipulator
DynamicsuxxWe を選ぶとき, (23)(27)(28) 式で構成される動的視覚フィード

uence定理 2 は (26) 式のエネルギー関数を蓄積関数とみなすことで証明される. この制御性能解析では観測対象の運動を外乱として捉えているために, γ が小さいコントローラであれば観測対象Fig. 3:動的視覚フィードバックシステムのブロック図の運動が状態や入力に与える影響がより少ないことを示しており, γ を外乱抑制レベルを示す指標とみなすことができる.

ドバックシステムは, (15) 式の視覚フィードバックシステムの4受動性を保存しているとみなすことができる. Fig. 3 に示す閉

おわりにループ系のブロック図からも, 動的視覚フィードバックシステム

本稿では, 従来研究で提案していた受動性に基づく動的視覚がマニピュレータと Fig. 2 で示した視覚フィードバックシステフィードバック制御を, 四つの座標系を有する固定カメラ構造のムを結合したシステムであると解釈できる.

視覚フィードバックシステムへ展開した. さらにマニピュレータ

3.3

ダイナミクスを考慮することで, 動的視覚フィードバックシステ

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

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

2Kξ
u = −Kν = −KN x, K := 4 000Kc0300 5Keムを構成した. そして, システムの有する性質を示したあと, 閉ループ系の漸近安定性と L2 ゲイン制御性能解析をおこなった.
参考文献(27)

Kξ := diag{kξ1 , · · · , kξn } は各関節に対するゲインであり,Kc := diag{kc1 , · · · , kc6 } と Ke := diag{ke1 , · · · , ke6 } は x軸, y 軸, z 軸の並進と回転における制御偏差と推定偏差に対するゲインである. ただし, ゲインにおける各要素はすべて正とす

1) S. Hutchinson, G. D. Hager and P. I. Corke: A Tutorial on Visual Servo Control, IEEE Trans. Robotics andAutomation, 12-5, 651/670 (1996)
2) 橋本, 田中, 則次: 視覚サーボにおけるポテンシャル切り替え制御, 計測自動制御学会論文集, 36-8, 660/667 (2000)

る. このとき, 補題 2 で示した動的視覚フィードバックシステム

3) Y. Mezouar and F. Chaumette: Optimal Camera Trajec-の受動性に基づくことで, 安定性に関してつぎの定理が導かれる.

tory with Image-Based Control, Int. Journal of Robotics

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

Research, 22-10/11, 781/804 (2003)
4) R. Kelly: Robust Asymptotically Stable Visual Servoingof Planar Robots, IEEE Trans. Robotics and Automation, 12-5, 759/766 (1996)

この定理 1 は (26) 式のエネルギー関数をリアプノフ関数候補5) E. Zergeroglu, D. M. Dawson, M. S. de Queirozとすることにより示される.

and A. Behal: Vision-Based Nonlinear Tracking Con-3.4

trollers With Uncertain Robot-Camera Parameters,

L2 ゲイン制御性能解析

本節では外乱が存在する場合について考察する. 特に, 本稿でIEEE/ASME Trans. Mechatronics, 6-3, 322/337 (2001)

は外乱抑制問題を考えることで L2 ゲイン制御性能解析をおこ

6) R. Kelly, R. Carelli, O. Nasisi, B. Kuchen and F. Reyes:

なう. 外乱抑制問題を考えるために, (23) 式の動的視覚フィード

Stable Visual Servoing of Camera-in-Hand Robotic Sys-

バックシステムとつぎの (28) 式を一般化プラントとして考える.
»–εxz=(28)ρutems, IEEE/ASME Trans. Mechatronics, 5-1, 39/48

シンポジウム 資料, 327/330 (2003)

ここで, ある正の数 γ としてつぎの行列を定義する.

ただし W:= diag{I, 0, We2 } とする.

7) 河合, 村尾, 藤田: 受動性に基づく動的視覚フィードバック
制御の安定性と L2 ゲイン制御性能解析, 第 32 回 制御理論

ただし, ε と ρ は状態と入力に対する重み行列である.

111
P := N T KN − εT ε − ρK2 − 2 W
222γ(2000)(29)

このとき, 制御性能解析に関するつぎの定理が示される.
定理 2 与えられた γ および被制御出力の重み行列 ε, ρ に対して, P ≥ 0 を満たすようにゲイン Kξ , Kc, Ke および重み Wc ,8) R. Murray, Z. Li and S. S. Sastry: A Mathematical Introduction to Robotic Manipulation, CRC Press (1994)
9) F. Bullo and R. Murray: Tracking for Fully ActuatedMechanical Systems: a Geometric Framework, Automatica, 35-1, 17/34 (1999)