知能システム論1...

知能システム論1　(14)

2013.7.10情報システム学研究科

情報メディアシステム学専攻

知能システム学講座

末廣尚士

2013年度前学期電気通信大学大学院　情報システム学研究科情報メディアシステム学専攻　知能システム論１ 2

17 座標系のキャリブレーション

座標系表現でロボットを動かすには座標系(座標変換)を正しく定める必要がある

前回の，「物体の位置・姿勢計測」もその一つ

depthカメラと物体の座標変換行列を精度良く(計測点の誤差が最小2乗になるように)求めた

今回は，座標系の根っこの部分の関係の定め方について学ぶ


- 座標系を用いた表現

table

box

placeBplaceA

worldbase

hand

camera


- 座標系の関係とその計測

table

boxplaceB placeA

world

base

hand

wrist

camera

T hw

T warm

T armworld

T tworld

T cworld

T armt

T At

T Bt

T bt 計測

（計測）


- 座標系をどうやって決めるか

設計値

CADやレイアウト図など

実際にどれくらい精確に設計値が実現できているかは場合によって変わる

計測

物差し，距離計，カメラなどで測る

実はロボットアームも立派な3次元計測器


- 座標系の決まり方

T hw

T warm

T armworld

T tworld T c

world

T armt

T At

T Bt

T bt

場所を表すだけの形のない座標系は計測が難しい．多くの場合，設計値で決めるしかない

アームの設計値と順運動学で決まる．関節角原点のオフセットなどの精密計測が必要な場合もある．

設備のレイアウト設計値で大まかな値が決まる．作業を行う場合には，さらに精密な計測が必要となる．移動ロボットなどの場合は移動ベース座標の計測が必要となる．

ハンドの据え付け設計値できまる．道具として「持った」場合は，計測が必要なことも多い．

工場などでは操作対象は設計時に決められた場所に置くのが基本．安定稼働には現場での微調整やアームでの直接計測も重要となる．日常生活，未整備環境では，アームが操作する前にカメラなどによる計測が必要となる．作業をスムーズに行うには計測値とアームの動作との一致度が高いほど良い．


- 座標系の関係の決まり方

table

boxplaceB placeA

world

base

hand

wrist

camera

T hw

T warm

T armworld

T tworld

T cworld

T armt

T At

T Bt

T bt 計測

（計測）


- 座標系計測の基本手順

world座標系を定めるアームベースチェスボード（キャリブレーションボード）

world-計測系のキャリブレーション worldがアームベースの場合，ハンドアイキャリブ

レーション worldがチェスボードの場合，，，openCVの外部

パラメタ．実は物体の位置姿勢計測問題．

後は，計測系⇔物体，アーム系の繰り返し物体の位置・姿勢計測ハンドアイキャリブレーション


- ハンドアイキャリブレーション ver.00 物体の位置・姿勢計測の直接利用計測系で直接アームベースの位置・姿勢計測

を行う．これで，が求められる．

問題点：多くの場合，それでは作業対象などの計測ができ

ない．アームベースが必ずしも計測しやすい形状ではな

い．このような単体計測は誤差が小さくしにくい

T armc


- ハンドアイキャリブレーション ver.0 物体の位置・姿勢計測の直接利用計測系で直接ハンドの位置・姿勢計測を行う．これで，が求められる

ここから　　　　　　　　　　　　　　　を計算する

．

問題点：ハンドが必ずしも計測しやすい形状ではない．このような単体計測は誤差が大きくなりやすい

T hc

T armc = T h

c ( T harm )−1


- ハンドアイキャリブレーション ver.1

ハンドから既知の位置に固定されたマークの位置をアームの姿勢を変えながら多数回計測し，最小2乗推定を行う．

base

hand

cameraph

T harm

pc

T armc

pc = T armc T h

arm ph


- キャリブレーション ver.1 (解法1-1)pc = T arm

c parmpc = T arm

c T harm ph ⇒

ここで　　を回転と並進で表すと

Rarmc

=(e xT

e yT

e zT) とおくと pc =(

e x⋅ parm

e y⋅ parm

e z⋅ parm )+(x⋅dy⋅dz⋅d )

pc =(parm 0 0 x0 parm 0 y0 0 parm z )(

e xe ye zd)

T armc Rarm

c d pc = Rarmc parm

+d

これを書き直すと


- キャリブレーション ver.1 (解法1-2)n個の計測点で連立させて

(⋮pic

⋮ )=(⋮

piarm 0 0 x

0 piarm 0 y

0 0 piarm z

⋮)(e xe ye zd)

これをb=A(

e xe ye zd) とおいて解く．


- キャリブレーション ver.1 (解法1-3)疑似逆行列を使って解くと

(e xe ye zd)=A+b

すなわち

Rarmc

=(e xT

e yT

e zT)

この解法の問題点は　　　　　が直交行列である保証がないこと．

ある程度の歪みも含めてキャリブレーションできたと考えることも出来るが，計測点数が不十分だと過学習の可能性もある．

Rarmc

T armc

=( Rarmc d

0 0 0 1)


- キャリブレーション ver.1 (解法2-1)

pc = T armc T h

arm ph

f ( x , y , z ,α ,β ,γ )= T armc T ( x , y , z ,α ,β ,γ ) T h

arm ph − pc

q=(x , y , z ,α ,β ,γ )T

これを

として，ニュートン法を使う．

T (q)=T xyz (x , y , z)T α (α )T β (β )T γ (γ )


- キャリブレーション ver.1 (解法2-2) 　を　　　　　で偏微分する．

∂T xyz∂ x

=(0 0 0 10 0 0 00 0 0 00 0 0 0

)∂T α

∂α=(0 0 0 00 0 −1 00 1 0 00 0 0 0

) ∂T β

∂ β=(

0 0 1 00 0 0 0

−1 0 0 00 0 0 0

) ∂T γ

∂γ=(0 −1 0 01 0 0 00 0 0 00 0 0 0

)

T (q)

∂T xyz∂ y

=(0 0 0 00 0 0 10 0 0 00 0 0 0

) ∂T xyz∂ z

=(0 0 0 00 0 0 00 0 0 10 0 0 0

)

q=0


- キャリブレーション ver.1 (解法2-3) 　を　　　　　で偏微分する．

∂ f∂ x

= T armc (

0 0 0 10 0 0 00 0 0 00 0 0 0

) T harm ph = T arm

c (100)

∂ f∂α

= T armc (

0 0 0 00 0 −1 00 1 0 00 0 0 0

) T harm ph = T arm

c ((100)× T h

arm ph )∂ f∂β

= T armc ((010)× T h

arm ph )∂ f∂γ

= T armc ((001)× T h

arm ph )

f (q)

∂ f∂ y

= T armc (

010)

∂ f∂ z

= T armc (

001)

q=0



(∂ f∂q )=(

⋮ ⋮ ⋮ ⋮ ⋮ ⋮

x y z T armc ((

100)× T h

arm ph ) T armc ((

010)× T h

arm ph ) T armc ((

001)× T h

arm ph )⋮ ⋮ ⋮ ⋮ ⋮ ⋮

)T armc

=(x y z)ただし

偏微分行列（ヤコビアン）を書き下す



f (q)≈ f (0)+(∂ f∂q )q=0

(∂ f∂q )q＝− f (0)

q＝−(∂ f∂q )

+

f (0)

T k+1＝T kT (q)

これを疑似逆行列を使って解く

として収束するまで繰り返す


- ハンドアイキャリブレーション ver.2

ハンドから未知の位置に固定されたマークの位置をアームの姿勢を変えながら多数回計測し，最小2乗推定を行う．

base

hand

cameraph

T harm

pc

T armc

pc = T armc T h

arm ph


- キャリブレーション ver.2 (解法)

pc = T armc T h

arm ph

f (q)= f ( x , y , z ,α ,β ,γ , p x , p y , pz)

q=(x , y , z ,α ,β ,γ , p x , p y , pz)T

これを

として，ニュートン法を使う．

T (x , y , z ,α ,β , γ)

= T armc T (x , y , z ,α ,β ,γ ) T h

arm( ph + p( px , p y , p z))− pc

=T xyz( x , y , z)T α (α )T β (β )T γ (γ )


- ハンドアイキャリブレーションのまとめ

考え方は物体の位置・姿勢計測と同じ作業全体に影響するので「広範囲」での精度が

必要作業領域全体で広く多数の計測を行う

アーム自身の誤差の影響も考慮する必要があるいろいろなアームの姿勢で計測を行う場合によってはアームパラメタ自身のキャリブレー

ションも必要


18 多様な面へのフィッティング

平面，円筒面，球面など様々な形状の面を持つ物体の位置・姿勢計測

それぞれの面は式が簡単になるような面の基準座標で表現する

面の基準座標は物体座標系からの相対座標で与えられる

depthセンサでの計測点がどの面と対応するものかは分かっているとする


-一般面のフィッティング

計測点が面上にある条件

T objc

T sobj

計測系から見た物体系

物体系から見た面座標系

pic

計測系から見た計測点

pis

面座標系から見た計測点

f ( pis )=0

　　　　　　　が点と面との距離になっていればニュートン法で距離の最小２乗に収束する∣ f ( pi

s)∣


- 面上にあるための条件

f ( p)= f ( p(q))

これを記号を簡略化して，整理する

q=(x , y , z ,α ,β ,γ )T

p(q)= T s−1obj T−1(q)T 0

−1 pc


- 面上にあるための条件の偏微分

合成関数の偏微分になるので

∂ f∂q

=∂ f∂ p

∂ p∂q

∂ f∂ q

=(∂ f∂ x

⋯∂ f∂γ )

∂ f∂ p

=( ∂ f∂ px

,∂ f∂ p y

,∂ f∂ pz)

∂ p∂ q

=(∂ px∂ x

⋯∂ px∂γ

∂ p y∂ x

⋯∂ p y∂γ

∂ pz∂ x

⋯∂ pz∂γ

)∂ f∂ p

∂ p∂qは面に依存，　　　　　は点の座標変換なので皆同じ


- 参考：　　　　を偏微分する

　を　　　　　で偏微分する．

T−1(q)

∂T xyz−1

∂ x=(0 0 0 −10 0 0 00 0 0 00 0 0 0

)∂T α

−1

∂α=(0 0 0 00 0 1 00 −1 0 00 0 0 0

) ∂T β−1

∂ β=(0 0 −1 00 0 0 01 0 0 00 0 0 0

) ∂T γ−1

∂γ=(

0 1 0 0−1 0 0 00 0 0 00 0 0 0

)

T−1(q)

∂T xyz−1

∂ y=(0 0 0 00 0 0 −10 0 0 00 0 0 0

) ∂T xyz−1

∂ z=(0 0 0 00 0 0 00 0 0 −10 0 0 0

)

q=0


- 共通部分　　　を求める∂ p∂q

∂ p∂q

= T s−1obj ∂T−1

∂qT 0

−1 pc

∂ p∂ x

= T s−1obj (0 0 0 −10 0 0 00 0 0 00 0 0 0

)T 0−1 p=− T s−1obj (100)

∂ p∂ z

=− T s−1obj (001)

∂ p∂ y

=− T s−1obj (010)

(x , y , z )


- 共通部分　　　を求める∂ p∂q

∂ p∂q

= T s−1obj ∂T−1

∂qT 0

−1 pc

∂ p∂α

= T s−1obj (0 0 0 00 0 1 00 −1 0 00 0 0 0

)T 0−1 p=− T s−1obj ((

100)×T 0−1 p)

∂ p∂β

=− T s−1obj ((

010)×T 0−1 p) ∂ p

∂γ=− T s

−1obj ((001)×T 0−1 p)

(α ,β ,γ )


- 平面のフィッティング

xy平面とする．z方向が面の外向き法線．

T objc

T sobj

pis = T s

−1obj T obj−1c pi

c

計測点が面上にあるとすれば




pic


pis


z⋅ pis =0


-平面上にあるための条件

z⋅ T s−1obj T obj

−1c pic =0

すなわち

f ( pis )=z⋅ pi

s =0

∂ f∂ p

=( ∂ f∂ px

,∂ f∂ p y

,∂ f∂ p z)=(0,0,1)

したがって


-平面のフィッティング条件の偏微分

∂ f∂ x

=−z⋅ T s−1obj (100)=− T s

obj z⋅(100)

∂ f∂ y

=−z⋅ T s−1obj (010)=− T s

obj z⋅(010)

∂ f∂ z

=−z⋅ T s−1obj (001)=− T s

obj z⋅(001)


-平面のフィッティング条件の偏微分

∂ f∂α

=−z⋅ T s−1obj ((

100)×T 0−1 pc )=− T s

obj z⋅(100)×T 0−1 pc

∂ f∂β

=−z⋅ T s−1obj ((

010)×T 0−1 pc )=− T s

obj z⋅(010)×T 0−1 pc

∂ f∂γ

=−z⋅ T s−1obj ((

001)×T 0−1 pc )=− T s

obj z⋅(001)×T 0−1 pc


- 円筒面のフィッティング

中心軸はz軸で半径rとする

T objc

T sobj

pis = T s


c





√ px , is 2+ p y , is 2

−r=0

pic


pis



-円筒面上にあるための条件

∂ f∂ p

=( ∂ f∂ px

,∂ f∂ p y

,∂ f∂ p z)=(

−px

√ px2+ p y2,

−p y

√ p x2+ p y2,0)

これを偏微分すると

f ( pis

)=√ px , is 2+ p y , is 2

−r=0


- 円筒面のフィッティング条件の偏微分

∂ f∂ x

=−( −p x

√ p x2+ p y2,

−p y

√ px2+ p y2,0) T s

−1obj (100)

∂ f∂ y

=−( −p x

√ p x2+ p y2,

−p y

√ px2+ p y2,0) T s

−1obj (010)

∂ f∂ z

=−( −p x

√ p x2+ p y2,

−p y

√ px2+ p y2,0) T s

−1obj (001)


-円筒面のフィッティング条件の偏微分

∂ f∂α

=−( −p x

√ p x2+ p y2,

−p y

√ px2+ p y2,0) T s

−1obj ((100)×T 0−1 pc )

∂ f∂β

=−( − px

√ px2+ p y2,

− p y

√ px2+ p y2,0) T s

−1obj ((010)×T 0−1 pc )

∂ f∂γ

=−( −p x

√ p x2+ p y2,

−p y

√ px2+ p y2,0) T s

−1obj ((001)×T 0−1 pc )


- 球面のフィッティング

中心は原点で半径rとする

T objc

T sobj

pis = T s


c





√ pis⋅ pis

−r=0

pic


pis



- 球面上にあるための条件

∂ f∂ p

=( −p x

√ px2+ p y2+ pz2,

−p y

√ p x2+ p y2+ pz2,

−p z

√ p x2+ p y2+ p z2)=−p∣p∣

これを偏微分すると

f ( pis

)=√ pis⋅ pis

−r=√ p x , is 2+ p y , is 2

+ pz , is 2

−r=0


- 球面のフィッティング条件の偏微分

∂ f∂ x

=− p∣p∣

⋅ T s−1obj (100)

∂ f∂ y

=− p∣p∣

⋅ T s−1obj (010)

∂ f∂ z

=− p∣p∣

⋅ T s−1obj (001)


-球面のフィッティング条件の偏微分

∂ f∂α

=− p∣p∣

⋅ T s−1obj ((

100)×T 0−1 pc )

∂ f∂β

=− p∣p∣

⋅ T s−1obj ((

010)×T 0−1 pc )

∂ f∂γ

=− p∣p∣

⋅ T s−1obj ((

001)×T 0−1 pc )


- 透視投影変換(おまけ)

T objc

pc = T objc pobj


u=F xpxc

pzc +C x

pobj 物体系から見た特徴点

pc 計測系から見た計測点

(u v)T 特徴点を計測系画面上に投影した点

v=F y

p yc

pzc +C y

　　　　　　　はピクセル単位での焦点距離および画像中心

F x , F y ,C x ,C y


- 画像特徴点へのフィッティング条件

f u=u−uc=F xp xc

pzc +C x−uc

(uc vc)T 計測系上で抽出された特徴点

f v=v−vc=F y

p yc

pzc +C y−vc

記号を簡略化して，整理する

q=(x , y , z ,α ,β ,γ )Tp(q)=T objT (q) pobj


- 　　　を求める∂ p∂q

∂ p∂q

=T obj∂T∂q

pobj

∂ p∂ x

=T obj(0 0 0 10 0 0 00 0 0 00 0 0 0

) p=T obj(100)∂ p∂ z

=T obj(001)

∂ p∂ y

=T obj(010)

(x , y , z )


- 　　　を求める∂ p∂q

∂ p∂α

=T obj(0 0 0 00 0 −1 00 1 0 00 0 0 0

) p=T obj((100)× p)∂ p∂β

=T obj((010)× p) ∂ p∂γ

=T obj((001)× p)

(α ,β ,γ )

∂ p∂q

=T obj∂T∂q

pobj


- 透視投影変換の偏微分

∂u∂ px

=F xpz

∂ v∂ p y

=F y

pz

∂u∂ pz

=−F x p xpz2

∂u∂ p y

=0

∂v∂ pz

=−F v pvpz2

∂ v∂ p y

=0

∂ f∂ p

=(F x

pz0

−F x pxpz2

0F y

pz

−F v pvpz2 )


- 透視投影変換の偏微分

∂ f∂q

=(F x

pz0

−F x p xpz2

0F y

pz

−F v pvpz2 )∂ p

∂q


- 全点について連立させてニュートン法

f (q)≈ f (0)+(∂ f∂q )q=0

(∂ f∂q )q＝− f (0)

q＝−(∂ f∂q )

+

f (0)

T k+1＝T kT (q)

これを疑似逆行列を使って解く

として収束するまで繰り返す


- 最小２乗であること疑似逆行列は線型連立方程式の最小２乗解を与える．つまり，

で求めたにより以下が最小，

　　　　が面と点の距離ならば，その２乗和が最小

　　　が収束したときにも近傍でこれが成り立つ

q＝−(∂ f∂q )

+

f (0)

q

∣ f (q)∣2≈∣ f (0)+(∂ f∂ q )q∣

2

q∣ f (q)∣


次回予告

マニピュレーションスキル授業アンケート

知能システム論1...

Documents