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

受動性に基づく 2 次元視触覚フィードバック制御の安定性と L2 ゲイン制御性能解析○河合宏之 （金沢工業大学） 村尾俊幸 藤田政之 （東京工業大学）

Passivity-based Visual Force Feedback Control for Planar Manipulators∗H. Kawai (Kanazawa Institute of Technology), T. Murao and M. Fujita (Tokyo Institute of Technology)
Abstract– This paper investigates visual force feedback control for planar manipulators. The vision/force
contorl is applied to horizontal/vertical direction for the environment which is thought as a frictionless,elastically compliant plane. We show passivity of the visual force feedback system which allows us to provestability in the sense of Lyapunov. The L2 -gain performance analysis for the disturbance attenuation problem
is considered via the dissipative systems theory. Finally simulation results are shown to verify the stability
and L2 -gain performance of the visual force feedback system.
Key Words: visual feedback control, force control, passivity, stability, L2 -gain performance analysis

1

はじめにロボットマニピュレータをはじめとするダイナミカ
ルシステムに対して, 視覚情報をフィードバックループに組み込んだ制御は視覚フィードバック制御と呼ばれる 1) . この視覚フィードバック制御においては, 安定性の大域的化 2) やカメラ視野を議論した研究 3) などがおこなわれている. 筆者らは, 視覚フィードバックシステ
ムが受動性を有することを示し, その受動性に基づいて安定性や運動する観測対象を外乱として捉えた制御性能解析をおこなっている 4)5) . また, 実システムへの応用例では, 工場における組み立て用ロボット以外にもDNA を細胞へ注入するロボット 6) や腹腔鏡を把持する手術支援ロボット 7) に視覚フィードバック制御が適用されている. この腹腔鏡把持ロボットにおいては術者の視野を確保するための視覚フィードバック制御のほかに, 鉗子が臓器に触れるため力覚・触覚の情報も必要となってくる. そのため, 視覚と力覚・触覚を併せてひとつのシステムとして捉えることが重要となる.
そこで本稿では, 視覚フィードバック制御と力制御を融合するために, 2 自由度マニピュレータに対する 2 次元視触覚フィードバックシステムを構成する. そして,従来研究で得られている視覚フィードバック制御の受動性 4) と力制御における受動性 8) を利用し, 2 次元視触覚フィードバックシステムに対して受動性に基づく制御則を提案する. 提案する制御則に対し, 安定性と制御性能解析をおこなう.

22 次元視触覚フィードバックシステム
M (q)q̈ + C(q, q̇)q̇ + g(q) = τ(1)で表される. q, q̇, q̈ ∈ Rn はそれぞれ各関節の角度, 角速度, 角加速度を表し, τ ∈ Rn は入力トルクである. また, M (q) ∈ Rn×n は慣性行列, C(q, q̇)q̇ ∈ Rn は遠心力・コリオリ力項を表すベクトル, g(q) ∈ Rn は重力項を表すベクトルである. マニピュレータの手先に加わる外力を F とすると, F に等価な関節力 τF は

τF = JpT (q)F

Camera Frame yixc

(2)

pcofx

ffyyc Scpwolzcpwc yw

Manipulatorzw

Image PlaneSwWorld FramexwFig. 1: Pinhole camera model with perspective projectionで表される 9) . ただし Jp (q) はマニピュレータヤコビアンである. 同様に, 外乱外力を d とすると, マニピュレータの手先に外力と外乱が加わった場合のマニピュレータダイナミクスはM (q)q̈ + C(q, q̇)q̇ + g(q) = τ + JpT (q)(F + d) (3)となる.
一方, 本稿で考えるカメラをマニピュレータの手先に取り付けた Eye-in-hand 構造において, カメラから得られる画像面上での視覚情報 f := [fx fy ]T はf=n 自由度のマニピュレータダイナミクスはTarget Objectxi

λ T
R (θ)(pwo − pwc)
zwo(4)

で表される 4) . λ は焦点距離 (一画素あたりの長さの逆数 [pixels/m] を与えるスケーリングパラメータを含む) である. 本稿では, 平面 2 自由度マニピュレータを考えるため, zwo はカメラから観測対象までの距離となる (Fig. 1 参照). 基準座標系 Σw := {xw , yw , zw }およびカメラ座標系 Σc := {xc, yc , zc } の X − Y 平面は画像平面と平行であるとし, 観測対象はこの X − Y
平面に平行なある平面上を運動するものとする. pwcが手先位置を表し, R(θ) が手先の姿勢を表している.
pwo は未知な観測対象の位置である. 本稿で考える視yw-q2cy,Fy fycx,Fxfx

q1xw
Fig. 2: Coordinate frames for planar manipulators
覚フィードバック制御は, 運動する観測対象に手先を追従させることである. このとき視覚情報 f の時間微分は, ṗwc = Jp (q)q̇ の関係を用いることでλ T
λ TR Jp q̇ − RT Ṙf +R ṗwo
f˙ = −zwozwo(5)で与えられる.
2 自由度マニピュレータにおいて, 弾性環境の力座標系 ΣF を基準座標系 Σw と同じ向きにとる. 弾性環境に対する垂直方向の力を Fx , 平行な方向を Fy とすると, F = [Fx Fy ]T となる. ここで, 画像平面 fx − fy を光軸まわりに回転させ, 姿勢のみを力座標系と一致させたものを仮想的な画像平面と考え, 弾性環境に垂直な方向を cx , 平行な方向を cy で表すことにする. Fig. 2に 2 自由度マニピュレータにおける座標系を示す. したがって, c := [cx cy ]T と定義すると
cos(θ) − sin(θ)fx(6)
c = Rf =fysin(θ) cos(θ)となる.
また, 手先が触れる弾性環境に対しては, 弾性係数を
Ke = diag{kex, key } とすると

F = Ke (pwc − pwc0 )(7)が成り立つ. ただし, pwc0 は接触力が 0 となる手先位置である. このとき, 手先が初期状態で接地面に触れていて, 接地面から離れないとし, 力, 位置, 視覚情報の変化分をそれぞれ F̃ , p̃wc , c̃ で表すとF̃ = Ke p̃wc(8)が成り立つ. また, 透視変換の関係からc̃ =λp̃wczwo(9)となる.
弾性環境に対する力の目標値を Fd = [Fdx Fdy ] とし, 画像面上での目標値は観測対象を中心でとらえるように fd = [0 0]T とする. このとき, 仮想カメラ座標系での目標値 cd = [cdx cdy ]T は c = R(θ)f の関係
から cd = [0 0]T となる. ここで, それぞれの偏差をFe := F − Fd , fe := f − fd , ce := c − cd と定義すると(8)(9) 式より
zwozwo kex 0cex
(10)
Fe =Ke ce =cey0 keyλλが成り立つ.
ここで, 本稿で考える問題をあらためて示すと, つぎのようになる.
問題設定 平面 2 自由度マニピュレータにおいて, 手先が触れる弾性環境の垂直方向に力制御を, 平行方向に視覚フィードバック制御するための制御則 τ を決定せよ.
上述の問題を考えるため, その状態を s = [Fex cey ]Tとおく. また, α := zwoλkex と定義する. このとき,(6)(10) 式より
 
α cos(θ) −α sin(θ)α0cex=fe (11)s=ceysin(θ)
cos(θ)
0 1と表すことができる. 上式を時間微分し, (5) 式の関係を用いて各項を展開して考えると

00kexkex
Jṗwo (12)ṡ = −q̇+pλλ00zwozwoが導かれる.
以上より, ロボットの 2 次元視触覚フィードバック
システムは

M q̈ + C q̇ + g = τ + JpT (F + d)ṡ = −Kxy Jp q̇ + Kxy ṗwo(13)λで表される. ただし, Kxy := diag{kex, zwo} と定義した. ここで, マニピュレータの動作範囲 Q をつぎのように仮定しておく.

仮定 1 Fex = 0, cey = 0 を達成するマニピュレータ姿
勢は動作範囲 Q に存在し, かつすべての q ∈ Q に対して Jp (q) は正則である.

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

33.1

2 次元視触覚フィードバックシステムの入出力間の関係
2 次元視触覚フィードバックシステムに対して, つぎの制御則を提案する.
τ = uξ + JpT KF c s + M u̇d + Cud + g(q) − JpT F (14)

ここで KF c := diag{kF , kc} で定義され, kF と kc は,それぞれ力の偏差と視覚の偏差にかかる正の定数ゲインである. uξ , ud はそれぞれ後に提案される新たな入力と関節角速度の目標値である. 新たな変数として
ξ := q̇ − ud を定義する. このとき, (13)(14) 式より得られる 2 次元視触覚フィードバックシステムは
  −M −1 Cξ + M −1 JPT KF c sξ˙=−Kxy Jp ξṡ −1 −1 TM0M JP 0+u+w (15)0 −Kxy Jp0Kxyとなる. ここで, 状態と入力をそれぞれ x := [ξ T sT ]T ,u := [uTξ uTd ]T と定義する. また, w := [dT ṗTwo ]T と定義する.
注意 1 観測可能な情報はマニピュレータの関節角度 q,関節角速度 q̇, 弾性環境へ垂直にかかる力 Fx, 視覚情報 f である. このとき, 弾性環境に対して平行な視覚情報 cy は f と θ = q1 + q2 から得られる. したがって,s を構成する Fex = Fx − Fd と cey = [cos(θ) sin(θ)]feが得られるため, 状態 x は観測可能である.

このとき, つぎの補題が成り立つ.
補題 1 外乱が存在しない (すなわち w = 0) とする. 出力をI0ν := N x, N :=0 −JPT KF cをとするとき, (15) 式の 2 次元視触覚フィードバックシステムの入出力間に

 T0uT νdτ ≥ −β, ∀T > 0(16)が成り立つ. ただし, β はある非負の定数である.

(証明) エネルギー関数として
V =1 T
1−1s
ξ M (q)ξ + sT KF c Kxy22(17)

を考える. このエネルギー関数の解軌道に沿った時間微分は=u ν(18)0と導かれる. N が正則であり K が正定行列であることから, (15) 式 (20) 式で構成される閉ループ系の平衡点 x = 0 は漸近安定となる.
（Q.E.D.）
つぎに, 外乱が存在する場合における制御性能解析について考える. ここで, 被制御出力を z := εx とする. ただし ε は, εξ = diag{εξ1 , εξ2 } > 0, εs =diag{εs1 , εs2 } > 0 を対角要素にもつ重み行列とする.
また, ある正の数 γ に対してつぎの行列を定義する.
Kξ − 12 εTξ εξ − 2γ1 2 JPT JP
P :=0
0(22)KFT c Jp Kp JpT KF c − 12 εTs εs − 2γ1 2 KFT c KF c
このとき, つぎの定理が示される.

(証明) 蓄積関数として (17) 式を考える. その解軌道にそった時間微分はu νdτ = V (T ) − V (0) ≥ −V (0) = −β (19)+ξ T JpT d + xT KF c ṗwoTが成り立つ. ただし, β は初期状態のみに依存する, ある非負の定数である.
（Q.E.D.）
この補題 1 は, (15) 式の 2 次元視触覚フィードバックシステムにおいて, u と ν をそれぞれ入力と出力とみなすと, システムが受動性を有していることを示している.

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

I0Kξ 0xu=−0 Kp
0 −JPT KF c= −KN x(21)

V̇ = −ξ T Kξ ξ − sT KFT c Jp Kp JpT sとなる. 両辺を積分することで

 T

V̇ = uT ν = xT N T u = −xT N T KN x定理 2 ある正の数 γ が与えられたとき, P ≥ 0 を満たすようにゲイン Kξ , Kp , KF c および重み ε1 , ε2 を選ぶとき, (15)(20) 式で構成される 2 次元視触覚フィー
ドバックシステムは γ 以下の L2 ゲインを有する.

1−1V̇ = ξ T M (q)ξ˙ + ξ T Ṁ (q)ξ + sT KF c Kxyṡ
2
1= ξ T uξ + ξ T (Ṁ − 2C)ξ + ξ T JpT kF c s
2−sT KF c Jp ξ − sT KF c Jp udT(証明) (17) 式のエネルギー関数をリアプノフ関数候補とする. (15)(20) 式の解軌道に沿った時間微分は, (19)
式より

(20)

ただし, Kξ := diag{kξ1 , kξ2 }, Kp = diag{kp1 , kp2 } は各軸にかかるゲインであり
Kξ 0K :=0 Kpで定義される. このとき, つぎの定理が成り立つ.
定理 1 外乱が存在しない (すなわち w = 0) とき, (15)式と (20) 式の入力で構成される閉ループ系の平衡点
x = 0 は漸近安定である.

(23)となる. 上式を平方完成を利用して整理すると

γ21V̇ + z2 − w2221≤ −ξ T Kξ ξ − sT KFT c Jp Kp JpT s + (εξ ξ)T εξ ξ2
11 T T
1T+ (εs s) εs s + 2 ξ Jp Jp ξ + 2 sT KFT c KF c s22γ2γ(24)= −xT P xが成り立つ. ゲイン Kξ , Kp , KF c および重み ε1 , ε2を, P ≥ 0 を満たすように選ぶとき

1γ2V̇ + z2 − w2 ≤ 022(25)が満たされる. 上式を 0 から T で積分し, V (x(T )) ≥ 0であることに注意すると
 T T22z ≤ γw2 + 2V (x(0))(26)00が成り立つ. 以上より 2 次元視触覚フィードバックシステムは γ 以下の L2 ゲインを有することが示される.
（Q.E.D.）定理 1 および定理 2 は, 補題 1 で示した 2 次元視触覚フィードバックシステムの受動性を利用することで導かれた結果であり, (17) 式のエネルギー関数が重要な役割をはたしている.

000.5x2 [rad/s]

4

1.0time [s]1.5

2.0

20-2-400.5

1.0
time [s]1.5

2.0

Fig. 3: Time response of ξ(t)Fex [N]cey [pixel]0.1002.04.0time [s]6.08.02.04.0time [s]

6.0

8.0

0.2
0.100

段が γ = 5.54 の場合であり, 下段が γ = 2.92 の場合である. 観測対象の運動が同じであるため波形は似ているが, その大きさは γ の値が小さい場合の方が小さいことがわかる.

0-10-200.2Fig. 5: Euclidean norm of the controlled output z

1000.5

1.0time [s]1.5

1.0time [s]1.552.0

本稿では, 2 自由度マニピュレータにおいて, 視覚フィードバック制御と力制御を融合した 2 次元視触覚フィードバック制御について考えた. 提案した制御則は, 弾性環境に力が加わる方向には力制御を, 力が加わらない方向には視覚フィードバック制御を考えている
ため, 位置と力のハイブリッド制御法を拡張したものだと捉えることができる. 構成した 2 次元視触覚フィー
ドバックシステムが受動性を有することを示し, そのエネルギー関数をそれぞれリアプノフ関数と蓄積関数とみなすことで安定性と L2 ゲイン制御性能解析をおこなった.

0-400
00.5

おわりに

2.0-200

Fig. 4: Time response of s(t)4|| z || (g = 5.54)

20|| z || (g = 2.92)

x1 [rad/s]

40

シミュレーション

本節のシミュレーションでは, 水平 2 自由度マニピュレータである SICE-DD アームを考える. 弾性環境における弾性係数を kex = 200 [N/m] とし, 力の目標値を Fdx = 18 [N] とした. また, 視覚情報の画像面上での目標値は cdy = 0 [pixel] である.
まず安定性を確かめるために, 外乱が存在しない状態でマニピュレータの初期姿勢を, q1 (0) = π/2 [rad],q2 (0) = −π/2 [rad], q̇1 (0) = 0 [rad/s], q̇2 (0) = π/2[rad/s] とした. この初期姿勢において, 力と視覚の偏差は Fex = −18 [N], cey = −310 [pixel] となった. このとき, (20) 式の入力に対するゲインをKξ = diag{0.1, 0.1}, Kp = diag{1, 0.1}, KF c =diag{10, 0.1} と選んだ. このシミュレーション結果をFig. 3 と Fig. 4 に示す. 両図より, 状態 x = [ξ T , sT ]Tが 0 に漸近的に収束していることから, 漸近安定性が確認される.
つぎに, 観測対象を運動させた場合についてのシミュレーションを示す. なお, ここでは外乱として観測対象の運動のみを考えるが, 手先にかかる外力も外乱として考えることが可能である. 観測対象は仮定 1 を満足する範囲内で, yw 軸と平行に運動させる. 初期状態を x(0) = 0 とし, 状態にかける重みをそれぞれ
ε = diag{2.0, 2.0, 0.25, 0.01} とした. γ = 5.54 を満たすゲインを Kξ = diag{5, 5}, Kp = diag{80, 40},
KF c = diag{0.5, 0.015} と選んだ. また, γ = 2.92 を満たすゲインを Kξ = diag{5, 5}, Kp = diag{5, 2.5},
KF c = diag{5, 0.4} と選んだ. Fig. 5 にそのシミュレーション結果として被制御出力のノルム z を示す. 上

参考文献1) S. Hutchinson, G. D. Hager and P. I. Corke, “A Tutorial on Visual Servo Control,” IEEE Trans. Roboticsand Automation, Vol. 12, No. 5, pp. 651–670, 1996.
2) 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.
3) 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.
4) A. Maruyama and M. Fujita, “Robust Control for Planar Manipulators with Image Feature Parameter Potential,” Advanced Robotics, Vol. 12, No. 1, pp. 67–80,1998.
5) H. Kawai and M. Fujita, “Passivity-based Dynamic
Visual Feedback Control for Three Dimensional Target Tracking:Stability and L2 -gain Performance Analysis,” Proc. of the 2004 American Control Conference,
pp. 1522–1527, 2004.
6) S. Yu and B. J. Nelson, “Autonomous Injection of Biological Cells Using Visual Servoing,” In:D. Rus andS. Singh (Eds), Experimental Robotics VII, Springer–Verlag, pp. 169–178, 2001.
7) K. Omote et al., “Self-Guided Robotic Camera Control for Laparoscopic Surgery Compared with HumanCamera Control,” The American Journal of Surgery,Vol. 177, No. 4, pp. 321–324, 1999.
8) 田中, 藤田, “SP-D 制御法に基づくマニピュレータの位置/力制御とトルク外乱抑制,” 電気学会論文誌 (C),Vol. 118, No. 2, pp. 271–276, 1998.
9) 吉川, ロボット制御基礎論, コロナ社, 1988.