第 6 回計測自動制御学会制御部門大会
2006 年 5 月 31 日–6 月 2 日 ウィルあいち

Eye-in-Hand システムに対する予測ビジュアル
フィードバック制御の一考察○村尾俊幸 山田照樹 藤田政之 （東京工業大学）

A Study on Predictive Visual Feedback Control with Eye-in-Hand System∗T. Murao, T. Yamada and M. Fujita (Tokyo Institute of Technology)
Abstract– This paper investigates vision based robot control based on a receding horizon control strategy,
as a first step for a predictive visual feedback control. Firstly, the brief summary of the 3D dynamic visualfeedback system with eye-in-hand configuration is given. Next, a stabilizing receding horizon control for the
3D dynamic visual feedback system, a highly nonlinear and relatively fast system, is proposed. The stabilityof the receding horizon control scheme is guaranteed by using the terminal cost derived from an energyfunction of the visual feedback system.
Key Words: Visual feedback control, Receding horizon control, Passivity, Stability, Control Lyapunov
function1

はじめにロボットやメカニカルシステムに自律的な振る舞いをさせるには, 多くの情報が必要となる. その中でも,視覚情報をシステムのフィードバックループに組み込んだ視覚フィードバック制御が盛んに研究されており
1), 最近では, 位置ベース法と回転, 並進, 光軸方向への動きを視覚情報に基づいて切り替える手法 2) や幾何学的な表現のシステムに対して Navigation 関数を用いる手法などが提案されている 3) . 著者らは Eye-in-hand
構造の視覚フィードバックシステムを幾何学的な表現で表し, システムの有する受動性を用いることで視覚
フィードバック制御則を提案している 4) .
一方, オンラインで有限時間区間の最適制御問題を繰り返し解く, モデル予測制御の研究も盛んにおこなわれており, 非線形システムにおける安定性解析に対して近年では様々な手法が提案されている 5) . Jadbabaie ら
は評価関数の終端コストに Control Lyapunov 関数を用いることで, 安定化モデル予測制御則を提案しており
6), 提案した制御則を Caltech Ducted Fan に適用することで, より積極的な動きを実証している 7) .
しかし, Jadbabaie らが適用したシステムは視覚情報を組み込んだシステムではない. 人間は前方に障害物があるとき, 視覚情報から予測的な制御をおこない, つまずかないように前もって動作を変えている. つまり人間は, 視覚フィードバック制御に予測制御も加えた予
測ビジュアルフィードバック制御をおこなっており, マ
ニピュレータにとっても予測ビジュアルフィードバッ
ク制御を考えることは非常に有用であると考えられる.
本稿では予測ビジュアルフィードバック制御の第一歩として, 比較的高速で非線形性の強い 3 次元動的視覚フィードバックシステム 4) に対して, 安定化モデル
予測制御則を提案する. 文献 8) と比較すると, この 3次元動的視覚フィードバックシステムは位置だけでなく姿勢も考慮できるため, 応用できる場面は確実に広がるはずである. 視覚フィードバックシステムのエネル
ギー関数が Control Lyapunov 関数であることを示した後に, エネルギー関数を評価関数の終端コストとすることで, 安定化モデル予測制御則を提案する.

Camera5c

Robot
Manipulator

Image
gwc=
gco=
^ wc)
^ ) (pwc,eNG(pco,eNGco^
NG
5w
5o gwo=(pwo,e wo)
Target Object
World Frame

Fig. 1: Eye-in-hand visual feedback system.

2
2.1

Eye-in-Hand 構造の 3 次元動的視覚フィードバックシステムと安定化制御則
3 次元動的視覚フィードバックシステム

本稿では, Fig. 1 に示すようなマニピュレータの手
先にカメラが取り付けられた Eye-in-hand 構造の視覚
フィードバックシステムを考える. Eye-in-hand 構造の視覚フィードバックシステムは三つの座標系により構成されており, マニピュレータの台座に基準座標系
Σw , カメラにカメラ座標系 Σc , 観測対象に観測対象座標系 Σo を定義する. このとき, 基準座標系からみたカ
メラの位置姿勢, 観測対象の位置姿勢をそれぞれ gwc ,gwo と, カメラ座標系からみた観測対象の相対位置姿勢
を gco と表す. 本システムの視覚フィードバックシス
テムの制御目的は, カメラ座標系からみた観測対象の相対位置姿勢 gco を, 目標相対位置姿勢である gd に常に一致させておくことである.
本稿で考えている視覚フィードバックシステムは観b
測対象の速度 Vwo
を外乱として捉えているために, カ
メラ座標系からみた観測対象の相対位置姿勢 gco が未
知情報となる. したがって, オブザーバを設計することにより, gco の推定値 ḡco を得ることを考える. これにより, 推定値 ḡco と 実値 gco の偏差である推定偏差
gee と, ḡco と目標値 gd の偏差である制御偏差 gec がともに 0 となることで, 制御目的が達成される. ただし, ḡco とカメラから得られる視覚情報 f , さらにその推定値 f¯ を用いることで, 推定偏差 gee をベクトル表

現した ee を求めることができるために, 実値 gco は推定偏差を測るために必要とされない. 同様に制御偏差
gec をベクトル表現したものを ec と定義しておく.
以上のことをふまえて, さらに n 自由度のマニピュ
レータダイナミクスを考慮した, Eye-in-hand 構造の 3次元動的視覚フィードバックシステムは次式で与えられる 4) .
2

3

2

−1

−1

JbT AdT(g−1 ) ec

(1)

2.2 エネルギー関数と安定化制御則
文献 4) ではさらに, 視覚フィードバックシステム (1)のもつ受動性という概念を用いることで安定化制御則を導出している. 受動性の証明をおこなうために, 次式のエネルギー関数を提案する.

ここで E(gab ) :=

1
ξ̂θab
2
)
2 pab  + φ(e(2)

であり, 関数ξ̂θabφ(e) は回転行列に対する誤差関数を表す正定関数である. 受動性に基づき, 制御則をつぎのように提案する.
"

u = −Kν := uk ,2I
6 0
ν := N x := 4
0

K :=

Kξ
0
0

0
−AdT(g−1 )d

AdT(eξ̂θec )

0
0
Kc
0
0 Ke
3

0
0 7x
5
−I

#

(3)

+

u

x

Passivity-based Control Law(Stabilizing Control Law)

KNx

Fig. 2: Block diagram of the passivity based visualfeedback control.
しかし, 提案されている安定化制御則 uk は陽に最適性を考慮した制御則ではない. したがって, 次節では最適理論に基づいた安定化モデル予測制御則を提案する.

3

予測ビジュアルフィードバック制御

本節は最適理論に基づいた予測ビジュアルフィード
バック制御について述べる. 視覚情報は現在の瞬間的な情報のみならず, より多くの情報を含んでいる. この特性を利用することで観測対象の動きを予測できるな
ら, 制御性能をより向上させることができるはずである. 本稿では予測ビジュアルフィードバック制御の第一歩として, 文献 4) の視覚フィードバックシステムに対し, 最適制御理論から発展したモデル予測制御則を提案する.

3.1 Control Lyapunov 関数
本節では, はじめにモデル予測制御の問題設定について述べる. 視覚フィードバックシステム (1) に対し, ある時刻 t のとき, 状態 x(t) = x0 , 入力 u(τ, x(τ )), τ ∈[t, t + T ] に関してつぎの評価関数を最小化する有限時間区間の最適制御問題を考える.
 t+T
l(x(τ ), u(τ ))dτ + F (x(t + T )) (6)J(x0 , u, T ) =tl(x(t), u(t)) = xT (t)Q(t)x(t) + uT (t)R(t)u(t)(7)Q(t)≥0, R(t) > 0, F (x(t + T ))≥0(4)

こ こ で, Kξ := diag{kξ1 , · · · , kξn } は マ ニ ピュ
レ ー タ の 各 関 節 に 対 す る ゲ イ ン で あ り, Kc :=diag{kc1 , · · · , kc6 }, Ke := diag{ke1 , · · · , ke6 } は x 軸,
y 軸, z 軸の並進と回転における制御偏差と推定偏差に対するゲインである. ただし, ゲインにおける各要素はすべて正とする.
エネルギー関数を解軌道に沿って時間微分し, 提案した (3) 式の制御則 uk を代入すると, 外乱がない (w = 0)ときには, つぎのように計算できる.

V̇ (x) = ν T u = −xT N T KN x

N

Vecb
Veeb

uk

ただし, 入力, 外乱, 状態を, それぞれ u := [uTξ uTd uTe ]T ,b T T
) ] , x := [ξ T ec T eTe ]T と定義していw := [τdT (Vwoる. ここで M は正定な慣性行列, C は遠心力・コリオ
リ力項, ξ は角速度偏差, Jb はマニピュレータヤコビア
ン, Ad(gab ) は同次表現 gab の随伴写像を表している.
bb
また, Vec, Vee
はそれぞれ制御偏差 gec , 推定偏差 geeのボディ速度を表し, 簡単に解釈すると, (1) 式の左辺
は状態 x の時間微分を表したものである.

1 T
ξ M (q)ξ + E(gec ) + E(gee )2u3−M Cξ + M
ξ̇
d
b 5
4 Vec
5
=4
−Ad(ḡco−1 Jb ξ
)
b
Vee
0
3
2 −1
" −1 #
0
0
M
M
0
5u +
I
−1
+ 4 0 −Ad(ḡco
0 0 w
)
0 I
0
0 −Ad(gee−1)

V (x) =

Visual Feedback System(5)

したがって, エネルギー関数 (2) を Lyapunov 関数とみなすことで, 外乱がない (w = 0) とき, 動的視覚フィー
ドバックシステム (1) と (3) 式の入力 uk で構成される
閉ループ系の平衡点 x = 0 は漸近安定となり, 制御則
uk は安定化制御則とみなすことができる. 受動性に基づく視覚フィードバック制御のブロック線図を Fig. 2に示す.

初期状態 x0 に対して, この評価関数を計算することにより求められる最適な入力を u∗ (τ, x(τ )), τ ∈ [t, t + T ]と表す. モデル予測制御では初めのサンプリング周期
δ [s] だけシステムに u∗ (τ, x(τ )) を加える. そして δ [s]後には, 時刻を t とし直し最適制御問題を再び解く. すなわち, モデル予測制御は

uRH := u∗ (0, x0 )(8)という入力を時刻を進めながら繰り返し加えていくことになる (Fig. 3 参照)6) .
本稿では, 動的視覚フィードバックシステム (1) とモ
デル予測制御則 (8) で構成される閉ループ系を安定とさせるような制御則の提案を考える. そのために, 安
定性に対して非常に大きくかかわる Control Lyapunov
関数を定義する.
定義 1 l(x, u) を正定な関数とし, 微分可能な正定関数
M (x) が以下の条件式を満たすとき, M (x) は Control
Lyapunov 関数である 6) .

inf [Ṁ (x) + l(x, u)] ≤ 0u(9)Output

証明 : 区間 [t + δ, t + T + δ] での準最適な入力として次式を考える.

Referencej

Predictive Outputũ =Predictive Input

Fig. 3: Receding horizon scheme.

t

この Control Lyapunov 関数についてつぎの補題を提案する.
補題 1 つぎの関数 P を定義する.

ρ > 0 (10)

外乱がなく (w = 0), P ≥ 0 ならば, 3 次元動的視覚
フィードバックシステム (1) のエネルギー関数 ρV (x)は Control Lyapunov 関数とみなすことができる.
証明 : エネルギー関数をシステムの解軌道に沿って時間微分した (5) 式と, (7) 式の正定な関数 l(x(t), u(t)),(3) 式の安定化制御則 uk を用いることにより, (9) 式はつぎのように変形される.

(11)

仮定より P ≥ 0 であるので, (9) 式の Control Lyapunov
関数の定義式より, エネルギー関数 ρV (x) は Control
2
Lyapunov 関数とみなすことができる.
補題 1 より, P ≥ 0 のとき視覚フィードバックシステ
ム (1) のエネルギー関数 ρV (x) を Control Lyapunov 関
数とみなせることが示された. この補題で用いた P ≥ 0という仮定は ρ の値を十分大きくとることで満たすことができる.

3.2

3 次元動的視覚フィードバックシステムに対する安定化モデル予測制御
評価関数の終端コストを ρV (x) とすることで, 視覚
フィードバックシステムに対するモデル予測制御の安定性に関してつぎの定理を提案することができる.
定理 1 Eye-in-Hand 構造の 3 次元動的視覚フィード
バックシステム (1) に対してつぎの評価関数を考える.
 t+T
J(x0 , u, T ) =l(x(τ ), u(τ ))dτ + F (x(t + T ))(12)t

F (x) = ρV (x),Q(t)≥0, R(t) > 0
ρ>0

(16)
∗

となる. ここで x は最適な状態を表している. さらにJ(x∗ (t + δ), u∗ , T ) ≤ J(x∗ (t + δ), ũ, T ) より,J(x∗ (t + δ), u∗ , T ) − J(x∗ (t), u∗ , T )≤ ρ[V (x(t + T + δ)) − V (x∗ (t + T ))]Z t+T
Z t+T +δ−l(x∗ (τ ), u∗ )dτ +l(x∗ (τ + T ), uk )dτ
t

t+T

(17)

と計算できる. (7) 式のステージコスト l(x(t), u(t)) と(3) 式の安定化制御則 uk を用い, (17) 式の両辺を δ で割り δ → 0 の極限をとることで, つぎのように変形さ
れる.
lim

u

l(x(t), u(t)) = xT (t)Q(t)x(t) + uT (t)R(t)u(t)t+TJ(x∗ (t + δ), u∗ , T ) − J(x∗ (t), u∗ , T )δ→0δ≤−x∗T (t + T )(ρN T KN −Q−N T K T RKN )x∗ (t + T )−x∗T (t)Qx∗ (t) − u∗T Ru∗= −x∗T (t + T )P x∗(t + T ) − x∗T (t)Qx∗(t) − u∗T Ru∗(18)

inf [Ṁ (x) + l(x, u)]uhi= inf ρV̇ + xT Qx + uT Ru
u hi= inf ρxT N T u + xT Qx + uT Ru= −xT (ρN T KN − Q − N T K T RKN )x= −xT P x

(15)

J(x∗ (t + δ), ũ, T )= J(x(t), u∗ , T ) + ρ[V (x(t + T + δ)) − V (x∗ (t + T ))]Z t+T
Z t+T +δ−l(x∗ (τ ), u∗ )dτ +l(x∗ (τ + T ), uk )dτt +T

P := ρN T KN − Q − N T K T RKN,τ ∈ [t + δ, t + T ]τ ∈ [t + T, t + T + δ]

ただし, 区間 [t + T, t + T + δ] の入力 uk は視覚フィー
ドバックシステム (1) の安定化制御則である. この入力
ũ に対する評価関数はControl Inputtu∗ (τ )uk (τ )

(13)(14)

外乱がなく (w = 0), P ≥ 0 ならば, 3 次元動的視覚
フィードバックシステム (1) と, 評価関数 (12)–(14) に対して解かれるモデル予測制御則 (8) で構成される閉ループ系の平衡点 x = 0 は漸近安定である.

はじめの δ [s] は u∗ = uRH であり, さらに P ≥ 0 という仮定により, J(x∗ (t), uRH , T ) の微分値は負定となる. したがって, J(x∗ (t), uRH , T ) を Lyapunov 関数とみなすことで, 閉ループ系の平衡点 x = 0 の漸近安定
2性が示される.
定理 1 により, 終端コストに Control Lyapunov 関数
を用いることで, 比較的高速で高い非線形性を有する 3
次元動的視覚フィードバックシステムに対するモデル
予測制御の安定性が証明された. モデル予測制御が最適制御理論を発展させた理論であるということに注目すると, 評価関数のパラメータを適切なものに選べば,安定化制御則 (3) に比べ制御性能を向上させることができると考えられる. さらに文献 8) と比較すると, 従
来研究では平面マニピュレータに限定されており, さらに位置問題しか考慮できなかったことに対して, 本シス
テムは位置に加えて姿勢も考慮できる点から, 応用範囲は格段に広がると考えられる. また, 推定偏差を予測しているので, カメラから観測対象の相対位置姿勢の推定値を間接的に予測していることになり, 本システム
はオブザーバにもモデル予測制御を適用していると捉えることができる. 本稿では予測ビジュアルフィード
バック制御の第一歩として, 拘束を考慮しない安定化モ
デル予測制御則を提案したが, 今後はモデル予測制御の重要な利点でもある拘束に対して, レベル集合を用いる
ことで考慮していきたい 6) . 予測ビジュアルフィード
バック制御のブロック線図を Fig. 4 に示す.
さらに逆最適性に焦点をあてることで, つぎの系を提案する.

JointVisual Feedback System

N

u

Vecb
Veeb

となる. ここで, 評価関数の最適値 J(x0 , uRH , T ) をリ
アプノフ関数候補とし, システム (1) の解軌道に沿って
時間微分をおこなうと,rq Angleu+x

˙ 0 , uRH , T ) = ρV̇ (x) = −ρJ(xReceding Horizon Control Law

uRH
Fig. 4: Block diagram of the predictive visual feedback control.
系 1 Eye-in-Hand 構造の 3 次元動的視覚フィードバッ
クシステム (1) に対して評価関数 (12)–(14) を考える.
ただし, 評価関数の重みはQ(t) = qN T (t)KN (t),−1

R(t) = rK ,√
ρ = 2 qr

q≥0

r>0

(19)
(20)
(21)

とする. 外乱がない (w = 0) ならば, 3 次元動的視覚
フィードバックシステム (1) と, 上記の評価関数に対して解かれるモデル予測制御則 (8) で構成される閉ループ系の平衡点 x = 0 は漸近安定となり, そのモデル予
測制御則 uRH は解析的につぎのように求めることが
できる.

q
KN x
(22)
uRH = −r
また評価関数の最適値はつぎのように与えられる.

J(x0 , uRH , T ) = ρV (x)(23)

証明 : まずはじめに, 重みを (19)–(21) のように設定したステージコスト (13) と終端コスト (14) に関してつ
ぎの計算を試みる.
Ḟ (x) + l(x, u)= ρV̇ + xT Qx + uT Ru√= 2 qrxT N T u + qxT N T KN x + ruT K −1 u
`√
´T
´
`√
√
√
=
ru + qKN x K −1
ru + qKN x
‚
‚√
√
2
(24)
= ‚ ru + qKN x‚K −1

上記の結果を用いることで, 評価関数 (12) はつぎのよ
うに計算される.
Z t+T “”xT Qx + uT Ru dτ + ρV (x(t + T ))J(x0 , u, T ) =Z t+T “t”‚‚√‚ ru + √qKN x‚2 −1 − ρV̇ (x(τ )) dτ
=
K
t

Z t+T
=
t

+ρV (x(t + T ))‚‚√‚ ru + √qKN x‚2 −1 dτ + ρV (x(t))K(25)

したがって, 評価関数を最小化するモデル予測制御則はr
uRH = −

q
KN x
r

(26)

と解析的に求めることができ, その最適値はJ(x0 , uRH , T ) = ρV (x)

(27)

q T T
x N KN xr(28)となる. したがって, 3 次元動的視覚フィードバックシ
ステム (1) と, 解析的に生成されるモデル予測制御則(22) で構成される閉ループ系の平衡点 x = 0 は漸近安
2定である.
系 1 は逆最適性に基づく結果から得られたものである. 評価関数の重みを (19)–(21) とおくと, モデル予測
制御則は inf u [Ṁ (x) + l(x, u)] = 0 を満足するような解析的な制御則を得ることとなる.

4

おわりに

本稿では予測ビジュアルフィードバック制御の第一歩として, 比較的高速で非線形性の強い 3 次元動的視覚フィードバックシステムに対する安定化モデル予測
制御則を提案した. 視覚フィードバックシステムのエ
ネルギー関数を終端コストにおくことで, モデル予測
制御の安定性を示した. 原稿のスペース上省略するが,評価関数の重みを適切に選ぶことで, 特に過渡応答が安定化制御則 (3) に比べ向上していることを, シミュレー
ションにより検証できている. この検証結果より, 予測ビジュアルフィードバック制御は視覚情報を用いて観測対象の動きを予測し, より積極的にマニピュレータを動かすことができるのではないかと考えられる.

参考文献
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) G. Chesi, K. Hashimoto, D. Prattichizzo and A. Vicino, “Keeping Features in the Field of View in EyeIn-Hand Visual Servoing: A Switching Approach,”
IEEE Trans. on Robotics, Vol. 20, No. 5, pp. 908–
913, 2004.
3) N. J. Cowan and D. E. Chang, “Geometric Visual
Servoing,” IEEE Trans. on Robotics, Vol. 21, No. 6,
pp. 1128–1138, 2005.
4) H. Kawai and M. Fujita, “Passivity-based Dynamic
Visual Feedback Control for Three Dimensional Target Tracking: Stability and L2 -gain Performance
Analysis,” Proc. 2004 American Control Conference,
pp. 1522–1527, 2004.
5) D. Q. Mayne, J. B. Rawlings, C. V. Rao and P. O.
M. Scokaert, “Constrained Model Predictive Control:
Stability and Optimality,” Automatica, Vol. 36, No. 7,
pp. 789–814, 2000.
6) A. Jadbabaie, J. Yu and J. Hauser, “Unconstrained
Receding-Horizon Control of Nonlinear Systems,”
IEEE Trans. Automatic Control, Vol. 46, No. 5, pp.
776–783, 2001.
7) A. Jadbabaie and J. Hauser, “Control of a ThrustVectored Flying Wing: a Receding Horizon – LPV
Approach,” International Journal of Robust andNonlinear Control, vol. 12, pp. 869–896, 2002.
8) M. Fujita, Y. Kawai and Y. Nakaso, “An Experimental Study of Stabilizing Receding Horizon Control of
Visual Feedback System with Planar Manipulators,”
Proc. 2005 International Workshop on Assessment
and Future Directions of Nonlinear Model Predictive
Control, pp. 319–326, 2005.