Σ A Σ B r Σ A (Σ A ): A r = [ A r A x r A y r z ] T Σ B : B r = [ B r B x r B y r z ] T A r = A x B B r x + A y B B r y + A z B B r z A r = A R B B r

(1)

剛体の位置と姿勢

• 3

次元空間内の剛体の位置・姿勢の表現方法

–

基準座標系

: Σ

_A

= O

_A

–{X

_A

, Y

_A

, Z

_A

}

–

剛体座標系

: Σ

_B

= O

_B

–{X

_B

, Y

_B

, Z

_B

} – O

_B _{の位置ベクトル}

:

^A

p

_B

– X

_B

, Y

_B

, Z

_B の方向を向く単位ベクトルを

Σ

_Aで表したもの

:

^A

x

_B

,

^A

y

_B

,

^A

z

_B

– Σ

_Aから見た剛体の位置

:

^A

p

_B

– Σ

_Aから見た剛体の姿勢

: {

^A

x

_B

,

^A

y

_B

,

^A

z

_B

}

•

回転行列

–

^A

R

_B

= [

^A

x

_B ^A

y

_B ^A

z

_B

] – (

^A

R

_B

)

^T

(

^A

R

_B

) = I

₃

A −1 A T

(2)

ベクトルの表現

• Σ

_Aと

Σ

_B の原点は一致しているとする

•

あるベクトル

r

– Σ

_Aから見たとき

(Σ

_Aでの表現

):

^A

r = [

^A

r

_x ^A

r

_y ^A

r

_z

]

^T

– Σ

_Bでの表現

:

^B

r = [

^B

r

_x ^B

r

_y ^B

r

_z

]

^T

A

r =

^A

x

_B^B

r

_x

+

^A

y

_B^B

r

_y

+

^A

z

_B^B

r

_z

A

r =

^A

R

_B^B

r

同様に，

(Σ

_A_と

Σ

_B _{の立場を逆にすれば}

)

B

r =

^B

R

_A^A

r

•

上の関係は任意の

r

について成り立つから

(

^A

R

_B

)(

^B

R

_A

) = I

₃

(3)

座標系の関係

• Σ

_A，

Σ

_B と原点が一致している第

3

の座標系

Σ

_C

• r

の

Σ

_Cにおける表現^C

r

B

r =

^B

R

_C^C

r,

^A

r =

^A

R

_C^C

r

A

R

_C^C

r =

^A

r =

^A

R

_B^B

r =

^A

R

_B^B

R

_C^C

r

A

R

_C

=

^A

R

_B^B

R

_C

,

^A

R

_B

= (

^C

R

_A

)

^{T C}

R

_B

A

R

_B

=







(

^C

x

_A

)

^{T C}

x

_B

(

^C

x

_A

)

^{T C}

y

_B

(

^C

x

_A

)

^{T C}

z

_B

(

^C

y

_A

)

^{T C}

x

_B

(

^C

y

_A

)

^{T C}

y

_B

(

^C

y

_A

)

^{T C}

z

_B

(

^C

z

_A

)

^{T C}

x

_B

(

^C

z

_A

)

^{T C}

y

_B

(

^C

z

_A

)

^{T C}

z

_B







(4)

同次変換行列

• 2

つの座標系

Σ

_A

, Σ

_B

(

原点が一致していない

)

• Σ

_Aに対する

Σ

_B の位置

:

^A

p

_B

• Σ

_Aに対する

Σ

_B の姿勢

:

^A

R

_B

• Σ

_B _に関して^B

r

_{で表示された点を}

Σ

_A_で表示

A

r =

^A

R

_B^B

r +

^A

p

_B





A

r 1





=





A

R

_B ^A

p

_B

0 0 0 1









B

r 1





•

同次変換行列

A

T

_B

=





A

R

_B ^A

p

_B

0 0 0 1





• [

注意！

]:

A

r

と





A

r 1



 ，^B

r

と





B

r 1



 を同一視することがある．

(5)

同次変換

•

同次変換

A

r =

^A

T

_B^B

r

•

_クイズ

– Σ

_Bは

Σ

_Aと原点が一致していて，

Z

_Aまわりに

α

回転した位置にある．このとき^A

T

_B を求めよ．

– Σ

_B_は

Σ

_A_に対して

Y

_A_方向に

2

_，

Z

_A_方向に

1

だけ並進した位置にある．このとき^A

T

_B _を求めよ．

(6)

同次変換の分解

1 0 1 2 3 4 5 6

1 0 1 2 3 4 5

Σ_A

Σ_B p

B

Ar

r

A B

•

同次変換行列





A

R

_B ^A

p

_B

0 1





=





I

₃ ^A

p

_B

0 1









A

R

_B

0 0 1





•

同次変換

A

r =

^A

T

_B^B

r

•

ベクトルの回転・並進としての解釈

– Σ

_A_において^B

r

_{で表現される点} を

Σ

_A _に関して^A

R

_B_{回転し，そ} れを

Σ

_Aに関して^A

p

_B 並進すると^A

r

を得る．

– Σ

_B _{における表現が}^B

r

_である点の

Σ

_A _{における表現は} ^A

r

_である．

(7)

座標系の回転・並進としての解釈

−1 0 1 2 3 4 5

• Σ

_Aを

Σ

_Aに関して^A

R

_B回転し，それを

Σ

_A に関して ^A

p

_B 並進すると

Σ

_Bを得る．

• Σ

_A_を

Σ

_A _に関して^A

p

_B _{並進し，そ} の場で^A

R

_B _{回転すると}

Σ

_B_を得る．

(8)

座標系の回転・並進としての解釈 [ _注意 1]

−1 0 1 2 3 4 5

• Σ

_Aを

Σ

_Aに関して^A

R

_B回転し，回転後の座標系

Σ

⁰_Aに関して^A⁰

p

_B並進しても

Σ

_B にはならない！

(9)

座標系の回転・並進としての解釈 [ _注意 2]

−1 0 1 2 3 4 5 6

−1 0 1 2 3 4 5

• Σ

_A_を

Σ

_A_に関して^A

p

_B _並進し，

Σ

_A に関して^A

R

_B _{回転しても}

Σ

_B _にはならない！

(10)

同次変換の積

• 3

つの座標系

Σ

_A

, Σ

_B

, Σ

_Cがあり，

Σ

_Aと

Σ

_B の関係が^A

T

_B，

Σ

_B と

Σ

_C の関係が^B

T

_Cで与えられるとき，

Σ

_Aと

Σ

_Cの関係は？

A

T

_C

=

^A

T

_B^B

T

_C

•

ここで

A

T

_B

=





A

R

_B ^A

p

_B

0 1





=





I

₃ ^A

p

_B

0 1









A

R

_B

0 0 1





つまり

A

T

_C

=





I

₃ ^A

p

_B

0 1









A

R

_B

0 0 1









I

₃ ^B

p

_C

0 1









B

R

_C

0 0 1





に注意する．

(11)

同次変換の積の解釈

1.

左から解釈する方法

(a) Σ

_Aを

Σ

_Aに関して^A

p

_B 並進

(b)

それをその場で^A

R

_B回転

−→ Σ

_B を得る

(c) Σ

_B_を

Σ

_B _に関して^B

p

_C_並進

(d)

それをその場で^B

R

_C回転

−→ Σ

_Cを得る

2.

右から解釈する方法

(a) Σ

_A_を

Σ

_A_に関して^B

R

_C_回転

(b)

それを

Σ

_Aに関して^B

p

_C並進

−→ Σ

⁰_B を得る

(c) Σ

⁰_Bを

Σ

_Aに関して^A

R

_B 回転

(d)

それを

Σ

_Aに関して^A

p

_B並進

−→ Σ

_Cを得る

(12)

同次変換の積の例題，逆変換

•

クイズ

A

T

_B

=







√ 3/2 −1/2 0 2 1/2 √

3/2 0 1

0 0 1 0

0 0 0 1







,

^B

T

_C

=







1/ √

2 1/ √

2 0 1

−1/ √

2 1/ √

2 0 1

0 0 1 0

0 0 0 1







左から解釈する場合と右から解釈する場合の図を書き，同じ結果を得ることを確認せよ．

•

逆変換

B

T

_A

= (

^A

T

_B

)

⁻¹

=





(

^A

R

_B

)

^T

−(

^A

R

_B

)

^{T A}

p

_B

0 1





(13)

線形代数の応用

•

ロボットのキネマティクス

•

_ステレオ

•

キャリブレーション

(14)

PUMA _{ロボットのリンク構造}

(15)

PUMA ロボットの同次変換行列 (0 → 1)

• Σ

₀に対し

Σ

₁は並進なしで

Σ

₀の

z

軸まわりの回転．

0

T

₁

=







C

₁

−S

₁

0 0 S

₁

C

₁

0 0 0 0 1 0 0 0 0 1







ただし

C

₁

= cos(θ

₁

), S

₁

= sin(θ

₁

)

である．

(16)

PUMA ロボットの同次変換行列 (1 → 2)

• Σ

₁に対し

Σ

₂は

...

– Σ

₁を

x

軸まわりに

−π/2

回転．

–

回転後の

z

方向に

l

_b

− l

_d並進し，その場で

z

軸まわりに

θ

₂回転．

1

T

₂

=







1 0 0 0 0 0 1 0 0 −1 0 0 0 0 0 1













1 0 0 0 0 1 0 0 0 0 1 l

_b

− l

_d

0 0 0 1













C

₂

−S

₂

0 0 S

₂

C

₂

0 0 0 0 1 0 0 0 0 1







=







C

₂

−S

₂

0 0 0 0 1 l

_b

− l

_d

−S

₂

−C

₂

0 0

0 0 0 1







•

次のように考えても同じ

– Σ

₁を

y

方向に

l

_b

− l

_d並進し，その場で

x

軸まわりに

−π/2

回転．

–

_回転後の

z

_方向に

θ

₂_回転．

(17)

PUMA ロボットの同次変換行列 (2 → 3)

• Σ

₂に対し

Σ

₃は

...

– Σ

₂を

x

方向に

l

_c並進．

–

その場で

z

軸まわりに

θ

₃回転．

2

T

₃

=







1 0 0 l

_c

0 1 0 0 0 0 1 0 0 0 0 1













C

₃

−S

₃

0 0 S

₃

C

₃

0 0 0 0 1 0 0 0 0 1







=







C

₃

−S

₃

0 l

_c

S

₃

C

₃

0 0 0 0 1 0 0 0 0 1







(18)

PUMA ロボットの同次変換行列 (3 → 4)

• Σ

₃に対し

Σ

₄は

...

– Σ

₃を

x

方向に

l

_e並進し，その場で

x

軸まわりに

−π/2

回転．

– z

軸方向に

l

_f並進し，その場で

z

軸まわりに

θ

₄回転．

3

T

₄

=







1 0 0 l

_e

0 1 0 0 0 0 1 0 0 0 0 1













1 0 0 0 0 0 1 0 0 −1 0 0 0 0 0 1













1 0 0 0 0 1 0 0 0 0 1 l

_f

0 0 0 1













C

₄

−S

₄

0 0 S

₄

C

₄

0 0 0 0 1 0 0 0 0 1







=







C

₄

−S

₄

0 l

_e

0 0 1 l

_f

−S

₄

−C

₄

0 0

0 0 0 1







(19)

PUMA ロボットの同次変換行列

•

同様にして

4

T

₅

=







C

₅

−S

₅

0 0 0 0 −1 0 S

₅

C

₅

0 0

0 0 0 1







5

T

₆

=







C

₆

−S

₆

0 0 0 0 1 0

−S

₆

−C

₆

0 0 0 0 0 1







(20)

ロボットキネマティクス ( _機構学 )

•

関節角度

θ

から手先

(

または手先に固定されたツール

)

の位置⁰

r

と姿勢 ⁰

R

₆ を求めること

•

一般的には

A

r =

^A

R

_B^B

r +

^A

p

_B

を

A = 5, B = 6

から順に基準座標系に向かって解いていく．

•

同次変換行列の積で書くとコンパクト

0

T

₆

=

⁰

T

₁¹

T

₂

· · ·

⁵

T

₆

(21)

インバースキネマティクス ( _逆機構学 )

•

手先の位置姿勢⁰

T

₆を実現する関節角度

θ

を求めること

•

非線形であり，解は一意ではない

•

例

–

右利き・左利き

–

エルボーアップ・エルボーダウン

(22)

透視射影変換

Image Plane

Y

X

Z

y

x

Object Coordinate System Camera

Coordinate System

Camera

f

cprel

•

_幾何光学

(

_{レンズのモデル}

) –

深さ

Z

，焦点距離

f

x = f X

Z , y = f Y Z

•

透視投影変換

s







x y 1







=







f 0 0 0 0 f 0 0 0 0 1 0













X Y Z 1







•

透視投影変換行列

A =







f 0 0 0 0 f 0 0 0 0 1 0







– CCD

の縦横の解像度の違い，

CCD

面のゆがみなどを考慮すると

A

はさらに複雑になる．

(23)

ビジョンシステム

World Coordinate System

Image Plane

Camera

y

x f

Z

X

Y

•

対象物とカメラの関係

(

^C

T

_O

)

–

対象物の座標系において表現^O

r

_iをもつ点は，カメラ座標系では





C

r

_i

1





=

^C

T

_O





O

r

_i

1





,

^C

r

_i

=







X

_i

Y

_i

Z

_i







–

_{撮像面上の位置}

s

_i







x

_i

y

_i

1







= A

^C

T

_O





O

r

_i

1





= A(

^W

T

_C

)

^−1W

T

_O





O

r

_i

1





(24)

キャリブレーション

Image Plane

Camera

y

x f

Z

X

Y

•

正確に測定された対象位置

(

^W

T

_O

)

と物体形状

(

^O

r

_i

)

に基づいて，カメラで物体を計測し

(x

_i

, y

_i

)

，カメラの内部パラメータ

(A)

_{および外部パラメータ}

(

^W

T

_C

)

_{を同定する問題．}

s

_i







x

_i

y

_i

1







= A(

^W

T

_C

)

^−1W

T

_O





O

r

_i

1





•

あとで詳しく説明する．

(25)

透視射影変換

Image Plane

Y

X

Z

y

x

Camera

f

cprel

•

_幾何光学

(

_{レンズのモデル}

) –

深さ

Z

，焦点距離

f

x = f X

Z , y = f Y Z

•

透視投影変換

s







x y 1







=







f 0 0 0 0 f 0 0 0 0 1 0













X Y Z 1







•

透視投影変換行列

A =







f 0 0 0 0 f 0 0







(26)

ビジョンシステム

Image Plane

Camera

y

x f

Z

X

Y

• 透視投影変換

s







x y 1





 = A







X Y Z

1







• 対象物とカメラの関係(^CTO)

– 対象物の座標系において表現^Or_iをもつ点のカメラ座標系での位置



 Cr_i 1



 = ^CT_O



 Or_i 1



, ^Cr_i =







Xi

Y_i Z_i







– 撮像面上の位置

s_i







xi

y_i 1





 = A^CT_O



 Or_i 1





= A(^WTC)^−1WTO



 Or_i 1





(27)

キャリブレーション

Image Plane

Camera

y

x f

Z

X

Y

s

_i







x

_i

y

_i

1







= A(

^W

T

_C

)

^−1W

T

_O





O

r

_i

1





•

正確に測定された

–

対象位置

:

^W

T

_O

–

物体形状

:

^O

r

_i

–

_{物体の画像}

: x

_i

, y

_i

ただし

(i = 1, . . . , N )

に基づいて，カメラの

–

内部パラメータ

: A

–

外部パラメータ

:

^W

T

_C

(28)

ビジョンシステムのキャリブレーション

Image Plane

Camera

y

x f

Z

X

Y

s

_i







x

_i

y

_i

1







= A(

^W

T

_C

)

^−1W

T

_O





O

r

_i

1





= P

^W

T

_O





O

r

_i

1





• P

のサイズは

3 ×4

であり，自由度は

12

．ただし，

P

⁰

= αP

に対しても上式は成立するため，

本質的には

11

_自由度．

• 1

点の観測につき

2

自由度の情報が得られるため，

6

点を観測すれば

P

は決定可能．

•

しかしノイズの影響などにより，精度よく推定するためには数十程度の点を計測する必要があり，現在もまだアクティブな研究対象である．

(29)

キャリブレーションアルゴリズム

透視変換のモデル

:

s

_i







x

_i

y

_i

1







= P

^W

T

_O





O

r

_i

1





問題

: Find P ∈ R

^3×4

s.t.

X

i

°°

s

_i







x

_i

y

_i

1







− P







X

_i

Y

_i

Z

_i

1







°°

2

→ min

アルゴリズム

:

点

i

_に関して

• P = [P

_jk

]

とおくと，最小化すべきベクトルの要素は

s

_i

x

_i

− (P

₁₁

X

_i

+ P

₁₂

Y

_i

+ P

₁₃

Z

_i

+ P

₁₄

)

s y − (P X + P Y + P Z + P )

(30)

キャリブレーションアルゴリズム

• P

の要素を縦に並べたベクトルを

P = [P

₁₁

, P

₁₂

, . . . , P

₃₄

]

^T とし，パラメータ

s

_i を消去すると，上式は

·

X

_i

Y

_i

Z

_i

1 0 0 0 0 −x

_i

X

_i

−x

_i

Y

_i

−x

_i

Z

_i

−x

_i ^¸

P

·

0 0 0 0 X

_i

Y

_i

Z

_i

1 −y

_i

X

_i

−y

_i

Y

_i

−y

_i

Z

_i

−y

_i ^¸

P

となる．

• P

の自由度を用いて

P

₃₄

= 1

となるように規格化する．

P ˆ = [P

₁₁

, P

₁₂

, . . . , P

₃₃

]

^T とおき，点

i = 1, · · · , N

に関して前の式を連立すると最小化すべき式として







x

₁

y

₁

x

₂

y

₂

...

y

_N







−







X

₁

Y

₁

Z

₁

1 0 0 0 0 −x

₁

X

₁

−x

₁

Y

₁

−x

₁

Z

₁

0 0 0 0 X

₁

Y

₁

Z

₁

1 −y

₁

X

₁

−y

₁

Y

₁

−y

₁

Z

₁

X

₂

Y

₂

Z

₂

1 0 0 0 0 −x

₂

X

₂

−x

₂

Y

₂

−x

₂

Z

₂

0 0 0 0 X

₂

Y

₂

Z

₂

1 −y

₂

X

₂

−y

₂

Y

₂

−y

₂

Z

₂

...

0 0 0 0 X

_N

Y

_N

Z

_N

1 −y

_N

X

_N

−y

_N

Y

_N

−y

_N

Z

_N







P ˆ

を得る．

•

したがって，キャリブレーション問題は下記の最小

2

乗問題となる

min x ky − M xk

²

(31)

キャリブレーションの例

•

透視投影モデル

:

s

_i







x

_i

y

_i

1







= P

^W

T

_O





O

r

_i

1





, P = A(

^W

T

_C

)

⁻¹

•

_{カメラ内部パラメータ}

A =







f

_x

α c

_x

0 0 f

_y

c

_y

0 0 0 1 0







= [B 0]

•

カメラ外部パラメータ

(

^W

T

_C

)

⁻¹

=

^C

T

_W

=





C

R

_W ^C

t

_W

0 0 0 1





•

カメラモデル

P = A(

^W

T

_C

)

⁻¹

=

^·

B

^C

R

_W

B

^C

t

_W^¸

(32)

数値例

•

観察対象

:

一辺

80cm

平面，観察点数

64

•

観察条件

:

画素整数，分散

1.5 pixel

のガウス分布ノイズ

•

カメラパラメータ

B = tc = 1.0e+03 * Rc =

-300 0 1 0.0510 0.9820 -0.0600 0.1790

0 -300 -2 -0.1571 0.0046 0.9555 0.2950

0 0 1 -1.0693 -0.1888 -0.2889 0.9386

•

カメラモデル

P = Phat =

0.2757 -0.0166 0.0493 15.2973 0.2762 -0.0170 0.0000 15.2083 0.0009 0.2675 0.0845 -46.0803 0.0010 0.2674 0.0000 -46.7265

0.0002 0.0003 -0.0009 1.0000 0.0002 0.0003 0 1.0000