論文

ナビゲーション関数を用いた視覚フィードバック制御による障害物回避
正 員

村尾 俊幸∗,∗∗

非会員

河合

宏之∗∗∗

非会員

鶴尾 有生∗∗∗

非会員

藤田政之∗∗∗∗

Obstacle Avoidance of Visual Feedback Control via Navigation Function
Toshiyuki Murao∗,∗∗ , Member, Hiroyuki Kawai∗∗∗ , Non-member, Yusei Tsuruo∗∗∗ , Non-member,
Masayuki Fujita∗∗∗∗ , Non-member
This paper investigates passivity-based pose control via an obstacle avoidance navigation function for threedimensional (3-D) eye-in-hand visual feedback systems. Firstly, we describe the representation of a relativepose and a camera model. Next, a stabilizing pose control law for a 3-D visual motion error system which atime-varying desired pose can be applied to is proposed. Moreover, a path planner to be appropriate for thevisual motion error system is designed through an obstacle avoidance navigation function to keep collisionfree. It should be noted that we develop a visual motion observer for path planning in order to estimate theposition of the obstacles from the camera. The main contribution of this paper is to provide that the path
planner which can achieve obstacle avoidance during the servoing is designed for the 3-D eye-in-hand visualfeedback control. Finally, we present simulation results and actual nonlinear experimental results using anaerial vehicle in order to verify control performance of the proposed control scheme with obstacle avoidance.
キーワード：視覚フィードバック制御，障害物回避，ナビゲーション関数，受動性，安定性，飛行ビークル

Keywords: visual feedback control, obstacle avoidance, navigation function, passivity, stability, aerial vehicleは古くからおこなわれており，ロボットを制御対象とした

1. はじめに研究成果も継続的に数多く発表されている (2)〜(4) 。

近年，視覚フィードバック制御は，工場をはじめとした

一方，その適用範囲を広げるために障害物回避問題を考あらかじめ可動範囲内の状況を整えられるような環境へのえることは重要であり，ロボットを目標点まで移動させる

適用だけでなく，より複雑な環境や幅広い分野，例えば医問題において，障害物の判断に視覚情報を用いる手法は非療分野や福祉分野，環境分野においても適用が見込まれる

常に有益となる。視覚フィードバック制御の研究と同様にロようになってきている(1)

ボットの障害物回避の研究も古くから注目されており，な。視覚フィードバック制御の研究

かでも Rimon ら (5) によって提案されたナビゲーション関∗ 早稲田大学 理工学術院

数を用いる手法は，障害物回避に対する非常に有効な手法〒 169–8555 東京都新宿区大久保 3–4–1
Faculty of Science and Engineering, Waseda University
3–4–1 Okubo Shinjuku-ku, Tokyo 169–8555∗∗ 独立行政法人科学技術振興機構, CREST〒 332–0012 埼玉県川口市本町 4–1–8
Japan Science and Technology Agency, CREST
4–1–8 Honcho Kawaguchi-shi, Saitama 332–0012∗∗∗ 金沢工業大学 工学部〒 924–0838 石川県白山市八束穂 3–1
College of Engineering, Kanazawa Institute of Technology
3–1 Yatsukaho Hakusan-shi, Ishikawa 924–0838∗∗∗∗ 東京工業大学 大学院理工学研究科〒 152–8550 東京都目黒区大岡山 2–12–1
Graduate School of Science and Engineering, Tokyo Institute of Technology
2–12–1 Ookayama Meguro-ku, Tokyo 152–8550
電学論 C，125 巻 1 号，2005 年として知られている。ナビゲーション関数は局所的な最小点への収束を回避できる人工的なポテンシャル関数であり，この関数を利用することで経路探索と軌道生成を同時におこなうことができる。
ナビゲーション関数と視覚フィードバック制御を融合させた研究では，Cowan ら (6) が視覚情報を組み込んだナビ
ゲーション関数を，ロボットマニピュレータへのトルク入力に直接用いる手法を提案している。さらに，Chen ら (7) は2-1/2D 視覚フィードバック制御則に，また著者ら (8) は安定化予測視覚フィードバック制御則に，画像空間上のナビ
ゲーション関数を用いて生成した目標軌道を適用させる手法をそれぞれ提案している。しかし，これまでのナビゲー
1

ミュレーション結果と実験結果をそれぞれ示し，最後に 6

Target Object

節でまとめをおこなう。

2. 安定化視覚フィードバック制御

So

Sb1

Sw

本節は目標位置姿勢を時不変としていた文献 (11) の手法

Obstacle 1に対して，時変の場合に拡張した安定化視覚フィードバッ
ク制御について示す。なお本節での目的は，カメラからみた対象物の相対位置姿勢を目標位置姿勢に一致させること

Sc

Camera-mounted
Vehicleとなる。
〈2・1〉 剛体運動の表現とカメラモデル

Sbn

Obstacle n

本論文では，

Fig. 1 に示すような，カメラが取り付けられたビークル (以下，カメラと省略する) と目標となる対象物 (以下，対象物と省略する) および n 個の障害物からなる Eye-in-Hand 構

Fig. 1. Visual feedback system with aneye-in-hand configuration.

造の視覚フィードバックシステムについて考える。本論文でション関数と視覚フィードバック制御を組み合わせた研究

は，対象物は静止しているとする。基準とする座標系として

は，制御の途中でカメラ視野から画像特徴点を外さないこ

基準座標系 Σw ，カメラにカメラ座標系 Σc ，対象物に対象とを目的として提案されたものであり，障害物回避を目的

物座標系 Σo ，各障害物に障害物座標系 Σbi (i = 1, · · · , n)とした手法ではなかった。

を定義する。また，任意の二つの座標系 Σa から Σb の位

一方，ナビゲーション関数を用いずに障害物回避と視覚

置 pab ∈ R3 と姿勢を表す回転行列 eξ̂θab ∈ SO(3) を要

フィードバック制御を組み合わせた最近の研究として，文素に持つ同次表現 gab = (pab , eξ̂θab ) を以下のように定義

献 (9) では地面を走行するモバイルロボットに対して外部する。

環境モデルが不要な手法を提案している。この手法は，障害gab =物が現れない場合の目標画像群に対する視覚フィードバッ
ク制御と障害物が現れた場合の 2 次元空間に特化した障[eξ̂θab0]
pab1∈ R4×4 · · · · · · · · · · · · · · · · (1)

害物回避制御を切り替えた制御手法となっている。また文

ただし，演算子 ∧(wedge) はパラメータベクトル表現を行

献 (10) では飛行ビークルに対して，安定性に対する議論は列表現に変換する演算子であり，3 次元の場合は 3 次元ベ

されていないものの，モデル予測制御を用いることで入力

クトルを 3 × 3 の歪対称行列へ写像し a ∈ R3 と b ∈ R3

やカメラ視野に対する拘束を設けながら障害物回避をおこに対して âb = a × b を満たす。また，その逆写像は ∨(vee)

なう手法が提案されている。しかし，これらの手法で定義される (12) 。

(9) (10)

本論文の視覚フィードバックシステムは対象物と障害物はカメラだけでなくレーザレンジファインダなどの測距セがカメラの観測対象となるが，本節では冗長な議論を避けンサも別途必要となる。

るため，座標系 Σo と Σbi を一時的に観測対象座標系 Σq

そこで，本論文では Fig. 1 に示すようなカメラのみを用いた 3 次元の視覚フィードバックシステムに対して，障として議論していく† 。カメラ座標系からみた各観測対象の害物回避のためのナビゲーション関数を用いた安定化視覚

相対位置姿勢 gcq は同次表現の合成則から以下のように導フィードバック制御則を提案する。はじめに，目標位置姿かれる。

勢が時変となる場合の受動性に基づく安定化視覚フィード−1
gcq = gwc
gwq · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (2)バック制御則を提案する。つぎに，障害物を回避しながら設定する最終目標位置姿勢へ収束させるような安定化視覚また，カメラ座標系からみた各観測対象の相対速度は，相フィードバック制御則への目標位置姿勢を，障害物回避ナ

対位置姿勢 gcq を時間微分することで，ビゲーション関数を用いることで生成する手法を示す。ナ

b
b
b
−1 V
Vcq
= −Ad(gcq) wc + Vwq · · · · · · · · · · · · · · · · · · (3)ビゲーション関数をリアプノフ関数候補とすることで設計する目標位置姿勢が最終目標位置姿勢に収束することを示−1bのように導かれる (11) 。ここで，Vab= (gabġab )∨ ∈ R6 はしている。さらに，提案手法の有効性を検証するために，飛剛体運動のボディ速度を表しており，Ad(gab ) ∈ R6×6 は行ビークルを用いた実験をおこなう。本手法は文献 (9) (10)同次表現 gab の随伴写像である (12) 。
一方，本論文の視覚フィードバックシステムでは，観測出の手法に対して，レンジセンサを用いずに一台のカメラの力として得られる情報はピンホールカメラから得られる画みで障害物回避をおこなえるという利点がある。
本論文の構成は以下のとおりである。2 節では時変の目

像特徴量のみである。ここで，各観測対象には mq (≥ 4) 点

標位置姿勢に対する安定化視覚フィードバック制御についの特徴点が取り付けられており，すべて観測可能であるとすて述べる。3 節で，障害物回避のためのナビゲーション関† すなわち添え字の q を o と bi に読み替えることで，それぞれの数を用いた目標位置姿勢生成をおこなう。4 節と 5 節でシ

相対位置姿勢を表すことになる。

2

IEEJ Trans. EIS, Vol.125, No.1, 2005

ナビゲーション関数を用いた視覚フィードバック制御による障害物回避る。このとき，観測対象座標系 Σq の原点から各特徴点まですることになる。ここで，次式の制御則を提案する。

のベクトルを pql ∈ R (l = 1, · · · , mq ) とすると，カメラ3

u = −K(−x), K := diag{Kc , Ke } · · · · · · · · · (10)からみた相対的な特徴点 pcql ∈ R3 は pcql = gcq pql で表される。ただし，特徴点 pql はすべて既知であるとし，同次表

ただし Kc > 0，Ke > 0 は，それぞれ制御偏差と推定偏差T(12)
現でよく用いられる表記法 ([pT)にab 1] を pab と表記より表したものである。画像特徴量を fcql := [fcqxl fcqyl ]Tに対するゲインである。

と定義すると，特徴量は透視変換を用いることで，〔定理 1〕

fcql =

λ
zcql

[

xcql
ycql

このとき，安定性に関してつぎの定理が導かれる。

]

(11)

b
対象物が静止している (Vwo
= 0) とき，視覚フィードバック偏差システム (8) と (10) 式の入力で構· · · · · · · · · · · · · · · · · · · · · · · · · (4)成される閉ループ系の平衡点 x = 0 は漸近安定である。
この定理 1 は，視覚フィードバック偏差システム (8) が入で表される。 ただし，λ ∈ R はカメラの焦点距離であり，

力 u から出力 −x に対して受動性を有することに基づくこpcql の各要素を pcql = [xcql ycql zcql ]T として表した。こTTの fcql を縦に並べたベクトル fcq = [fcq1· · · fcqm
]T ∈q
R2mq が各観測対象に関してカメラから得られる視覚情報とで，文献 (11) と同様の手法でリアプノフの安定定理によとなる。

bのボディ速度 Vcd
が組み込まれる点で，文献 (11) の定理〈2・2〉 受動性に基づく視覚フィードバック制御

り証明することができる。ただし，本論文では目標位置姿
勢 gcd を時変としたことから入力 uc に目標位置姿勢 gcd
を拡張したものとなっている。すなわち，(9) 式の入力 uc

一般bにおいて，Vcd= 0 とすれば文献 (11) と一致する。

的な位置ベース法の視覚フィードバックシステムの制御目
的は，相対位置姿勢 gco を目標位置姿勢 gcd に常に一致さ

3. 障害物回避ナビゲーション関数を用いた目標位
置姿勢生成

せておくことである。本研究ではこの目標位置姿勢 gcd を時変とする。制御則を構成するにあたり相対位置姿勢 gco
は直接計測することができないため，その推定値 ḡco を得

前節では，カメラからみた対象物の相対位置姿勢を目標

るために (3) 式に基づいたつぎの動的視覚オブザーバを構位置姿勢に一致させる安定化制御則を提案したが，障害物成する。

回避については考慮されておらず，本論文で時変としている目標位置姿勢に対する議論もなかった。障害物回避を考

b
b
−1 V
V̄co
= −Ad(ḡco) wc + ue · · · · · · · · · · · · · · · · · · · · (5)慮した視覚フィードバック制御の制御目的は，障害物を回避ξ̂θcdfue ∈ R6 は推定偏差の振舞いを安定にするために加えられしながら設定する最終目標位置姿勢 gcdf = (pcdf , eξ̂θcdに収束するような時変の目標位置姿勢 gcd = (pcd , e

る入力であり，後に制御則の一部として提案する。
ここで，制御偏差 gec = (pec , eξ̂θec ) と推定偏差 gee =ξ̂θee(pee , eを生成し，それにカメラからみた対象物の相対位置姿勢

gco = (pco , eξ̂θco ) を一致させることとなる。ただし，本研
究では実システムを考慮し，最終目標位置姿勢 gcdf は一)を−1
gec = gcd
gco · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(6)定値とする。

−1
gee = ḡcogco · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · ·(7)そこで本節では，障害物を回避しながら最終目標位置姿と定義する。また，rei := sk(eξ̂θei )∨ ∈ R3 ，sk(eξ̂θei ) :=勢 gcdf へ収束するような目標位置姿勢 gcd のボディ速度
b
b T
b T T
b
Vcd
= [(vcd) (ωcd) ] を設計する。目標速度 Vcd
が設計されれば，gcd はつぎの微分方程式を解くことで生成で1 ξ̂θei− e−ξ̂θei ) を用いて，制御偏差ベクトルを ec :=
2 (e
T T
T T
[pTec rec] ∈ R6 と，推定偏差ベクトルを ee := [pTee ree] ∈6
R と定義する。推定偏差ベクトル ee は視覚情報 fco と推定値 ḡco から得ることができる

(11)

きる。

。また，目標位置姿bġcd = gcd V̂cd
· · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (11)

勢 gcd と推定偏差ベクトル ee を用いると，制御偏差ベク

前節で提案した視覚フィードバック制御則 (10) と (9) 式でトル ec も計算可能となる (13) 。

b表される偏差システムの入力により，カメラ入力 Vwc
はEye-in-Hand 構造の視覚フィードバック偏差システムは(6)(7) 式を時間微分することで
[
]
[ ]
] [
b
−1
−Ad(gec
0
Vec
I
)
b
Vwo
(8)
u+
=
b
−1
I
Vee
0
−Ad(gee
)

(
)
b
b
Vwc
= Ad(gcd ) Kc ec − Vcd· · · · · · · · · · · · · · · · · (12)bと目標位置姿勢のように構成されるため，目標速度 Vcd

gcd を用いることで生成可能となる。
〈3・1〉 目標軌道生成に対する姿勢偏差と導出できる。ただし，入力を

u := [uTc uTe ]T ,b
目標速度 Vcdを設計するにあたり，本節では姿勢に対して議論する。障bbuc := Ad(g−1 ) Vwc+ Vcd· · · · · (9)

害物回避は，位置成分を用いて達成させるために，姿勢成

cd

分は目標姿勢 eξ̂θcd をその最終値 eと定義している。Eye-in-Hand 構造の視覚フィードバックξ̂θcdfに収束させることのみを目的とする。そのため，つぎの姿勢偏差を定義する。

T T
偏差システムにおいて，状態 x := [eTc ee ] を 0 にとどめeξ̂θed = e−ξ̂θcdf eξ̂θcd · · · · · · · · · · · · · · · · · · · · · · · · (13)ておくことで相対位置姿勢 gco が目標位置姿勢 gcd に一致
電学論 C，125 巻 1 号，2005 年))

3

また，偏差ベクトルを red := 12 (eξ̂θed − e−ξ̂θed )∨ として定義する。最終目標位置姿勢 gcdf が時不変であることを考慮

F の中に存在する n 個の障害物を表す集合であり，中心に座標系 Σbi を持つ半径 ρi の球体で定義する。本論文ですると，(13) 式を時間微分することで，姿勢偏差速度の関は，移動可能な領域の境界と障害物および障害物同士は重ならないとする。すなわち，∥pcd − p0 − pcbi ∥ + ρi < ρ0 ，

係式はつぎのように導出することができる。

∥pcbi − pcbj ∥ > ρi + ρj ，(1 ≤ i, j ≤ n) とする。

b
b
ωed
= ωcd
· · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (14)〈3・2〉 障害物回避のための動的視覚オブザーバ

つぎに，ナビゲーション関数という特殊人工ポテンシャ

カル関数を導入する。
〔定義 1〕

メラに障害物を回避させるには，カメラ座標系からみた障(5) (14)滑らかなモース関数 φ(pcd ) : D → [0, 1]

害物の位置情報 pcbi が必要になる。位置情報はカメラからが以下の性質を有するとき，φ はナビゲーション関数で直接計測できないために，対象物のときと同様に，障害物ある。

の位置情報を推定するためにつぎの動的視覚オブザーバを∈ D のとき唯一の最小値が存在し，φ(pcdf ) = 0 となる。
（ 2 ） D の境界で最大値を持つ。
（ 1 ） pcdf

設計する。
b
b
V̄cbi
= −Ad(ḡ−1 ) Vwc+ ubi · · · · · · · · · · · · · · · · · · (15)

障害物回避のためのナビゲーション関数を具体的に設計

cbi

ただし，ḡcbi はカメラ座標系からみた障害物の相対位置姿していく。はじめに目標位置 pcd とその最終値 pcdf との勢 gcbi の推定値であり，ubi ∈ R6 は後に提案する推定偏間の偏差に関するつぎの関数を定義する。

差の振舞いを安定にするための入力である。

s(pcd ) = ∥pcd − pcdf ∥2κ · · · · · · · · · · · · · · · · · · · · (21)

ここで，障害物の位置姿勢に対する推定偏差 gebi をつぎのように定義する。

ここで，κ は形成されるポテンシャル場を変化させるため−1
gebi = ḡcbi
gcbi · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (16)のパラメータである。つぎに，障害物関数 η をつぎのように定義する。

T T
6
また，推定偏差ベクトルを ebi := [pT
ebi rebi ] ∈ R と定義する。障害物に対する推定偏差ベクトル ebi は視覚情報

η(pcd ) =fcbi と推定値 ḡcbi からつぎのように得ることができる。

n∏ηi (pcd ) · · · · · · · · · · · · · · · · · · · · · · · · (22)i=0{

ebi = Je† (ḡcbi )(fcbi − f¯cbi )· · · · · · · · · · · · · · · · · · · (17)ηi (pcd ) =ここで，f¯cbi は画像特徴量 fcbi の推定値を表し，Je (·) は

ρ20 − ∥pcd − p0 ∥2∥pcbi ∥2 − ρ2i(i = 0)(i = 1, · · · , n)

画像ヤコビアンの一種である (11) 。また，(16) 式を時間微

ただし，η0 は移動可能領域 F の内部にカメラを留めるた

分することで，障害物に対する推定偏差システムは下記のめの関数である。障害物関数 η(pcd ) で必要となるカメラ

ように導出することができる。

座標系からみた障害物の位置 pcbi は前節で設計した動的視

b
b
Vebi
= −Ad(g−1 ) ubi + Vwbi
· · · · · · · · · · · · · · · · · ·(18)

覚オブザーバにより得られることに注意されたい。この障

上述の障害物に対する動的視覚オブザーバを用いることことを示している。さらに，パラメータ κ を用いたモデル

で，カメラ座標系からみた障害物の位置 pcbi は (16)(17) 式

空間上のナビゲーション関数 φ̃(x) ∈ R → [0, 1] をつぎのより，ように定義する。

ebiˆξ̄ θ̄cbi

pcbi = e

害物関数は 0 に近づくほど領域の境界や障害物に近くなる(

pebi + p̄cbi · · · · · · · · · · · · · · · · · · · · · · (19)φ̃(x) =と計算することができる。
〈3・3〉 障害物回避ナビゲーション関数

x
µ+x

) κ1

· · · · · · · · · · · · · · · · · · · · · · · · · (23)

本節では，障ここで，µ は収束速度を調整するためのパラメータである。

害物を回避しながら最終目標位置へ収束させるためのナビ(21)–(23) 式の関数を組み合わせることで，障害物回避のゲーション関数 φ(pcd ) を設計する。

ためのナビゲーション関数をはじめに，カメラが移動可能でかつ障害物に衝突しないs
φ(pcd ) = φ̃ ◦ (pcd ) =η集合 D をつぎのように定義する。

D=F −

n
∪

Bi · · · · · · · · · · · · · · · · · · · · · · · · · · · · (20)(s(pcd )µη(pcd ) + s(pcd )

) κ1
(24)

と設計する。ここで，◦ は合成演算子を表している。紙面

i=1の都合上証明は省略するが，κ を適切に設定した (24) 式のここで，F はカメラが移動可能な領域を中心 p0 ，半径 ρ0関数がナビゲーション関数となることは，文献 (14) の定理の球体で定義した集合である† 。また，Bi は移動可能領域

4 と同様の手法で証明できる。
〈3・4〉 目標速度の提案と収束解析† 本論文では対象物は静止しているとしているため，p は対象物座0
標系 Σo を基準として考えることができる。たとえば，p0 = [0 0 0]T

ナビゲーション

関数および姿勢偏差を用いることで，つぎの目標速度とそとすると，対象物を中心に半径 ρ0 の空間がカメラの移動可能領域となり，p0 = pco (0) とするとカメラの初期値が中心の空間となる。

れを生成するための推定入力を提案する。
4

IEEJ Trans. EIS, Vol.125, No.1, 2005

ナビゲーション関数を用いた視覚フィードバック制御による障害物回避

gcdf

...

...

Visual Motion
Observer

fcbn
b
Vwc

Obstacle Avoidance
Navigation Functionbased Path Planner

gcb1
Navigation
eb1 Function-based b
vcd
Translational
gcbn
Velocityebn Path PlannergcdbAngularωcd
Velocity
gcdf
Path Planner

b
Vcd

fco
b
Vwc

Visual Motion
Observer

gco
ee

b
b
Vcd
vcd
b
ωcd

gcd
g_cd = gcdV^cdb

b
Vwbi

gcd
b
Vwc

Pose
Controller

fco
Vision fcb1
Camerafcbn...

Visual Motion
Observer

fcb1

...

b
Vwc

Visual Motion Observerbased Pose Controller

Fig. 2. Block diagram of visual motion observer-based pose control with obstacle avoidancenavigation function-based path planner.
[
b
Vcd
=

−e−ξ̂θcd Kdp ∇φ(pcd )−Kdr e−ξ̂θed red]· · · · · · · · · · · · (25)+n∑eTbi Ad(eξ̂θebi ) (−Ad−1(gebi ) ubi )i=1= −(∇φ)T eξ̂θcd e−ξ̂θcd Kdp ∇φ
n
∑
( T
)
T ξ̂θed−rede
Kdr e−ξ̂θed red +−ebi Kb ebi

ubi = Kb ebi · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · · (26):= diag{kdp1 , kdp2 , kdp3 } と Kdr :=diag{kdr1 , kdr2 , kdr3 } は，それぞれ x 軸，y 軸，z 軸の並進と回転における目標速度に対するゲインであり，Kb :=diag{kb1 , · · · , kb6 } は推定入力に対するゲインである。ま)T(∂φ(pcd )た，∇φ(pcd ) :=は φ(pcd ) の勾配ベクトルを∂pcd
表し，計算すると次ページ下部に示す (27) 式となる。
提案した目標速度 (25) と推定入力 (26) を用いることで，た だ し ，Kdp

i=1

= −(∇φ)T Kdp ∇φ − (e−ξ̂θed red )T Kdr e−ξ̂θed redn∑−eTbi Kb ebi · · · · · · · · · · · · · · · · · · · · · · · · · · (29)i=1

この計算過程において，推定偏差 p̂ebi の歪対称性が用いられている。(28)(29) 式から，初期状態 pcd (0) ∈ D に対つぎの定理が成立する。

して，pcd (t) ∈ D, ∀t > 0 となることがわかる。推定偏〔定理 2〕 pcd (0) ∈ D かつ対象物および障害物が静止して差 ebi が 0 となるために，障害物関数で用いているカメラbb= 0) とする。このとき，(25)(26) 式
いる (Vwo= 0，Vwbi座標系からみた障害物の相対位置の推定値 p̄cbi が真値に一によって生成される目標トラジェクトリーは pcd (t) ∈ D

致する。またナビゲーション関数では ∇φ(pcd ) → 0 のとであり，かつ目標位置姿勢 gcd (t) は漸近安定な平衡点

きには pcd (t) → pcdf となる (14) 。さらに，e−ξ̂θed red → 0

gcdf を持つ。
証明 : つぎの正定な関数を考える。
n∑E(gebi )· · · · · · · · (28)
Vn = φ(pcd ) + ϕ(eξ̂θed ) +は red → 0 となるために，姿勢偏差の定義より eξ̂θcd (t) →eξ̂θedf となる。したがって，gcd (t) → gcdf が導かれる。2定理 2 はナビゲーション関数をリアプノフ関数候補の一部とすることで証明している。この定理 2 により，提案しi=1

ここで E(gab ) :=1
2
2 ∥pab ∥たナビゲーション関数を用いた軌道生成をおこなうことで，+ ϕ(eξ̂θab ) であり，関数

目標位置姿勢 gcd (t) が障害物を回避しながら最終目標位置ϕ(eξ̂θab ) は回転行列に対する誤差関数を表す正定関数である。(14)(18)(25)(26) 式を用いると，(28) 式の正定関数

姿勢 gcdf に収束することが示された。Fig. 2 に障害物回避のためのナビゲーション関数を用いた受動性に基づく視覚を解軌道に沿って時間微分することにより，つぎの関係式

フィードバック制御のブロック図を示す。3 節で提案したが得られる。
T ξ̂θed bωed +V̇n = (∇φ)T ṗcd + reden∑

目標位置姿勢生成部分では，カメラから取得した各障害物
bに対する画像特徴量 fcbi とカメラ入力 Vwc
を用いて動的beTbi Ad(eξ̂θebi ) Vebi

視覚オブザーバにより障害物の位置姿勢を推定し，障害物

i=1bT ξ̂θed b
ωcd
+ red
e
= (∇φ)T eξ̂θcd vcd

電学論 C，125 巻 1 号，2005 年bを避けるような目標速度 Vcdと目標位置姿勢 gcd を生成し5ている。そして，2 節で提案した動的視覚オブザーバを用らみた対象物の相対位置姿勢 gco が目標位置姿勢 gcd に追b
従するようなカメラ入力 Vwc
を生成している。

-3

本論文の障害物回避ナビゲーション関数を用いた安定化

0
xrer

xrep

いた安定化視覚フィードバック制御則を用いて，カメラか

1
0
-1
0

2

-0.2
-0.4

4

8

に限定されず，さらにレーザレンジファインダなどの測距

4センサのようなカメラ以外の付加的なセンサを必要としな0いため適用範囲は広いと考えられる。また，障害物回避に2を通した画像特徴量から障害物の位置を推定する問題も含

2

2

4

0

2

4

0

2
time [s]

4

0.2
0

4

0

0
-2

んだ上で目標位置姿勢の生成に対する収束性の議論ができ

0

zrer

zrep

おいては障害物までの距離が重要となるが，カメラモデル

yrer

yrep

視覚フィードバック制御の手法は，地面を走行するビークル

0

0.4

0

2
time [s]ていることから，本手法は非常に有益であると考えられる。

4

-0.2
-0.4

Fig. 3. Pose control error.

本論文ではカメラモデルとしてピンホールカメラモデルを用いたが，文献 (15) で導出した画像ヤコビアン J(·) を用いることで，全方位カメラを用いた視覚フィードバック
制御においても本論文で提案した手法を適用できると考え4られる。また，本論文ではマニピュレータダイナミクスをダイナミクスを考慮した安定化視覚フィードバック制御を[m]

陽に考慮していないが，文献 (11) (13) ではマニピュレータ
提案している。本論文では紙面の都合上割愛したが，同様

zcwpの方法により，陽にマニピュレータのダイナミクスを考慮3
pwc(4)
2

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

1pwc(0)

4. シミュレーション検証-100pwcx [m]20本節では，提案した制御手法の有効性を確認するために

4
pwcy [m]6

Fig. 4. Trajectory of camera in the case of thestatic obstacle (Solid: with proposed method,
Dashed: with previous one (11) ).

二つの障害物が存在する状況のシミュレーション検証をおこなう。
はじめに，二つの障害物を静止させたシミュレーション
検証をおこなう。初期設定は pwc = [0 0 1]T m，ξθwc =[π/12 − π/12 π/12]T rad，pwo = [−5 6.5 3]T m，ξθwo = [0 0 0]T rad とする。また最終目標位置姿勢は
pcdf = [0 1.5 0]T m，ξθcdf = [0 0 0]T rad とする。移動
可能範囲及び障害物の設定は p0 = [0 0 0]T m，ρ0 = 8.5 m，
pwb1 = [−2 2 1.5]T m，ρ1 = 1 m，pwb2 = [−5 3 2.5]T m，
ρ2 = 1 m とする。さらに，本シミュレーションにおけるゲインの設定や各パラメータは以下のようにした。Kc = 5I ，
Ke = 10I ，Kdp = 50I ，Kdr = 2I ，Keb1 = 10I ，
Keb2 = 10I ，κ = 3，µ = 4。
Fig. 3 に相対位置姿勢 gco (t) と最終目標位置姿勢 gcdfとの間の偏差 er を示す。Fig. 3 より，全ての偏差が定常状態において 0 に収束していることが確認できる。したがっバック制御において，カメラが最終目標位置姿勢に収束することが示された。Fig. 4 にカメラのトラジェクトリーを示す。図中の実線が提案手法により目標軌道を与えたとき，破線が従来手法 (11) となる時不変の目標位置姿勢を与えた
ときのトラジェクトリーとなっている。pwc (0) と pwc (4)はカメラの初期値と最終値を表しており，また図中の球体が障害物を，直方体が対象物を表している。つまり，Fig. 4において，カメラは球体を避けて直方体の前まで進まなけ
ればならない。Fig. 4 より提案手法による目標軌道を与えることで，障害物をかわしながら対象物に向かっていることがわかる。一方，従来研究 (11) である時不変の目標位置
姿勢を与えた場合は，最終目標位置姿勢に収束しているもて，障害物回避ナビゲーション関数を用いた視覚フィード∇φ(pcd ) =2µ1+κκ(µη + s) κのの障害物にぶつかっていることから，実際の実験においκη(pcd − pcdf ) + ∥pcd − pcdf ∥2 

n
∏

i=1

6

ηi (pcd − p0 ) −

n
n
∑
∏


ηi pcbj  · · · · · · · · · · · (27)j=1 i=0,i̸=j

IEEJ Trans. EIS, Vol.125, No.1, 2005

[m]ナビゲーション関数を用いた視覚フィードバック制御による障害物回避
55. 実験検証

4

本論文では Fig. 6 に示す小型自律飛行ロボットを用いた

zcwp

3 次元空間上での検証実験をおこなった。小型自律飛行ロボットは 2 台のカメラ (水平カメラと垂直カメラ) が標準搭載された Parrot 社の AR.Drone を用いる。AR.Drone
は x 軸，y 軸，z 軸方向の並進運動と z 軸周りの回転運動が可能である。本実験で使用した水平カメラは画角 93 degの CMOS イメージセンサを用いており，画像取得周期の公称値は 30 fps である。実験した際の実測での画像取得周期は約 13 fps であり，動的視覚オブザーバを用いた閉ループシステム全体の処理周期は約 0.6 ms である。水平カメラで取得した画像は，AR.Drone に搭載されたアドホック
モードで動作する 802.11b/g 集積モジュールにより PC に3
pwc(4)21
8

pwc(0)
642
pwcy [m]

0

-2 -8

-6

-4

0
-2
pwcx [m]2

Fig. 5. Trajectory of camera in the case of themoving obstacle (Solid: with moving obstacle,
Dashed: with static one).

送られる。観測対象の特徴量は，得られた画像から画像処理ライブラリ OpenCV を用いて抽出する。目標軌道や視覚フィードバック制御則は Visual Studio 2008 (Microsoft
Target Object

Co.) により C++ 言語を用いてプログラミングされ，カSo

メラ速度入力をロール・ピッチ・ヨー角に対する入力に変換した後，AR.Drone に送信する。
本実験検証は，一つの静止した障害物が存在している状

Obstacle

Sb1

況にておこなった。本手法は障害物の特徴点が常に観測可

yczc

能でなければならないが，本実験のカメラでは，障害物をScかわした後は障害物を観測することができない。したがっxcて，障害物をかわした後は障害物に再び当たらないと仮定

AR.Drone

し，障害物の特徴点が取得できなくなった後に (22) 式の障害物関数を下記のものに切り替えて目標軌道生成をおこな

Swうこととする。

Fig. 6. Experimental environment.

η(pcd ) = ρ20 − ∥pcd − p0 ∥2 · · · · · · · · · · · · · · · · · · (30)
初期設定および最終目標位置姿勢は pwc = [0 0 0.65]T m，つぎに，理論的には示していないがより実用面を考慮しξθwc = [0 0 0]T rad，pwo = [−2.5 5.5 2.15]T m，ξθwo =[0 0 0]T rad，pcdf = [0 2.5 0]T m，ξθcdf = [0 0 0]T radとする。移動可能範囲及び障害物の設定は p0 = [0 0 0]T m，
ρ0 = 10 m，pwb1 = [−1 1.75 1.05]T m，ρ1 = 0.95 m とて，障害物が運動しているときのシミュレーション検証を示する。さらに，ゲインの設定や各パラメータは以下のようにす。前述のシミュレーション設定から pwb2 = [−5+0.2t 3−
0.25t 2.5 + 1.5t]T m に変更する。Fig. 5 にカメラのトラジェクトリーを示す。図中の破線が Fig. 4 中で実線で示しした。Kc = diag{1.5, 1.5, 0.6, 0.1, 0.1, 0.1}，Ke = 20I ，ては失敗することに注意されたい。したがって，Figs. 3, 4より，提案手法は障害物回避をおこないつつ，最終目標位
置姿勢に収束していることが示された。

ことにより変化した実線の軌道は，障害物を回避しながらKdp = diag{6, 6, 3}，Kdr = 0.5I ，Keb1 = 20I ，κ = 2，
µ = 4。ただし，比較実験としておこなった従来手法 (11) の場合においては，位置に対する制御ゲインのみすべて 1/10倍とした。なお，本実験で用いた AR.Drone は空中で完全に静止させる事が困難であるため，予備実験を通して x 軸と y 軸の位置においては目標位置から ±0.35 m の範囲に，
z 軸の位置においては ±0.15 m の範囲に，また z 軸の回転においては ±0.1 rad の範囲に収まった時点で収束した

対象物の前まで移動していることがわかる。したがって，本と判断する。この収束範囲はこれから示す実験結果では点シミュレーションでは，障害物が運動している状況におい線で示すこととする。

たトラジェクトリーであり，実線が障害物が運動している
状況においてリアルタイムで目標軌道を生成した場合のカメラのトラジェクトリーを示している。今回は障害物が運動しているために破線の軌道ではカメラが障害物に衝突してしまうのに対して，リアルタイムで目標軌道を生成するても，リアルタイムで目標軌道を生成することで，障害物Figs. 7, 8 に実験結果を示す。Fig. 7 の実線は上から 3を回避しながら最終目標位置姿勢に収束していることが示

次元空間での x，y ，z 軸の位置の推定値と z 軸周りの回転された。

角度を表している。また，破線は障害物関数ナビゲーション

電学論 C，125 巻 1 号，2005 年

7の有効性が示された。

]m[ xocp

2
0
-2
-4

6. おわりに
0

10

20

30

本論文では 3 次元 Eye-in-Hand 構造の視覚フィードバッ

40

クシステムに対して，障害物回避のためのナビゲーション]m[ yocp6関数を用いた安定化視覚フィードバック制御手法を提案し42た。制御則への目標位置姿勢生成を障害物回避のためのナ
0

10

20

30

ビゲーション関数を用いて設計し，ナビゲーション関数を40]m[ zocp2

リアプノフ関数候補とすることで設計する目標位置姿勢が1

設定する最終目標位置姿勢に収束することを示している。

0

提案手法は障害物を回避しながら最終目標位置姿勢へ収束

-1

0

10

20

30

する安定化視覚フィードバック制御手法となる。最後にシ

40]dar[ zoc qx

ミュレーションと飛行ビークルを用いた検証実験により，提0.2

案した制御手法の有効性を示した。

0（平成 24 年 10 月 12 日受付，平成 25 年 3 月 1 日再受付）

-0.2
0

10

20
time [s]

30

40

文

Fig. 7. Estimated relative position and theactual rotation along the z-axis (Solid: actual
trajectory, Dashed: reference one).

（ 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 ） 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)（ 3 ） 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)
加藤健一, 平田健太郎, 水野貴志, 山田晃平: “視覚フィードバックとロバスト制御による大道芸「くわえ撥」の実現”, 計測自動制御学
会論文集, Vol. 45, No. 10, pp. 530–537 (2010) (in Japanese)（ 4 ） 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)（ 5 ） E. Rimon and D. E. Koditschek: “Exact Robot Navigation
using Artificial Potential Functions”, IEEE Trans. Robotics
and Automation, Vol. 8, No. 5, pp. 501–518 (1992)（ 6 ） 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)（ 7 ） 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)（ 8 ） T. Murao, H. Kawai and M. Fujita: “Stabilizing Predictive
Visual Feedback Control via Image Space Navigation Function”, IEEJ Trans. Electronics, Information and Systems,
Vol. 132, No. 5, pp. 721–729 (2012) (in Japanese)
村尾俊幸, 河合宏之, 藤田政之: “安定化予測視覚フィードバック制
御に対するナビゲーション関数を用いた目標位置姿勢生成”, 電気学会論文誌 C, Vol. 132, No. 5, pp. 721–729 (2012) (in Japanese)（ 9 ） A. Cherubini and F. Chaumette: “Visual Navigation with
Obstacle Avoidance”, Proc. 2011 IEEE/RSJ International
Conference on Intelligent Robots and Systems, pp. 1593–
1598 (2011)（10） D. Lee, H. Lim and H. J. Kim, “Obstacle Avoidance using Image-based Visual Servoing Integrated with Nonlinear
Model Predictive Control”, Proc. 50th IEEE Conference on
Decision and Control and European Control Conference,0]m[ zocp

1
2
1

-2
0

-1
pcox [m]0pco(0)-24

献

2
pcoy [m]-3 6

Fig. 8. Trajectory of camera (Solid: withproposed method, Dashed: with previous
one (11) ).

関数を用いて生成した目標位置姿勢を表している。シミュ
レーション検証に比べて収束時間が長いのは，モータに対して電圧制限がかからないように重みや目標軌道生成のためのゲインを設定しているためである。Fig. 7 より，目標
位置姿勢に対して相対位置姿勢が追従しているのがわかる。
また，Fig. 8 に，カメラのトラジェクトリーを示している。
実線が提案手法，破線が従来手法 (11) の実験結果である。従来手法は最終目標位置姿勢へ直線的に動いているものの，障害物を考慮していないために，8 s で障害物に衝突し実験は失敗してしまった。それに対して，提案手法では障害物を回避するように目標位置姿勢を設計しているために，障害物に衝突せずにカメラが対象物の前まで動いていることが確認できる。以上のことからも，障害物回避のためのナ
ビゲーション関数を用いた安定化視覚フィードバック制御
8

IEEJ Trans. EIS, Vol.125, No.1, 2005

ナビゲーション関数を用いた視覚フィードバック制御による障害物回避
藤 田

pp. 5689–5694 (2011)（11） 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)（12） R. Murray, Z. Li and S. S. Sastry: A Mathematical Introduction to Robotic Manipulation, CRC Press (1994)（13） T. Murao, H. Kawai and M. Fujita: “Stabilizing Predictive
Visual Feedback Control for Fixed Camera Systems”, Electronics and Communications in Japan, Vol. 94, No. 8, pp. 1–
11 (2011)（14） D. E. Koditschek and E. Rimon: “Robot Navigation Functions on Manifolds with Boundary”, Advances in Applied
Mathematics, Vol. 11, No. 4, pp. 412–442 (1990)（15） 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)

村 尾俊

同年金沢大学助手，同講師，助教授を経て，1992
年北陸先端科学技術大学院大学助教授。1999 年金
沢大学教授。2005 年東京工業大学教授。2011〜

2012 年 IEEE CSS Vice President，2012 年
SICE 制御部門長，2008〜2011 年 IEEE CSSBoard of Governor，2005〜2006 年 SICE 理事，
2010 年 IEEE MSC General Chair，IEEE TAC，IEEE TCST，
Automatica，Asian J. Control の Associate Editor，SICE JCMSIの Editor。2008 年 IEEE TCST Outstanding Paper Award。環境・エネルギーのためのシステム制御，受動性に基づく視覚と制御の融合，ロバスト制御とその応用の研究に従事。工学博士。IEEE CSSなどの会員。

幸 （正員） 2005 年金沢大学大学院自然科学研究科電子情報システム専攻博士前期課程修了，2008 年
ム専攻博士後期課程修了。2006 年産業技術大学
院大学研究員，2007 年同助教。2013 年早稲田大
学次席研究員（研究院講師），独立行政法人科学技術振興機構 CREST 研究員となり現在に至る。
視覚フィードバック制御に関する研究に従事。博士（工学）。計測自動制御学会，システム制御情報学会，IEEE などの会員。

宏之 （非会員） 2001 年金沢大学大学院自然科学研究
科電子情報システム専攻博士前期課程修了，2004
年金沢大学大学院自然科学研究科機能開発科学専
攻博士後期課程修了，同年法政大学情報技術 (IT)
研究センターポストドクター。2005 年金沢工業大学講師。2010 年同准教授。2008 年 IEEE TCST

Outstanding Paper Award。視覚フィードバッ
ク制御とその応用に関する研究に従事。博士（工学）。計測自動制御学会，システム制御情報学会，IEEE の会員。

鶴 尾有

生 （非会員） 2011 年金沢工業大学工学部ロボティ
クス学科卒業。同年金沢工業大学大学院工学研究科博士前期課程入学。飛行体の視覚フィードバッ
ク制御に関する研究に従事。計測自動制御学会の会員。

電学論 C，125 巻 1 号，2005 年

之 （非会員） 1984 年早稲田大学大学院理工学研究科
博士前期課程修了。1985 年同博士後期課程中退。

東京工業大学大学院理工学研究科機械制御システ

河 合政9