Microsoft PowerPoint - ロボットの運動学forUpload'C5Q [互換モード]

ロボットの運動学

ロボットの運動学

順運動学とは

順運動学とは

座標系の回転と並進

同次座標変換行列

Denavit-Hartenbergの表記法

多関節ロボットの順運動学

多関節ロボットの順運動学

レポート課題

＆中間試験について

逆運動学とは

ヤコビアン行列

ロボットの運動学・動力学

(

)

P

θ

( )

θ

( )

θ

( )

θ

( )

Equation of motion ( ( ), ( ), ( ))

f θ



t

θ



t

θ

t

=

τ

( )

t

運動方程式

(微分方程式)

姿勢

(関節角の組合せ)

Posture ( )

θ

t

=

(

θ

₁

( )

t

θ

₂

( )

t

"

θ

_N

( )

t

)

(

)

Tip position ( )

p

t

=

x t

( )

y t

( )

z t

( )

姿勢

(関節角の組合せ)

手先位置

(順)

運動

学

Kinematics

θ

⇒

p

姿勢

向

を求める(関節角)から

手先位置・方

逆

運動

学

Inverse kinematics

p

⇒

θ

(目標とする)

手先位置・方向

を 実現する

姿勢

(関節角)を求める

運動

_{(軌道)計画}

(順)

動力

学

_τ

_{( )}

_t

⇒

_θ

_{( )}

_t

_{運動方程式}

_を解いて

_トルク

_{パタ ン}

運動

_{(軌道)計画}

(順)

動力

学

Dynamics

逆

動力

学

( )

t

⇒

( )

t

τ

θ

運動方程式

を解いて

トルク

パターン から

姿勢変化

(

時間変化

)を求める

( )

t

⇒

( )

t

θ

τ

(目標とする)

姿勢変化

を実現する

シミュレーション

逆

動力

学

Inverse Dynamics

θ

( )

t

⇒

( )

τ

t

(目標とする)

姿勢変化

を実現する

トルク

パターン(

時間関数

)を求める

制御

ロボットの順運動学

順運動学とは，ロボットの

各関節の変位量

が 与えられたとき，ロボットの

手先の位置と方向

が

基準座標系

基準座標系

から見てどのようになるかを求める 問題である． [1] 回転運動に伴う座標変換 座標変換

O x y z

O x y z

[2] 並進運動を伴う座標変換 座標変換行列

R

1 1 1

O x y z

−

O x y z

−

_{2 2 2} [ ] 並進運動を伴う座標変換 同次座標変換行列

T

1 2

( )

z

T

30

θ

=

D 2 1

y

2

y

2 y 0 5

( )

1

x

y

z

⎛ ⎞

⎜ ⎟

=

_{= ⎜ ⎟}

⎜ ⎟

p

u

2

( )

( ) z θ R 2 1

z

z

1 2

y

2

x

2

O

1 y 0 0.86 0.5 θ p [3] 多関節ロボットの座標変換

( )

1

1

⎜ ⎟

_{⎜ ⎟}

z

⎝ ⎠

x

O

2

y

1

y

θ 1 2 1

x

1

O

2

O

1 x x2 [3] 多関節ロボットの座標変換 0 0 1 1 1

( )

1 2

( )

2

( )

n n

q

q

n

q

n −

=

T

T

T

"

T

1

x

2

x

θ

例１

手先位置の計算

関節の回転角から手先位置を求める 1 1 2 1 2 1 1 2 1 2

cos

cos(

)

sin

sin(

)

e e

e

l

l

l

l

x

y

θ

θ θ

θ

θ θ

+

+

⎛

⎞ ⎛

⎞

=

_⎜

_{⎟ ⎜}

=

_⎟

+

+

⎝

⎠

⎝

⎠

p

平面

₂

リンクの場合

1

sin

1 2

sin(

1 2

)

e

l

l

y

_⎝

θ

+

θ θ

+

_⎠

⎝

⎠

3 1 2 2

2

2.23

e

x

⎛

⎞

⎛

⎞

× +

_⎛

_⎞

⎜

⎟

⎜

⎟

y

2 2 3 1 2 2

2.23

1.87

2

e e

x

y

⎛

⎞

_⎛

_⎞

= ⎜

⎟

⎜

⎟

_⎜

_⎟

⎜

_{⎟ ⎜}

_{× +}

_⎟

_⎝

_⎠

⎝

_{⎠ ⎝}

_⎠



意味を考えると

2

y

e

p

1 3 2 2

2

e

x

⎛

⎞

⎛ ⎞

⎛

⎞

_×

⎜ ⎟

⎜

⎟

⎜

⎟

意味を考えると 2

1

l

=

2

30

θ

=

D 2 3 1 2 2

2

e e

y

=

⎜

⎟

+ ⎜ ⎟

⎜

⎟

⎜

⎟ ⎜

_×

⎟ ⎜ ⎟

⎝

⎠ ⎝

⎠ ⎝ ⎠

O

から

p

移動

p

から

p

へ移動

1

2

l

=

1 2

60

θ θ

+

=

D

cos(

)

l

θ θ

+

p

₀

3 1 3 3 2 2 2 2

2

⎛

⎞

⎛

⎞

−

⎛ ⎞

=

_⎜

_⎜

_⎟

_⎟

+ ⎜

⎟

_{⎜ ⎟}

_{⎜ ⎟}

⎜

⎟

O

から

p

₀ へ移動

p

₀ から

p

_e

へ移動 1

2

l

=

1

30

θ

=

D 2

cos(

1 2

)

l

θ θ

+

1 _{1 3} 1 2 _{2 2} 2

2

=

_⎜

_⎜

_⎟

_⎟

+ ⎜

⎟

_{⎜ ⎟}

_{⎜ ⎟}

⎜

⎟

⎝

⎠

_⎝

_⎠

⎝ ⎠

30度の回転

1

2

1

30

θ

x

1

cos

1

l

θ

O

例１

つづき

各関節の座標系から見たリンク先端位置座標を組合わせて基準座標系における手先位置を得る

⎛

⎞

2

1

y

2

p

0

3

1

⎛

⎞

= ⎜ ⎟

⎝

⎠

p

座標系

O

₁

-x

₁

y

₁から 見た

O

₂の位置

O

₁

x

₁

y

_1から

O

₂

x

₂

y

₂への

平行移動

2

30

D 2

y

p

2 3 2 1

2

⎛ ⎞

= ⎜ ⎟

p

座標系

O

₂

-x

₂

y

₂から

O

₁

-x

₁

y

_1から

O

₂

-x

₂

y

₂への

平行移動

(

)

3 2 3 1 1 2

cos

sin

i

30

30

30

30

⎛

₋

⎞

−

=

_{= ⎜}

_⎜

_⎟

_⎟

R

1

O

30

2

x

p

1

p

2

⎜ ⎟

⎝ ⎠

12

p

見た

p

₂の位置

(

)

1 3 2 2

sin

30

cos

30

⎜

⎜

⎟

⎟

⎝

⎠

R

2

O

30

D 0

p

座標系

O

₁

-x

₁

y

_{1から見た座標系}

O

₂

-x

₂

y

₂の

回転

平行移動と回転を

変換 表す

( )

0

( )

2

=

T

u

=

R

p

p

u

1

2

x

₁

30

1

O

平行移動と回転を１つの変換で表す

( )

( )

1

=

T

u

2

=

0

1

1

u

3 2 −1 2 3 _{3 2}

⎛

3 1 2+

⎞

_2.23

⎛

_{⎞ ⎛}

_⎞

_⎛

_⎞

⎛

_p

⎞

_⎜

_{⎟ ⎜}

_⎟

_⎜

_⎟

1 2 3 2 1 0 0 1 1 3 2 1 1 _{1 2}3 2 1

1

2.23

1.87

1

1

+

⎛

⎞

_⎛

_⎞

⎛

⎞

_⎜

_{⎟ ⎜}

_⎟

_⎜

_⎟

⎜

⎟

=

_⎜

_⎟

_⎜

_⎟

=

_⎜

_⎟

⎜

⎟

⎜

⎟

⎜

⎟

⎜

⎟

⎝

⎠

=

_⎝

_⎠

_⎝

_⎠

_⎝

_⎠

_⎝

_⎠

p

u



i

θ

θ

⎛

⎛ ⎞

⎞

⎛ ⎞

座標系の回転

2次元の例

2

y

1 2 1 2

cos

sin

sin

cos

x

x

y

y

θ

θ

θ

θ

⎛

⎛ ⎞

⎛ ⎞

=

⎜ ⎟

⎜ ⎟

⎝ ⎠

⎟

−

⎠

⎝

⎞

⎜

⎠

⎝

2

y

0 87

1

⎛

⎞

⎛ ⎞

1 2

y

y

⎝ ⎠

⎝

⎠

⎝ ⎠

1

p

_θ

_回転

p

₂ 1

y

0

0.5

1

1 2

0.87

0.5

1

0

⎛

⎞

⇒

=

⎛ ⎞

= ⎜ ⎟

⎝

⎠

⎜

⎝

⎠

⎟

p

p

3

⎛ ⎞

⎛

⎞

30

θ

= °の場合

0.87

θ

p

3 2 3 1 2 2 1 2

₂

3

1

2

0.87

0.5

1

0

1

−

⎛ ⎞ ⎛

⎞

=

⎛ ⎞

_{⎜ ⎟}

=

⎝

⎛

⎞

⎜

⎟

⎜ ⎟ ⎜

_⎟

⎜ ⎟ ⎝

⎠

⎝ ⎠

⎝

⎠

⎠

p



2

y

1

x

x

2 2 2

⎝

⎝ ⎠

₂

⎝

⎠

⎝

⎠

⎠

2 1

0

1

=

⎛

⎞

⎛ ⎞

⎜ ⎟

⎝ ⎠

p

1

y

p

3 2 3 1 2 2 1 2 1 ₃ 2 1 2

0.5

0.87

0

1

=

⎛

⎜

−

⎞

⎟

=

⎛

_⎜

⎜

−

⎟ ⎜

_{⎟ ⎝}

⎞

=

⎛

−

⎞

_⎟

⎠

⎝

⎠

⎝

⎠

⎛ ⎞

⎜ ⎟

⎝ ⎠

→

p

θ

2 2

⎝

2

⎠

⎝

⎠

1

x

x

2 移動後の座標系で与えられた位置が

基準座標系から見てどの位置になるか

を座標変換行列で計算する

演習

座標系の回転

回転後の座標系で与えられた位置

_{基準座標系から見てどの位置になるか}

が 座標変換行列を 計算 なさ

y

1 2 1 3

?

⎛ ⎞ = ⎜_⎜ ⎟_⎟ ⎝ ⎠ ⇒

p

=

p

30

θ

= °

の場合

座標変換行列を用いて計算しなさい 2

y

0

p

p

2 ⎜_⎝ 3⎟_⎠

p

1 3 2 − 12

⎛

1

⎞

₂3 ₂3

0

⎜

⎟

⎜

⎟

⎛

⎞ ⎛ ⎞

=

⎛

_⎜

⎞

_⎟

=

_⎜

−

_{⎟ ⎜ ⎟}

=

p

場合

1

y

0

θ

p

?

3 1 2 2 3 1 2 2

3

2

1

=

⎜

_⎝

_⎠

⎟

⎜

_⎝

⎜

_⎠

⎟

⎟

=

⎜

_⎝

₊

⎟ ⎜ ⎟

_⎠

=

_⎝

_⎠

p

2 ⎛ ⎞ 2

x

1 2 2 0

?

⎛ ⎞ = ⎜ ⎟ ⎝ ⎠ ⇒

p

=

p

45

θ

=

°

の場合

2

y

1

x

2 まず

₄₅

○_{回転の行列を求める} 2 2 2 2

cos 45

° −

sin 45

°

⎛

−

⎞

⎛

⎞

⎜

⎟

⎜

_{⎟ ⎜}

=

_⎟

1

y

?

まず

₄₅

○_{回転の行列を求める} 2 2 2 2 2 2

2

2

0

2

−

⎛

×

⎞ ⎛

⎞

⎛

⎞

⎛ ⎞

−

2 2 2

2

sin 45

°

cos 45

°

⎜

⎟

⎜

_{⎟ ⎜}

_⎟

⎝

_{⎠ ⎝}

_⎠

θ

p

?

2 2 2 2 2 2 2 2 2

2

0

2

2

0

2

2

0

1

×

⎛

⎞ ⎛

⎞

=

=

_⎜

_⎜

_{⎟ ⎜}

=

_⎟

⎟ ⎝ ⎠

⎝

⎛

⎞

⎜

⎟

⎝

⎠

⎛

×

−

⎠

_⎠

⎞

⎜ ⎟

⎝

p

1

x

x

2

p

?

座標変換行列

R

任意軸

転

⎛ ⎞ ⎛

x

cos

θ

−

sin

θ

0

⎞⎛ ⎞

x

z

軸周りに

_θ回転

2 1 rx (1 C ) C r rx y(1 C ) r Sz r rx z(1 C ) r Sy 2 x ⎛ − θ + θ − θ − θ − θ + θ ⎞ x ⎛ ⎞ _{⎜ ⎟}⎛ ⎞ ⎜ ⎟ ⎜ ⎟

任意軸

r

周りに

_θ回転

1 2 1 2 1 2

cos

sin

0

sin

cos

0

0

0

1

x

x

y

y

z

z

θ

θ

θ

θ

⎛ ⎞ ⎛

⎞⎛ ⎞

⎜ ⎟ ⎜

₌

⎟⎜ ⎟

⎜ ⎟ ⎜

⎟⎜ ⎟

⎝ ⎠ ⎝

⎠⎝ ⎠

2 2 1 2 1 2 where (1 ) (1 ) (1 ) (1 ) (1 ) (1 ) cos , sin x y z y y z x x z y y z x z r r C r S r C C r r C r S r r C r S r r C r S r C C y y z z C S θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ − + − + − − − − − + − + ⎜ ⎟ ⎜ ⎟ _{= ⎜ ⎟}⎜ ⎟ ⎜ ⎟ _{⎜ ⎟}⎜ ⎟ ⎝ ⎠ _{⎝ ⎠}⎝ ⎠ = =

⇒

p

₁

=

R

_z

( )

θ

p

₂ θ θ x

r

⎛ ⎞

⎜ ⎟

z

1

0

_x

( )

θ

⎛ ⎞

=

_{⎜ ⎟}

⇒

R

r

⎛ ⎞ ⎛

x

1

1

0

0

⎞⎛ ⎞

x

2

⎜ ⎟ ⎜

⎟⎜ ⎟

x

軸周りに

_θ回転

x y z

r

r

⎛ ⎞

= ⎜ ⎟

_{⎜ ⎟}

⎝ ⎠

r

θ

0

x

( )

⎜ ⎟

⎝ ⎠

0

1

( )

θ

⎛ ⎞

=

_{⎜ ⎟}

⇒

R

r

1 2 1 2

0 cos

sin

0 sin

cos

( )

y

y

z

z

θ

θ

θ

θ

θ

⎜ ⎟ ⎜

₌

₋

⎟⎜ ⎟

⎜ ⎟ ⎜

⎟⎜ ⎟

⎝ ⎠ ⎝

⎠⎝ ⎠

R

x

y

θ

1

0

y

( )

θ

=

_{⎜ ⎟}

⇒

⎝ ⎠

R

r

0

0

( )

θ

⎛ ⎞

=

_{⎜ ⎟}

⇒

R

r

⇒

p

₁

=

R

_x

( )

θ

p

₂

y

軸周りに

_θ回転

x

0

1

z

( )

θ

=

_{⎜ ⎟}

⇒

⎝ ⎠

R

r

1 2 1 2

cos

0 sin

0

1

0

x

x

y

y

θ

θ

⎛ ⎞ ⎛

⎞⎛ ⎞

⎜ ⎟ ⎜

₌

⎟⎜ ⎟

⎜ ⎟ ⎜

⎟⎜ ⎟

y

1

sin

0 cos

2

_y

( )

z

θ

θ

z

θ

⇒

⎜ ⎟ ⎜

⎟⎜

⎝

−

⎟

⎝ ⎠ ⎝

⎠

⎠

=

1 2

p

R

p

R

y だけ

sin

の符号が違うことに注意

並進を伴う座標変換

y

₁

y

2

y

1

O

1

(3)

3.5

1

⎛ ⎞

0.5

2

O

1

1

3

⎛ ⎞

= ⎜ ⎟

⎝ ⎠

0

p

1

x

θ

1

p

₁

0

⎛ ⎞

= ⎜ ⎟

⎝ ⎠

2

p

2

x

0.87

1.87

1 0.87 3 0.5 ⎛ ⎛ ⎞ ⎛ ⎞⎞ = + ⎜ _{⎜ ⎟ ⎜ ⎟}⎟ ⎝ ⎠ ⎝ ⎠ ⎝

p

1 ⎠

=

+

1

2

0

p

Rp

p

3 3 1 2 2 1 0 _{1 1 0.87 1 1.87} − ⎛ _{⎞⎛ ⎞ ⎛ ⎞ ⎛ ⎞ ⎛ ⎞ ⎛ ⎞} ⎜ _{⎟⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟} ⎜ ⎟ 2

x

3 0.5 ⎝ ⎠ ⎝ ⎠ ⎝ ⎠ 単純な掛算で表したい 3 2 1 2 0 0 0 1 0 3 0.5 3 3.5 0 0 0 0 0 ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ = _{⎜ ⎟ ⎜ ⎟ ⎜}+ = _{⎟ ⎜ ⎟ ⎜}+ = _⎟ ⎜ _{⎟⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟} ⎜ _{⎟⎝ ⎠ ⎝ ⎠ ⎝ ⎠ ⎝ ⎠ ⎝ ⎠} ⎝ ⎠ 1

p

p

(30)

R

p

(１つの変換行列)

⎛ ⎞ ⎛

p

₁

R p

₀

⎞⎛ ⎞

p

₂ 0

p

(30)

₂

R

_z

p

0

1

1

1

⎛ ⎞ ⎛

⎞⎛ ⎞

=

⎜ ⎟ ⎜

⎟⎜ ⎟

⎝ ⎠ ⎝

⎠⎝ ⎠

1 0 2

p

p

p

同次座標変換行列

1 2 1 2 1 2 x x y y ⎛ ⎞ ⎛ ⎞ ⎜ ⎟ ⎜ ⎟ ⎛ ⎞ _{⎜ ⎟} ⎛ ⎞ _{⎜ ⎟} = _{⎜ ⎟} = = _{⎜ ⎟} = ⎜ ⎟ ⎜ ⎟

p

p

u

u

3 2 3 1 2 2 1 3 1 ₂ 2 2 0 1 1 1 1.87 0 3 3.5 0 3 − ⎛ ⎞_{⎛ ⎞ ⎛ + ⎞ ⎛ ⎞} ⎜ ⎟_{⎜ ⎟ ⎜ ⎟ ⎜ ⎟} ⎜ ⎟_{⎜ ⎟ ⎜ + ⎟ ⎜ ⎟} = _{⎜ ⎟} = _{⎜ ⎟} = ⎜ ⎟ ⎜ ⎟

u

1 2 , 1 1

1

= _⎝⎜

₁

⎟_⎠ = _{⎜ ⎟z}

2

= ⎜_⎝

₁

⎟_⎠ = _{⎜ ⎟z} ⎜ ⎟ ⎜ ⎟ ⎝ ⎠ ⎝ ⎠

u

u

=

2 2 1 _{0 0 0} 0 0 1 0 0 0 0 1 1 1 1 ⎜ ⎟ ⎜ ⎟ = _{⎜ ⎟} = _{⎜ ⎟} = ⎜ ⎟ ⎜ ⎟ ⎜ ⎟_{⎜ ⎟ ⎜ ⎟ ⎜ ⎟} ⎜ ⎟_{⎝ ⎠} ⎜ ⎟ _{⎝ ⎠} ⎝ ⎠ ⎝ ⎠

u

同次座標変換

行列

T

2

u

=

位置座標に形式的に

１

を加える

同次座標変換

行列

T

1

y

O 3.5 1 (3) 1

z

3 1 2 2 0 1 1 ⎛ ₋ ⎞ ⎜ ⎟ ⎜ ⎟ 2

y

1 O 2 O 1 3 0 ⎛ ⎞ = ⎜ ⎟_{⎜ ⎟} ⎝ ⎠ 0 p 3 1 2 2 0 3 0 0 1 0 0 0 0 1 1 2 ⎜ ⎟ = ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎝ ⎠

T

1

x

0.5 θ 2 O 1 2 1 0 0 ⎛ ⎞ = ⎜ ⎟_{⎜ ⎟} ⎝ ⎠ p 1 1.87 3.5 0 ⎛ ⎞ = ⎜_⎜ ⎟_⎟ ⎝ ⎠ p 座標系

O

2

-x

2

y

2

z

2

から 座標系

O

₁

-x

₁

y

₁

z

₁

への ⎝ ⎠ 1 2

x

0.87 1.87 ⎝ 0 ⎠ ⎝ ⎠0 座標系

O

1

-x

1

y

1

z

1

への 同次座標変換行列

多関節ロボットの座標変換

座標系

O

基準座標系

O

座標変換行列

0

T

座標系

O

_n

-x

_n

y

_n

z

_nから

基準座標系

O

₀

-x

₀

y

₀

z

_0への

座標変換行列

0

T

_n 各関節間の座標変換行列の積 0 0 1 1 1

( )

1 2

( )

2

( )

n n

q

q

n

q

n −

=

T

T

T

"

T

0 0 ( 90) ( 90) ( 90) ( 90) int 90 1 0 0 0 0 cos sin 0 1 0 sin cos 0 0 0 1 x z l _l θ − − − − − = =− ⎛ ⎞ ⎜ ⎟ = ⎜_⎜ ⎟_⎟ ⎝ ⎠

T

0 0 0 1 1 0 0 0 0 0 1 0 _{0 1 0} x l ⎝ ⎠ ⎛ ⎞ ⎜ ⎟ = ⎜ − ⎟ ⎜0 1 0 0 ⎟ 0 0 0 1l ⎜ ⎟ ⎜ ⎟ ⎝ ⎠ 2 2 2 2 90 90 1 cos sin 0 0 1 0 0 0 0 cos sin 0 sin cos 0 0 z l θ θ θ θ = − − ⎛ ⎞⎛ ⎞ ⎜ ⎟⎜ ⎟ = ⎜ ⎟⎜ ⎟

T

1 2 2 2 2 2 1 _{90 90} 90 2 0 0 1 0 0 sin cos 0 0 0 1 0 0 0 1 cos sin 0 0 1 0 0 0 0 0 1 0 i z x z l l θ θ θ θ θ = = = − = ⎜_⎜ ⎟⎜_⎟⎜ ⎟_⎟ ⎝ ⎠ ⎝ ⎠ ⎛ ⎞⎛ ⎞ ⎜ ⎟⎜ ⎟

T

1 2 2 0 0 1 0 sin cos 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 = ⎜⎜ θ θ ⎟⎜_⎟⎜ − _l ⎟_⎟ ⎜ ⎟ ⎜ ⎟_{⎝ ⎠} ⎝ ⎠ cosθ 0 sinθ 0 ⎛ ⎞ 1 2 2 2 2 cos 0 sin 0 sin 0 cos 0 0 1 0 0 0 0 1 l θ θ θ − θ ⎛ ⎞ ⎜ ⎟ = ⎜ ⎟ ⎜ ⎟ ⎝ ⎠

多関節ロボットの座標変換

ｰ同次変換行列の別誘導法ｰ

前頁

1

は次

よう 求める とも きる

2 2 cos 0 sin 0 1 0 0 0 0 cos sin 0 0 1 0 0 1

_T

_{θ θ}

⎛

⎜

90 − 90

⎞

⎟

⎜

⎛

θ θ

⎞

⎟

前頁の

1

_T

₂

は次のように求めることもできる

2 2 1 1 2 90 0 cos sin 0 0 1 0 0 1

2 0 sin cos sin 0 cos 0 0 0 0 1 0 0 0 1 0 i 0 1 0 0 0 90 90 90 90 y x z l

l

θ θ θ θ θ θ θ = = = −

⎜

⎟

⎜

⎟

=

_⎜

_⎟

_⎜

_⎟

⎜

⎟⎜

⎟

⎝

⎠⎝

⎠

⎛

⎞

⎛

⎞

T

1 2 2 2 2 cos 0 sin 0 1 0 0 0 0 0 1 0 0 1 0 0 0 1 0 sin 0 cos 0 0 0 0 1 0 0 0 1

_l θ θ θ θ − −

⎛

⎞

⎛

⎞

⎜

⎟

⎜

⎟

=

_⎜

_⎟

_⎜

_⎟

⎜

⎟⎜

⎟

⎝

0 0 0 1

⎠⎝

0 0 0 1

⎠

⎝

⎠⎝

⎠

2 2 2 2 cos 0 sin 0 sin 0 cos 0 θ θ θ − θ

⎛

⎞

⎜

⎟

= ⎜

⎟

1 0 1 0 0 0 0 l1

= ⎜

⎟

⎜

⎟

⎝

⎠

どんな順序で座標系の変換を行 たと考えるかで

どんな順序で座標系の変換を行ったと考えるかで

誘導は異なるが，結果は同じ

演習

極座標型ロボットの運動学

θ

x

2

_r

1

z

e

p

1. O

₂

-x

₂

y

₂

z

₂から

O

₁

-x

₁

y

₁

z

₁への変換行

手順

0

z

2

z

1

x

h

d

. O

_列21

_T

x

2

y

2 2から

O

1

x

1

y

1 1 の変換行 2 (y 方向へ d 平行移動＋ y 軸 周りにφ回転),

O

₁

-x

₁

y

₁

z

₁から

O

₀

-φ

y

2 1

y

h

x

₀

y

₀

z

₀へとするの変換行列

0

T

₁

(

z

方 向へ

h

平行移動＋

z

軸周りにθ回転) 1. 座標系

O

₂

-x

₂

y

₂

z

₂から基準座標系 0

x

0

y

を求め，

T

=

0

T

₁1

T

₂ とする.

2. T

に

h=3

,

θ

=

30

,

d=2

,

φ

=

270

,

r=5

O

₀

-x

₀

y

₀

z

₀への座標変換行列

T

を 求めよ． 2 O₀からO₁への平行移動量

h=3 z

₀ を代入して

T

を具体的に数値で表し，

O

₂

-x

₂

y

₂

z

₂から見た手先位置の座標

(0 0 )

から作 た

p

(0 0

r

1)

T 2. O₀からO₁への平行移動量

h 3

,

z

₀ 軸回転角

θ

=

30

°, O₁からO₂への 平行移動量

d=2

,

y

₁軸回転角

(0, 0, r)

から作った

p

₂

= (0 0

r

1)

T を掛けて

p

_e

=

T p

₂を得る．

y

₁

φ

=

270

°,

r=5

のとき，

O

₀

-x

₀

y

₀

z

₀ から見た

p

_eの座標を求めよ．

演習 極座標型ロボットの運動学 (解)

1 変換行列1

T

0

T

は それぞれ 1. 変換行列1

T

₂

,

0

T

₁ は，それぞれ

cos

sin

0 0

sin

cos

0 0

0

0

0

1

1

h

_h

θ

θ

θ

−

θ

⎛

⎞

⎜

⎟

= ⎜

⎟

T

cos 0 sin 0 0 1 0 i 0 0 1 2 d d

φ

φ

φ

φ

⎛

⎞

⎜

⎟

= ⎜

⎟

T

₀

₀

₁

1

1

0

0

0

z z h

_h

θ θ==

⎜

⎜

_⎝

⎟

⎟

_⎠

sin 0 cos 0 2 0 0 0 1 y y d θ φ==

⎜

⎜

_⎝

−

φ

φ

⎟

⎟

_⎠

T

よって

_⎛

_{cos cos}

_θ

_φ

_−sin

_θ

_{cos sin}

_θ

_φ

_−dsin

_θ

_⎞

_⎛

_{c sθ φ −sθ c sθ φ −d sθ}

_⎞

⎜

⎟

0 1 1 2 c s c s s c s 0 c 0 0 0 1

cos cos sin cos sin sin

cos sin cos sin sin cos

sin 0 cos 0 0 0 1 d h d d h θ φ θ θ φ θ φ θ θ θ φ θ φ φ

θ

φ

θ

θ

φ

θ

φ

θ

θ

θ

φ

θ

φ

φ

− −

⎛

⎞

⎛

⎞

⎜

⎟

⎜

⎟

=

=

_⎜

_⎟

_{= ⎜}

_⎟

⎜

_{⎟ ⎜}

_⎟

⎝

_{⎠ ⎝}

_⎠

T T

T

0 0 0 1 0 0 0 1

⎝

_{⎠ ⎝}

_⎠

2. T

に

h=3

, θ=

30

,

d=2

, φ=

270

,

r=5

を代入すると よく行われる略記法 3 1 ⎛ ⎞

θ

_r

2

z

2

x

1

z

e

p

d

なので

p

₂= (0 0

5

1)T を掛けて 3 1 2 2 3 1 2 2 0 1 0 3 1 0 0 3 − − − − ⎛ ⎞ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟

=

T

φ

0

z

2 2

y

1

x

1

y

h

d

0 0 0 1 ⎜ ⎟ ⎜ ⎟ ⎝ ⎠ 5 3 2 5 3 1 2 2 3 1 0 1 0 3 1 3

0

5.33

0

0 77

− − −

⎛

− −

⎞

⎛

_{⎞⎛ ⎞}

_⎛

_⎞

⎜

⎟

⎜

_{⎟⎜ ⎟}

_⎜

₋

−

_⎟

T

φ

0

x

0

y

5 2 3 1 2 2 0 3 1 0 0 3 0 0 0 1 2 ₃ 3 1

0

0.77

5

1

3

1

e − − +

⎜

⎟

⎜

_{⎟⎜ ⎟}

_⎜

_⎟

= ⎜

⎟

⎜

_{⎟⎜ ⎟}

_⎜

_⎟

⎜ ⎟

_⎜

_⎟

⎜

⎟

⎜

_{⎟⎝ ⎠}

_⎝

_⎠

⎝

⎠

⎝

⎠

=

=



p Tp

DH

法

法

－

Denavit-Hartenberg

g

の表記法

表

ー

4つの変数(

リンクパラ メータ

_{)で多リンク機構を系統的に表現}

リンク座標系

_{∑ を ∑}

から見た場合の

同 次

変換で記述する

リンク座標系

_{∑i を ∑i-}

₁

から見た場合の

同 次

変換で記述する

リンクパラメータ

変換行列

リ ク

ラ

タ

(i)

x

_i

軸 に沿って

a

_i-1

だけ

並進

(ii)

x

軸回りに

α

だけ

回転

(

1 1

)

T

X

i−

,

a

i−

T

変換行列

(

)

R

X

α

T

(ii)

x

_i-1

軸回りに

α

_i-1

だけ

回転

(iii)

z

_i

軸 に沿って

d

_i

だけ

並進

(

1 1

)

R

X

i−

,

α

i−

T

(

)

T

Z d

i

,

i

T

(

Z

θ

)

T

(iv)

z

_i

軸回りに

θ

_i

だけ

回転

T

_R

(

Z

i

,

θ

i

)

長さ

a

_i-1

の線分は，

関節

_{i-1 の回転軸にも，}

関節

_{i の回転軸にも直交する}

DH法による変換行列

(

1 1

)

1 T 1 0 0 0 1 0 0 0 0 1 0

,

i i i a

X

a

− − −

⎡

⎤

⎢

⎥

=

⎢

⎥

T

(

)

1 1 1 1 1 1 R 1 0 0 0 0 cos sin 0 0 sin cos 0

,

i i i i i i

X

α

_α

α

−

_α

α

− − − −

⎡

⎤

⎢

⎥

=

⎢

⎥

T

0 0 1 0 0 0 0 1

⎢

⎥

⎣

⎦

⎣

⎢

0 sin0 0

α

i−1 cos0

α

i−1 10

⎥

⎦

(

)

⎡

_⎢

1 0 0 00 1 0 0

⎤

_⎥

(

)

_⎢

⎡

cossin

θ

θ

i −cossin

θ

θ

i 0 00 0

⎤

_⎥

(

)

T 0 1 0 0_{0 0 1} 0 0 0 1

,

i i i d

Z d

=

⎢

⎥

⎢

⎥

⎣

⎦

T

_R

(

)

sin₀ cos_{0 1 0}0 0 0 0 0 1

,

i i i i

Z

θ

=

⎢

θ

θ

⎥

⎢

⎥

⎣

⎦

T

(

) (

) (

) (

)

1 1 1 R 1 1 T R 1 T

1

0

0

0

0 0

1 0 0

1 0 0 0

,

,

,

,

i i i i i i i i i i i i i

C

S

a

X

a

X

Z d

Z

θ θ

α

θ

− − − − −

−

⎡

⎤

⎡

⎤

⎡

⎤

⎡

⎤

=

T T

T

T

T

1 1 1 1 1

0 0

0 0 0

0 1 0 0

0

0

0 1 0 0

0 0

0 0 1 0

0

0

0 0 1

0

0

1 0

0 0 0 1

0

0

0

1

0 0 0 1

0

0

0 1

i i i i i i i i i i

a

C

S

S

C

S

C

d

θ θ α α θ θ α α − − − − −

−

⎡

⎤

⎡

⎤

⎡

⎤

⎡

⎤

⎢

⎥

⎢

⎥

⎢

⎥

⎢

⎥

=

⎢

⎥

⎢

⎥

⎢

⎥

⎢

⎥

⎣

0 0 0

⎦

⎣

0

0

0

1

⎦

⎣

0 0 0

⎦

⎣

0

0

0 1

⎦

⎣

⎦

⎣

⎦

⎣

⎦

⎣

⎦

1 1 1 1 1

0

i i i i i i i i i i

C

S

a

C S

_α θ _θ

C C

−

_α θ _θ

S

_α

S d

_α−

⎡

⎤

⎢

₋

₋

⎥

⎢

1 1 1 1

⎥

1 1 1 1

0

0

0

1

i i i i i i i i i i i i i i

S S

_αα₋− _θθ

S C

_αα₋− _θθ

C

_αα₋−

C d

_αα₋−

= ⎢

⎥

⎢

⎥

⎣

⎦

ロボットの逆運動学

手先の位置･方向が与えられた時に， それを実現する各関節の変位量を求 める 1 1 2 1 2 1 1 2 1 2

cos

cos(

)

sin

sin(

)

T T

x

l

l

y

l

l

θ

θ θ

θ

θ θ

=

+

+

=

+

+

_演習

例1の設定の場合， める問題 1 1 2

(

1 2

)

T

y

2 2 2 2 1 2

2

1 2

cos

2 T T

x

+

y

= + +

l

l

l l

θ

θ1,θ2はいくらか？

演習

2 2 2 2 1 2 1 2 1 2

cos (

T 2T

)

x y l l l l

θ

₌

− + − −

解は

唯一

ではない

右手系と左手系

_{( ,}

_{x y}

_{T T}

₎

解は

唯

ではない

θ

2

l

2

θ

2

l

1

l

各関節角をどう変化させたら ワークを把持できるか？ 1

θ

1

ヤコビアン行列

(Jacob行列 , Jacobian)

各関節の

回転

速度と

手先

速度の関係を表す行列

= 

ω θ

v

並進速度に関するヤコビアン行列

J

(

_J

_J

)

v

_J

θ

p

_e

= f(

q

)

: 各関節の回転角度と手先位置の関係

並進速度に関するヤコビアン行列

(

)

L1 L2 L

= J

J

J

e

d

∂

_d

_d

=

p

=

f q

≡

=



v

J

q

J q

e

( )

x

y

=

p

v

θ

J

L2 2

θ

J

L L

dt

dt

dt

=

=

≡

=

∂q

v

J

J q

( )

2

θ

L1 1

θ

J

L

=

J

θ



回転関節のみなら⇒

e

p

：基準座標系上での手先位置

(e: end-effecter)

θ

L

q

：ロボットの一般化座標

関節の回転角 (回転関節)

θ

，

可変リンク長 (直動関節)

d

1

θ

(回転関節) (直動関節)

回転速度に関するヤコビアン行列

A

J

ω = J q

_A

例題

ヤコビアン行列と手先速度

(1) ヤコビアン行列J

_L

を求めよ

(2)

l

₁

=

2,

l

₂

=

1,

θ θ

₁

=

₂

=

30

D

の時の

_J

_L

を計算せよ

(2)

の時の

_J

_L

を計算せよ

(3) (2)の時

,

とすると

手先速度はいくらか

1

30

[deg sec]

θ



=

θ



₂

= −

45

[deg sec] 1

2,

2

1,

1 2

30

l

l

θ θ

手先速度はいくらか

(1) 手先位置p の座標(x y)を

解法手順

y

e

p

(1) 手先位置p

_e

の座標

_(x,y)を

θ

₁

θ

₂

で表す．

x x θ θ ∂ ∂ ⎛ ⎞ ⎜ _{∂ ∂} ⎟

算

θ

2

l

e

p

1 2 1 2 L _{y y} θ θ θ θ ⎜ _{∂ ∂} ⎟ ⎜ ⎟ = ⎜ ∂ ∂ ⎟ ⎜ _{∂ ∂} ⎟ ⎝ ⎠

J

を計算する．

( )

に

を代入する

θ

2

θ

1

l

(2) J

_L

に

を代入する

( )

30

[deg sec]

45

(3)

J

_L

に

θ

=

1 2

30

θ θ

=

=

D

x

1

θ

( )

L

( )

-45

を掛ける

例題

ヤコビアン行列と手先速度

（解答例）

(

)

(

)

1 1 2 1 2 1 1 2 1 2

cos

cos

e

=

⎛ ⎞

⎜ ⎟

x

_y

= ⎜

⎛

l

_l

_sin

θ

_θ

+

₊

l

_l

_sin

_{θ θ}

θ θ

₊

+

⎞

⎟

⎝ ⎠ ⎝

⎠

p

(1)

(

)

1 1 2 1 2

y

⎝ ⎠ ⎝

⎠

(

)

(

)

1 2 1 1 2 1 2 2 1 2 L

sin

sin

sin

x x

l

l

l

θ θ

θ

θ θ

θ θ

∂ ∂ ⎛ ⎞ _{⎛ ⎞} ⎜ _{∂ ∂} ⎟ _{⎜ ⎟} ⎜ ⎟ = _{= ⎜ ⎟} ⎜ ⎟

−

−

+

−

+

J

(

)

(

)

1 2 1 1 2 1 2 2 1 2

L _{y y}

_l

_cos

_l

_cos

_l

_cos

θ θ

θ

θ θ

θ θ

⎜ ⎟ = _{= ⎜ ⎟} ⎜ ∂ ∂ ⎟ _{⎜ ⎟} ⎜ _{∂ ∂} ⎟ _{⎝ ⎠} ⎝ ⎠

+

+

+

J

3 3 1 2 2 3 3 1 2 2 2 2

1.87

0.87

− ⋅ − − _{− − −}

⎛

_{⎞ ⎛}

_⎞

⎜

⎟

_⎜

_⎟

_⎛

_⎞

⎜

⎟

_⎜

−

−

_⎟

J

(2)

_L 2 2 1 1 2 2 2 3 _{1 1} 2 2 2 2 3 2 2

2.23

0.5

⋅ + +

⎛

⎞

⎜

⎟

=

_⎜

_⎟

=

_⎜

_⎟

_⎜

_⎟

⎝

⎠

⎜

_{⎟ ⎜}

_⎟

⎝

⎠ ⎝

⎠



J

(2)

L 3 3 1 2 2 1 1 17.0 44 5 6

0.3

0 78

[m sec]

180

π

π

π

− − − ₋

⎛

_⎞⎛

_⎞

⎛

⎞

⎛

⎞

⎜

_⎟⎜

_⎟

=

=

_⎜

_⎟⎜

_⎟

=

_⎜

_⎟

_⎜

_⎟

⎝

⎠ ⎝

⎠

⎜

⎟

⎜

⎟

−





J θ

v

(3)

1 1 3 2 2 44.5 4

180

0.78

π

+

⎜

⎜

−

⎟

⎟

⎝

⎠ ⎝

⎠

⎜

_{⎟⎝ ⎠}

_⎠

⎝

ヤコビ行列と特異姿勢

1

⎛ ⎞

(4) 手先速度を

とするためには各関節をどのような

速度

で回転させればよいか？

1

0

⎛ ⎞

⎜ ⎟

⎝ ⎠

=

v

θ

速度

θ

で回転させればよいか？

3 3 1 2 2 L 3 1 2 2 1 1 1

1

₁

1

₄

0.25

− − − −

_⎛

_⎞

⎛

⎞

⎛

_{⎞ ⎛ ⎞}

_{⎛ ⎞}

_⎛

_⎞

⎜

⎟

= ⎜

⎟

=

=

⎜

_{⎟ ⎜ ⎟}

_{⎜ ⎟}

_⎜

_⎟

⎜

⎟

⎜

⎟

_⎜

_⎟



₌

₌

θ J v

3 2 L _{1 3 1} 2 2 4 1 1 2 2 3 1 3 +

0

2

⎜

− − −

⎟

0

− − −

1.12

⎜

_{⎟ ⎜ ⎟}

_{⎜ ⎟}

_⎜

_⎟

⎜

⎟

_{⎝ ⎠}

⎜

⎟

_{⎝ ⎠}

_⎜

_⎟

_⎝

_⎠

⎝

⎠

⎝

⎠

⎝

⎠

も

1

が存在 な 場合

もし

1

が存在しない場合は？

L −

J

1 1 1 1 1 2 L

2sin

sin

sin

sin

sin

180

θ

θ

θ

θ

θ

θ

=

D

⇒

_J

=

⎛

_⎜

−

+

⎞ ⎛

_{⎟ ⎜}

=

−

⎞

_⎟

2 L

1 1 1 1 1

2cos

θ

cos

θ

cos

θ

cos

θ

cos

θ

⎜

₋

₋

⎟ ⎜

₋

⎟

⎝

⎠ ⎝

⎠

180 θ = D

特異姿勢

2 180 θ = 2 0 θ = 1 θ θ1