受動性に基づく可動カメラを用いた視覚フィードバック制御の一考察○村 尾 俊 幸 ∗ · 河 合 宏 之 ∗∗ · 藤 田 政 之 ∗∗金 沢 大 学, ∗∗ 法 政 大 学

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

In this paper, we discuss the dynamic visual feedback control with a movable camera instead of a ﬁxed camerain the ﬁxed camera conﬁgulation. Firstly the brief summary of the control and the estimated error system is given.
Secondly we construct the camera ﬁeld error system in order to consider the camera ﬁeld of view. Next we derive
the passivity of the dynamic visual feedback system by combining the passivity of both the visual feedback systemand the manipulator dynamics. Finally the stability and L2 -gain performance analysis are discussed based on thepassivity.

1

CameraはじめにImage

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

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

gwc
gco

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

Shgho

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

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

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

gwh
SwRobotManipulator

Fig. 1:視覚フィードバックシステムに対し, ロボットは単なる位置決めをするための装置として捉えられている. 一方, マニピュレータダイナミクスまで考慮した研究で, 平面マニピュレータに限定されない制御則を Kelly ら 4)用いた視覚フィードバックシステムを構成する. さらにマニピュが提案している. しかし, 観測対象が静止している場合の安定性レータダイナミクスを考慮することで, 動的視覚フィードバック解析まではおこなっているが, 観測対象が運動する場合や制御性システムを構成し, 閉ループ系の漸近安定性と L2 ゲイン制御性能解析については十分な議論がなされていない. それに対して能解析をおこなう.

従来研究 5) では, 固定カメラ構造の視覚フィードバックシステムについて安定性や制御性能解析について議論した.

2

しかし, 従来研究 5) のシステムではカメラが固定されているためにカメラ視野を広くとることができない. Chaumette は文献6)においてカメラ視野の問題は非常に重要であると述べてお

2.1

四つの座標系をもつ視覚フィードバックシステム

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

本稿では, Fig. 1 に示すような四つの座標系によって表されり, マニピュレータの作業スペースを広げるためにも, より多くる視覚フィードバックシステムについて考える. Fig. 1 中の四の視野が必要となる.

つの座標系をそれぞれ基準座標系 Σw , 手先座標系 Σh , カメラ

したがって, 本稿ではカメラを固定させるのではなく, 可動カ座標系 Σc および観測対象座標系 Σo と定義する. このとき, 基メラを用いることによって視野をより広げることを考える. その準座標系からみた手先の位置姿勢, カメラの位置姿勢および観測ために従来研究 5) の四つの座標系を有する固定カメラ構造の視対象の位置姿勢をそれぞれ gwh , gwc , gwo で表す. 同様に, カメ

覚フィードバックシステムに対して, 固定カメラの部分を可動カ

ラ座標系からみた手先の位置姿勢と観測対象の位置姿勢をそれメラに置き換えて議論する. また, 可動カメラの動きを決定する

ぞれ gch , gco とし, 手先座標系からみた観測対象の位置姿勢をために, カメラ視野偏差システムを新たに構成し, 可動カメラをgho と表す. 視覚フィードバックシステムの制御目的は, 手先座

標系からみた観測対象の相対位置姿勢 gho を目標相対位置姿勢bとして表される 5) . ここで Vwhはマニピュレータの手先の速度gd に常に一致させておくことである. ここで, 同次表現 gab やであり, Vdb は手先座標系からみた観測対象の相対位置姿勢 gho

演算子 ∧(wedge) の詳しい説明については文献

7)を参照していただきたい.の目標速度である.

(5) 式の制御偏差システムと (8) 式の推定偏差システムで構成

これらの座標系から, 視覚フィードバックシステムにおける三される視覚フィードバックシステムは受動性を有している 5) .

つの座標系間に成り立つ剛体運動の表現を導出する. まず, カメ

3

ラ座標系からみた観測対象の相対位置姿勢 gco は−1
gco = gwc
gwo(1)bbで表される. 一方, カメラの速度を Vwc, 観測対象の速度を Vwo

3.1

可動カメラを用いた視覚フィードバックシステム

カメラ視野偏差システム

従来研究 5) と比較すると, 2 章の四つの座標系をもつ視覚と定義すると, カメラ座標系からみた観測対象の相対的な位置姿

フィードバックシステムでは, カメラを固定させずに議論してきbは, (1) 式を時間微分することで, gco の基勢に対する速度 Vco

たが, カメラ視野を広げるためのカメラの動きの決め方について本式としては議論してこなかった. したがって, 本章ではカメラの動きを決bbb
Vco= −Ad(gco−1 Vwc + Vwo)

(2)と導かれる. ここで Ad(gab ) は同次表現 gab の随伴写像である
7).

2.2めるために, カメラ座標系からみた観測対象への目標相対位置姿勢を設定し, カメラ座標系からみた観測対象への推定値との偏差をとることで, カメラ視野偏差システムを構成する.
まずはじめに, カメラの動きを決めるために, カメラ座標系からみた観測対象への相対位置姿勢の目標値を gcd で表すことと推定偏差システムと制御偏差システム

本節は, 従来研究 5) 8) である推定偏差システムと制御偏差システムについて述べる.

する. このとき, カメラ座標系からみた観測対象への相対位置姿勢の推定値とその目標相対位置姿勢 gcd との偏差 gev を−1gev = gcdḡco

カメラから得られる視覚情報 f は, 相対位置姿勢 gco を直接得ることはできないため, その推定値 ḡco を得るためにオブザーバを構成する. (2) 式の基本式に基づき, 推定値 ḡco の運動モデ
ルを(9)と定義する. また, カメラ視野偏差に対するカメラ視野偏差ベクξ̂θev TT)] と定義する. このとき (9) 式を時
トルを ev := [pTev eR (e間微分することで, カメラ視野偏差ベクトル ev を状態とするカbb= −Ad(ḡcoV̄co−1 Vwc + ue)(3)

メラ視野偏差システムはと構成する. ue は推定偏差の振る舞いを安定にするために加え

b−1bb
Vev= (gevġev )∨ = ue − Ad(ḡco−1 Vwc − Ad −1 Vcd (10))(gev )られる入力である.
つぎに, カメラ座標系から観測対象への相対位置姿勢の真値bと導出される. ここで Vcdはカメラ座標系からみた観測対象の相対位置姿勢 gco の目標速度である.と推定値の偏差 gee を−1gee := ḡcogco(4)と定義する. この推定偏差に対する推定偏差ベクトルを ee :=[pTee eTR (eξ̂θee )]T と定義する. このとき, 推定偏差ベクトル ee を状態とした推定偏差システムは次式のように表される 8) .
b−1b= (geeġee )∨ = −Ad(g−1 ) ue + Vwo
Veeee

(5)

3.2

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

可動カメラを用いた剛体運動の視覚フィードバックシステム
を (5)(8)(10) 式より
322 b 3 2
3I0−Ad(ḡ−1 )0Vecho76b 5b4 Vee
0−Ad(gee0=4−15ucev +4 Vwo 5(11))
b0Vev
0I−Ad(ḡ−1 )co同様に, 手先座標系からみた観測対象の相対位置姿勢の推定と構成する. ただし,

23b+ Ad(gd ) VdbVwh5ueucev := 4bbVwc + Ad(gcd ) Vcd

値と目標相対位置姿勢の偏差 gec を

gec := gd−1 ḡho(6)と定義する. この手先座標系からみた観測対象の相対位置姿勢−1gwh が正確な値を得ることができるの推定値 ḡho は gch = gwcと仮定するとき,(12)

T Tであり, システムの状態を e := [eTc ee ev ] と定義しておく. このとき視覚フィードバックシステムに対して以下の補題が成り立つ.

−1ḡcoḡho = gch(7)という関係式から得ることができる. 制御偏差に対する制御偏Tξ̂θec T
差ベクトルを ec := [pT)] と定義するとき, 制御偏差
ec eR (eベクトル ec を状態にもつ制御偏差システムは
b−1bbVec= (gecġec )∨ = −Ad(ḡ−1 ) Vwh+ ue − Ad(gec−1 Vd)
hob
補題 1 観測対象が運動していない (すなわち Vwo= 0) とする.

出力を2−AdT(g−1 )6νcev :=Ncev e, Ncev :=4 Ad(e−ξ̂θec )
0d(8)

0−I0

30

Ad(e−ξ̂θev ) 75(13)−AdT(g−1 )cd+Visual Feedback Systemb
Vwh

gho

Ad(gd )Vdb
ue

Estimated
OMFCucev

gco , fb
Vwob
Vwc

OMFC

gco

Ad(gcd )Vcdb

度の目標値を q̇d で表すとすると, (17) 式と同様にしてマニピュgdgco+

一方, 手先の速度の目標値を ud とし, マニピュレータの関節角速gho +HMFC

Image JacobianJO

レータヤコビアンを用いることで ud = Jb (q)q̇d と表すことがで
eceeev

きる. マニピュレータの関節角速度に関する偏差 ξ ∈ Rn をNcevfξ := q̇ − q̇dncev

gco +と定義する.

gcd(18)また, 提案する制御則に設計の自由度を与える

ために, つぎのような重み行列 Wc := diag{wpc I3 , wrcI3 } ∈R6×6 , We:=

diag{wpe I3 , wre I3 }diag{wpv I3 , wrv I3 }Fig. 2:視覚フィードバックシステムのブロック図

∈R6×6

∈

R6×6 , Wv

を 定 義 す る.

:=

ただしwpc , wpe , wpv , wrc, wre , wrv ∈ R は正の定数とする.
このとき, マニピュレータへの入力トルクとしてとするとき, (11) 式の視覚フィードバックシステムの入出力間にZ T
0uTcev νcev dτ ≥ − βcev , ∀ T > 0

τ = M (q)q̈d + C(q, q̇)q̇d + g(q)+ JbT (q)AdT(g−1 ) Wc ec + uξ

(14)

(19)

dを考える. q̇d , q̈d はそれぞれ関節角速度と関節角加速度の目標が成り立つ. ただし βcev はある非負の定数である.

値である. また, uξ は後に提案する新たな入力である. このとき, (11)(16)(19) 式を用いることで, 可動カメラを用いた動的視証明は以下のエネルギー関数

Vcev = E(gec ) + E(gee ) + E(gev )(15)を用いることで, 文献 5) 8) と同様に可能である. ここで E(g) :=ξ̂θξ̂θ
122 p + φ(e ) であり, 関数 φ(e ) は回転行列に対する誤差関9)数を表す正定関数である.

この補題は, (11) 式の視覚フィードバックシステムが受動性を有していることを示唆しており, 出力 νcev をネガティブフィードバックしたものを入力 ucev として用いると, 視覚フィードバッ

覚フィードバックシステムは
323 2−M −1 Cξ + M −1 JbT AdT(g−1 ) Wc ecξ̇d67b 76 Vec−Ad(ḡ−1 ) Jb ξ
76 b 7=6ho
74 Vee 5 6450bVev
0
3 22 −1
3000MM −1 0
76 0 −Ad −1I00 077 66(ḡho )7w+67u+60−Ad −105 4 0 I 54 00

クシステムが安定となることが示される.

0(gee )I−Ad(ḡco−1)00(20)

Fig. 2 に視覚フィードバックシステムのブロック図を示す.
図中の OMFC と Estimated OMFC はそれぞれカメラからみで表される. ただし,

23uξ
»
–
6 ud + Ad(g ) Vdb 7τdd7 , w :=u := 6b45ue
Vwobb
Vwc+ Ad(gcd ) Vcd

た観測対象の運動とその推定モデルを表しており, HMFC はカメラからみた手先の運動を表している.

44.1

動的視覚フィードバック制御(21)で定義される. 動的視覚フィードバックシステムにおける制御目
動的視覚フィードバックシステム

的は, 運動する観測対象にマニピュレータの手先を追従させるこ

本節では, 可動カメラを用いた剛体運動の視覚フィードバックシステムにマニピュレータダイナミクスを加えることで, 動的視とであり, 状態 ξ, ec , ee および ev を 0 に留めておくことで制御目的が達成される.

覚フィードバックシステムを構成する.

n 自由度のマニピュレータダイナミクスは次式で表される.

4.2

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

M (q)q̈ + C(q, q̇)q̇ + g(q) = τ + τd(16)q, q̇, q̈ ∈ R はそれぞれの各関節の角度, 角速度, 角加速度を表し,nτ ∈ Rn は入力トルク, τd ∈ Rn はトルク外乱, M (q) ∈ Rn×nは正定な慣性行列, C(q, q̇)q̇ ∈ Rn は遠心力・コリオリ力項,g(q) ∈ Rn は重力項を表す.
また, マニピュレータの手先の速度はマニピュレータヤコビア
ン Jb (q) を用いることでつぎのように表される.
bVwh= Jb (q)q̇(17)の構成に対して重要な役割を果たす動的視覚フィードバックシステムの有する性質を示す.
補題 2 外乱がない (すなわち w = 0) とする. このとき, 出力をν := N2x323I000ξ
T
6 0 −Ad −1 Wc 0
706 ec 7(gd )677
N :=67, x :=64 ee 5
4 0 Ad(e−ξ̂θec ) Wc −We Ad(e−ξ̂θev ) Wv 5ev
000 −AdT(g−1 ) Wv
cdとするとき, 動的視覚フィードバックシステム (20) の入出力間にZ TuT νdτ ≥ −β0 , ∀ T > 0(22)して捉えているために, γ が小さいコントローラであれば観測対が成り立つ. ただし β0 はある非負の定数である.

象の運動が状態に与える影響がより少ないことを示しており, γ0定理 2 は (23) 式のエネルギー関数を蓄積関数とみなすことで証明される. この制御性能解析では観測対象の運動を外乱とを外乱抑制レベルを示す指標とみなすことができる. 本稿では,補題 2 はエネルギー関数

H∞ 制御における最も基本となる外乱抑制問題を考えた. 本稿

1V (x) = ξ T M ξ + EW (gec ) + EW (gee ) + EW (gev ) (23)2を用いて, Ṁ −2C の歪対称性と p̂ec , p̂ee と p̂ev の歪対称性を利用で提案した手法は, 適切な一般化プラントを構成することで, 外乱抑制問題以外にも拡張可能である.

することで証明される. ここで, EW (g) := 12 wp p2 + wr φ(eξ̂θ )であり, wp と wr はそれぞれの重み行列の各要素である.
この補題は, (20) 式の動的視覚フィードバックシステムが受動性を有していることを示唆している. したがって動的視覚フィードバックシステムは, (11) 式の視覚フィードバックシステムの受5

おわりに

本稿では, カメラ視野を広げるために, 従来研究で提案していた固定カメラ構造の視覚フィードバックシステムに対して, 可動カメラを用いて議論をおこなった. また, カメラ視野偏差システ
ムを新たに構成し, 可動カメラを用いた視覚フィーバックシステ

動性を保存しているとみなすことができる.

ムを構成した. さらにマニピュレータダイナミクスを考慮するこ

4.3

動的視覚フィードバック制御則と安定性とで, 動的視覚フィードバックシステムを構成し, 閉ループ系の動的視覚フィードバックシステム (20) に対し, 外乱がない (す漸近安定性と L2 ゲイン制御性能解析をおこなった.

なわち w = 0) 場合に平衡点 x = 0 を安定とする制御則として参考文献

次式を提案する.

200 77(24)
0 5Kv1) S. Hutchinson, G. D. Hager and P. I. Corke, “A Tuto-Kξ := diag{kξ1 , · · · , kξn } は各関節に対するゲインであり,

2) 橋本, 田中, 則次, “視覚サーボにおけるポテンシャル切り替

Kc := diag{kc1 , · · · , kc6 }, Ke := diag{ke1 , · · · , ke6 } とえ制御,” 計測自動制御学会論文集, Vol. 36, No. 8, pp. 660–

Kv := diag{kv1 , · · · , kv6 } は x 軸, y 軸, z 軸の並進と回転

667, 2000.

Kξ
6 06u = −Kν = −KN x, K := 4000Kc0000Ke03

rial on Visual Servo Control,” IEEE Trans. Robotics andAutomation, Vol. 12, No. 5, pp. 651–670, 1996.

における制御偏差と推定偏差とカメラ視野偏差に対するゲイン

3) Y. Mezouar and F. Chaumette, “Optimal camera trajec-である. ただし, ゲインにおける各要素はすべて正とする. このtory with image-based control,” Int. Journal of Robotics

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

Research, Vol. 22, No. 10–11, pp. 781–804, 2003.に基づくことで, 安定性に関してつぎの定理が導かれる.

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

定理 1 w = 0 のとき, 動的視覚フィードバックシステム (20) と“Stable Visual Servoing of Camera-in-Hand Robotic Sys-

(24) 式の入力で構成される閉ループ系の平衡点 x = 0 は漸近安tems,” IEEE/ASME Trans. Mechatronics, Vol. 5, No. 1,定である.

pp. 39–48, 2000.

この定理 1 は (23) 式のエネルギー関数をリアプノフ関数候補とすることにより示される.

4.4

5) 村尾, 河合, 藤田, “受動性に基づく視覚フィードバック制
御の固定カメラ構造への展開,” 第 4 回 制御部門大会 資料,

pp. 327–330, 2004.

L2 ゲイン制御性能解析

6) F. Chaumette, “Potential Problems of Stability and Con-

本節では外乱が存在する場合について考察する. 特に, 本稿でvergence in Image-Based and Position-Based Visual Ser-は外乱抑制問題を考えることで L2 ゲイン制御性能解析をおこ

voing,” In:D. J. Kriegman, G. D. Hager and A. S. Morse

なう.

(Eds), The Confluence of Vision and Control, Springer–

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

11
P := N T KN − 2 W − I
2γ2Verlag, pp. 66-78, 1998.
(25)

ただし W := diag{I, 0, We2 , 0} とする. このとき, 制御性能解析に関するつぎの定理が示される.
定理 2 与えられた γ に対して, P ≥ 0 を満たすようにゲイン

7) R. Murray, Z. Li and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994.
8) 河合, 村尾, 藤田, “受動性に基づく動的視覚フィードバック
制御の安定性と L2 ゲイン制御性能解析,” 第 32 回 制御理論シンポジウム 資料, pp. 23–26, 2003.

Kξ , Kc, Ke , Kv および重み Wc , We , Wv を選ぶとき, (20)(24)

9) F. Bullo and R. Murray, “Tracking for Fully Actuated式で構成される動的視覚フィードバックシステムは γ 以下の L2

Mechanical Systems: a Geometric Framework,” Auto-ゲインを有する.

matica, Vol. 35, No. 1, pp. 17–34, 1999.