1

システム制御情報学会論文誌，Vol. 0, No. 0, pp. 1–7, 0000

論

文

受動性に基づく固定カメラ構造の 3 次元視触覚フィード
バック制御*
河合 宏之 † ・村尾 俊幸 ‡ ・藤田

政之 §Passivity-based Visual Force Feedback Control for Fixed
Camera Systems*
Hiroyuki Kawai†, Toshiyuki Murao‡ and Masayuki Fujita§This paper investigates passivity based 3D visual force feedback control for fixed camera systems.
In our approach, we can control not only the position but also the orientation of the robot hand witha contact force by using visual information. The proposed method can be regarded as an extensionof the hybrid position/force control to the hybrid vision/force control. The main contribution ofthis paper is to show that the 3D visual force feedback system has the passivity which allows us toprove stability in the sense of Lyapunov. Both the passivity of the manipulator dynamics and thevisual feedback system are preserved. Finally simulation results on 3DOF planar manipulator arepresented to verify the stability of the 3D visual force feedback system and understand our proposed
method simply.

1.

はじめに

るため力覚・触覚の情報も必要となってくると考えられる. そのため, 視覚と力覚・触覚を併せてひとつのシス

視覚情報をフィードバックループに組み込んだ制御はテムとして捉えることが重要となる.

視覚フィードバック制御と呼ばれ, 多くの制御方法が提外界センサとして視覚と力覚・触覚を両方用いてシ

案されている [1,2]. 近年では, 実システムへの応用例とステムを構成することは, 視覚フィードバック制御の研して, 工場における組み立て用ロボット以外にも DNA究が盛んになり始めた頃からすでにおこなわれており,を細胞へ自動注入するロボット [3] や腹腔鏡を把持する

Nelson ら [5] は分解能の異なる視覚センサと力覚センサ

手術支援ロボット [4] に視覚フィードバック制御が適用を切り替える方法を提案し, 実験により有効性の検証をされている. 細胞注入ロボットにおいては, 視覚フィーおこなっている. 細田ら [6] は未知環境下における視覚とドバック制御にて注入箇所までピペットを誘導すること

力覚のハイブリッド制御法を提案しており, 並木ら [7] はに加えて, 適切な力で挿入することが望ましく, また, 腹視触覚フィードバックにより物体の把握行動を実現して腔鏡把持ロボットにおいては術者の視野を確保するためいる. これらは技術的に実現可能な方法を提案し, 実シの視覚フィードバック制御のほかに, 鉗子が臓器に触れステムを用いてその有用まで示している一方で, 安定性∗

原稿受付 200x 年 x 月 xx 日†
金沢工業大学 工学部 College of Engineering, Kanazawa

解析などの理論的な議論が十分なされていない. それに対して, Dean-León ら [8] は視触覚フィードバックシス

Institute of Technology; 7-1 Ohgigaoka Nonoichi,
Ishikawa 921-8501, JAPAN
‡
産業技 術大学院 大学 産業 技術研究 科 School of Industrial Technology, Advanced Institute of Industrial Technology; 1-10-40 Higashi-Ohi Shinagawa-ku, Tokyo 1400011, JAPAN§
東京工業大学 大学院理工学研究科 Graduate School of
Science and Engineering, Tokyo Institute of Technology;
2-12-1 Ookayama Meguro-ku, Tokyo 152-8550, JAPANKey Words: visual feedback control, manipulator dynamics, passivity, Lyapunov stability, L2 -gain performance
analysis.

テムに対するリアプノフ関数を与え, その安定性解析までおこなっているが, 提案する制御則の適用可能な対象
が平面 2 自由度マニピュレータに限定されている.
本論文では対象とするロボットを平面 2 自由度マニ
ピュレータに限定せずに, Fig. 1 に示すような固定カメ
ラ構造の 3 次元視触覚フィードバック制御について考える. 制御目的は位置姿勢が未知の対象物に対して, 与えられた接触力で手先を適切に接触させることであり, そのため視覚と触覚のフィードバックが必要となる. 従来
研究 [9][10] で提案している 3 次元視覚フィードバック制

–1–

2

システム制御情報学会論文誌

第0巻

Image
Camera

gch

5c
gco

5h

第0号

(0000)(pho ,eξ̂θho ) と表す.

Force
Sensor

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

gwh

gwc

する. まず, カメラ座標系からみた観測対象の相対位置
姿勢 gco は

gho
gwo

−1
gco = gwc
gwo

5w

World Frame
5oContact Surface
Target Object(2)で表される. また, カメラ座標系からみた観測対象の相対速度は, gco を時間微分することで,Fig. 1Visual force feedback system with a fixed camera
configuration.

bbb= −Ad(gco−1 Vwc + Vwo
Vco
)

(3)

−1
b
のように導かれる [10]. ここで, Vab= (gabġab )∨ ∈ R6

御と, 受動性に基づく位置と力のハイブリッド制御 [11]は剛体運動のボディ速度を表しており, Ad(gab ) ∈ R6×6を融合することで, 視触覚フィードバック制御則を提案

は同次表現 gab の随伴写像である [12].

する. その際, 構成する視触覚フィードバックシステム

一方, カメラから得られる視覚情報 f(gco ) は, 相対位が受動性を保存していることを示し, リアプノフ関数を置姿勢 gco により変化することからその情報を含んでは与えることで安定性解析をおこなう.

いるが, 2 次元情報であるため直接 gco を観測すること

本論文の構成は以下のとおりである. 2. で固定カメラはできない. そこで, オブザーバを構成することで相対ˆ

構造の 3 次元視覚フィードバックシステムについてまと

位置姿勢の推定値 ḡco = (p̄co ,eξ̄θ̄co ) を得ることを考える.

める. 3. で力制御と融合することで, 視触覚フィードバッ

本論文では固定カメラ構造の視触覚フィードバックシス

ク制御則を提案し, 構成した視触覚フィードバックシスb= 0) を考えるため, gco の運動モデル
テム (すなわち Vwc

テムが受動性を有することを示した後, 安定性解析をおは (3) 式よりこなう. 4. で 3 自由度マニピュレータによるシミュレー

b
b
Vco
= Vwo

ション結果を示し, 最後に 5. でまとめをおこなう.

2.

(4)

となる. そこで (4) 式から, 推定値 ḡco の運動モデルを視覚フィードバックシステム

2.1

剛体運動の表現と推定偏差システム
本論文では, Fig. 1 に示すような四つの座標系によっ

b
= ue
V̄co

(5)

て表される視触覚フィードバックシステムについて考と構成する. ue ∈ R6 は推定偏差の振る舞いを安定にす

える. 図中の四つの座標系をそれぞれ基準座標系 Σw ,るために加えられる入力である.

手先座標系 Σh , カメラ座標系 Σc および観測対象座標

推定偏差システムを (4)(5) 式から構成する. まず, カ系 Σo と定義する. このとき, 基準座標系からみた手

メラ座標系からみた観測対象の相対位置姿勢の真値と推定値の偏差 gee = (pee ,eξ̂θee ) を先の位置姿勢, カメラの位置姿勢および観測対象の位置
姿勢をそれぞれ gwh = (pwh ,eξ̂θwh ), gwc = (pwc ,eξ̂θwc ),−1
gee := ḡcogco

gwo = (pwo ,eξ̂θwo ) で表す. ここで, 任意の二つの座標系
を ΣA , ΣB としたとき, pab ∈ R3 は ΣA の原点から ΣBの原点への位置ベクトルを表し, eξ̂θab ∈ SO(3) (ただし,ξab ∈ R3 , θab ∈ R であり, 簡単化のため混乱がない限り
添え字は θ のみにつけて表記する) は ΣA を基準とした
ΣB の姿勢を意味する [12]. 演算子 ∧(wedge) は 3 次元ベクトルを 3 ×3 の歪対称行列へ写像する演算子であり,その逆写像は ∨(vee) で定義される. gab は混同しない限
り, よく知られた 4 × 4 行列の同次表現eξ̂θab pab
(1)
gab =
0
1

(6)

と定義する. この推定偏差に対する推定偏差ベクトルをee ∈ R6 として, ee := [pTee eTR (eξ̂θee )]T と定義する. た1
だし eR (eξ̂θ ) := sk(eξ̂θ )∨ , sk(eξ̂θ ) := (eξ̂θ −e−ξ̂θ ) であ2
る. この推定偏差ベクトル ee はカメラから得られる視覚情報 f と推定モデルから得られる視覚情報の推定値 f¯
および画像ヤコビアンの擬似逆行列 J † (ḡco ) を用いて

ee = J † (ḡco )(f − f¯)(7)と導出される [10]. このとき, 推定偏差ベクトル ee を状態とした推定偏差システムは, (6) 式を時間微分し, (4)(5)
式を代入することでとして用いる [12]. 基準座標系からみたそれぞれの位置bb−1 ue + V
Vee
= −Ad(geewo)

姿勢と同様に, カメラ座標系からみた手先の位置姿勢と観ξ̂θch

測対象の位置姿勢を gch = (pch ,eξ̂θco), gco = (pco ,e)と導出される.

とし, 手先座標系からみた観測対象の位置姿勢を gho =

–2–

(8)

3

河合・村尾・藤田：受動性に基づく固定カメラ構造の 3 次元視触覚フィードバック制御

2.2

b
Vwo

制御偏差システム

視覚フィードバックシステムにおける制御目的を達成ξ̂θec

で, 制御偏差 gec = (pec ,e)を

ue

gec := gd−1 gho(9)Fig. 2で定義する. このとき gd は相対位置姿勢の目標値であec
eeBlock diagram of the visual feedback system.

 T

る. また, 制御偏差に対する制御偏差ベクトルを ec ∈ R6
T ξ̂θec Tとして, ec := [pT)]
ec eR (eVisualFeedback
Systemb
Vwh

するために, 制御偏差システムについて議論する. ここ0と定義する. (9) 式より,

uTceνcedt ≥ −βce , ∀T > 0(14)が成り立つ. ただし, βce はある非負の定数である.

制御偏差 gec は推定偏差 gee を用いて以下のように表す

証明は以下のエネルギー関数を用いて行われる.

ことができる.
−1−1
gco = gd−1 gchḡco gee
gec = gd−1 gho = gd−1 gch

Vce = E(gec ) + E(gee )

(10)

(15)

1
pab 2 + φ(eξ̂θab ) であり, φ(eξ̂θab )2

ただし, カメラ座標系からみた手先の位置姿勢 gch は同

ただし, E(gab ) :=定により得られるとする. (10) 式において, 目標値 gd

1:= tr(I − eξ̂θab ) は回転行列に対するエネルギー関数を2表す正定関数である [13]. Fig. 2 に視覚フィードバックシステム (12) のブロック図を示す. この補題は, 視覚
フィードバックシステムにおいて入力を uce , 出力を νceとするとき, その間に受動性が成り立つことを意味している.

と推定値 ḡco は既知情報であるため, 推定偏差 gee が得られれば, 相対位置姿勢 gho を直接得なくとも制御偏差

gec は求まることがわかる. 文献 [14] より, 推定偏差 geeππは − ≤ θee ≤ の範囲内で推定偏差ベクトル ee から22求められることがわかっているため, (10) 式より制御偏差 gec を導出することが可能となる.
(9) 式で定義した制御偏差 gec を時間微分し, (4) 式を用いることで, 制御偏差システムは
b
b
b
Vec
= −Ad(gec−1 Ad −1 Vwh + Vwo
)
(gd )

3.

本論文で考える制御目的は, 観測した対象物に対する(11)

マニピュレータの手先の相対位置姿勢を目標相対位置姿
勢 gd に一致させたうえで, 与えられた接触力 λd で接触と導出される.

2.3

視触覚フィードバック制御

させることである. そこで, 前節で示した固定カメラ構

視覚フィードバックシステムの受動性

造の視覚フィードバックシステムにおいて接触力を考慮

推定偏差システム (8) と制御偏差システム (11) を併せすることで, 視触覚フィードバックシステムを構成する.

ることで, 固定カメラ構造における視覚フィードバック

そして, 受動性に基づく視触覚フィードバック制御則をシステムをつぎのように構成する.

提案する.



b
Vec
b
Vee





=

−Ad(gec−10)−10−Ad(gee
)



 
I
b
Vwo
uce +
I
(12)

3.1

視触覚フィードバックシステム
接触力を考慮した n 自由度のマニピュレータダイナミ
クスは次式で表される [11].

b T
ただし, uce := [(Ad(g−1 ) Vwh) uTe ]T を入力として定義

M (q)q̈ + C(q, q̇)q̇ + g(q) = τ + Jϕ (q)λ

(16)

d

する. ここでは, マニピュレータダイナミクスを考慮せq, q̇, q̈ ∈ Rn はそれぞれ各関節の角度, 角速度, 角加速度を表し, τ ∈ Rn は入力トルク, λ ∈ R は接触力, M (q) ∈Rn×n は正定な慣性行列, C(q, q̇)q̇ ∈ Rn は遠心力・コリ
オリ力項, g(q) ∈ Rn は重力項を表す. Jϕ (q) は接触点における手先拘束が ϕ(q) = 0 (ϕ(q) ∈ R) で表されるとき,bを直接決められると仮定する (次節以ず, 手先速度 Vwh

降はこの仮定は不要). 制御偏差ベクトル ec と推定偏差ベクトル ee を視覚フィードバックシステムの状態とし

e := [eTc eTe ]T と定義する. このとき, つぎの補題が成り立つ [10].
b【補題 1】 観測対象が静止している (すなわち Vwo=0) とする. 出力を
νce := −e

その時間微分により

JϕT (q)q̇ = 0,

Jϕ (q) =

∂ϕ(q)
∂q

T
∈ Rn

(17)

(13)
で定義される. ただし, 本論文では対象物の接触面の形とするとき, (12) 式の視覚フィードバックシステムの入状は既知とする.

出力間にここで, マニピュレータへの入力トルクとして

τ = M (q)q̈r + C(q, q̇)q̇r + g(q)–3–4システム制御情報学会論文誌+JbT (q)AdT(g−1 ) ec − Jϕ λd + us + Jϕ uF

第0巻

第0号

(0000)T

Ad(gd-1)ec

(18)

w

d

を考える. Jb (q) はボディマニピュレータヤコビアン, λd
および接触力に関する新しい入力である. また, q̇r は

q̇r := Qϕ(q)q̇d + αJϕ(q)Fe

Manipulator
Dynamics& Auxiliary
Controller

uh
us
uF

は接触力の目標値であり, us と uF は後で定める角速度(19)Fig. 3bVwh(=Jb(q)Qq)
_

s
Fe

ue

VisualFeedback
Systemeceeで定義される新たな変数であり, α は正の定数とする.

Block diagram of the dynamic visual force feedback system.

Qϕ (q) は次式で定義される関節空間から接触面への射影⎡

M −1 M −1 Jϕ
0
⎢ 0
0
0
⎢
+⎢
0 −Ad(g−1 )⎣ 0行列である [11].

Qϕ(q) = I − Jϕ (q)JϕT (q)

(20)

ec

0

0

0

また, Fe は接触力に関する偏差として

 t
Fe :=



λ − λd dτ =

t0

⎤

0
0
0

⎡ ⎤
0

⎥
⎢0⎥
⎥
⎥
⎥u + ⎢⎣ I ⎦w⎦−Ad(g−1 )
ee

I
(26)

 t
eλ dτ ∈ Rと構成される. ここで, システムの状態, 入力および外乱

(21)

t0

をそれぞれ

⎤
⎡
⎤
us
s
⎢
⎥
⎢F ⎥
uF
⎢
⎥
⎢ e⎥
b
x := ⎢ ⎥ , u := ⎢⎥ , w := Vwo⎣ Ad(g−1 ) uh ⎦⎣ ec ⎦deeue⎡と定義する. ただし, t0 は手先が対象物に接触した時刻であり, eλ := λ − λd である. このとき

Ḟe = eλ(22)が成り立つ.

と定義する. Fig. 3 に視触覚フィードバックシステム (26)

一方, マニピュレータの手先の速度はボディマニピュ

のブロック図を示す.

レータヤコビアン Jb (q) を用いることでつぎのように表視触覚フィードバックシステムにおける制御目的は未される [12].
b= Jb (q)q̇ = Jb (q)Qϕ q̇Vwh

知な位置姿勢の対象物に, 適切な接触力でロボットの手(23)

先を接触させることである. そこで, 本論文で考える問

ここで, Qϕ の定義により得られる関係 Qϕ Qϕ q̇ = Qϕ q̇ = q̇

題をあらためて示すと, つぎのようになる.

を用いている. 同様に, 視覚情報を用いて構成される手

問題設定 固定カメラ構造の視触覚フィードバックシステ

先の速度の目標値を uh とし, マニピュレータの関節角速ム (26) において, 対象物が静止しているとき,

度の目標値を qd で表すと, ボディマニピュレータヤコビ

lim s = 0, lim Fe = 0, lim ec = 0, lim ee = 0t→∞

アンを用いることで uh = Jb (q)Qϕ q̇d と表すことができる. ここで, マニピュレータの関節角速度に関する偏差3.2

s := q̇ − q̇r ∈ R

t→∞

t→∞

を満たすように制御入力 u を決定せよ.

s ∈ Rn をnt→∞

視触覚フィードバックシステムの受動性制御入力 u を決定するために, 視触覚フィードバック(24)

システムの性質としてつぎの補題を示す.

と定義する. このとき, 関節角速度の偏差 s と接触力に【補題 2】 外乱が存在しない (すなわち w = 0) とす

関する偏差 Fe の間にる. 出力をJϕT s = JϕT (q̇ − q̇r ) = −JϕT q̇rν := N x, N := diag{In ,−α,−I6 ,−I6 }= −JϕT Qϕq̇d − αJϕT Jϕ Fe = −αFe(25)とするとき, (26) 式の視触覚フィードバックシステムのなる関係がある. ただし, JϕT Jϕ = I と JϕT Qϕ = 0 を用い

入出力間に

 T

ている [11].
固定カメラ構造の視触覚フィードバックシステムは(12)(16)(18)(22) 式より

⎡

⎤

⎡

−M
ṡ
⎢ Ḟe ⎥ ⎢⎢⎥ ⎢⎣ Vecb ⎦ = ⎢
⎣
b
Vee

−1



Cs − JbT AdT(g−1 ) ec − Jϕ eλeλd−Ad(g−1 ) Jb s0

⎤

uT ν ≥ −β,

∀

T >0

(27)

が成り立つ. ただし, β はある非負の定数である.

(証明) エネルギー関数として

⎥
⎥
⎥
⎦

1
1
V = sT M s + αFe2 + E(gec ) + E(gee )
2
2

ho

0

–4–

(28)

5

河合・村尾・藤田：受動性に基づく固定カメラ構造の 3 次元視触覚フィードバック制御を考える. このエネルギー関数の解軌道に沿った時間微分は Ṁ −2C, p̂ec と p̂ee の歪対称性 (すなわち sT (Ṁ −Camera Framezc2C)s = 0, pTecp̂ec e−ξ̂θd ωud = −pTec (e−ξ̂θd ωud )∧ pec = 0,
pTee p̂ee ωue = −pTee ω̂ue pee = 0) を用いると

xcV̇ = −s C ṡ + s
T

T

5c

gch

d

gco
Gwo

5o

 T
uT νdτ = V (T ) − V (0) ≥ −V (0) = −β

5h
xh

gho

yo

q3

q2

yh gwoHand Frame

World Frame
zw
5w
q1
xw
yw
3DOF Robot
Manipulator

Contact Point

Target Object Frame(30)

Fig. 4が成り立つ. ただし, β は s, Fe , ec および ee の初期状
態のみに依存する, ある非負の定数である.

zo

zh
Gwh

xo

となる. 両辺を積分することで

Image

gwc

yc

JbT AdT(g−1 ) ec + sT Jϕ eλ + αFe eλ1−eTc Ad(g−1 ) Jb s + sT Ṁ s + xT N T uho2
1= sT (Ṁ − 2C)s + eTλ (JϕT s + αFe ) + xT N T u
2(29)= xT N T u

0includingfour feature points（Q.E.D.）

Coordinate frames for dynamic visual force feedback system with three degree of freedom manipulator.

補題 1 で示したように視覚フィードバックシステム

K が正定であり, N が正則であるため, システムの平衡（Q.E.D.）定理 1 では, 固定カメラ構造の 3 次元視触覚フィード
バックシステムに対し, リアプノフの安定定理によりその安定性を示した. この安定性解析は補題 2 で示した受動性に基づいておこなわれている. 従来の受動性に基づ(12) は受動性を有している. また, 文献 [11] で示されて

点 x = 0 は漸近安定となる.

いるように, マニピュレータダイナミクス (16) を含む
力制御における偏差システムも受動性を有している. 補
題 2 は, 固定カメラ構造の視触覚フィードバックシステ
T
T T T
ム (26) の入力 u = [uTs uF (Ad (g−1 ) uh ) ue ] から出力d

ν = [sT − αFeT − eTc − eTe ]T の間に受動性が成り立つ
ことを示しており, 上記のふたつの受動性を保存していると考えることができる.

く位置と力・触覚のハイブリッド制御では対象物の位置
姿勢は既知とされていたが, 本研究で提案する受動性に基づく視触覚フィードバック制御では未知な位置姿勢で3.3

受動性に基づく制御則と安定性解析
前述の補題を用いて, 3 次元視触覚フィードバックシ
ステムの入力として, つぎの受動性に基づく制御則を提案する.

u = −Kν = −KN x

も接触を可能としている. 特に, 手先の速度を (23) 式で表すことでマニピュレータダイナミクスを含む力制御における偏差システムと視覚フィードバックシステムを結合したことで, 受動性および安定性の議論が可能となっ(31)ている. また, 本論文では受動性に基づく視触覚フィード

K := diag{Ks ,kF ,Kc ,Ke } ∈ Rn+13

バック制御則を提案しているため, マニピュレータの受動性に基づく位置制御での議論と同様に, パラメトリッ

ただし Ks := diag{ks1 ,···,ksn } と kF は各関節およびクな不確かさに対するロバスト制御や適応制御への展開

接触力の偏差に関するゲインであり, Kc := diag{kc1 ,···,

も可能であると考えられる.

kc6 } と Ke := diag{ke1 ,···,ke6 } は x 軸, y 軸, z 軸の並進と回転における制御偏差と推定偏差に対するゲインで4.

シミュレーションある. ただし, ゲインにおける各要素はすべて正とする.

4.1

シミュレーション設定

このとき, 補題 2 で示した視触覚フィードバックシス

本節のシミュレーションでは, マニピュレータとしてテムの受動性に基づくことで, 安定性に関してつぎの定

Fig. 4 に示すような平面 3 自由度マニピュレータを考え理が導かれる.

る. しかし, 提案する制御則は 3 次元空間に置かれた対【定理 1】 w = 0 のとき, 固定カメラ構造の 3 次元視

象物 (並進 3 自由度, 回転 3 自由度) に手先を接触させ触覚フィードバックシステム (26) と (31) 式の入力で構ることが可能な 3 次元視触覚フィードバック制御則であ成される閉ループ系の平衡点 x = 0 は漸近安定である.

る. 座標系は Fig. 4 に示すように, マニピュレータの台(証明) 補題 2 の証明における (28) 式のエネルギー関数
V をリアプノフ関数候補とすると, (26)(31) 式の閉ループ系の解軌道に沿った時間微分は (29) 式より
V̇ = −x N KN x
T

T

座を基準座標系の原点とし, 四つの座標系を定める.
観測する対象物の特徴点は立方体上部の四つの頂点とし, 立方体の中心が対象物の座標系の原点で pwo =[0.47 0.05 0]T [m], ξθwo = [0 0 0]T [rad] に配置されており, 接触点はその側面とする. マニピュレータのリンクの長さを台座側からそれぞれ, l1 = 0.2 [m], l2 = 0.2 [m],(32)と導かれる. ゲイン Ks , kF , Kc , Ke の正定性より行列

–5–

6

第0巻

0.2

6

0

5

0.4

0

0.2

0.4

0.6

0.8

1.0

contact force λ [N]NGecz [rad]pecy [m]pecx [m]システム制御情報学会論文誌

0
-0.4
0.6
0
-0.6
-1.2

NGeez [rad]peey [m]peex [m]Fig. 5

0

0

0.2

0.2

0.4

0.6

0.4
0.6
time [s]

0.8

0.8

1.0

1.0

0.2

0.4

0.6

0.8

1.0

0.8
0.4

0

0.2

0.4

0.6

0.8

1.0

0

0.2

0.4
0.6
time [s]

0.8

1.0

1

Contact Transition
0

0.2

0.4
0.6
time [s]0.81.0The contact force trajectory λ.

シミュレーション結果

提案する (31) 式の制御則における各ゲインを Ks =diag{10,10,10}, kF = 25, α = 1, Kc = diag{40,40,20,
20,20,40}, Ke = 50I6 と定めた. このときのシミュレー
ション結果を Figs. 5∼7 に示す. Figs. 5∼7 はそれぞれ視覚に関する制御偏差 ec と推定偏差 ee , 接触力 λ を表している. Figs. 5, 6 においては, 平面 3 自由度マニピュ
レータを考えているため, x 軸と y 軸の並進および z 軸の回転のみを表している. 制御偏差 ec と推定偏差 ee が0 に収束していることから, 視覚情報を用いて相対位置
姿勢 gho が目標位置姿勢 gd に一致することが確認される. Fig. 7 において, 0.2 [s] 付近でパルス状の変化が見られるのは, 手先が対象物に接触した瞬間を表している.
その後, 時間の経過とともに接触力 λ が目標値 5 [N] に収束していることが確認される. 以上により, 提案する

0.5
0

2

4.2

0
0

3

Fig. 70.8
0.4

1(0000)

4

0

The control error ec which defined the error between the relative rigid body motion of the robot
hand gho and desired one gd . Initial control errors are pecx = 0.12 [m], pecy = −0.27 [m] andξθecz = −π/2 [rad], respectively.

第0号

0The estimation error ee which defined the error
between the estimated value ḡco and the actual
relative rigid body motion gco . Initial estimation
errors are peex = 0.53 [m], pecy = −0.95 [m] andξθecz = π/4 [rad], respectively.

手法において位置姿勢が未知の対象に対して与えられた

l3 = 0.1 [m] とし, 各関節角度の初期値を q1 (0) = −π/2[rad], q2 (0) = π/2 [rad], q3 (0) = π/2 [rad] とする. このとき, 相対位置姿勢は pho = [0.15 − 0.27 0]T [m],ξθho = [0 0 − π/2]T [rad] となる. 手先が対象物に接触するように, 相対目標位置姿勢を pd = [0.03 0 0]T [m],ξθd = [0 0 0]T [rad] と定め, 目標接触力を λd = 5 [N]とする. このとき, 視覚と接触力に関する初期偏差はそ
れぞれ pec = [0.12 − 0.27 0]T [m], ξθec = [0 0 − π/2]T[rad], pee = [0.53 0.95 0]T [m], ξθee = [0 0 π/4]T [rad],λe = −5 [N] となる. ただし, 推定偏差は現実的な値の範囲で適当に与え, 接触力には文献 [8][11] の実験で見られるような観測ノイズを付加した.

本論文では, 受動性に基づく視覚フィードバック制御

Fig. 6

接触力で手先を適切に接触されていることがシミュレー
ションによって確認された.

5.

おわりに

と力制御を融合することで, 固定カメラ構造に対する受動性に基づく視触覚フィードバック制御について考えた.
提案する手法は力制御の従来研究で示されていた位置と力のハイブリッド制御法を, 視覚と力のハイブリッド制
御法へと拡張したものであると捉えることができる. 構
成した 3 次元視触覚フィードバックシステムが受動性を有することを示し, そのエネルギー関数をリアプノフ関数とみなすことで安定性解析をおこなった.

参 考 文 献[1] F. Chaumette and S. Hutchinson: Visual Servo Control, Part I: Basic Approaches; IEEE Robotics & Automation Magazine, Vol. 13, No. 4, pp. 82–90 (2006)[2] F. Chaumette and S. Hutchinson: Visual Servo Con-–6–

河合・村尾・藤田：受動性に基づく固定カメラ構造の 3 次元視触覚フィードバック制御

7

cal Introduction to Robotic Manipulation, CRC Press(1994)[13] F. Bullo and A.D. Lewis: Geometric Control of Mechanical Systems, Springer–Verlag (2004)[14] 村尾, 河合, 藤田: 固定カメラシステムに対する安定化
予測ビジュアルフィードバック制御; 電気学会論文誌 C,(投稿中)

trol, Part II: Advanced Approaches; IEEE Robotics& Automation Magazine, Vol. 14, No. 1, pp. 109–117(2007)[3] S. Yu and B. J. Nelson: Autonomous injection of
biological cells using visual servoing; Experimental
Robotics VII (D. Rus and S. Singh (Eds)), Springer–
Verlag, pp. 169–178 (2001)[4] K. Omote et al.: Self-Guided Robotic Camera Control for Laparoscopic Surgery Compared with Human
Camera Control; The American Journal of Surgery,Vol. 177, No. 4, pp. 321–324 (1999)[5] B. J. Nelson and P. K. Khosla: Force and Vision Resolvability for Assimilationg Disparate Sensory Feedback; IEEE Trans. Robotics and Automation, Vol. 12,
No. 5, pp. 714–731 (1996)[6] 細田, 五十嵐, 浅田: 未知環境内で動作するロボットのビジュアルサーボ/力サーボハイブリッド制御系; 日本ロボット学会誌, Vol. 15, No. 4, pp. 642–647 (1997)[7] 並木, 石川: 視触覚フィードバックを用いた最適把握行動; 日本ロボット学会誌, Vol. 18, No. 2, pp. 261–269(2000)[8] E.C. Dean-León, V. Parra-Vega and A. EspinosaRomero: Visual Servoing for Constrained Planar
Robots Subject to Complex Friction; IEEE/ASME
Trans. on Mechatronics, Vol. 11, No. 4, pp. 389–400(2006)[9] 河合, 村尾, 東, 藤田: 受動性に基づく 3 次元動的視覚
フィードバック制御の固定カメラ構造への展開; シス
テム制御情報学会論文誌, Vol. 18, No. 4, pp. 127–135(2005)[10] M. Fujita, H. Kawai and M. Spong: Passivity-based
Dynamic Visual Feedback Control for Three Dimensional Target Tracking: Stability and L2 -gain Performance Analysis; IEEE Transactions on Control Systems Technology, Vol. 15, No. 1, pp. 40–52 (2007)[11] Y. Liu , K. Kitagaki, T. Ogasawara and S. Arimoto:
Model-based Adaptive Hybrid Control for Manupilators Under Multiple Geometric Constraints; IEEE
Trans. on Control Systems Technology, Vol. 7, No. 1,
pp. 97–109 (1999)[12] R. Murray, Z. Li and S. S. Sastry: A Mathemati-

著 者 略 歴
かわ

い

ひろ

ゆき

宏 之 (正会員)
2001 年金沢大学大学院自然科学研究科
電子情報システム専攻博士前期課程修了,
2004 年金沢大学大学院自然科学研究科機能開発科学専攻博士後期課程修了, 同年法政大学情報技術 (IT) 研究センターポスト
ドクター. 2005 年金沢工業大学講師. 視覚フィードバック制御とその応用に関する研究に従事. 博士（工学）. 計測自動制御学会, IEEE の会員.
河 合

むら

お

とし

ゆき

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

ふじ

た

藤 田

まさ

ゆき

政 之 (正会員)
1984 年早稲田大学大学院理工学研究科
博士前期課程修了（電気工学専攻）, 1985年同博士後期課程中退. 同年金沢大学工学部助手. 同講師, 助教授を経て, 1992 年北陸先端科学技術大学院大学情報科学研究科
助教授. 1999 年金沢大学工学部教授. 2005
年東京工業大学大学院理工学研究科教授. ロバスト制御とその応用に関する研究に従事. 工学博士. 計測自動制御学会,
IEEE などの会員–7–