SO(3) 上の誤差関数

定し, マニピュレータダ イナミクスを含めたシステム全体の安定性を文献[34]で取り扱っ た. しかし対象の運動モデルが完全に既知でなくてはならず, 安定性の解析のみで制御性 能解析までは行っていない.

本章では3次元を運動する対象に追従させる視覚フィード バック制御において厳密な安 定性の解析とL₂ゲ インによる制御性能解析を行う. 対象との相対位置姿勢を非線形オブ ザーバを利用して推定し, その推定誤差やマニピュレータダ イナミクスまで含む系全体に 対して安定性及び制御性能解析を行う. このようなことが行えるようになった背景には前 章までで扱ってきた受動性に基づく平面マニピュレータの視覚フィード バック制御を拡張 したことと3次元の位置姿勢をあらわす多様体SE(3)上の誤差関数を定義したことにある.

本章の構成はつぎのとおりである.

6.2 節ではSE(3)上, 特にSO(3)上の誤差関数を定義してその性質を説明する.

6.3 節ではSE(3)上の剛体の運動モデルを紹介して3次元運動を行う場合のカメラの動

特性モデルを導出する.

6.4 節では視覚情報よりカメラと対象との相対位置姿勢を推定する問題を考察する.

6.5 節ではマニピュレータの手先速度が直接指令できる, つまりマニピュレータダ イナ ミクスが無視できる仮定のもとで, 視覚情報から相対位置姿勢を目標値と一致させる問題 を考える. 推定誤差と追従誤差がともに0へ収束する,もしくはL₂ゲイン制御性能条件を 満足する制御則を, 6.4 節で考察した相対位置姿勢オブザーバを利用して構成する.

6.6 節においてはマニピュレータダ イナミクスを含めた視覚フィード バックシステムを 対象として, 制御則の提案と解析を行う. その制御則の提案には前章まで考察した平面マ ニピュレータの視覚フィード バック制御の構成法を参考にした.

最後に6.5節で提案した制御手法の有効性をシミュレーションにより確認してまとめを 行う.

6.2 SO (3) ^{上の誤差関数}

3次元空間中での対象の相対位置姿勢を制御, 推定するためには回転群SO(3)上での誤 差を考える必要がある. 誤差関数としてφ:SO(3)→R+を

φ(R) := 1

2tr(I−R) (6.1)

60 第6章 視覚フィード バックを利用した3次元相対位置姿勢制御

で与える. また誤差ベクトルe_R(R) := sk(R)^∨ ∈R³ を考える¹. この関数φと誤差ベクト ルe_Rに対してはつぎの補題が成立する.

補題 6.1 R∈SO(3)を与える.

1.φ(R)≥0であり, φ(R) = 0の必要十分条件はR =Iである.

2. ˙φ=e^T_R(R)(R^TR)˙ ^∨ =e^T_R(R)( ˙RR^T)^∨. 3.φ(R)<1を満足するRに対して,

b₂ke_R(R)k² ≤φ(R)≤b₁ke_R(R)k² (6.2) を満足するb₁ ≥ ¹₂ ≥b₂ >0が存在する.

この証明のはじめにつぎの命題および補題を示しておく.

命題 6.1 任意の回転行列R ∈SO(3)に対して,

R =I+ ˆωsinθ+ ˆω²(1−cosθ) (6.3) を満足する単位ベクトルω∈R³, kωk= 1とθ ∈Rが存在する.

この命題はロド リゲスの公式とよばれるものであり, 証明は文献[47]などを参照. 補題 6.2 a∈R³, b∈R³とする. ならば, ¹₂tr(ˆaˆb) = −a^Tbが成立する.

証明 : a= [a₁ a₂ a₃]^T, b= [b₁ b₂ b₃]^T とすると,

1

2tr(ˆaˆb) =1 2tr

















0 −a₃ a₂ a₃ 0 −a₁

−a₂ a₁ 0

















0 −b₃ b₂ b₃ 0 −b₁

−b₂ b₁ 0

















=1 2tr

















−a₃b₃−a₂b₂ ∗ ∗

∗ −a₁b₁−a₃b₃ ∗

∗ ∗ −a₂b₂−a₁b₁

















=−(a₁b₁ +a₂b₂+a₃b₃) =−a^Tb

(Q.E.D.)

1skの定義については1.4節参照

6.2. SO(3)上の誤差関数 61

この命題および補題を利用して補題6.1を証明する.

補題6.1の証明 :

1.R=Iならばあきらかにφ(R) = φ(R^T) = 0. 任意のR∈SO(3)に対しては,ロド リゲ スの公式(6.3)より,

φ(R) =1 2

−tr(ˆω) sinθ−tr(ˆω²)(1−cosθ)

=−1

2tr(ˆω²)(1−cosθ)

= 1−cosθ

が成立する. 最後の変形では補題6.2を利用した. 1−cosθはθ = 2πn, n ∈Zの場合 に0となり,それ以外のときは正になる. θ = 2πnはR=Iと等価であることとφ(R^T) も同様に証明可能であるので, 1. の証明は完了する.

2.行列の集合は線形空間であるので任意の行列A∈R^3×3は,

A= sk(A) + sym(A) (6.4)

に分解可能である². この性質を利用すると, φ˙ =−1

2tr(RR^TR) =˙ −1

2tr(sk(R)(R^TR))˙ − 1

2tr(sym(R)(R^TR))˙ となる. ここでR^TR˙ ∈so(3)であることを利用すると

tr(sym(R)(R^TR)) = tr((R˙ ^TR)˙ ^T(sym(R))^T) =−tr(sym(R)(R^TR))˙ であるのでtr(sym(R)(R^TR)) = 0˙ が成立する. つまり,

φ˙ =−1

2tr(sk(R)(R^TR))˙ である. 最後に ¹₂tr(ˆaˆb) =−a^Tbの関係式を利用すると,

φ˙ = (sk(R)^∨)^T(R^TR)˙ ^∨ =e^T_R(R)(R^TR)˙ ^∨ が成立する. ˙φ=e^T_R(R)( ˙RR^T)^∨ に関しても同様に証明可能.

2symの定義は1.4節参照.

62 第6章 視覚フィード バックを利用した3次元相対位置姿勢制御

3.ロド リゲスの公式(6.3)を利用してe_R(R)を計算すると, e_R(R) = ωsinθ が成立する. kωk= 1を利用すると,

keR(R)k² = sin²θ (6.5)

が成立する. 三角関数の公式を利用すると任意のθ ∈Rに対して, 1−cosθ≥ ¹₂sin²θ が成立するので, 0< b₂ ≤ ¹₂とすれば,

φ(R) = 1−cosθ≥ 1

2sin²θ ≥b₂ke_R(R)k²

が成立する. またφ = 1−cosθ <1が成立する仮定のもとでは, cosθ >0が成立する ので, つぎの関係式を満足する定数0< ≤1が存在する.

sin²θ= 1−cos²θ = (1 + cosθ)(1−cosθ)≥(1 +)(1−cosθ) つまりb₁ := ₁₊¹ ≥ ¹₂とすれば,

b₁ke_R(R)k² ≥ 1

1 +sin²θ≥1−cosθ=φ(R) を満足するb₁が存在する.

(Q.E.D.) この補題はSO(3)上で定義される系の安定性や収束を議論するために非常に重要な役 割をはたす. たとえばφは正定関数であるのでリアプノフ関数候補として利用できる可能 性を有している. また誤差ベクトルeRができるだけ0になるように制御則などを構成す ることで, 3番目の上限および下限の性質より回転行列Rを単位行列Iに近づけることを できる.

またこのSO(3)上の誤差関数を利用してSE(3)上の誤差関数を容易に定義できる. g =

(p, R)∈SE(3)に対して

V(g) =φ(R) + 1

2kpk² (6.6)

と定義すれば, V(g) ≥0であり等号が成立するのは(p, R) = (0, I)のときのみである. そ のうえベクトルe∈R⁶を

e:=



 p e_R



 (6.7)

6.3. 3次元運動に対する視覚フィード バックシステムのモデル 63

と選ぶならば, V(g)<1を満足するgに対して

β₂kek² ≤V(g)≤β₁kek² (6.8) を満足するβ₁ ≥ ¹₂ ≥β₂ >0が存在することは補題6.1よりあきらかである.

6.3 3 次元運動に対する視覚フィード バックシステムのモデル

3–5 章までは2次元平面運動を仮定することで視覚フィード バックシステムのモデルを 与えた. しかし本章では3次元運動に対する視覚フィード バック制御問題を取り扱うため に, 3次元空間での剛体の運動モデルが非常に重要となる.

6.3.1 ^{座標の設定}

本章では座標系を4つ設定する(図6.1参照).

まず基準座標系Σ_W = {X_W, Y_W, Z_W}は, その位置姿勢が時間的に不変であり, 各座標 系の位置姿勢を記述する上で基準となる座標系である. カメラ座標系Σ_C ={X_C, Y_C, Z_C} はカメラに取り付けられた座標系である. 一般性を失わない仮定として光軸はΣ_C のZ_C 方向と一致すると考える.

画像座標系Σ_I ={X_I, Y_I, Z_I}はカメラの画像面に設定される座標系であり, X_IとY_Iは それぞれX_C, Y_Cと平行に設定されX_I-Y_I平面とX_C-Y_C平面は焦点距離λ >0[m]離れて

いる. またΣ_Iの原点はZ_C(つまりカメラの光軸)とX_I-Y_I平面との交点におかれている.

なおカメラ座標系との相対位置姿勢は時間的に変化しない.

最後に観測対象に対象座標系Σ_Oを設定する. この座標系の位置姿勢が対象の位置姿 勢をあらわす. 観測対象はn個の点からなり, それぞれの点のΣOにおける座標poi = [xoi yoi zoi]^T, i= 1, . . . , nがその対象の形状とする. xoi, yoi, zoi はそれぞれXO, YO, ZO方 向の座標をあらわす. なお対象は剛体としてその形状poiは時間的に不変とする.

6.3.2 ^{座標系の変換}

各座標系の位置姿勢は基準座標系Σ_W を基準にして表現される. カメラ座標系Σ_C は g_wc = (p_wc, R_wc)∈R³ ×SO(3) =SE(3), で, 対象座標系Σ_Oはg_wo = (p_wc, R_wc)∈ SE(3)

64 第6章 視覚フィード バックを利用した3次元相対位置姿勢制御

Σ

O

Σ

^W

Σ

^C

Σ

I

λ

p

_oi

p

_ci

f

_i

図 6.1: Coordinates for 3D visual feedback system

でそれぞれの位置姿勢は与えられる. g_wcとg_woを利用するとΣ_Cから見たΣ_Oの相対位置 姿勢g_co= (p_co, R_co)はつぎのように与えられる.

p_co=R^T_wc(p_wo−p_wc) (6.9)

R_co=R^T_wcR_wo (6.10)

Σ_Cから見たΣ_Oの相対位置姿勢g_coが与えられたならば,対象の形状p_oiをΣ_C基準で与 えることができる. そのベクトルをp_ci = [x_ci y_ci z_ci]^T, i= 1, . . . , nとすると,

p_ci=R_cop_oi+p_co (6.11)

で計算できる.

6.3. 3次元運動に対する視覚フィード バックシステムのモデル 65

6.3.3 ^{剛体の運動モデル}

視覚フィード バックシステムでは座標間の相対位置姿勢運動が非常に重要となる. 本 研究ではアイインハンド 構造を仮定して議論を進めていくので, g_wcとg_woがそれぞれ独 立に時間の関数で与えられる. g_wcとg_woのボデ ィー速度をそれぞれ V_wc = [v_wc^T ω^T_wc]^T, V_wo= [v_wo^T ω^T_wo]^T とする. ここでv_wc ∈R³,v_wo∈R³はそれぞれΣ_CとΣ_Oの原点のΣ_Wに 対する速度ベクトルをそれぞれの座標系を基準に見たベクトルであり,ω_wc ∈R³,ω_wo∈R³ はそれぞれの座標系の基準座標に対する回転速度ベクトルをそれぞれの座標系を基準に見 たベクトルである. このボデ ィー速度V_wc, V_woを利用してg_wcとg_woが満足する微分方程 式を記述すると,

R˙_wc=R_wcωˆ_wc (6.12)

˙

p_wc=R_wcv_wc (6.13)

R˙_wo=R_woωˆ_wo (6.14)

˙

p_wo=R_wov_wo (6.15)

で与えられる. この関係を利用して, R_coとp_coの微分方程式を導出すると, R˙_co= ˙R^T_wcR_wo+R^T_wcR˙_wo

=−R^T_wcR˙_wcR^T_wcR_wo+R^T_wcR_woR^T_woR˙_wo

˙

p_co= ˙R^T_wc(p_wo−p_wc) +R^T_wc( ˙p_wo−p˙_wc)

=−R^T_wcR˙_wcR^T_wc(p_wo−p_wc) +R^T_wcR_woR_wo^T p˙_wo−R^T_wcp˙_wc となる. つまり,

R˙_co=−ωˆ_wcR_co+R_coωˆ_wo (6.16)

˙

pco= ˆpcoωwc+Rcovwo−vwc (6.17) である微分方程式がえられる. この式を行列表現すると,



 p˙_co ( ˙R_coR_co^T)^∨



=



 −I pˆ_co 0 −I



V_wc+



 R_co 0 0 R_co



V_wo (6.18)

となる.

66 第6章 視覚フィード バックを利用した3次元相対位置姿勢制御

本研究ではカメラを手先効果器に取り付けたアイインハンド 構造を対象に議論を進めて きた. 本章でもアイインハンド 構造を対象とする. つまり, g_wcとV_wcはそれぞれマニピュ レータの関節角度qの関数となる. g_wcはマニピュレータの運動学により与えられる. V_wc はボデ ィマニピュレータヤコビアンJ_b(q)∈R^6×n(nはqの次元)により,

V_wc =J_b(q) ˙q (6.19)

で与えられることが知られている[47]. よって式(6.18)は,



 p˙_co ( ˙R_coR^T_co)^∨



=



 −I pˆ_co 0 −I



J_b(q) ˙q+



 R_co 0 0 R_co



V_wo (6.20)

となる.

注意 6.1 本章で対象とする視覚フィード バックシステムは,簡単にいえばカメラと対象の 相対位置姿勢g_coをある目標値g_d = (p_d, R_d) ∈ SE(3)に一致するようにカメラ情報を利 用してマニピュレータの関節速度q˙を決定することにある. しかし式(6.20)の状態空間は

SE(3)上で定義されていることやq˙を直接与えることはできず, マニピュレータダ イナミ

クスにしたがった動特性が存在することに本研究で対象とする視覚フィード バックシステ ムの難しさが存在する.

6.3.4 カメラの透視変換モデル

カメラからの情報を元にマニピュレータを制御するためには, 画像の幾何モデルを理解 しなくてはならない. カメラの幾何モデルは最も標準的にはピンホールカメラを仮定して 透視変換モデルを利用する場合が多い. 6.3.1 節での仮定のもとでは透視変換は,

f_i = λ zci



 x_ci y_ci



 (6.21)

で与えられる. ここでf_i ∈ R², i = 1, . . . , nは画像座標系でのX_I-Y_I平面上の点像位置を あらわす. またλは焦点距離をあらわし, p_ci = [x_ci y_ci z_ci]^T である. 式(6.11)より, p_ciは g_coの関数であるのでf_iはg_coの関数となる. よってf := [f₁^T . . . f_n^T]^T ∈R²ⁿ と定義する と, π :SE(3) →R²ⁿなる関数を利用して,

f =π(g_co) (6.22)

でカメラモデルは記述できる. ここでπは式(6.11)と(6.21)により定義される.

ドキュメント内 藤田 政之 助教授 (ページ 66-74)