Li ni u

Trong s nghip cng nghip ha, hin i ha t nc vn t ng ha

sn xut c vai tr c bit quan trng .
Nhm nng cao nng sut dy chuyn cng ngh, nng cao cht lng v
kh nng cnh tranh ca sn phm, ci thin iu kin lao ng , nng cao nng
sut lao ngt ra l h thng sn xut phi c tnh linh hot cao. Robot cng
nghip l mt b phn quan trng to ra nhng h thng t ng .
Robot cng nghip c mt trong sn xut t nhiu nm trc, ngy nay
robot cng nghip c dng nhiu lnh vc sn xut. l xut pht t nhng
u im m robot c chn v c kt li trong qu trnh sn xut,robot c
nhng tnh nng m con ngi khng th c c, kh nng lm vic n nh,lm
vic trong mi trng c hi Do vic u t nghin cu, ch to ra ngng
loi robot phc v cho cng cuc t ng ha sn xut l rt cn thit cho hin ti
v trong tng lai.
Mn hc N C IN T gip chng em bc u lm quen,tm hiu,
hc cch ch to v rt c ch cho chng em sau ny.
Trong qu trnh hc mn N C IN T chng em cn nhiu thiu
st mong cc thy ch bo thm cho chng em.
Chn thnh cm n
Nhm sinh vin :
Nguyn nh Qung
Nguyn ng Thi
Nguyn Vn Tho
Ng Vn Thng
1
I-Xy dng s ng hc v cc ma trn biu din cu trc
1-S ng hc ca PUMA.
2- Cc ma trn chuyn biu din cu trc ca PUMA.
1
1
1
1
]
1

1 0 0 0
1 0 0
0 0 6 6
0 0 6 6
6
6
d
C S
S C
A
1
1
1
1
]
1

1 0 0 0
0 0 1 0
0 5 0 5
0 5 0 5
5
C S
S C
A
1
1
1
1
]
1

1 0 0 0
0 1 0
0 4 0 4
0 4 0 4
4
4
d
C S
S C
A
1
1
1
1
]
1

1 0 0 0
0 0 1 0
3 3 0 3
3 3 0 3
6
3
3
3
a S C S
a C S C
A
1
1
1
1
]
1

1 0 0 0
1 0 0
2 0 2 2
2 0 2 2
2
2
2
2
d
a S C S
a C S C
A
1
1
1
1
]
1

1 0 0 0
0 0 1 0
0 1 0 1
0 1 0 1
1
C S
S C
A
Trong : tin theo di ta k hiu
Cx =cos(x): x l cc bin khp
Sx= sin(x)
II-Tnh Ton ng Hc Thun:
2
Bng thng s DH:

khu
i

i
a
i
d
1 1
*
-90 0 0
2
*
2
0 a2 d2
3
*
3
90 a3 0
4
*
4
-90 0 d4
5
*
5
90 0 0
6
*
6
0 0 d6


1
1
1
1
]
1

1 0 0 0
1 0 0
0 0 6 6
0 0 6 6
6
6
d
C S
S C
A

1
1
1
1
]
1

1 0 0 0
0 0 1 0
0 5 0 5
0 5 0 5
5
C S
S C
A

1
1
1
1
]
1

1 0 0 0
0 0 6 6
5 5 6 5 6 5
5 5 6 5 6 5
.
6
6
6
5
5 6
4
C S
d C C S S C S
d S S S C C C
T A T

1
1
1
1
]
1

1 0 0 0
0 1 0
0 4 0 4
0 4 0 4
4
4
d
C S
S C
A

1
1
1
1
]
1

+
+ +


1 0 0 0
5 5 6 5 6 5
5 4 5 4 6 4 6 5 4 6 4 6 5 4
5 4 5 4 6 4 6 5 4 6 4 6 5 4
.
4 6
6
6
6
4
4 6
3
d d C C S S C S
d S S S S C C S C S S C C C S
d S C S C C S S C C S S C C C
T A T
3

1
1
1
1
]
1

1 0 0 0
0 0 1 0
3 3 0 3
3 3 0 3
6
3
3
3
a S C S
a C S C
A

6
3
3 6
2
. T A T
cc h s trong ma trn:

. 6 5 4 ) 4 , 3 (
; 5 4 ) 3 , 3 (
; 6 4 6 5 4 ) 2 , 3 (
; 6 4 6 5 4 ) 1 , 3 (
; 3 3 ) 4 6 5 ( 3 6 5 4 3 ) 4 , 2 (
; 5 3 5 4 3 ) 3 , 2 (
; 6 5 3 ) 6 4 6 5 4 ( 3 ) 2 , 2 (
; 6 5 3 ) 6 4 6 5 4 ( 3 ) 1 , 2 (
; 5 3 5 4 3 ) 3 , 1 (
; 6 5 3 ) 6 4 6 5 4 ( * 3 ) 2 , 1 (
; 6 5 3 _ 6 4 6 5 4 ( 2 ) 1 , 1 (
6
2
6
2
6
2
6
2
6
2
6
2
6
2
6
2
6
2
6
2
6
2
d S S T
S S T
C C S C S T
S C C C S T
a S d d C C d S C S T
C C S C S T
S S C C S S C C S T
C S C S S C C C T
C S S C C T
S S S S S S C C C T
S S S S S C C C C T

+
+
+ +


+
+
+



1
1
1
1
]
1

1 0 0 0
1 0 0
2 0 2 2
2 0 2 2
2
2
2
2
d
a S C S
a C S C
A

6
2
2 6
1
. T A T
cc h s trong
6
1
T
l:
4

2 6 5 4 ) 4 , 3 (
5 4 ) 3 , 3 (
6 4 6 5 4 ) 2 , 3 (
6 4 6 5 4 ) 1 , 3 (
2 2
] 3 3 ) 4 6 5 ( 3 6 4 3 [ 2 ] 3 3 ) 4 6 5 ( 3 6 5 4 3 [ 2 ) 4 , 2 (
) 5 3 5 4 3 ( 2 ) 5 3 5 4 3 ( 2 ) 3 , 2 (
] 6 5 3 ) 6 4 6 5 4 ( 3 [ 3 ] 6 5 3 ) 6 4 6 6 4 ( 3 [ 2 ) 2 , 2 (
] 6 5 3 ) 6 4 6 5 4 ( 3 [ 2 ] 6 5 3 ) 6 4 6 5 4 ( 3 [ 2 ) 1 , 2 (
2 2
] 3 3 ) 4 6 5 ( 3 6 5 4 3 [ 2 ] 3 3 ) 4 6 5 ( 3 6 5 4 3 [ 2 ) 4 , 1 (
) 5 3 5 4 3 ( 2 ) 5 3 5 4 3 ( 2 ) 3 , 1 (
6 5 23 ) 6 4 6 5 4 ( 23 ) 2 , 1 (
6 5 23 ) 6 4 6 5 4 ( 2 ) 1 , 1 (
6
1
6
1
6
1
6
1
6
1
6
1
6
1
6
1
6
1
6
1
6
1
6
1
d d S S T
S S T
C C S C S T
S C C C S T
a S
a S d d C C d C S C a C d d C S d S C C S T
C C S C S C C S S C C S T
S S C C S S C C S C S S S C S S C C C S T
C S C S S C C C S C C S S S S C C C C S T
a C
a S d d C C d S C S S a C d d C S d S C C C T
C C S C S S C S S C C C T
S S S C S S C C C T
C S S S S C C C C T
+

+
+
+
+ + + + + +
+ +
+ +
+ +
+
+ + + + +
+
+ +


1
1
1
1
]
1

1 0 0 0
0 0 1 0
0 1 0 1
0 1 0 1
1
C S
S C
A

6
1
1 6
0
. T A T

cc h s trong
6
0
T
l:
1[ 23( 4 5 6 4 6) 23 5 6] 1( 4 5 6 4 6]
x
N C C C C C S S S S S S S C C C S +
1[ 23( 4 5 6 4 6) 23 5 6] 1( 4 5 6 4 6)
x
O C C C C S S C S S S S C S C C + + +
1[ 2( 3 4 5 3 5) 2( 3 4 5 3 5) 1 4 5
x
A C C C C S S C S S C S C C S S S +
5
1[ 2[ 3 4 5 6 3( 5 6 4) 3 3] 2[ 3 4 5 6
3( 5 6 4) 3 3] 2 2] 1( 4 5 6 2)
x
P C C C C S d S C d d C a S S C S d
C C d d S a C a S S S d d
+ + +
+ + + +
1[ 23( 4 5 6 4 6) 23 5 6] 1( 4 5 6 4 6)
y
N S C C C C S S S S S C S C S C S + +
1[ 23( 4 5 6 4 6) 23 5 6] 1( 4 5 6 4 6)
y
O S C C C S S C S S S C S C S C C + + + +
1[ 2( 3 4 5 3 5) 2( 3 4 5 3 5) 1 4 5
y
A S C C C S S C S S C S C C C S S + +
1[ 2[ 3 4 5 6 3( 4 6 4) 3 3] 2[ 3 4 5 6 3( 5 6 4)] 2 2]
1( 4 5 6 2)
Py S C C C S d S C d d C a S S C S d C d d C a
C S S d d
+ + + + +
+ +
[ 2( 3( 4 5 6 4 6) 3 5 6) 2( 3( 4 5 6 4 6) 3 5 6)]
z
N S C C C C S S S S C C S C C C S S C S S + +
[ 2( 3( 4 5 6 4 6) 3 5 6) 2( 3( 4 5 6 4 6) 3 5 6)]
z
O S C C C S S C S S S C S C C S S C C S S + +
[ 2( 3 4 5 3 5) 2( 3 4 5 3 5)]
z
A S C C S S C C S C S C C + +
[ 2( 33 4 5 6 3( 5 6 4) 3 3) 2[ 3 4 5 6 3( 5 6 4) 3 3] 2 2]
z
P S C C S d S C d d C a C S C S d C C d d S a S a + + + + + + +
III. PHNG TRNH NG HC NGC ROBOT PUMA
Bi ton ng hc ngc l rt kh, i vi tay my 6 khp c ba khp cui
ng quy ti mt im, ta c th tch bi ton ng hc ngc thnh hai bi ton
n gin hn l ng hc ngc v tr v ng hc ngc hng
+ Tm v tr giao im cc trc c tay (tm c tay)
+ Tm hng ca c tay.
Ta biu din thnh hai h phng trnh nh sau:
Trong Ov R l hng v v tr ca dng c, c biu din i vi h ta
c nh bn ngoi (world coordinate system). Ta phi gii bi ton trn i vi cc
n 6 1
, , q q
.
C cu 6 khp quay c 3 khp cui giao nhau c phng php gii nh sau:
V tr ca tm c tay,P
C
xc nh qua v tr cng c (The given tool position)
v phng ca Tool pointing (
6
Z
).Do v tr ca tm c tay ph thuc vo 3 bin
khp u tin.
The relative wrist oriention
6
3
R

6
Cc bin khp 4 5 6
, ,
xc nh t ma trn nh hng c tay (The arm
orientation)
3
0
R
v ma trn nh hng cng c (The given tool orientation)
0
6
R
.
+ Ma trn trng thi (The given tool pose)
0
6
T
+ Solve porions ca ng hc ngc tm ra
0
3
R
1 2 3
( , , )
v
3
6 4 5 6
( , , ) R
.
+ nh v tr ca tm c tay c
O
c ta cho trc nh sau:
0
6
0
. . 0
1
C
O O d R
1
1

1
1
]
trong
O
v R l hng v v tr ca dng c, c biu
din i vi h ta c nh bn ngoi (world coordinate system).
c
P
as
6
d
(ct cui cng ca
0
6
T
)
6
d
(tool offset length)*
6
Z
(3 ct ca ma trn
0
6
T
)
+ Thit lp c
P
= ct cui cng ca
0
3
R
1 2 3
( , , )
tm ra cc bin khp
1 2 3
, ,
Tnh
1
3 0 0
6 3 6
R R R

sau khi thay gi tr ca cc bin khp vo 1 2 3

, ,
vo
0
3
R
1 2 3
( , , )
+So snh
3
6
R
v
3
6 4 5 6
( , , ) R
rt ra
4 5 6
, ,
Ta ca im C trong h ta 6
R
:
6
r
C
=
0
0
6
d
t
1
1
1
1

1
1
]
Ta ca C trong h ta 0
R
:
r
C
=
E
T
.
6
r
E
=
6
6
6
.
0
0 .
.
6
1
1
0 0 0 1
x p
y p
z p
n s a p
a d x
x x x x
n s a p
a d y
y y y y
d
a d z
n s a p
z z z z
1
1
1
+
1
1
1
1
1
+ 1
1
1
1
1
+ 1
1
1
1
1
1 1 ]
]
]
(*)
Ma trn chuyn t h ta 0

3:
7
0
R
3=
A
1.
A
2.
A
3
=
23 3 23 2 2 1 2
3 23 2 2 1 2
3 23 2 2
. ( . . ) .
1 23 1 1
1
. ( . . ) .
1 1 23 1
1
. .
0
23 1 23
0
1 0 0
C S C C C a C a C S d
S
S S S S a C a C C d
C
S a S a S d C
1
+

1
1
+
1
1 +
1
1
]

Ta ca C trong
0
R
:
r
C
=
0
R
3
.
4
r
C =
4 3 23 2 1 2 1 2
4 3 23 2 1 2 1 2
4 23 3 23 2 2 1
. . . . . .
1 23 1
. . . . . .
1 23 1
. . .
C S d a C C a C C S d
S S d a S C a S C C d
d C a S a S d
t
1
+ +
1
1
+ + +
1
1 +
1
]
(**)
i chiu (*) v (**) ta c h 3 phng trnh 3 n :
1 23 4 3 23 2 2 1 2 6
1 23 4 3 23 2 2 1 2 6
4 23 3 23 2 2 6 1
( . . . ) . .
( . . . ) . .
. . . .
p x
p y
p z
C S d a C a C S d x a d
S S d a C a C C d y a d
d C a S a S z a d d
+ +
+ + +

Nhn (2) vi cos 1

tr i (1) nhn vi sin 1

:
2 6 1 6 1
2
1 6 6
2 2
6 6
( ) os ( )sin
cos tan 2( , )
( ) ( )
p y p x
p x p y
p y p x
d y a d c x a d
d
Ar A x a d y a d
y a d x a d

_

t

+
,
Nhn (2) vi sin
1

cng vi (1) nhn vi cos

1

v kt hp vi (3) ta c h
23 4 3 23 2 2 6 6 1
23 4 3 23 2 2 6 1
. . . ( . ) sin ( . ) os
. . . .
p y p x x
p z y
S d a C a C y a d x a d c P
C d a S a S z a d d P
+ + +


Bnh phng 2 v cng li ta c:
2 2 2 2 2
4 3 2 4 2 3 2 3 3
2 2 2 2 2
4 2 3 2 3 3 3 2 4
2 sin 2 os
2 sin 2 os ( )
x y
x y
d a a d a a a c P P
d a a a c P P a a d


+ + + + +
+ + + +
2 2 2 2 2
3 2 4
3 4 2 2 3
2 2
2 4 3
( )
cos tan 2( , )
2
x y
P P a a d
Arc A d a a a
a d a

_
+ + +
t +

+
,
Khai trin h phng trnh lin kt ta c:
8
3 3 2 3 4 2 3 4 3 3 2
3 4 3 3 2 2 3 4 3 3 2
( . . ) ( . .sin ) sin
( . . ) os ( . . os ) sin
x
y
a C a S d C C d a P
C d a S c a S d a c P


+ + +
+ +
2 2
3 3 2 3 4 3 4 3 3
3 3 2 3 4 3 4 3 3
4 3 3 3 2 4 3 3 3
( . . ) ( . . )
( . . ) ( . . )
( . ) ( . )
c x y
s x y
a C a S d C d a S
a C a S d P C d a S P
d C a S P a d S a C P
1 + + +
]
1 + + +
]
1 + +
]
2
tan 2( , )
s c
A

2 4 3 3 3 2 4 3 3 3 2 4 3 3 3 4 3 3 3
tan 2 ( ) ( ) , ( ) ( )
x y x
A d C a S P a d S a C P a d S a C P d C a S 1 + + + + +
]
Ta c
3
6
R
=
4 5 6
A A A
=
4 4
4 4
4
0 0
0 0
0 1 1
0 0 0 1
C S
S C
d
1
1
1
1
1
]
5 5
5 5
0 0
0 0
0 1 0 0
0 0 0 1
C S
S C
1
1

1
1
1
]
6 6
6 6
6
0 0
0 0
0 0 1
0 0 0 1
C S
S C
d
1
1
1
1
1
]
=
Mt khc:
1 2 3
A A A
4 5 6
A A A =
E
T
0 3
3 6
.
E
R R T
3 0( 1)
6 3
.
E
R R T

1 23 1 23 23 1 23 1 23 23 1 23 1 23 23 14
1 1 1 1 1 1 24
1 23 1 23 23 1 23 1 23 23 1 23 1 23 23 34
0 0 0 1
x y z x y z x y z
x y x y x y
x y z x y z x y z
C C n S C n S n C C s S C s S s C C a S C a S a f
S n C n S s C s S a C a f
C S n S S n C n C S s S S s C s C S a S S a C a f
+ + + 1
1
+ + +
1
1 + + +
1
]

Ch cn quan tm ti phn nh hng tc l ma trn [3,3].
So snh cc phn t ca 2 ma trn [3,3] :
[ ]
3
6
3, 3 R
cos 5

23 1 1 23
( )
x y z
S C a S a C a +

5

23 1 1 23
cos ( )
x y z
Arc S C a S a C a 1 t +
]
Nu sin
5
0
9
[ ]
[ ]
3
6 4 5 1 1
1 1
4
5
3
6 4 5 23 1 1 23
23 1 1 23
4
5
1 1 23 1 1 23
4
5 5
2, 3 sin *sin * *
* *
sin
sin
1, 3 os *sin ( * * )
( * * )
os
sin
* * ( * * )
tan 2( , )
sin sin
x y
x y
x y z
x y z
x y x y z
R S a C a
S a C a
R c C C a S a S a
C C a S a S a
c
S a C a C C a S a S a
A

+
+

+
+

+ +

[ ]
[ ]
3
6 6 5 1 23 1 23 23
1 23 1 23 23
6
5
3
6 6 5 1 23 1 23 23
1 23 1 23 23
6
5
23 1 1 23 23 1 1
6
5
3, 2 sin *sin
sin
sin
3,1 os *sin
os
sin
( ) ( )
tan 2( ,
sin
x y z
x y z
x y z
x y z
x y z x y
R C S s S S s C s
C S s S S s C s
R c C S n S S n C n
C S n S S n C n
c
S C s S s C s S C n S n C
A

+ +
+ +

+ +
+ +

+ + + +

23
5
)
sin
z
n

Vy bi ton ng hc ngc cho ta kt qu nh sau:

2
1 6 6
2 2
6 6
cos tan 2( , )
( ) ( )
p x p y
p y p x
d
Ar A x a d y a d
y a d x a d

_

t

+
,
2 4 3 3 3 2 4 3 3 3 2 4 3 3 3 4 3 3 3
tan 2 ( ) ( ) , ( ) ( )
x y x
A d C a S P a d S a C P a d S a C P d C a S 1 + + + + +
]
2 2 2 2 2
3 2 4
3 4 2 2 3
2 2
2 4 3
( )
cos tan 2( , )
2
x y
P P a a d
Arc A d a a a
a d a

_
+ + +
t +

+
,
1 1 23 1 1 23
4
5 5
* * ( * * )
tan 2( , )
sin sin
x y x y z
S a C a C C a S a S a
A

+ +

5 23 1 1 23
cos ( )
x y z
Arc S C a S a C a 1 t +
]
23 1 1 23 23 1 1 23
6
5 5
( ) ( )
tan 2( , )
sin sin
x y z x y z
S C s S s C s S C n S n C n
A

+ + + +

10
V.THIT K M HNH ROBOT PUMA DNG SOLIDWORK.
1. Nhim v ca robot.
Gp mt vt nng khi lng di 1Kg di chuyn trong min lm vic
2. Phn tch v la chn kt cu.
2.1 La chn bi, ln cho c cu.
- Do tnh cht lm vic chu ti tng i nh (ti trng c khi lng di 1kg)
ca Robot nn ta s chn ln cho cc khu nh sau:
+ Khu 1: Dng bi mt dy c k hiu 305.
+ Khu 2: Dng bi mt dy c k hiu 1000902.
+ Khu 3: Dng bi mt dy c k hiu 1000901.
+ Khu 4: Dng bi mt dy c k hiu 1000901.
+ Khu 5: Dng bi mt dy c k hiu 1000099.
2.2 La chn ng c cho Robot.
Do yu cu iu khin chnh xc v tr im tc ng cui ca robot, ta chn
ng c cho cc khp quay l loi ng c Servo.
11
y l loi ng c cho php ta iu khin chnh xc c tc v v tr ca
rotor. ng c tch hp mt mch encoder, phn hi trng thi v tr ca trc ra,
da vo ta gim thiu c sai s do ti trng bn ngoi thay i
Da vo yu cu lm vic ca robot, ta chn c ng c cho cc khp nh
sau:
Khp S hiu DC Khp S hiu
1 T720-012 4 T406-012
2 T511-012 5 T404-012
3 T506-012 6 T402-011
Kch thc v thng s k thut cc ng c nh sau:
12
13
14
3. Thit k m hnh dng phn mm SolidWorks
Bn v chi tit phn thit k (2D v 3D) ca cc khu ca robot c km
theo phn ph lc. Say y l mt s hnh nh m hnh 3D ca robot.
15
VI. M PHNG NG HC ROBOT PUMA DNG VISUAL C++ V OPENGL
Giao din phn mm m phng c xy dng nh sau:
16
Phn mm c xy dng bng visual C++. Phn ha c lp trnh
bng cc th vin OpenGL. Giao din chng trnh gm hai phn chnh. Khung
bn phi cha cc nt lnh iu khin, v chn cc nhim v ca Robot cn thc
hin. Khung bn phi dng th hin m hnh ca robot c dng m phng.
M hnh robot c v bng phn mm SolidWork v xut ra file cad 3D di
dng STL sau dng Visual C++ c cc file ny vo chng trnh. Sau dng
cc hm OpenGL v cc m hnh ny.
Phn ng hc thun v ngc trong chng trnh c tnh trong cc hm
thnh vin SolveInverseKinematic v SolveForwardKinematic ca lp
CSerialRobot.
Cc hm ny nh sau:
void CSerialRobot::SolveInverseKinematic(const CFrame& end ,float* jointVar)
{
CVector nn =end.GetXDir ().GetDirection ();
17
CVector ss=end.GetYDir ().GetDirection ();
CVector aa=end.GetZDir ().GetDirection ();
//CVector aa =CrossProduct (nn,ss);
CPoint3D pp =end.GetOrigin ();
float nx=nn.GetX ();
float ny=nn.GetY ();
float nz=nn.GetZ ();
float sx =ss.GetX ();
float sy =ss.GetY ();
float sz =ss.GetZ ();
float ax =aa.GetX ();
float ay =aa.GetY ();
float az =aa.GetZ ();
float px =pp.GetX ();
float py=pp.GetY ();
float pz=pp.GetZ ();
//joint vars are contained here
float tt1,tt2,tt3,tt4,tt5,tt6;
//link and joint configue
float a2=ROBOT_A2;
float a3=ROBOT_A3;
float d1 =ROBOT_D1;
float d2=ROBOT_D2;
float d4=ROBOT_D4;
float d6=ROBOT_D6;
// calculate theta 1
18
float m =px -ax*d6;
float n =py -ay*d6;
tt1= acosf(d2/sqrt(m*m+n*n)) -atan2(m,n);
float s1=sinf(tt1);
float c1=cosf(tt1);
float K1 = n*s1 + m * c1;
float K2 = pz -az*d6 -d1;
//calulate theta 3
float p=K1*K1+K2*K2 -(a3*a3+a2*a2 +d4*d4);
float q=2*a2*sqrt(d4*d4+a3*a3);
tt3= acosf(p/q) + atan2(d4,a3);
//check the criteria
float c3=cosf(tt3);
float s3=sinf(tt3);
//calculate theta 2
p=a3*c3 +a2 +s3 *d4;
q=d4*c3-a3*s3;
float delta =p*p +q*q;
float deltac =p*K1 +q*K2;
float deltas =q*K1 - p*K2;
tt2= atan2(deltas,deltac);
float s2=sinf(tt2);
float c2=cosf(tt2);
//calculate theta 5;
19
float s23 =sinf(tt2+tt3);
float c23 =cosf(tt2+tt3);
m=c1*ax+s1*ay;
tt5 = acosf(s23*m- c23*az);
float c5= cosf(tt5);
float s5= sinf(tt5);
//calculate theta4
m=c1 *ax +s1*ay;
n=-s1*ax +c1*ay;
if(tt5!=0)
{
tt4 =atan2(n/s5, (c23*m - s23*az)/s5);
}
else
{
tt4=0;
}
//calculate theta 6
tt6 =atan2((s23*(c1*sx +s1*sy)+c23*sz)/s5,
-(s23*(c1*nx +s1*ny)+c23*nz)/s5);
tt1=r2d(tt1);
tt2 =r2d(tt2);
tt3 =r2d(tt3);
tt4 =r2d(tt4);
tt5 =r2d(tt5);
20
tt6 =r2d(tt6);
jointVar[0] =tt1;
jointVar[1] =tt2;
jointVar[2] =tt3;
jointVar[3] =tt4;
jointVar[4] =tt5;
jointVar[5] =tt6;
MoveTo (tt1,tt2,tt3,tt4,tt5,tt6);
}
void CSerialRobot::SolveForwardKinematic
(const float tt1,const float tt2,const float tt3,const float tt4,
const float tt5,const float tt6,CFrame* frm)
{
float c1 =cosf(d2r(tt1));
float c2 =cosf(d2r(tt2));
float c3 =cosf(d2r(tt3));
float c4 =cosf(d2r(tt4));
float c5 =cosf(d2r(tt5));
float c6 =cosf(d2r(tt6));
////
float s1 =sinf(d2r(tt1));
float s2 =sinf(d2r(tt2));
float s3 =sinf(d2r(tt3));
21
float s4 =sinf(d2r(tt4));
float s5 =sinf(d2r(tt5));
float s6 =sinf(d2r(tt6));
float d6=ROBOT_D6;
float d4=ROBOT_D4;
float d2=ROBOT_D2;
float a3=ROBOT_A3;
float a2=ROBOT_A2;
float px =d6*(c5*(c1*c2*s3 + c1*c3*s2) - s5*(s1*s4 - c4*(c1*c2*c3 -
c1*s2*s3))) - d2*s1 + d4*(c1*c2*s3 + c1*c3*s2) + a2*c1*c2 - a3*c1*s2*s3 +
a3*c1*c2*c3;
float py =c1*d2 + d6*(c5*(c2*s1*s3 + c3*s1*s2) + s5*(c1*s4 +
c4*(c2*c3*s1 - s1*s2*s3))) + d4*(c2*s1*s3 + c3*s1*s2) + a2*c2*s1 - a3*s1*s2*s3
+ a3*c2*c3*s1;
float pz =d4*(c2*c3 - s2*s3) - a2*s2 + d6*(c5*(c2*c3 - s2*s3) -
c4*s5*(c2*s3 + c3*s2)) - a3*c2*s3 - a3*c3*s2;
frm->SetOrigin (CPoint3D(px,py,pz));
}
Mt s kt qu m phng:
- im tc ng cui chy theo ng xon c:
22
- Bin dng chy theo ng helixcal:
23
- M phng gp mt vt:
24
25
KT LUN.
Robot cng nghip l mt b phn rt quan trng trong nn sn xut hin i.
N gp phn thay th sc lao ng ca con ngi, tng nng sut lao ng v to ra
nhng sn phm ngy cng phc tp v p ng nhu cu ngy cng cao ca con
ngi. Thng qua n ny, chng em phn no hiu r c tm quan trng
. Vi ti tm hiu v robot Puma, mt loi robot cng nghip ph bin v in
hnh, nhm phn tch, thit k v m phng c hot ng ca loi robot ny.
Cc vn c bn c gii quyt nh tnh ton ng hc thun, ng hc
ngc, thit k dng phn mm CAD, v m phng bng Visual C++ v OpenGL.
Bn cnh , do thi gian gp rt, vn cn cc vn khc cn tip tc gii quyt.
Chng em xin kin ng gp ca cc thy ti c hon thin hn. Mt ln
na nhm xin chn thnh cm n s hng dn tn tnh ca thy Nguyn Tun
Khoa v thy ng Bo Lm trong thi gian qua.
26
PH LC
27