論 文

安定化予測視覚フィードバック制御に対するナビゲーション関数を用いた目標位置姿勢生成
員

村尾

俊幸∗

非会員

藤田

政之∗∗∗

正

非会員

河合

宏之∗∗

Stabilizing Predictive Visual Feedback Control via Image Space Navigation Function
Toshiyuki Murao∗ , Member, Hiroyuki Kawai∗∗, Non-member, Masayuki Fujita∗∗∗ , Non-member
This paper investigates stabilizing receding horizon control via an image space navigation function forthree-dimensional (3-D) visual feedback systems. Firstly, we describe the representation of a relative rigid
body motion and a camera model. Next, the visual motion error system is reconstructed in order to apply
to time-varying desired motion. Then, visual motion observer-based stabilizing receding horizon control for
3-D visual feedback systems is proposed. Moreover, a path planner to be appropriate for the visual motion
error system is designed through an image space navigation function to keep all features into the camera ﬁeldof view. The main contribution of this paper is to show that the path planner which always remains in thecamera ﬁeld of view during the servoing is designed for the position-based visual feedback receding horizon
control based on optimal control theory. Finally, we present simulation and actual nonlinear experimental
results in order to verify control performance with visibility maintenance of the proposed control scheme.
キーワード：視覚フィードバック制御，Receding Horizon 制御，ナビゲーション関数，受動性，安定性

Keywords: visual feedback control, receding horizon control, navigation function, passivity, stability

題を繰り返し解く，モデル予測制御 (6) を視覚フィードバッ

1. はじめにクシステムに展開した発展的な手法も最近では研究されて近年，視覚フィードバック制御は工場だけでなくより複いる。Sauvée らは非線形モデル予測制御を一般的な画像

雑なシステムや幅広い分野，例えば医療分野や生命化学のベース法の視覚フィードバック制御に適用させ (7) ，さらに分野においても適用が見込まれるようになってきている (1) 。

文献 (8) において超音波画像を用いた手術用器具の制御に

これらの視覚フィードバック制御の研究に対しては，特に展開している。Allibert ら (9) は内部モデル制御構造におけ

Chaumette (2) がカメラ視野等の問題に対して議論して以来，るモデル予測制御の提案だけでなく，予測するための視覚

ますます盛んにより有効的な制御則が提案されており，多フィードバック制御のモデルについても複数挙げ比較検討。

している。しかし，上述の研究は安定性について十分な議そのなかでも，オンラインで有限時間区間の最適制御問論がなされていない。著者らは従来研究 (10) (11) において，くの研究成果がここ数年においても発表されている(3)〜(5)

安定性を保証した視覚フィードバックシステムのモデル予
測制御則を提案している。しかし，外乱がある場合におい∗ 産業技術大学院大学 産業技術研究科ても再計算することにより最適な安定化制御入力を適用で〒 140–0011 東京都品川区東大井 1–10–40
School of Industrial Technology, Advanced Institute of Industrial Technology
1–10–40 Higashi-Ohi Shinagawa-ku, Tokyo 140–0011∗∗ 金沢工業大学 工学部〒 924–0838 石川県白山市八束穂 3–1
College of Engineering, Kanazawa Institute of Technology
3–1 Yatsukaho Hakusan, Ishikawa 924–0838∗∗∗ 東京工業大学 大学院理工学研究科〒 152–8550 東京都目黒区大岡山 2–12–1
Graduate School of Science and Engineering, Tokyo Instituteof Technology
2–12–1 Ookayama Meguro-ku, Tokyo 152–8550
電学論 C，132 巻 5 号，2012 年

きる文献 (10) (11) の手法は，制御の途中でカメラ視野から
画像特徴点が外れる可能性があるという問題 (2) に対しては議論されていなかった。
一方，カメラ視野の問題に対する軌道生成の手法も様々な形で提案されており，なかでもナビゲーション関数といわれる人工的なポテンシャル関数を用いる手法が，局所的な極小点への収束を避ける有効な手法として議論されている。
このナビゲーション関数を用いた視覚フィードバック制御の例としては大きく二つに分類でき，一つは Cowan らが提1

xi

Camera5c

fcoyi

Camera Frame
xc
zc

l

fcoi

gco

zo

gwc

pcoi

gco

P

TargetObject

poi

5o

gwo

5w

xo

World Frame

5o

5c
yc
yi

fcoxi
zw

Image Plane

yo

xw
yw

5w

World Frame

Target Object

Fig. 2. Pinhole camera model.

Fig. 1. Visual feedback system with aneye-in-hand conﬁguration.

Fig. 1 に示すような，三つの座標系によって表される Eyein-Hand 構造の視覚フィードバックシステムについて考える。基準とする座標系として基準座標系 Σw ，カメラにカメラ座標系 Σc ，観測対象に観測対象座標系 Σo を定義する。
また，任意の二つの座標系 Σa から Σb の位置 pab ∈ R3と姿勢を表す回転行列 eξ̂θab ∈ SO(3) を要素に持つ同次表
現 gab を以下のように定義する。
eξ̂θab pab
gab =∈ R4×4 · · · · · · · · · · · · · · · · (1)01案している制御則の生成に直接ナビゲーション関数を用いる手法である。文献 (12) ではロボットマニピュレータに対して，ナビゲーション関数を用いた視覚フィードバック制御則が提案されている。さらに文献 (13) では，Eye-in-Handと固定カメラの両構造を同じ形式で扱える球体を用いたモデルに対して上記の手法を提案している。もう一つは制御則に与える目標値生成に対してナビゲーション関数を適用させる手法である。Chen ら (14) は画像空間上のナビゲー
ション関数を用いて目標軌道を生成し，2-1/2D 視覚フィー

ただし，演算子 ∧(wedge) は 3 次元ベクトルを 3 × 3 のドバック制御則に適用させている。これらの手法はカメラ

歪対称行列へ写像する演算子であり，その逆写像は ∨(vee)

視野の問題に対して有効な手段として提案されているが，で定義される (15) 。

制御則の最適性は考慮されていない。

カメラ座標系からみた観測対象の相対位置姿勢 gco は同

そこで，本研究では Fig. 1 に示すような 3 次元の視覚

次表現の合成則から以下のように導かれる。

フィードバックシステムに対して，画像空間上のナビゲー
ション関数を用いた安定化モデル予測制御則を提案する。は−1
gco = gwc
gwo · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (2)じめに，目標位置姿勢が時変となる場合の受動性に基づく視覚フィードバックシステムに対して，安定化モデル予測

また，カメラ座標系からみた観測対象の相対速度は，gco を制御を提案する。さらに，カメラ視野の中に常に画像特徴

時間微分することで，点を保持できるように，画像空間上のナビゲーション関数

b
b
b
Vco
= −Ad(gco−1 V) wc + Vwo · · · · · · · · · · · · · · · · · · (3)を用いることでその目標位置姿勢を生成する。画像空間上の偏差ではなく相対位置姿勢の偏差を用いる本論文の安定−1bのように導かれる (16) 。ここで，Vab= (gabġab )∨ ∈ R6 は化予測視覚フィードバック制御においては，目標画像を与剛体運動のボディ速度を表しており，Ad(gab ) ∈ R6×6 はえる文献 (14) の手法に対して，目標値を相対位置姿勢で与同次表現 gab の随伴写像である (15) 。

える点と，モデル予測制御を用いることで最適性を陽に考一方，上述の相対位置姿勢 gco は観測対象の位置姿勢 gwo

慮することができる点で大きく異なる。また文献 (10) (11)が測定不可能であるために未知情報となる。本論文では，観の制御手法と比較すると，常に画像特徴点をカメラ視野に測出力として得られる情報はピンホールカメラ (Fig. 2 参保持できるという利点がある。

照) から得られる画像特徴量とし，観測対象に取り付けら

本論文の構成は以下のとおりである。2 節では，時変のれた 4 点の特徴点の画像面上における x 座標と y 座標を

目標位置姿勢に対する安定化予測視覚フィードバック制御

考える。このとき，観測対象座標系 Σo の原点から各特徴

について提案する。3 節では，画像空間上のナビゲーショ

点までのベクトルを poi ∈ R3 (i = 1, · · · , 4) とすると，カン関数を用いた目標位置姿勢生成をおこなう。 4 節と 5 節

メラからみた相対的な特徴点 pcoi ∈ R3 (i = 1, · · · , 4) はでシミュレーション結果と実験結果をそれぞれ示し，最後pcoi = gco poi で表される。ただし，特徴点 poi はすべて既T知であるとし，同次表現でよく用いられる表記法 ([pTab 1]
を pab と表記 (15) ) により表したものである。
画像特徴量を fcoi := [fcoxi fcoyi ]T (i = 1, · · · , 4) と定に 6 節でまとめをおこなう。

2. 安定化予測視覚フィードバック制御〈2・1〉 剛体運動の表現とカメラモデル

義すると，特徴量は透視変換を用いることで，本論文では，
2

IEEJ Trans. EIS, Vol.132, No.5, 2012

安定化予測視覚フィードバック制御に対するナビゲーション関数を用いた目標位置姿勢生成

1fcoi =

λαx
0

zcoi

0
λαy



xcoi
ycoi



Eye-in-Hand 構造の視覚フィードバック偏差システムにおT Tいて，状態 x := [eTc ee ] を 0 にとどめておくことで相対位置姿勢 gco が目標位置姿勢 gcd に一致することになる。
この視覚フィードバック偏差システム (9) は観測対象が静b= 0) しているとき，入力 u から出力 −x
止 (すなわち Vwo· · · · · · · · · · (4)で表される。ただし，λ ∈ R はカメラの焦点距離, αx , αy ∈R は 1 画素の実質的な縦横の大きさの逆数であり，pcoi の各要素を pcoi = [xcoi ycoi zcoi ]T として表した。この fcoi
T
T T
を縦に並べたベクトル fco = [fco1· · · fco4
] ∈ R8 が実に対して受動性を有することを，以下のエネルギー関数を用いることで証明することができる。

際にカメラから得られる視覚情報となる。
〈2・2〉 視覚フィードバック偏差システム

V (x) = E(gec ) + E(gee )· · · · · · · · · · · · · · · · · · · · (11)一般的な位置ベース法の視覚フィードバックシステムの制御目的は，

ここで E(gab ) := 12 pab 2 +φ(eξ̂θab ) であり，関数 φ(eξ̂θab )カメラ座標系 Σc からみた観測対象座標系 Σo の相対位置

は回転行列に対する誤差関数を表す正定関数である (17) 。

姿勢 gco を目標位置姿勢 gcd に常に一致させておくことで〈2・3〉 安定化予測視覚フィードバック制御ある。ただし，目標位置姿勢 gcd は時変とするが，実シスは，視覚フィードバック偏差システム (9) の閉ループ系を安テムを考慮に入れ，最終値は一定値とする。制御則を構成定化させるモデル予測制御則を提案する。本論文では，視するに当たり gco を直接得ることはできないので，gco の覚フィードバック偏差システム (9) に対し，ある時刻 t の推定値 ḡco を得るために (3) 式に基づいたつぎの視覚オブ

とき，状態 x(t) = x0 ，入力 u(τ, x(τ )), τ ∈ [t, t + T ] に関

ザーバを構成する。

してつぎの評価関数を最小化する有限時間区間の最適制御

本節で

b
b
−1 V
V̄co
= −Ad(ḡco) wc + ue · · · · · · · · · · · · · · · · · · · · (5)問題を考える。

ue ∈ R は推定偏差の振舞いを安定にするために加えられJ(x0 , u, T )=る入力であり，後に制御則の一部として提案する。

l(x(t), u(t))=Eqc (gec (t))+Eqe (gee (t)) + uT (t)R(t)u(t)· · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (13)

 t+T

6

ここで，制御偏差 gec と推定偏差 gee をつぎのように定義する。

F (x)=ρV (x) · · · · · · · · · · · · · · · · · · · · · · · · · · · · (14)−1
gec := gcd
gco · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (6)qpi (t)≥0, qri (t)≥0, R(t) > 0, ρ > 0−1
gee := ḡcogco · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (7)ただし，R(t) は対角行列とし，Eqi (gei (t)) :=また，rei := sk(eξ̂θei )∨ ∈ R3 ，sk(eξ̂θei ) := 12 (eξ̂θei −qpi (t)pei (t)2 + qri (t)φ(eξ̂θei (t) ), i = c, e である。初期
状態 x0 に対して，この評価関数を計算することにより求められる最適な入力を u∗ (τ, x(τ )), τ ∈ [t, t + T ] と表す。
モデル予測制御では，初めのサンプリング周期である δ 秒
だけシステムに u∗ (τ, x(τ )) を加える。そして δ 秒後には，その時刻を改めて t とし，再び最適制御問題を解く。すな−ξ̂θeiT Te) を用いて，制御偏差ベクトルを ec := [pTec rec
] ∈
6
T
T T
6
R と，推定偏差ベクトルを ee := [pee ree ] ∈ R と定義する。推定偏差ベクトル ee は視覚情報 fco と推定値 ḡcoからつぎのように得ることができる。

ee = Je† (ḡco )(fco − f¯co )· · · · · · · · · · · · · · · · · · · · · · (8)わち，モデル予測制御は

ここで，f¯co は画像特徴量 fco の推定値を表し，Je (·) は画

uRH := u∗ (δ, x0 ) · · · · · · · · · · · · · · · · · · · · · · · · · · (15)像ヤコビアンの一種である (16) 。また，目標値 gcd と推定という入力を，時刻を進めながら繰り返し加えていくこと

偏差ベクトル ee を用いると，制御偏差ベクトル ec は計算
可能である

(11)

になる (6) 。

。

視覚フィードバック偏差システム (9) と (15) 式のモデ

視覚フィードバック偏差システムは (6)(7) 式を時間微分

ル予測制御則との閉ループ系に関して，つぎの定理が成りすることで下記のように導出することができる。



b
Vec
b
Vee





=

l(x(τ ), u(τ ))dτ +F (x(t + T )) (12)t−Ad(gec−10)−10−Ad(gee
)





u+

I
I



立つ。
b
(9)
Vwo

〔定理 1〕 Eye-in-Hand 構造の 3 次元視覚フィードバッ
ク偏差システム (9) に対して (12)〜(14) 式の評価関数をb考える。観測対象が静止しており (Vwo
= 0)，θec ≤ π2 ，

ただし，入力を

u := [uTc uTe ],θee ≤ π2 ，ρ2 I≥4QR であるならば，視覚フィードバッ
ク偏差システム (9) と，評価関数 (12)〜(14) に対して解
かれるモデル予測制御則 (15) で構成される閉ループ系の平衡点 x = 0 は漸近安定である。
この定理 1 は評価関数の終端コスト ρV (x) が制御リアプ
ノフ関数であることを利用することで，目標位置姿勢 gcd
が時変となっても，文献 (10) (11) と同様の手法で証明すbbuc := Ad(g−1 ) Vwc+ Vcd· · · · · (10)cdと定義している。目標位置姿勢を時不変としていた文献(10) (11) の視覚フィードバック偏差システムと本論文の偏差システム (9) は同じ形となっているが，(10) 式にあるよ
うに，入力 u の一部に目標位置姿勢の速度 (以下では，目b
標速度とする) Vcd
が陽に入るところが従来研究と異なる。
電学論 C，132 巻 5 号，2012 年

3

ることができる。モデル予測制御が最適制御理論を発展さ

ル fcd と作業空間上で定義される目標位置姿勢 gcd の間のせた理論であるということに注目すると，評価関数のパラ関係性を導出する。目標位置姿勢におけるカメラからみたメータを適切なものに選ぶことで，望まれる制御性能を陽相対的な特徴点を pcdi = [xcdi ycdi zcdi ]T と表すと，目標に得られると考えられる。

画像特徴量は

3. 画像空間上のナビゲーション関数を用いた目標
位置姿勢生成〈3・1〉 目標速度の設計

fcdi =



1
zcdi

0
λαy

λαx
0



xcdi
ycdi


· · · · · · · · (18)となる。(18) 式を時間微分することで，目標画像特徴量 fcdi

画像面上の偏差を減らす画と目標特徴点の位置 pcdi の間に像ベース法に比べ，作業空間上で偏差を定義する位置ベー

 λα

ス法の視覚フィードバック制御は，画像特徴点をカメラ視
野内に常に留めておくことが難しい (2) 。提案した安定化予

λαy
zcdi

0

測視覚フィードバック制御則は偏差を画像空間ではなく作− λαzx2xcdi

0

x

zcdi

f˙cdi =

ṗcdi · · · · · · · (19)cdi
λαy ycdi
2
zcdi−

業空間で定義しているため，カメラを制御している途中でが成り立つ。さらに，pcdi = gcd poi であることから，目標

カメラ視野から画像特徴点を外してしまう可能性がある。

b
特徴点の位置 pcdi と目標速度 Vcdとの間の関係式として

本節では，常にカメラ視野内に画像特徴点を維持するようつぎの式が得られる。

な，安定化予測視覚フィードバック制御に適した画像空間ṗcdi = eξ̂θcd

上のナビゲーション関数を用いた軌道生成について述べる。



I

−p̂oi



b
Vcd
· · · · · · · · · · · · · · · · (20)これらより，

最終的な目標位置姿勢を gcdf と定義すると，本システムの制御目的はカメラ視野内に全ての画像特徴点が維持され，b· · · · · · · · · · · · · · · · · · · · · · · · · · · (21)f˙cd = JL (gcd )Vcd

かつ gcdf に収束するような時変の目標位置姿勢 gcd を生が成り立つ。ここで，JL (gcd ) : SE(3) → R8×6 は成し，それに対して gco を一致させることとなる。



提案した視覚フィードバック制御則 (15) と (10) 式で表JL (gcd ) :=される偏差システムの入力により，実際のカメラへの入力

TTT(gcd ) JL2(gcd ) · · · JL4(gcd )JL1 λαx

はつぎのように構成される。

zcdi

JLi (gcd ) :=



b
b
Vwc
= Ad(gcd ) uRH− Vcd· · · · · · · · · · · · · · · · · (16)
c

0

0

−

λαy
zcdi

−

T
(22)


λαx xcdiz2
cdi
λαy ycdiz2eξ̂θcd

I

−p̂oi

,

cdi

(i = 1, · · · , 4) · · · · · · · · · · · · · · · · · · · · · · · · · · · · (23)T
RH T T
ここで，uRH = [(uRHc ) (ue ) ] とした。目標位置姿で表される。

勢 gcd も制御入力には必要となるが，gcd は

この導出された (21) 式の関係とナビゲーション関数の定bġcd = gcd V̂cd
· · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (17)b
義より，目標速度 Vcd
をつぎのように設計する。
b
Vcd= −kcd JL† (gcd )∇ϕ(fcd )· · · · · · · · · · · · · · · · · (24)を解くことにより生成することができるため，目標位置姿bb
勢 gcd の速度 Vcd
を設計すれば，カメラ入力 Vwc
を生成

ここで，∇ϕ(fcd ) :=できる。

∂ϕ(fcd )∂fcd

T

は ϕ(fcd ) の勾配ベクトルを表している。また kcd ∈ R は正のゲインである。設定

今，あるカメラ位置姿勢に対して全ての観測対象の特徴点が視野内に留まる特徴点の集合を D ⊂ R8 と，また画像特徴するナビゲーション関数 ϕ(fcd ) は付録 1 に示す。

量の目標値を fcd := [fcdx1 fcdy1 · · · fcdx4 fcdy4]T ∈ R8

本手法は，(24) 式の目標速度と (17) 式で解かれる目標bとすると，D の中に fcd が常に属するような目標速度 Vcd

位置姿勢 gcd を用いて，予測視覚フィードバック制御則がを設計する必要がある。ここで，目標画像特徴量の最終値適用できることになる。次節では，この目標位置姿勢 gcd

fcdf := [fcdf x1 fcdf y1 · · · fcdf x4 fcdf y4 ]T ∈ R8 は D の内部に時不変な値として存在すると仮定する。fcd ∈ D とがその最終値 gcdf に収束し，かつ目標画像特徴量 fcd がなるような目標速度を生成するために，つぎで定義される〈3・2〉 目標位置姿勢の収束解析

ナビゲーション関数という特殊人工ポテンシャル関数を導式に代入することで，目標画像特徴量のトラジェクトリー

入する。

はつぎのように表されることになる。

〔定義 1〕

カメラ視野内に常に収まっていることを議論する。

滑らかなモース関数 ϕ(fcd ) : D →[0, 1] が以下の性質を有するとき，ϕ はナビゲーション(13) (14)目標速度 (24) を (21)f˙cd = −kcd JL (gcd )JL† (gcd )∇ϕ(fcd )· · · · · · · · · · (25)関数である。

ここで，∇ϕ(fcd ) ∈ N S(JLT (fcd )) という仮定を設ける。

（ 1 ） fcdf のとき唯一の最小値が存在する。

N S(·) は零空間演算子を表し，∇ϕ(fcd ) が JLT (fcd ) の零

（ 2 ） D の境界で最大値を持つ。

空間に入らないということを表している。

b
このナビゲーション関数を用いて目標速度 Vcd
を設計す

上記の仮定を設けることにより，設計した目標軌道に関してつぎの定理が成り立つ。

るために，画像空間上で定義される目標画像特徴量ベクト
4

IEEJ Trans. EIS, Vol.132, No.5, 2012

安定化予測視覚フィードバック制御に対するナビゲーション関数を用いた目標位置姿勢生成
gcdf
Image Space
Navigation
Function-based
Path Planner

gcd
b
Vcd
fco

Stabilizingb
Vwc
Predictive
Visual Feedback
Controller構成されるわけではないため，制御中に画像面から特徴点b
Vwoが外れる可能性がある。最適な安定化制御入力を視覚フィー

Vision fco
Cameraドバックシステムに適用した著者らの従来研究 (10) (11) においても，同様の問題を抱えていた。しかし，この位置ベース
法は目標画像を事前に必要とはせず，またこれまでに提案されているロボットの多様な位置制御の方法と組み合わせ

Fig. 3. Block diagram of stabilizing predictive
visual feedback control with image space
navigation function-based path planner.

易いことから有益な手法として知られている (1) 。したがって，特徴点を常にカメラ視野内に維持することができる画像空間上での目標軌道生成と，最適理論を元にした作業空〔定理 2〕 fcd (0) ∈ D かつ ∇ϕ(fcd ) ∈ N S(JLT (fcd )) を

間中での偏差を用いた安定化予測視覚フィードバック制御

保証するように目標特徴点が選ばれたとする。このとき，を組み合わせる本提案手法の適用範囲は広いと考えられる。

(25) 式によって生成される目標画像トラジェクトリーはfcd (t) ∈ D であり，かつ目標位置姿勢 gcd (t) は漸近安定な平衡点 gcdf を持つ。
証明 : つぎの正定な関数を考える。

本研究において，(21) 式で用いている目標画像特徴量ベbを関連づけるヤコビア
クトルの微分値 f˙cd と目標速度 Vcd

ン JL (·) と，(8) 式で用いている推定偏差ベクトル生成のために一次テーラー近似を用いて導出したヤコビアン Je (·)
が全く同じ形を取っていることは非常に興味深い†。これは

Vn (fcd (t)) = ϕ(fcd (t))· · · · · · · · · · · · · · · · · · · · · · (26)

文献 (18) で導出したヤコビアン J(·) を用いることで，全(25) 式の解軌道に沿って時間微分することにより，つぎの方位カメラを用いた視覚フィードバック制御においても，本関係式が得られる。

論文で提案した手法を同じ方法で適用できることを示唆している。

V̇n (fcd (t)) = (∇ϕ) f˙cd
= −kcd (∇ϕ)T JL JL† ∇ϕ= −kcd (JLT ∇ϕ)T (JLT JL )−1 JLT ∇ϕ≤ −kJLT ∇ϕ2 · · · · · · · · · · · · · · · · · (27)T

また，本稿ではマニピュレータダイナミクスを陽に考慮していないが，文献 (10) (11) ではマニピュレータダイナミ
クスも考慮した安定化予測視覚フィードバック制御を提案している。本稿では紙面の都合上割愛したが，同様の方法

ここで，ka2 ≤kcd aT (JLT JL )−1 a, ∀a ∈ R6 という関係

により，陽にマニピュレータのダイナミクスを考慮しなけを用いており，k はある正の定数を表している。この (27)

ればならないシステムに対しても，本手法を適用すること

式は Vn (fcd ) が下記の意味で非増加関数であるということが可能となる。

を表している。

4. シミュレーション検証

Vn (fcd (t)) ≤ Vn (fcd (0))· · · · · · · · · · · · · · · · · · · · (28)

本節では，提案した制御手法のシミュレーション検証(26)〜(28) 式から，いかなる初期状態 fcd (0) ∈ D に対して
も，fcd (t) ∈ D, ∀t > 0 となることがわかる。したがって，
ラサールの定理を用いることで，JLT (fcd )∇ϕ(fcd ) = 0

をおこなう。初期設定は pco = [0.2 0.2 −1.35]T m，ξθco = [0 0 0]T rad とする。また最終的な目標位置
姿勢は pcdf = [−0.22 − 0.31 −1.35]T m，ξθcdf =[0 0 π/2]T rad とする。これは観測対象が相対的にカメラ視野の端から端に 90 度回転して動く設定となっている。画像ピクセル値の最大値及び最小値は fxM = 240
pixel，fxm = −240 pixel，fyM = 320 pixel，fym =−320 pixel とした。さらに，本シミュレーションにおける評価関数の重みは以下のように設定した。qpc = 0.003，
qrc = 0.001，qpe = 0.0003，qre = 0.0001，R =diag{0.105, 0.105, 15, 15, 15, 0.105, 300, 300, 30, 30, 30,300}。また，ρ2 I≥4QR という条件式を満たすように ρ = 1とし，uRH をリアルタイムで求めるための最適化ソフト
ウェアは C/GMRES (19) を用いた。制御入力は 2 ms 毎に更新され，その間に uRH は計算されなければならない。予測ホライズンの長さは T = 0.04 s と設定した。一方，目標軌道生成のためのゲインは kcd = 6000000，Ks = 0.1I ，
κ = 2 と設定した。

を満足する正の不変集合が原点のみとなることが判明する。∇ϕ(fcd ) ∈ N S(JLT (fcd )) という仮定を置いていることから，∇ϕ(fcd ) = 0 が示される。付録 1 節より，

∇ϕ(fcd ) → 0 のとき fcd (t) → fcdf となるために，
fcd (t) → fcdf が証明される。さらに，画像特徴点を 4点取ることで，画像特徴量 fab から相対位置姿勢 gab の幾
何学的な解は唯一存在するため，fcd (t) → fcdf となるこ
2とから gcd (t) → gcdf が導かれる。
定理 2 はナビゲーション関数をリアプノフ関数候補とすることで証明している。この定理 2 により，画像空間上のナビゲーション関数を用いた軌道生成をおこなうことで，目標位置姿勢 gcd (t) が 最終目標位置姿勢 gcdf に，カメラ視野を維持したまま収束することが示された。Fig. 3 に
画像空間上のナビゲーション関数を用いた安定化予測視覚
フィードバック制御のブロック図を示す。
視覚フィードバック制御の中でも位置姿勢の偏差をフィー† このヤコビアン J (·) の更なる詳細は文献 (16) を参照されたい。
e

ドバックする手法は，画像空間上の偏差に基づいて制御則が電学論 C，132 巻 5 号，2012 年

5

1Table 1. Values of the integral cost withsimulation results.
Control Scheme|| x ||

Passivity based Control

0.5

0
0

0.5

time [s]1

cost
10.3159

Receding Horizon Control (T = 0.04 [s])0.5826

Receding Horizon Control (T = 0.4 [s])0.5813

Receding Horizon Control (T = 1 [s])0.5799Fig. 5 に画像特徴量 fco のトラジェクトリーを示す。図中

1.5の実線が提案手法により目標軌道を与えたとき，破線が従来
手法 (10) となる時不変の目標値を与えたときのトラジェクト

Fig. 4. Norm of the state.

リーとなっている。fco (0) := [f1T (0) f2T (0) f3T (0) f4T (0)]T300f4(1.5)f2(1.5)200

f3(1.5)と fco (1.5) := [f1T (1.5) f2T (1.5) f3T (1.5) f4T (1.5)]T はそf1(1.5)

れぞれ特徴量の値の初期値と最終値を表しており，また図中の長方形がカメラ視野内を表している。すなわち，長方

fy [pixel]

100

形の中に特徴量の軌跡がすべて入っていなければならない。

Fig. 5 より，位置ベース法である予測視覚フィードバック0

制御において，提案手法による目標軌道を与えることで，カ-100f2(0)f4(0)メラ視野を外れていないことがわかる。一方，従来研究 (10)である時不変の目標値の場合は，最終的な目標値に収束し-200

f1(0)f3(0)ているもののカメラ視野を一度外れていることから，実際

Camera Field of View

-300
-300

-200

-100
100
0
fx [pixel]

200の実験においては失敗することに注意されたい。

300

Fig. 6 に相対位置姿勢 gco (t) と最終的な目標位置姿勢
gcdf との間の偏差 er を示す。全ての偏差が定常状態において 0 に収束していることが確認できる。一方，x 軸方向及び y 軸方向の位置偏差，z 軸方向の姿勢偏差は単調減少

Fig. 5. Trajectory of image feature points(Solid: with proposed method, Dashed: withprevious one (10) ).

しているのに対して，その他の偏差は一度増加しているこ

0

0.5

1

1.5

pery [m]0-0.4perz [m]

0.5

0

0.5

1

1.5

xq erz [rad]-0.8
0

メラが後退し，かつ観測対象を捉えやすい姿勢に回転していることを表している。
0.5

1

1.5

最後に，予測視覚フィードバック制御の予測ホライズンの長さを変えたときのコストの違いに対して，以下の式で計算し，その結果を Table 1 に示す。

0.5

1

 I

1.5

0

0.5
1
time [s]

1.5

0

0
-1
0

l(x(τ ), u(τ ))dτ · · · · · · · · · · · · · · · · · (29)

Cost =

積分区間は I = 1.5 とした。評価関数のパラメータを適切

-0.5

-0.5
-1

とがわかる。これはカメラ視野を維持するために，一旦カ

xq erx [rad]

0.4
0

0.6
0.4
0.2
0
-0.2
0
0.6
0.4
0.2
0
-0.2
0
0.5

xq ery [rad]perx [m]0.8なものに選んだ安定化予測視覚フィードバック制御を用いた
0.5
1
time [s]1.5

ときのコストが受動性に基づく制御則を適用させたときのコストに比べて小さいことから，コストの面において制御

Fig. 6. Pose control error.

性能が高いことが確認できる。さらに，ホライズン T の長さを長くすると，より未来の状態を考慮しコストが最小になるように入力が計算されるために，よりコストが減少さ

Fig. 4 に提案手法を視覚フィードバックシステムに適用した場合の，状態 x のユークリッドノルム x を示す。初期姿勢では目標位置姿勢に一致しているため x(0) = 0よりも長く設定するとサンプリング時間内に制御入力を計であるが，目標位置姿勢がナビゲーション関数によって変算することができなくなった。

れることが確認できる。また，ホライズンの長さを T = 1

化するため，ノルム x も変化する。Fig. 4 より，目標位

5. 実験検証

置姿勢に収束した後の定常状態において平衡点 x = 0 に収束していることが確認される。したがって，目標位置姿勢

本稿では 3 次元の動的視覚フィードバック制御則を提案が変化した場合において，予測視覚フィードバック制御則したが，検証実験では簡単のために，Fig. 7 に示す土佐電

uRH の漸近安定性が示された。

子社のオムニホイールを有する全方向移動ロボットを用い
6

IEEJ Trans. EIS, Vol.132, No.5, 2012

安定化予測視覚フィードバック制御に対するナビゲーション関数を用いた目標位置姿勢生成

zc 5
c

0.3

pcox [m]Camera Frame

Object Frame

yo

yc

5o

0

0

2

4

6

8

10

0

2

4

6

8

10

00

2

4

6
time [s]

8

10

xc

pcoz [m]

-0.5

xo

zo

-1.5

xq coy [rad]0.6

Fig. 9. Estimated relative rigid body motion.

0.4

2

4

6

8

10

0

2

4

6

8

10

0

-0.4

0

-0.02
0.02
0

200
0

2

4

6

8

10

0

2

4

6
time [s]

8

-0.04

0

2

4

6

8

10

0.02

10

2

4

6
time [s]

8

-100

0

-0.02

0

Camera Field of View

100

fy [pixel]

0.40peez [m]

0.2
0

-0.6

xq ecy [rad]peex [m]

0.2
0

300

0.02

xq eey [rad]pecz [m]pecx [m]Fig. 7. Mobile robot and target object.

-300
-400

Fig. 8. Pose control error and estimation one.

f3(0)f1(1.2)f4(0)f2(1.2)f4(1.2) f2(0)

-200

10

f3(1.2) f1(0)-2000fx [pixel]

200

400

300
200

CCD カメラである KMT-1607N(30 fps) を搭載している。
カメラから画像を入力するボードとして，PCI バスカラー
画像入力ボードの PicPort-Color を使用する。また，画像処理は画像処理用ソフトウェア HALCON を使用して，観測対象上に取り付けた 4 点の中心点をそれぞれ算出する。
移動ロボットはディジタル制御装置として dSPACE 社のDS1104 を使用し制御する。サンプリング周期は 10 ms である。制御信号の送信には XBee を用いた。

100fy [pixel]

た 2 次元平面上での実験をおこなった。移動ロボットにはCamera Field of View
f3(10)

f3(0)
f4(0)

0

-100
-200
-300
-400

f1(10)-200

f4(10)

f2(10)

0fx [pixel]

200

f1(0)
f2(0)

400

Fig. 10. Trajectory of image feature points(Top: with previous method (10) , Bottom: withproposed one).

実験検証では，実験装置の可動範囲に制約が生じるため，
シミュレーション検証でおこなった光軸回転の検証ではな
く光軸以外の軸回転の検証をおこなう。具体的には，初期
設定は pco = [0.2 0 − 1.3]T m，ξθco = [0 π/12 0]T rad

イズ等の影響により微小な偏差は残っているものの，両偏と，また最終的な目標位置姿勢は pcdf = [0.03 0 − 0.6]

差とも 0 に向かって収束していることがわかる。 Fig. 9m，ξθcdf = [0 0 0]T rad とする。画像ピクセル値の最大値及び最小値は fxM = 350 pixel，fxm = −350 pixel，
fyM = 200 pixel，fym = −200 pixel とした。評価関数の実線は相対位置姿勢の推定値，破線は画像空間上のナビの重みや予測ホライズンの長さは以下のように設定した。

ものの，目標位置姿勢に対して相対位置姿勢が追従していqpc = 0.008，qrc = 0.005，qpe = 0.2，qre = 0.15，R =diag{1.5, 1.5, 1.75, 0.8, 0.8, 0.8, 0.05, 0.05, 0.05, 0.06, 0.06,
0.06}，ρ = 1，T = 0.05 s 。一方，目標軌道生成のためのゲ
インは kcd = 140000，Ks = diag{0.1, 0.05, 0.1, 0.05, 0.1,
0.05, 0.1, 0.05}，κ = 2 と設定した。
Figs. 8–10 に実験結果を示す。Fig. 8 は左に制御偏差，
右に推定偏差の実験結果を示している。上から 3 次元空間
での x，z 軸の位置偏差と y 軸周りの回転偏差を示す。シるのがわかる。また，Fig.10 には，画像特徴量 fco のトラT

ゲーション関数を用いて生成した目標位置姿勢を表している。モータへの電圧が制限されるために遅れが生じているジェクトリーを示している。上が従来手法 (10) ，下が提案手法の実験結果である。従来手法は 3 次元空間では偏差が減るように動いているものの，画像空間に対しては何も考慮していないために画像特徴点を外すようにカメラが動いてしまい，1.2 s で許容画像最大値を超え実験は失敗してしまう。それに対して，提案手法では画像空間を考慮して目標
位置姿勢を設計しているために，画像特徴量がカメラ視野

ミュレーション検証に比べて収束時間が長いのは，モータ内に留まる軌道でカメラが動いていることが確認できる。

に対して電圧制限がかからないように重みや目標軌道生成

さらに，予測ホライズンの長さを変えたときのコストののためのゲインを設定しているためである。摩擦や画像ノ違いを確認するために，(29) 式を計算し，その結果を Ta-

電学論 C，132 巻 5 号，2012 年

7Table 2. Values of the integral cost withexperimental results.
Control Schemeとロバスト制御による大道芸「くわえ撥」の実現”, 計測自動制御学
会論文集, Vol. 45, No. 10, pp. 530–537 (2010) (in Japanese)（ 5 ） J. Satake and J. Miura: “Person Following of a Mobile Robotusing Stereo Vision”, Journal of the Robotics Society ofJapan, Vol. 28, No. 9, pp. 1091–1099 (2010) (in Japanese)
佐竹 純二, 三浦 純: “ステレオビジョンを用いた移動ロボットの人物追従制御”, 日本ロボット学会誌, Vol. 28, No. 9, pp. 1091–1099(2010) (in Japanese)（ 6 ） 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. 6, pp. 789–814(2000)（ 7 ） M. Sauvée, P. Poignet, E. Dombre and E. Courtial: “Image based Visual Servoing through Nonlinear Model Predictive Control”, Proc. 45th IEEE Conference on Decision andControl, pp. 1776–1781 (2006)（ 8 ） M. Sauvée, P. Poignet and E. Dombre: “Ultrasound Imagebased Visual Servoing of a Surgical Instrument through Nonlinear Model Predictive Control”, The International Journalof Robotics Research, Vol. 27, No. 1, pp. 25–40 (2008)（ 9 ） G. Allibert, E. Courtial and F. Chaumette: “Predictive Control for Constrained Image-based Visual Servoing”, IEEE
Trans. on Robotics, Vol. 26, No. 5, pp. 933–939 (2010)（10） T. Murao, H. Kawai and M. Fujita: “Predictive Visual Feedback Control with Eye-in/to-Hand Configuration via Stabilizing Receding Horizon Approach”, Proc. of the 17th
IFAC World Congress on Automatic Control, pp. 5341–5346(2008)（11） T. Murao, H. Kawai and M. Fujita: “Stabilizing Predictive
Visual Feedback Control for Fixed Camera Systems”, IEEJ
Trans. Electronics, Information and Systems, Vol. 129,
No. 4, pp. 630–638 (2009) (in Japanese)
村尾俊幸, 河合宏之, 藤田政之: “固定カメラシステムに対する安定化
予測ビジュアルフィードバック制御”, 電気学会論文誌 C, Vol. 129,
No. 4, pp. 630–638 (2009) (in Japanese)（12） N. J. Cowan, J. D. Weingarten and D. E. Koditschek: “Visual
Servoing via Navigation Functions”, IEEE Trans. Robotics
and Automation, Vol. 18, No. 4, pp. 521–533 (2002)（13） N. J. Cowan and D. E. Chang: “Geometric Visual Servoing”, IEEE Trans. on Robotics, Vol. 21, No. 6, pp. 1128–1138(2005)（14） J. Chen, D. M. Dawson, W. E. Dixon and V. K. Chitrakaran:“Navigation Function-based Visual Servo Control”, Automatica, Vol. 43, No. 7, pp. 1165–1177 (2007)（15） R. Murray, Z. Li and S. S. Sastry: A Mathematical Introduction to Robotic Manipulation, CRC Press (1994)（16） 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)（17） F. Bullo and A. D. Lewis: Geometric Control of Mechanical
Systems, Springer-Verlag (2004)（18） H. Kawai, T. Murao and M. Fujita: “Passivity-based Visual
Motion Observer with Panoramic Camera for Pose Control”,
Journal of Intelligent and Robotic Systems, Vol. 64, No. 3–4,
pp. 561–583 (2011)（19） T. Ohtsuka: “A Continuation/GMRES Method for Fast
Computation of Nonlinear Receding Horizon Control”, Automatica, Vol. 40, No. 4, pp. 563–574 (2004)

cost

Passivity based Control

13.7056

Receding Horizon Control (T = 0.01 [s])11.4265

Receding Horizon Control (T = 0.05 [s])10.1662

Receding Horizon Control (T = 0.5 [s])7.9265

Receding Horizon Control (T = 1 [s])5.2540ble 2 に示す。積分区間は I = 15 とした。安定化予測視覚フィードバック制御を用いたときのコストの方が，受動性に基づく制御則を適用させたときのコストに比べて小さく，またホライズン T の長さを長くすると，よりコストが減少することが実験結果からも確認できた。以上のことより，画像特徴点をカメラ視野に保持できる安定化予測視覚
フィードバック制御の有効性が示された。

6. おわりに
本論文では 3 次元の視覚フィードバックシステムに対して，画像空間上のナビゲーション関数を用いた安定化モ
デル予測制御則を提案した。はじめに，目標位置姿勢が時変となる場合のシステムに対して，安定化予測視覚フィー
ドバック制御を提案した。そして，制御則への目標位置姿
勢生成を画像空間上のナビゲーション関数を用いる手法で設計した。ナビゲーション関数をリアプノフ関数候補とすることで設計する目標位置姿勢が最終的な目標位置姿勢に収束することを示している。提案手法は特徴点を常にカメラ視野内に維持しつつ最適理論に基づいた安定化予測視覚
フィードバック制御をシステムに適用できる手法となる。
最後にシミュレーションと検証実験により，光軸回転と光軸以外の回転の両方において，提案した制御手法の有効性を示した。
（平成 23 年 7 月 7 日受付，平成 23 年 12 月 26 日再受付）

文献（ 1 ） F. Chaumette and S. A. Hutchinson: “Visual Servoing
and Visual Tracking”, In:B. Siciliano and O. Khatib (Eds),
Springer Handbook of Robotics, Springer–Verlag, pp. 563–
583 (2008)（ 2 ） F. Chaumette: “Potential Problems of Stability and Convergence in Image-based and Position-based Visual Servoing”,
D. J. Kriegman, G. D. Hager and A. S. Morse (Eds.), The
Confluence of Vision and Control, Lecture Notes in Control and Information Sciences 237, Springer–Verlag, pp. 66–
78 (1998)（ 3 ） T. Saitoh, N. Tada and R. Konishi: “Indoor Mobile Robot
Navigation by Central Following based on Monocular Vision”, IEEJ Trans. Electronics, Information and Systems,
Vol. 129, No. 8, pp. 1576–1584 (2009) (in Japanese)齊藤 剛史, 多田 直也, 小西 亮介: “単眼カメラを用いた中央走行型の屋内移動ロボット”, 電気学会論文誌 C, Vol. 129, No. 8,
pp. 1576–1584 (2009) (in Japanese)（ 4 ） K. Katoh, K. Hirata, T. Mizuno and K. Yamada: “Development of “KUWAEBACHI” Robot via Visual Feedback Robust Control”, Trans. the Society of Instrument and Control
Engineers, Vol. 45, No. 10, pp. 530–537 (2010) (in Japanese)
加藤健一, 平田健太郎, 水野貴志, 山田晃平: “視覚フィードバック

付

録

1. 画像空間上のナビゲーション関数
本節では，全ての画像特徴量をカメラ視野に維持するための画像空間上のナビゲーション関数 ϕ(fcd ) (12) (14) について述べる。モデル空間上のナビゲーション関数を微分同相
写像を用いて変換することにより，画像空間上のナビゲー
ション関数を構築する。
まずはじめに二つの補助的な関数である η(fcd ) : R8 →[−1, 1]8 と s(η) : [−1, 1]8 → R8 をつぎのように定義する。
8

IEEJ Trans. EIS, Vol.132, No.5, 2012

安定化予測視覚フィードバック制御に対するナビゲーション関数を用いた目標位置姿勢生成

2
2
2
,
,···
fxM − fxm fyM − fym
fyM − fymη(fcd ) = diag−s(η) =

fyM + fym
fxM + fxm fyM + fym···
fxM − fxm fyM − fym
fyM − fym
η1 − ηf 1(1 − η12κ )

1
2κ

···

η8 − ηf 8(1 − η82κ )



河

fcd

合

宏

科電子情報システム専攻博士前期課程修了，2004

T

年金沢大学大学院自然科学研究科機能開発科学専(付 1)

攻博士後期課程修了，同年法政大学情報技術 (IT)

T

1
2κ

之 （非会員） 2001 年金沢大学大学院自然科学研究

研究センターポストドクター。2005 年金沢工業大· · · · · · · · · · ·(付 2)学講師。2010 年同准教授。2008 年 IEEE TCST

Outstanding Paper Award。視覚フィードバッ

ここで，η(fcd ) = [η1 (fcd ) η2 (fcd ) · · · η8 (fcd ) ]T であり，

ク制御とその応用に関する研究に従事。博士（工κ > 0 ∈ R は s(η) によって形作られるポテンシャル場を変化させるためのパラメータである。また，fxM ，fxm ，fyM ，
fym は x 軸，y 軸における画像ピクセル値の最大値及び最小値をそれぞれ表す。(付 1) 式の関数に対して，目標画像
特徴量ベクトルの最終値 fcdf に関しては ηf := η(fcdf ) と表すことにする。補助関数 η(fcd ) は現在の目標画像特徴学）。計測自動制御学会，システム制御情報学会，IEEE の会員。

藤

田

政

之 （非会員） 1984 年早稲田大学大学院博士前期課程修了。1985 年同博士後期課程中退。同年金沢大学助手，同講師，助教授を経て，1992 年北陸先端科学技術大学院大学助教授。1999 年金沢大学

量を画像ピクセル値の最大値及び最小値に対して正規化す教授。2005 年東京工業大学教授。1994〜1995 年

るための関数であり，また関数 s(η) は現在の目標画像特ミュンヘン工科大学客員研究員。2011 年 IEEE

徴量とその最終値との偏差を定義した関数となっている。

CSS Vice President, 2008〜2011 年 IEEE CSS

ϕ̃(η) ∈ R8 → [0, 1] で表されるモデル空間上のナビゲー

Board of Governor，2005〜2006 年 SICE 理事，
2010 年 IEEE MSC General Chair，IEEE TAC，IEEE TCST，ション関数はつぎのように定義される。

ϕ̃(η) :=

Automatica，Asian J. Control の Associate Editor，SICE JCMSI

ϕ̄
· · · · · · · · · · · · · · · · · · · · · · · · · · · · (付 3)
1 + ϕ̄

の Editor。2008 年 IEEE TCST Outstanding Paper Award，2000
年 ISCIE 論文賞，2009, 1997 年 SICE 論文賞，2010 年 SICE 教育

さらに (付 3) 式内の関数 ϕ̄(η) ∈ R8 → R はつぎのように貢献賞。ロバスト制御とその応用，ビジュアルフィードバックによる定義される。

協調制御の研究に従事。工学博士。IEEE CSS などの会員。

1
ϕ̄(η) := sT (η)Ks s(η)· · · · · · · · · · · · · · · · · · · · (付 4)2
ここで，Ks ∈ R8×8 は正定で対角なゲイン行列である。(付3) 式の関数がナビゲーション関数となることは文献 (12) のProposition 3 を参考にされたい。
ϕ(fcd ) ∈ D → R で表される画像空間上のナビゲーション関数はつぎのように表すことができる。

ϕ(fcd ) := ϕ̃ ◦ ϕ̄ ◦ s ◦ η(fcd )· · · · · · · · · · · · · · · · (付 5)
ここで，◦ は合成演算子を表している。したがって，勾配ベクトル ∇ϕ(fcd ) はつぎのように計算できる。

∇ϕ(fcd ) :=
=

∂ϕ
∂fcd

T

1
2
2
2
diag
,
,···,
(1 + ϕ̄)2
fxM − fxm fyM − fym
fyM − fym



×diag

1 − η12κ−1 ηf 1

2κ+1

(1 − η12κ ) 2κ,···,

1 − η82κ−1 ηf 8

2κ+1

(1 − η82κ ) 2κ

Ks s· · · (付 6)(付 1)〜(付 6) 式より，∇ϕ(fcd ) → 0 のとき fcd (t) → fcdfになることは明らかである。

村

尾

俊

幸 （正員） 2005 年金沢大学大学院自然科学研究科電子情報システム専攻博士前期課程修了，2008 年
東京工業大学大学院理工学研究科機械制御システ
ム専攻博士後期課程修了。2006 年産業技術大学
院大学研究員，2007 年同助教。視覚フィードバッ
ク制御に関する研究に従事。博士（工学）。計測
自動制御学会，システム制御情報学会，IEEE などの会員。

電学論 C，132 巻 5 号，2012 年

9