4

4.1

Fig. 4.1(a)

BASE
HAND = BASE ROBOT ROBOT ARM ARM HAND (4.1)

Fig. 4.1(b)



BASE n s a p
HAND = (4.2)
0 0 0 1

a (approach vector) s (open or slide

vector) o n (normal
vector) BASE ROBOT ARM HAND ROBOT ARM

BOX GRASP

BASE
ROBOT ROBOT ARM ARM HAND = BASE TABLE TABLE BOX BOX GRASP (4.3)

BASE TABLE TABLE BOX BOX GRASP

ROBOT ARM ROBOT
ARM

ROBOT
ARM = ROBOT BASE BASE TABLE TABLE BOX BOX GRASP HAND ARM (4.4)

qi

45
46

n
6 6
HAND
ROBOT 

@ GRASP  - a

@ ARM  
6
@

 "
"""   So
S

 
 
 
S

-  ?
? s
S
BASE 
 -   BOX 6 6
  6*  *
-
""  p S

 ? 
-  S -

HAND
TABLE  BASE

(a) (b)

4.1:

4.2

4.2.1
Fig. 4.2 n
1. i = 1, 2, . . . , n
2. i Oi
3. 0 Oi Oi+1 i n
4. i = 1, 2, . . . , n 1 Oi Oi+1 xi
5. i = 1, 2, . . . , n 1 yi i
6. 0 1 = 0 1
7. n = 0 xn1 On xn
yn n

ai Oi Oi+1
i xi1 xi

i1 i
1. i1 xi1 ai1 ai1
2. ai1 i
ai1 i qi = i )
Trans(ai1 , 0) Rot(i )

i1
Ti = Trans(ai1 , 0)Rot(i ) (4.5)
Intelligent Machine Lab. Meiji univ. 47

xn xn1
6  e
 e On1 0i+1
On C e
C 

C xi 
Ce O  i
y0 3 ai1   
 e

 


6
x1 e O
x2 e 
 :
Oi

e
 1- 2
- Oi1
xi1

O0 , O1 x0 (a) (b)

4.2:

Ci Si ai1

= Si Ci 0 (4.6)
0 0 1

0 T 1 a0 = 0 i1 T i i 1
i i, i + 1, i + 2, . . . , j 1, j

i
T j = i T i+1 i+1 T i+2 j1 T j (4.7)

ROBOT ARM = 0 T n

4.2.1 ( ) Fig.4.3 a0 = 0, a1 =
L1 , a2 = L2

C1 S1 0 C2 S2 L1
0 1
T1 = S1 C1 0 , T 2 = S2 C2 0 ,
0 0 1 0 0 1

C3 S3 L2
2
T3 = S3 C3 0 (4.8)
0 0 1

C123 S123 L1 C1 + L2 C12
ROBOT
ARM = 0 T 3 = S123 C123 L1 S1 + L2 S12 (4.9)
0 0 1
C123 = cos(1 + 2 + 3 )

BASE ARM
ROBOT = Trans(L0 , 0), Hand = Trans(L3 , 0) (4.10)
48

4.3:

BASE BASE
HAND = ROBOT 0 T 3 ARM
Hand

C123 S123 L0 + L1 C1 + L2 C12 + L3 C123

= S123 C123 L1 S1 + L2 S12 + L3 S123 (4.11)
0 0 1

//
2 T 3 = Trans(a2 , 0)Rot(3 ) 0 T 3

0
T 3 = 0 T 2 2 T 3 (4.12)

C12 S12 L1 C1 + L2 C12
0 0
T2 = T 2 Trans(a2 , 0) = S12 C12 L1 S1 + L2 S12 (4.13)
0 0 1

C3 S3 0
2
T3 = Rot(3 ) = S3 C3 0 (4.14)
0 0 1

0 T
2 2 T 3
0 T 2 3 = 0 0 T 3

4.2.2
n
4.4

1. i = 1, 2, . . . , n
2. 0 i i + 1 i n

Intelligent Machine Lab. Meiji univ. 49

3. i zi
4. zi zi+1 zi Oi zi zi+1

5. i = 1, 2, . . . , n 1 Oi zi+1 xi
6. i = 1, 2, . . . , n 1 yi i
7. 0 1
8. On zn1 zn zn xn n
xn1 yn
n
4.5

ai1 zi1 zi
i1 xi1 zi1 zi
di xi1 xi
i zi xi1 xi

ai1 i1 a0 = 0 0 = 0
di i i di i qi = i
di qi = di i i1 i
4.6

1. i1 xi1 ai1 ai1
2. ai1 xai1 i1 bi1
3. bi1 zi1
b
di ci1
4. ci1 zi1
c
i
Trans(ai1 , 0, 0) Rot(x, i1 ) Trans(0, 0, di )
Rot(z, i ) i1 i i1 T i

i1
Ti = Trans(ai1 , 0, 0)Rot(x, i1 )Trans(0, 0, di )Rot(z, i )

Ci Si 0 ai1

C Si C Ci S S di
= (4.15)
S S S C C di
i i C
0 0 0 1

Si = sin i Ci = cos i S = sin i1 C = cos i1 -

(Denabit-Hartenberg)

4.2.3 Puma
Puma Euler
Fig. 4.7 Table 4.1
i
50

4.4:

4.5:

4.6:
Intelligent Machine Lab. Meiji univ. 51

qi = i i1 T i

C1 S1 0 0 C2 S2 0 0

S1 C1 0 0 0 0 1 Lb Ld
0
T1 =
0 , 1T 2 =



0 1 0 S2 C2 0 0
0 0 0 1 0 0 0 1

C3 S3 0 Lc C4 S4 0 Le

S3 C3 0 0 0 0 1 Lf
2
T3 =
0
, 3
T4 =
0 1 0
S C
4 4 0 0
0 0 0 1 0 0 0 1

C5 S5 0 0 C6 S6 0 0

0 0 1 0 0 0 1 0
4
T5 =
S
, 5
T6 = (4.16)
5 C5 0 0

S C
6 6 0 0
0 0 0 1 0 0 0 1

ROBOT ARM ROBOT

ARM = 0 T 6 = 0 T 1 1 T 2 5 T 6

3
T 4 = Trans(a3 , 0, 0)Rot(x, 3 )Trans(0, 0, d4 ) Rot(z, 4 ) (4.17)

0
T 6 = 0 T 3 3 T 6 (4.18)

0 0
T3 = T 3 Trans(a3 , 0, 0)Rot(x, 3 )Trans(0, 0, d4 )

C1 C23 S1 C1 S23 px

S1 C23 C1 S1 S23 py
= (4.19)
S C23 pz
23 0
0 0 0 1
3
T6 = Rot(z, 4 )4 T 6 = Euler(4 , 5 , 6 ) (4.20)
px = C1 (Lc C2 + Le C23 Lf S23 ) (Lb Ld )S1 (4.21)
py = S1 (Lc C2 + Le C23 Lf S23 ) + (Lb Ld )C1 (4.22)
pz = Lc S2 Le S23 Lf C23 (4.23)

Rot(x, 90 )Rot(z, 5 )Rot(x, 90 ) = Rot(y, 5 ) 0 T

3
4 = 0 0 T 4 //

4.3
4.3.1

BASE
TROBOT 0 T n ARM HAND = BASE TABLE TABLE BOX BOX GRASP (4.24)
52

4.7: Puma
Intelligent Machine Lab. Meiji univ. 53

i ai1 i1 di i

1 0 0 0 1 + 0
2 0 90 Lb Ld 2 + 0
3 Lc 0 0 3 + 0
4 Le 90 Lf 4 + 0
5 0 90 0 5 + 0

6 0 90 0 6 + 0

4.1: Puma

0 T n 0 T n q1 , q2 , . . . , qn qi = i
qi = di 0 T n q1 , q2 , . . . , qn

q1 , q2 , . . . , qn 0 T n 0 T n

1.
2.
(1) Puma (2) T 3

1.
2.
3.

4.3.2
atan2(a, b) arctan(c)
tan() = c 90 < 90 atan2(a, b) b + aj 180
54

< 180

atan2(sin , cos ) = (4.25)

atan2(a, b) = atan2(a, b) (4.26)

atan2(a, b) = 180 atan2(a, b) (4.27)
atan2(a, b) = atan2(a, b) 180 (4.28)

atan2(a, b) = atan2(a, b) (4.29)

(b + aj)(d + cj) = (bd ac) + (bc + ad)j Eq.(4.27)

atan2(a, b) + atan2(c, d) = atan2(bc + ad, bd ac) (4.30)

atan2(a, b) atan2(c, d) = atan2(bc + ad, bd + ac) (4.31)

A sin + B cos = C sin( + ) = C cos( ) (4.32)

C = A2 + B 2 (4.33)
= atan2(B, A) (4.34)
= atan2(A, B) (4.35)

4.3.3
BASE ROBOT 0 T 3 ARM HAND =
4.2.1 H
1 , 2 , 3
H

0
T 3 (1 , 2 , 3 ) = ROBOT BASE H
HAND ARM 0 T 3 (4.36)

1 , 2 , 3 0 T
3

nx sx px
0
T 3 = ny sy py (4.37)
0 0 1

4.8

|L1 L2 | p2x + p2y L1 + L2 (4.38)

Intelligent Machine Lab. Meiji univ. 55

4.8:

p2x + p2y = L21 + L22 2L1 L2 cos (4.39)

L21 + L22 p2x p2y

cos = a (4.40)
2L1 L2

0 180 sin = 1 a2 = atan2( 1 a2 , a)

2 = 180 atan2( 1 a2 , a) (4.41)

2 = 180 + atan2( 1 a2 , a) (4.42)

2 = 180 atan2( 1 a2 , a) (4.43)

= 1 = 1

1 p2x + p2y = 0

L2 sin 2 L1 + L2 cos 2
sin =
, cos =
(4.44)
p2x + p2y p2x + p2y

1 = atan2(py , px ) atan2(L2 sin 2 , L1 + L2 cos 2 ) (4.45)

p2x + p2y = 0 L1 = L2 1 2 = 180
3
3 = atan2(ny sx , nx + sy ) 1 2 (4.46)
( Eq.(4.38))
( Eq. (4.43))
56

4.3.4

Euler RPY Puma

(a) Euler

Euler

Euler(, , ) = Rot(z, )Rot(y, )Rot(z, ) (4.47)

nx sx ax 0

ny sy ay 0
Euler(, , ) =
n

(4.48)
z sz az 0
0 0 0 1

, , Eq. (4.47) Eq. (4.48 Rot1 (z, )

C C C S S 0 fx (n) fx (s) fx (a) 0

S C 0 0 0
= fy (n) fy (s) fy (a) (4.49)
S C 0
0
S S C fz (n) fz (s) fz (a)
0 0 0 1 0 0 0 1

fx () = x C + y S (4.50)
fy () = x S + y C (4.51)
fz () = z , = n, s, a (4.52)

a2x + a2y = 0 (2,3)

ay ay
ax S + ay C = 0 = tan = or (4.53)
ax ax

= arctan(ay /ax ) = arctan(ay /ax ) + 180 (4.54)

(1,3) (3,3) (2,1) (2,2)

= atan2(ax C + ay S , az ) (4.55)
= atan2(nx S + ny C , sx S + sy C ) (4.56)

Intelligent Machine Lab. Meiji univ. 57

a2x + a2y = 0 az = 1

= (4.57)
= 90 90 az (4.58)
= atan2(nx S + ny C , sx S + sy C ) (4.59)

(b) RPY

RPY

RPY(, , ) = Rot(z, )Rot(y, )Rot(x, ) (4.60)

nx sx ax 0

ny sy ay 0
RPY(, , ) =
n
(4.61)
z sz az 0

0 0 0 1
Euler Rot1 (z, )

C S S S C 0 fx (n) fx (s) fx (a) 0

0 C S 0
= fy (n) fy (s) fy (a) 0 (4.62)
S C S C C 0 fz (n) fz (s) fz (a) 0


0 0 0 1 0 0 0 1

fx () = x C + y S (4.63)
fy () = x S + y C (4.64)
fz () = z , = n, s, a (4.65)

n2x + n2y = 0 (2,1)

ny ny
nx S + ny C = 0 = tan = or (4.66)
nx nx

= arctan(ny /nx ) = arctan(ny /nx ) + 180 (4.67)

(1,1) (3,1) (2,2) (2,3)

= atan2(nz , nx C + ny S ) (4.68)
= atan2(ax S ay C , sx S + sy C ) (4.69)

58

n2x + n2y = 0 nz = 1

= (4.70)

= 90 nz (4.71)
= atan2(ax S ay C , sx S + sy C ) (4.72)

(c) Puma

Puma 4.2.3
0
T 6 = 0 T 3 3 T 6 (4.73)


0 R1 p1
T3 = (4.74)
0 1


3 R2 0
T6 = Euler(4 , 5 , 6 ) = (4.75)
0 1

 nx sx ax px

R1 R2 p1 ny sy ay py
0
T6 = = 0 T 6

(4.76)
0 1 nz sz az pz

0 0 0 1

(pT1 , 1)T = (px , py , pz , 1)T 0 T 1 T T 0 1

1 (p1 , 1) = T 1 (px , py , pz , 1)
T

Le C23 Lf S23 + Lc C2 px C1 + py S1

Lb Ld = px S1 + py C1 (4.77)
Le S23 Lf C23 Lc S2 pz

p2x + p2y = 0 px S1 + py C1 = Lb Ld = atan2(py , px )

Lb Ld
sin( 1 ) =
a (4.78)
p2x + p2y

 
 

 Lb Ld 
|a| = 
 1 |Lb Ld | p2x + p2y
 (4.79)
 p2x + p2y 

cos( 1 ) = 1 a2

1 = atan2(py , px ) atan2(a, 1 a2 ) (4.80)
Intelligent Machine Lab. Meiji univ. 59

p2x + p2y = 0 Lb = Ld 1
Eq.(4.77) b = px C1 + py S1

Lc C2 = b Le C23 + Lf S23 (4.81)

L c S2 = pz Le S23 Lf C23 (4.82)

(4.81)2 +(4.82)2

L2c = b2 + p2z + L2e + L2f + 2(Le pz + bLf )S23 + 2(Lf pz bLe )C23 (4.83)

p2x + p2y + p2z = 0

L2c b2 p2z L2e L2f

(Le pz + bLf )S23 + (Lf pz bLe )C23 = (4.84)
2

= atan2(Lf pz bLe , Le pz + bLf )

L2c b2 p2z L2e L2f
c = (4.85)
2 (Le pz + bLf )2 + (Lf pz bLe )2

sin(2 + 3 + ) = c (4.86)
 
 L2c b2 p2z L2e L2f 
 
|c| =  1 (4.87)
 2 (Le pz + bLf )2 + (Lf pz bLe )2 

= 2 + 3 = atan2(c, 1 c2 ) (4.88)
Eqs. (4.81), (4.82), (4.88)

2 = atan2(pz Le S Lf C , b Le C + Lf S ) (4.89)
3 = 2 (4.90)

p2x + p2y + p2z = 0 L2c = L2e + L2f = 2 + 3

2 = atan2(Le S Lf C , Le C + Lf S ) (4.91)
3 = 2 (4.92)

Eq. (4.76)


3 0 1 0 RT1 0 R
6 0
T6 = T3 T6 = (4.93)
0 1

3 T 6 = Euler(4 , 5 , 6 )
Eq. (4.48) Eqs. (4.54) -(4.59) 4 , 5 , 6
Eqs.(4.80) (4.88) Puma

Puma
60

(Right Arm) (Left Arm)

(Above Elbow) (Below Elbow)
(Wrist Down) (Wrist Up)

4.9

4.4

(a) D-H

D-H
D-H

D-H i zi
ai1 ai



D-H
x
4.2.3 4.7
D-H

D-H D-H i
i Craig Paul
i i i1 Ti qi1 i
Paul D-H
4.2.3 0 T3

(b)

4.9

360
350 185 5
360 365
Intelligent Machine Lab. Meiji univ. 61

4.9: PUMA

185

4.5
1. 4.10

1 0 x
BASE
T0 = Trans(x, 0) = 0 1 0 ,
0 0 1

0 1 L3
3
T BODY = Trans(L3 , 0)Rot(90 ) = 1 0 0
0 0 1

(a) i1 T i , i = 1, 2, . . . , 5
L1 L2
(b) BASE BODY
TIPTOE
2. 4.11 BBR
(a)
(b)
(c) i1 T i , i = 1, 2, . . . , 6 PUMA 0 T 3 3 T 6
62

4.10:

3. 4.1 L1 = L2 = L3 = 1

1 0 10 1 0 10
BASE BASE
T0 = 0 1 0 , T BODY = 0 1 1+ 3
0 0 1 0 0 1

BASE
(a) T BODY = BASE T 0 0 T 3 3 T BODY 0 T 3
(b) 0 T 3 (1 , 2 , 3 ) = 0 T 3 1 , 2 , 3

(c) BASE (x, y) = (9 2/2, 3 2/2) 4 , 5

Intelligent Machine Lab. Meiji univ. 63

4.11: