NG H C ROBOT

H t a v t: H t a v t:
2.1. V tr v h ng c a v t r n trong khng gian
Bi u di n matr n
^ ^ ^ __
k c j b i a P
z y x
+ +
1
1
1
1
]
1

w
z
y
x
P
__
1
1
1
1
]
1

w
z
y
x
P
__
w
z
c
w
y
b
w
x
a
z y x
; ;
1
1
1
1
]
1

1 0 0 0
0
0
0
z z z
y y y
x x x
a o n
a o n
a o n
F
Quay:
1
1
1
1
]
1

1 0 0 0
1 0 0
0 1 0
0 0 1
z
y
x
d
d
d
T
Tnh ti n:
1
1
1
1
]
1

1 0 0 0
0
0
0
z z z
y y y
x x x
a o n
a o n
a o n
F
1
1
1
1
]
1

1 0 0 0
z z z z
y y y y
x x x x
P a o n
P a o n
P a o n
F
Quay v tnh ti n
z o y o x o O
z y x
' ' ' ' + +
H t a g c: Oyxz v i cc vc t n v l x, y, z
V tr v nh h ng c a c a v t r n trong khng gian
g n ln h quy chi u a ph ng Oxyz
1
1
1
]
1

z
y
x
o
o
o
o
'
'
'
'

H ng c a v t <== cc
vc t n v x, y, z:
z z y z x z z
z y y y x y y
z x y x x x x
z y x
z y x
z y x
' ' '
' ' '
' ' '
'
'
'
+ +
+ +
+ +
Cc thnh ph n c a cc vc t n v (x
x
, x
y
, x
z
) l cosin ch
ph ng c a cc tr c c a h t a a ph ng so v i h quy
chi u chung.
X
x
= X.cos(X,X),
Cc thnh ph n c a cc vc t n v (x
x
, x
y
, x
z
) l cosin ch
ph ng c a cc tr c c a h t a a ph ng so v i h quy
chi u chung.
X
x
= X.cos(X,X),
j [
1
1
1
]
1

z z z
y y y
x x x
z y x
z y x
z y x
z y x R
' ' '
' ' '
' ' '
' ' '
Ma tr n quay
Quay quanh tr c z
1
1
1
]
1

cos 0 sin
0 1 0
sin 0 cos
) (
y
R
1
1
1
]
1

1 0 0
0 cos sin
0 sin cos
) ( o o
o o
o
z
R
1
1
1
]
1

1
1
1
]
1

1
1
1
]
1

1
0
0
' ;
0
cos
sin
' ;
0
sin
cos
' z y x o
o
o
o
1
1
1
]
1

cos 0 sin
0 1 0
sin 0 cos
) (
y
R
1
1
1
]
1

cos sin 0
sin cos 0
0 0 1
) (
x
R
Quay m t vc t
1
1
1
]
1

1
1
1
]
1

z
y
x
z
y
x
p
p
p
p
p
p
p
p
'
'
'
' ;
j [ ' ' ' ' '
' ' ' ' ' ' '
Rp p z y x
z p y p x p p p
z y x

+ +
p R p
T
'
Hay:
p R p
T
'
Hay:
'
1 0 0
0 cos sin
0 sin cos
p p
1
1
1
]
1

o o
o o
Quay quanh tr c z
V d
Quay m t vc t quanh m t tr c b t k:
T ng h p cc ma tr n quay:
1
2
0
1
0
2
2
0
2
0
1
0
1
0
2
1
2
1
R R R
P R P
P R P
P R P

1
2
0
1
0
2
2
0
2
0
1
0
1
0
2
1
2
1
R R R
P R P
P R P
P R P

Php quay quanh tr c b t k

Php quay
quanh tr c
b t k
1- Bi n i tr c quay v trng v i 1 trong 3 tr c c b n c a h quy
chi u:
A2 = Rot(z, -a )
quay v quanh tr c z gc theo chi u kim ng h khi nhn t ng n v
g c tr c ztrng v i m t ph ng xoz.
A3 = Rot(y, -b)
quay v quanh tr c y gc theo chi u kim ng h khi nhn t ng n v
g c tr c yv trng v i tr c Oz,
bi n i v v trng v i Oz :
A1 = A2A3
v v trng v i tr c Ox ho c Oy t ng t .
Php quay
quanh tr c
b t k
1- Bi n i tr c quay v trng v i 1 trong 3 tr c c b n c a h
quy chi u:
A
2
= Rot(z, - ) : quay r quanh tr c z trng v i m t ph ng xoz.
A3 = Rot(y, ): quay r quanh tr c y trng v i tr c Oz,
bi n i v v trng v i Oz :
A
1
= A
2
A
3
r v trng v i tr c Ox ho c Oy t ng t .
2- A
4
quay v t quanh tr c r ( trng tr c c b n).
3- Tr k t qu v h quy chi u c b ng cch th c hi n ng c l i
nh ng g lm b c 1, ma tr n bi n i ng c l chuy n v
(ho c nghch o) c a ma tr n bi n i thu n.
Quay r ng c kim ng h quanh tr c Oy b ng ma tr n A
3
T
.
Quay r ng c kim ng h quanh tr c Oz b ng ma tr n A
2
T
.
T T
A A v z Rot A A v r Rot
2 3 3 2
). , ( . ) , (
M t t i thi u c a h ng
Gc Euler:
Ba l n quay quanh ba tr c c a ba h quy chi u khc nhau
V d m t b gc le l zyz, ngha l quay quanh tr c z, quay
quanh tr c y, r i l i quay quanh tr c z, t c l trong m t b gc le
c th quay quanh m t tr c t i a 2 l n, song ph i l 2 l n khng
lin ti p. V y kh i xu t n u m t tr c quay c th c m t hai l n th
ban u s c b 6 l n quay, quanh 6 tr c x, y, z, x, y, z.
C ba kh nng ch n tr c quay u tin ho c x, ho c y, ho c z.
C hai kh nng ch n tr c quay th hai, ch n 2 trong 3 tr c trn
tr tr c ch n b c tr c, v hai tr c quay gi ng nhau khng
c th c hi n lin t c.
C hai kh nng ch n tr c quay l n ba v c th ch n l p l i tr c
u tin v cn m t tr c ch a dng l n no.
V y s kh nng c a php quay le l k = 3.2.2 = 12)
V d m t b gc le l zyz, ngha l quay quanh tr c z, quay
quanh tr c y, r i l i quay quanh tr c z, t c l trong m t b gc le
c th quay quanh m t tr c t i a 2 l n, song ph i l 2 l n khng
lin ti p. V y kh i xu t n u m t tr c quay c th c m t hai l n th
ban u s c b 6 l n quay, quanh 6 tr c x, y, z, x, y, z.
C ba kh nng ch n tr c quay u tin ho c x, ho c y, ho c z.
C hai kh nng ch n tr c quay th hai, ch n 2 trong 3 tr c trn
tr tr c ch n b c tr c, v hai tr c quay gi ng nhau khng
c th c hi n lin t c.
C hai kh nng ch n tr c quay l n ba v c th ch n l p l i tr c
u tin v cn m t tr c ch a dng l n no.
V y s kh nng c a php quay le l k = 3.2.2 = 12)
Rot (Z, Y, X) Euler Angles
R R R R
2
3
1
2
0
1
0
3

Php quay le ZYZ
1
1
1
]
1

+ +

0 0 0
0 0 0
0 0 0
0
c s s c s
s s c c s c s s c c c s
s c c s s c c s s c c c
z Rot y Rot z Rot R
EUL
) , ( ). , ( ). , (
, , ,
1
1
1
]
1

+ +

0 0 0
0 0 0
0 0 0
0
c s s c s
s s c c s c s s c c c s
s c c s s c c s s c c c
z Rot y Rot z Rot R
EUL
) , ( ). , ( ). , (
, , ,
1/ R(x, )
P
(1)A
= R(x, )P
B
2/ R(x, )
P
(2)A
= R(y, )P
(1)A
= R(y, ) R(x, )P
B
3/R(z, )
P
(3)A
= R(z, )P
(2)A
= R(z, ) R(y, ) R(x, )P
B
1/ R(x, )
P
(1)A
= R(x, )P
B
2/ R(x, )
P
(2)A
= R(y, )P
(1)A
= R(y, ) R(x, )P
B
3/R(z, )
P
(3)A
= R(z, )P
(2)A
= R(z, ) R(y, ) R(x, )P
B
Trong bi ton nghch, n u cho tr c:
C th tnh cc gc quay:
Euler RPY
Quay ba l n quanh ba tr c
c a ba h quy chi u khc
nhau,
Xoay v t lin ti p ba l n
quanh ba tr c c a h quy
chi u ban u.
Quay ba l n quanh ba tr c
c a ba h quy chi u khc
nhau,
Xoay v t lin ti p ba l n
quanh ba tr c c a h quy
chi u ban u.
i t ng quay l h quy
chi u
i t ng quay trong
php quay ny l v t th )
G i A l ma tr n i m bi u di n i m mt vc t c n bi n hnh
trong c hai h quy chi u.
Php quay v t so v i h quy chi u c nh lin ti p:
(1)
Hay g i A1 l nh c a A qua nh x ta c:
(2)
Sau khi quay v t i l n th nh t b i php th c hi n bnh th ng
v tr c z lc ny l tr c c b n. L n quay th hai quanh tr c y
khng c ma tr n quay v y lc ny l tr c b t k, ta ph i lm
trng n v i m t tr c c a h quy chi u r i s d ng php quay c
b n quanh tr c y c , sau tr k t qu l i nh sau:
(3)
Lc ny tr c x l i l tr c b t k, c ma tr n quay ta l i ph i
lm trng tr c quay tr c khi quay, sau khi quay b ng ma tr n
quay tiu chu n tr k t qu l i nh sau:
(4)
V y bi u th c t c cu i cng y chnh l m t trnh t
ng c l i v i (2). Bi u th c (2) bi u thi php quay RPY cn
(4) bi u th Euler.
) , " ( ) , ' ( ) , ( 0 x R y R z R R
RPY

) , " ( ) , ' ( ) , ( .
1
0 x R y R z R A A
G i A l ma tr n i m bi u di n i m mt vc t c n bi n hnh
trong c hai h quy chi u.
Php quay v t so v i h quy chi u c nh lin ti p:
(1)
Hay g i A1 l nh c a A qua nh x ta c:
(2)
Sau khi quay v t i l n th nh t b i php th c hi n bnh th ng
v tr c z lc ny l tr c c b n. L n quay th hai quanh tr c y
khng c ma tr n quay v y lc ny l tr c b t k, ta ph i lm
trng n v i m t tr c c a h quy chi u r i s d ng php quay c
b n quanh tr c y c , sau tr k t qu l i nh sau:
(3)
Lc ny tr c x l i l tr c b t k, c ma tr n quay ta l i ph i
lm trng tr c quay tr c khi quay, sau khi quay b ng ma tr n
quay tiu chu n tr k t qu l i nh sau:
(4)
V y bi u th c t c cu i cng y chnh l m t trnh t
ng c l i v i (2). Bi u th c (2) bi u thi php quay RPY cn
(4) bi u th Euler.
) )R(z, (y, . ) , ( ) , ( ) , ( ) , ( .
1
0 0 R A z R y R z R z R A

) , ( ) , ( ) , ( . ) , ( ) , ( ) , ( ) , ( )R (z, )R )R(z, (y, .

-1 -1
0 0 0 0 z R y R x R A z R y R x R y R A
1
1
1
1
]
1

1
1
1
1
]
1

1
1
1
1
]
1

1 0 0 0
0 0
0 0
0 0 0 1
) , ( ;
1 0 0 0
0 0
0 0 1 0
0 0
) , ( ;
1 0 0 0
0 1 0 0
0 0
0 0
) , (




o o
o o
o
c s
s c
x Rot
c s
s c
y Rot
c s
s c
z Rot
Php bi n i thu n nh t:
Chuy n ng = tnh ti n + quay
1
1
1
1
]
1

1
1
1
1
]
1

1
1
1
1
]
1

1 0 0 0
0 0
0 0
0 0 0 1
) , ( ;
1 0 0 0
0 0
0 0 1 0
0 0
) , ( ;
1 0 0 0
0 1 0 0
0 0
0 0
) , (




o o
o o
o
c s
s c
x Rot
c s
s c
y Rot
c s
s c
z Rot
1
1
1
1
]
1

1 0 0 0
1 0 0
0 1 0
0 0 1
) , , (
P
N
M
P N M Trans
Bi ton ng h c
thu n c a tay my
Php chuy n i t a c bi u di n b ng ma tr n chuy n i
thu n nh t:
Trong l vc t nh v, l cc vc t nh
h ng d i d ng cosin ch ph ng c a ph n lm vi c. Ch ng h n
v i ma tr n thu n nh t c th ch n nh sau:
Cc ph n t a
12
; a
13
; a
23
l cc ph n t nh h ng, cc ph n t
a
14
; a
24
; a
34
l cc ph n t inh v. Nh v y ch c n 6 ph n t
m t nh v v nh h ng.
1
]
1

1 0 0 0
) ( ) ( ) ( ) (
) (
0 0 0 0
0
q p q a q s q n
q T
) (
0
q p
) ( ), ( ), (
0 0 0
q a q s q n
Php chuy n i t a c bi u di n b ng ma tr n chuy n i
thu n nh t:
Trong l vc t nh v, l cc vc t nh
h ng d i d ng cosin ch ph ng c a ph n lm vi c. Ch ng h n
v i ma tr n thu n nh t c th ch n nh sau:
Cc ph n t a
12
; a
13
; a
23
l cc ph n t nh h ng, cc ph n t
a
14
; a
24
; a
34
l cc ph n t inh v. Nh v y ch c n 6 ph n t
m t nh v v nh h ng.
1
1
1
1
]
1

1
) (
34
24 23
14 13 12
0
a
a a
a a a
q T
Nguyn l Denavit-Hartenberg (D-H)
a
i
: kho ng cch gi a hai kh p theo ph ng ng
vung gc chung.
d
i
: kho ng cch gi a giao i m c a hai ng vung
gc chung v i tr c quay, tnh theo ph ng c a ng
vung gc chung.
i
l gc quay quanh tr c xi zi-1 n trng v i zi.
i l gc quay quanh tr c zi-1 xi-1 n trng v i xi.
Bi n i h quy chi u O
i-1
trng v i h quy chi u O
i
:
Tnh ti n O
i-1

i
(d
i
theo tr c z
i-1
)
Quay Oi quanh tr c zi
b ng ma tr n quay R(z
i
,

)
1
1
1
1
]
1

1 0 0 0
1 0 0
0 0
0 0
) , 0 , 0 ( ). , (
1
'
i
i i
i i
i
i
d
c s
s c
di R i Z i R A
0 0
0 0

Tnh ti n Oi theo tr c
xi m t l ng a
i
Quay i b c trn
quanh tr c xi gc
hon thi n.
1
1
1
1
]
1

1 0 0 0
0 0
0 0
0 0 1
'
i i
i i
i
i
i
c s
s c
a
A
o o
o o
Ma tr n bi n hnh t ng h p t c b ng cch nhn hai
ma tr n trn c d ng:
1
1
1
1
]
1

1 0 0 0
0
) (
' 1
'
1
i i i
i i i i i i i
i i i i i i i
i
i
i
i i
i
i
d c s
s a s c c c s
c a s s c s c
A A q A
o o
0 o 0 o 0 0
0 o 0 o 0 0
1
1
1
1
]
1

1 0 0 0
0
) (
' 1
'
1
i i i
i i i i i i i
i i i i i i i
i
i
i
i i
i
i
d c s
s a s c c c s
c a s s c s c
A A q A
o o
0 o 0 o 0 0
0 o 0 o 0 0
M t s v
d ng
d ng quy
t c DH:
Tay my ba khu ph ng:
Thay cc thng s t ng ng vo cc ma tr n m u t ng qut ni
trn nh n c ma tr n bi n hnh cho t ng b c nh sau:
Khi nhn cc ma tr n ny v i nhau c ma tr n chuy n i t ng
h p:
y k hi u:
1
1
1
1
]
1

1 0 0 0
0 1 0 0
0
0
) (
1 i i i i
i i i i
i
i
i
s a c s
c a s c
A 0
Thay cc thng s t ng ng vo cc ma tr n m u t ng qut ni
trn nh n c ma tr n bi n hnh cho t ng b c nh sau:
Khi nhn cc ma tr n ny v i nhau c ma tr n chuy n i t ng
h p:
y k hi u:
1
1
1
1
]
1

+ +
+ +

1 0 0 0
0 1 0 0
0
0
) (
123 3 12 2 1 1 123 123
123 3 12 2 1 1 123 123
2
3
1
2
0
1
0
3
s a s a s a c s
c a c a c a s c
A A A q T
) cos( c
3 2 1 123
+ +
Z
0
X
0
Y
0
Z
3
X
2
Y
1
X
1
d
2
Z
1
X
3
3
O
2
O
1
O
0
O
Joint 1
Joint 2
Joint 3
Joint i
i
a
i
d
i

i
1 0 a
0
0
0
2 -90 a
1
0
1
3 0 0 d
2

2
a
0
a
1
Y
2
Joint 2
1
1
1
1
]
1

1 0 0 0
0
1
i i i
i i i i i i i
i i i i i i i
i
i
d C S
S a C S C C S
C a S S S C C
T
o o
o o
o o
1
1
1
1
]
1

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

a
a
T
1
1
1
1
]
1

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


a
a
T
) )( )( (
2 1 0
3 2 1 3
0
T T T T
1
1
1
1
]
1

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

a
a
T
1
1
1
1
]
1

1 0 0 0
0 sin cos
2 2
2
2
2 2 3
1 0 0
0 0 cos sin
0
d
T

Tay my t a c u:
S ng v b ng thng s DH cho th y nh
hnh v :
V z0 v z1 c t nhau nn d1 = 0. T b ng thng s DH c cc
ma tr n chuy n v thnh ph n nh sau:
Nhn cc ma tr n trn v i nhau c ma tr n chuy n v t ng
h p:
1
1
1
1
]
1

1 0 0 0
0 0 1 0
0 0
0 0
) (
1 1
1 1
1
0
1
c s
s c
A 0
1
1
1
1
]
1

1 0 0 0
0 1 0
0 0
0 0
) (
2
2 2
2 2
2
1
2
d
c s
s c
A 0
1
1
1
1
]
1

1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
) (
3
3
2
3
d
d A
V z0 v z1 c t nhau nn d1 = 0. T b ng thng s DH c cc
ma tr n chuy n v thnh ph n nh sau:
Nhn cc ma tr n trn v i nhau c ma tr n chuy n v t ng
h p:
1
1
1
1
]
1

+


1 0 0 0
0
) (
3 2 2 2
2 1 3 2 1 2 1 1 2 1
2 1 3 2 1 2 1 1 2 1
2
3
1
2
0
1
0
3
d c c s
d c d s s s s c c s
d s d s c s c s c c
A A A q T
Robot Stanford
Z1
X2
Z2
X3
Z3
X4
X5
X6
Z4
Z5
Z6
X7
Z7
X1
Y1
DH Parameters of Stanford Arm
i ai di oi i
1 0 d1 90 1
2 0 d2 90 2 2 0 d2 90 2
3 0 d3 (var) 90 90
4 0 0 90 4
5 0 d5 0 5
6 0 d6 0 6
Fig. 2.31 The frames of t he St anford Arm.
# d a o
1

1
0 0 -90
2

2 d
1
0 90
3
0
d
1
0 0
4

4
0 0 -90
5

5
0 0 90
6

6
0 0 0
Robot Scara
Khu
i

i
o L
i
D
i
1
*
1

0 L1 H
2
*
2

180
0
L2 0
3 0 0 0
*
3
d
4
*
4

0 0 d
4
1
1
1
1
]
1

1 0 0 0
1 0 0
0
0
) 0 , 0 , ( ). , (
1 1 1 1
1 1 1 1
1 1 01
H
S L C S
C L S C
l T z R T
1
1
1
1
]
1

1 0 0 0
0 1 0 0
0
0
) 0 , 0 , ( ). , (
2 2 2 2
2 2 2 2
2 2 12
S L C S
C L S C
l T z R T
1
1
1
1
]
1

1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
) , 0 , 0 (
3
3 23
d
d T T
1
1
1
1
]
1

1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
) , 0 , 0 (
3
3 23
d
d T T
1
1
1
1
]
1

1 0 0 0
1 0 0
0 0
0 0
) , (
4
4 4
4 4
4 34
d
C S
S C
z R T
1
1
1
1
]
1

+
+

1 0 0 0
1 0 0
0
0
.
12 2 1 1 12 12
12 2 1 1 12 12
12 01 02
H
S L S L C S
C L C L S C
T T T
1
1
1
1
]
1

+
+
+

1 0 0 0
1 0 0
0
0
. .
3
12 2 1 1 12 12
12 2 1 1 12 12
23 12 01 03
H d
S L S L C S
C L C L S C
T T T T
1
1
1
1
]
1

+
+
+

1 0 0 0
1 0 0
0
0
. .
3
12 2 1 1 12 12
12 2 1 1 12 12
23 12 01 03
H d
S L S L C S
C L C L S C
T T T T
1
1
1
1
]
1

+
+
+

1 0 0 0
1 0 0
0
0
. . .
4 3
12 2 1 1 123 123
12 2 1 1 123 123
34 23 12 01 04
H d d
S L S L C S
C L C L S C
T T T T T
Puma
Example: Puma 560
Joint i
i

i
a
i
(mm) d
i
(mm)
1
1
0 0 0
2
2
-90 0 d
2
3
3
0 a
2
d
3
4
4
90 a
3
d
4
5
5
-90 0 0
6
6
0 0 0
Example: Puma 560
Joint i
i

i
a
i
(mm) d
i
(mm)
1
1
0 0 0
2
2
-90 0 d
2
3
3
0 a
2
d
3
4
4
90 a
3
d
4
5
5
-90 0 0
6
6
0 0 0
Forearm of a PUMA
a
3
x
3
y
3
x
4
z
4
d
4
a
3
x
5
y
5
x
6
z
6
y
3
z
4
Spherical j oint
Example: Puma 560
Different Configurat ion
Link Coordinate Parameters
Joint i

i

i
a
i
(mm) d
i
(mm)
1

1
-90 0 0
2

2
0 431.8 149.09
3

3
90 -20.32 0
4

4
-90 0 433.07
5

5
90 0 0
6

6
0 0 56.25
PUMA 560 robot arm link coordinate parameters
Joint i

i

i
a
i
(mm) d
i
(mm)
1

1
-90 0 0
2

2
0 431.8 149.09
3

3
90 -20.32 0
4

4
-90 0 433.07
5

5
90 0 0
6

6
0 0 56.25
Example: Puma 560
ng h c ng c
I n v e r s e K i n e m a t i c s
From Position to Angles
ng h c ng c
I n v e r s e K i n e m a t i c s
From Position to Angles
V d
(x , y)
Tm:
)
x
y
arctan(
Trong tnh ton:
)
x
y
( 2 arctan

1
X
Y
S
)
x
y
( 2 arctan
K t qu :
) y (x S
2 2
+

2
(x , y)
l
2
C c u hai khu ph ng:
Given: l
1
, l
2
, x , y
Find:
1
,
2
(x , y)

1
l
1
Ph ng php hnh h c
l
1
l
2

(x , y) S d ng nh lu t Cosines:

,
_
,
,

+ +
+
2 1
2
2
2
1
2 2
2 1
2
2
2
1
2 2
2 1
2
2
2
1
2 2
2 2 2
2
arccos
2
) cos(
) cos( ) 180 cos(
) 180 cos( 2 ) (
cos 2
l l
l l y x
l l
l l y x
l l l l y x
C ab b a c
2
2
2 2
2

,
_
,
,

+ +
+
2 1
2
2
2
1
2 2
2 1
2
2
2
1
2 2
2 1
2
2
2
1
2 2
2 2 2
2
arccos
2
) cos(
) cos( ) 180 cos(
) 180 cos( 2 ) (
cos 2
l l
l l y x
l l
l l y x
l l l l y x
C ab b a c
2
2
2 2
2
S d ng nh lu t Cosines:

,
_
,

+
+

x
y
2 arctan

y x
) sin(
y x
) sin(180 sin
sin sin
1 1
2 2
2
2 2
2
2
1
l
c
C
b
B

,
_
,

,
_
,
,

x
y
2 arctan
y x
) sin(
arcsin
2 2
2 2
1
l

2
c th gc t a u tin hay
th t
Ph ng php i s
l
1
l
2

(x , y)
2 1
2 1 2 1 1
2 1 2 1 1
1 2 2 1
1 1
(3)
sin s y (2)
c c x (1)
) cos( c
cos c
+
+
+
+

+
+
+
l l
l l
, ) , )
, )

,
_
,
,

+

+ +
+ + +
+ + + + +
+ +
+ +
+ + + +
2 1
2
2
2
1
2 2
2
2 2 1
2
2
2
1
2 1 1 2 1 1 2 1
2
2
2
1
2 1 1 2 1
2
2 1
2
2
2
1
2
1 2 1 1 2 1
2
2 1
2
2
2
1
2
1
2 2 2 2
2
y x
arccos
c 2
) (sin s ) (c c 2
) (sin s 2 ) (sin s ) (c c 2 ) (c c
y x ) 2 ( (1)
l l
l l
l l l l
l l l l
l l l l l l l l
2 1
2 1 2 1 1
2 1 2 1 1
1 2 2 1
1 1
(3)
sin s y (2)
c c x (1)
) cos( c
cos c
+
+
+
+

+
+
+
l l
l l
Only
Unknown
) )(sin (cos ) )(sin (cos ) sin(
) )(sin (sin ) )(cos (cos ) cos(
:
a b b a b a
b a b a b a
Note
+

+
+

) )(sin (cos ) )(sin (cos ) sin(

) )(sin (sin ) )(cos (cos ) cos(
:
a b b a b a
b a b a b a
Note
+

+
+

) c ( s ) s ( c
c s c s s
sin s y
) ( ) c ( c
c c c
c c x
2 2 1 1 2 2 1
1 2 2 2 1 2 1 1
2 1 2 1 1
2 2 1 2 2 1 1
2 1 2 2 1 2 1 1
2 1 2 1 1
l l l
l l l
l l
s l s l l
s s l l l
l l
+ +
+ +
+
+
+
+
+
+
Chng ta bi t r ng
2
xc nh
trn slide. Chng ta c n tm
1
.
, )
2 2
2 2 2 2 1
1
2 2 1
2
2
2
1 1 2 2
2 2 1
2 2 1 1 2 2
2 2 1
2 2 1
2 2 1
2 2 1
1
y x
x ) c ( y
s
) c 2 ( s x
) c (
1
) c ( s ) s (
) c (
) ( x
y
) c (
) ( x
c
+
+

+ + +
+

+ +
+
+

+
+

s l l l
l l l l s l
l l
l l l
l l
s l s
l l
s l s
, )
2 2
2 2 2 2 1
1
2 2 1
2
2
2
1 1 2 2
2 2 1
2 2 1 1 2 2
2 2 1
2 2 1
2 2 1
2 2 1
1
y x
x ) c ( y
s
) c 2 ( s x
) c (
1
) c ( s ) s (
) c (
) ( x
y
) c (
) ( x
c
+
+

+ + +
+

+ +
+
+

+
+

s l l l
l l l l s l
l l
l l l
l l
s l s
l l
s l s
Thay th cho c1 v n gi n ha
nhi u l n
Ch ay l nh lu t cosin v c
th c thay th b ng x
2
+ y
2

,
_
,
,

+
+

2 2
2 2 2 2 1
1
y x
x ) c ( y
arcsin
s l l l
Ph ng php i s
Kh o st m t t ay my c 3 khu
T T T T T
B
W
2
3
1
2
0
1
0
3

Chng t a t hu c
ph ng t rnh ng h c:
Ph ng php i s
Kh o st m t t ay my c 3 khu
T T T T T
B
W
2
3
1
2
0
1
0
3

Chng t a t hu c
ph ng t rnh ng h c:
3

Y
1

Y
2

X 2

Y
3

X
0

Y
Ph ng php i s
D-H t ransfaormat ion
i o
i-1
o
i-1
d
i

i
1 0 0 0
1
2 0 L
1
0
2
3 0 L
2
0
3
1

X
0

X
Ph ng php i s
1
1
1
1
]
1

1
1
1
1
]
1

1
1
1
1
]
1

1
1
1
1
]
1

1 0 0 0
0 1 0 0
0 0
0
1 0 0 0
0 1 0 0
0 0
0
1 0 0 0
0 1 0 0
0 0
0 0
1 0 0 0
0
3 3
2 3 3
2
3
2 2
1 2 2
1
2
1 1
1 1
1 0 0 0 1 0 1
1 0 0 0 1 0 1
0 1 1
0
1






o o o o
o o o o

c s
L s c
T
c s
L s c
T
c s
s c
d c c s c s s
d s s c c c s
a s c
T
1
1
1
1
]
1

1
1
1
1
]
1

1
1
1
1
]
1

1
1
1
1
]
1

1 0 0 0
0 1 0 0
0 0
0
1 0 0 0
0 1 0 0
0 0
0
1 0 0 0
0 1 0 0
0 0
0 0
1 0 0 0
0
3 3
2 3 3
2
3
2 2
1 2 2
1
2
1 1
1 1
1 0 0 0 1 0 1
1 0 0 0 1 0 1
0 1 1
0
1






o o o o
o o o o

c s
L s c
T
c s
L s c
T
c s
s c
d c c s c s s
d s s c c c s
a s c
T
Ph ng php i s
Ta c:
1
1
1
1
]
1

+
+

1 0 0 0
0 1 0 0
0
0
12 2 1 1 123 123
12 2 1 1 123 123
0
3
s l s l c s
c l c l s c
T T
B
W
1
1
1
1
]
1

+
+

1 0 0 0
0 1 0 0
0
0
12 2 1 1 123 123
12 2 1 1 123 123
0
3
s l s l c s
c l c l s c
T T
B
W
Gi s
1
1
1
1
]
1

1 0 0 0
0 1 0 0
0
0
y c s
x s c
T
B
W


T ta c
12 2 1 1
12 2 1 1
123 123
s l s l y
c l c l x
s s c c
+
+


Bnh ph ng 2 ph ng
trnh cu i ta c:
Ph ng php i s
Bnh ph ng 2 ph ng
trnh cu i ta c:
2 2 1
2
2
2
1
2 2
2 c l l l l y x + + +
T y ta c bi u th c
cho c
2
2 1
2
2
2
1
2 2
2
2 l l
l l y x
c
+

). , ( 2 tan 1
2 2 2
2
2 2
c s A c s
V cu i cng:
Ta c c
12
=c
1
c
2
-s
1
s
2
v s
12
= c
1
s
2
-c
2
s
1
:
1 2 1 1
1 2 1 1
c k s k y
s k c k x
+

Ph ng php i s
1 2 1 1
1 2 1 1
c k s k y
s k c k x
+

Trong k
1
=l
1
+l
2
c
2
v k
2
=l
2
s
2
.
t
r=+\ k
1
2
+k
2
2
and =Atan2(k
2
,k
1
).
Ph ng php i s
k
1
k
2

2
l
1
l
2

Trong : k
1
=r cos , k
2
=r sin v chng ta
c th vi t
x/r= cos cos
1
- sin sin
1
y/r= cos sin
1
+ sin cos
1
ho c: cos(+
1
) = x/r, sin(+
1
) =y/r
Trong : k
1
=r cos , k
2
=r sin v chng ta
c th vi t
x/r= cos cos
1
- sin sin
1
y/r= cos sin
1
+ sin cos
1
ho c: cos(+
1
) = x/r, sin(+
1
) =y/r
Do
+
1
= atan2(y/r,x/r) = atan2(y,x)
Suy ra :

1
= atan2(y,x) - atan2(k
2
,k
1
)
Cu i cng,
3
c th c rt ra t

1
+
2
+
3
=
Ph ng php i s
Do
+
1
= atan2(y/r,x/r) = atan2(y,x)
Suy ra :

1
= atan2(y,x) - atan2(k
2
,k
1
)
Cu i cng,
3
c th c rt ra t

1
+
2
+
3
=