Lun vn

Thit k qu o chuyn ng
cho c cu robot hai bc t do

Li ni u

LI NI U
Theo qu trnh pht trin ca x hi, nhu cu nng cao sn xut v cht lng sn
phm ngy cng i hi ng dng rng ri cc phng tin t ng ha sn xut. Xu
hng to ra nhng dy chuyn v thit b t ng c tnh linh hot cao hnh thnh v
pht trin mnh m V th ngy cng tng nhanh nhu cu ng dng ngi my to ra
cc h sn xut t ng linh hot. Robot ng dng rng ri v ng vai tr quan trng
trong sn xut cng nh trong i sng. Robot l c cu a chc nng c kh nng lp
trnh c dng di chuyn nguyn vt liu, cc chi tit, cc dng c thng qua cc
truyn ng c lp trnh trc. Khoa hc robot ch yu da vo cc php ton v i
s ma trn. Robot c th thao tc nh con ngi v c th hp tc vi nhau mt cnh
thng minh. Robot c cnh tay vi nhiu bc t do v c th thc hin c cc chuyn
ng nh tay ngi v iu khin c bng my tnh hoc c th iu khin bng
chng trnh.
n ca em c ti thit k qu o chuyn ng cho c cu robot hai bc t
do gm 4 chng:
Chng 1: Tng quan v robot cng nghip.
Chng 2: Xy dng phng trnh ng hc thun v ngc cho robot hai bc t do.
Chng 3: Thit k qu o cho robot.
Chng 4: Biu din qu o trn th.
Do kin thc v thi gian c hn nn n ca em khng th trnh c cc sai st
mong c thy c v cc bn bo ban, nhc nh thm.
Em xin chn thnh cm n TS. Nguyn Mnh Tin v cc bn trong nhm nhit
tnh gip em hon thnh n ny.
Sinh vin thc hin

Chng 1. Tng quan v robot cng nghip.

Mc Lc
LI NI U .....................................................................................................................1
CHNG 1 ......................................................................................................................... 3
TNG QUAN V ROBOT CNG NGHIP .....................................................................3
1.1. T ng ha v robot cng nghip.........................................................................3
1.2. Lch s pht trin ca robot ....................................................................................3
1.3. Cc c tnh ca robot cng nghip .......................................................................3
1.4. Cu to robot cng nghip .....................................................................................4
1.5. H thng iu khin robot ......................................................................................5
CHNG 2 ......................................................................................................................... 6
XY DNG PHNG TRNH NG HC THUN V NGC CHO ROBOT HAI
BC T DO ........................................................................................................................6
2.1. Cu to robot 2 thanh ni .......................................................................................6
2.2. Phng php t khung ta D-H .......................................................................6
2.3. Xy dng phng trnh c hc thun v ngc ...................................................7
CHNG 3 .......................................................................................................................11
THIT K QU O CHO ROBOT ..............................................................................11
3.1. Xy dng qu o ................................................................................................11
3.2. Xy dng qu o cho robot 2 thanh ni .............................................................12
CHNG 4 .......................................................................................................................14
BIU DIN QU O TRN TH ..........................................................................14
4.1. th qu o. .....................................................................................................14
4.2. Kt qu .................................................................................................................14
KT LUN ........................................................................................................................19
TI LIU THAM KHO .................................................................................................20

Chng 1. Tng quan v robot cng nghip.

CHNG 1
TNG QUAN V ROBOT CNG NGHIP
1.1. T ng ha v robot cng nghip
Robot c nh ngha di dnh cc kha cnh khc nhau, Robot c coi l mt
tay my c vi bc t do, c th iu khin bng my tnh. Mt nh ngha khc v robot
cng nghip hin nay l: robot cng nghip l mt c cu c kh c th lp trnh c v
c th thc hin nhng cng vic c ch mt cch t ng khng cn s tr gip trc tip
ca con ngi. hip hi nhng nh ch to nh s dng a ra nh ngha robot nh
sau: Robot l mt thit b c th thc hin c cc chc nng bnh thng nh con
ngi v c th hp tc vi nhau mt cch thng minh c tr tu nh con ngi
T ng ha v k thut robot l 2 lnh vc c lin quan mt thit vi nhau. V
phng din cng nghip, t ng ha l mt cng ngh lin kt vi s dng cc h
thng c kh, in t v h thng my tnh trong vn hnh v iu chnh qu trnh sn
xut. Robot c th coi l mt dng ca thit b t ng ha cng nghip.
1.2. Lch s pht trin ca robot
T th k 17, mt s thit b my mc c ch to c mt s t tnh lm vic
nh robot cng nghip hin nay. Jacques de Vancanson ch to mt vi bp b nhc
s
Vo nhng nm u th k 20, iu khin s v c cu iu khin t xa l hai cng
ngh quan trng cho s pht trin ca robot. Gip cho robot lm vic mt khong cch
xa di s iu khin ca con ngi, cng nh lm vic trong cc mt trng c hi
hoc nhng ni m con ngi khng th tip xc c.
Robot hin i l s kt hp ca k thut iu khin s v c cu t ng iu khin t
xa, c iu khin bng cc thit b c kh nng lp trnh.
1.3. Cc c tnh ca robot cng nghip
- Ti trng: Ti trng l trng lng robot c th mang v gi trong khi vn m bo
mt t tnh no . Ti trng cng phi nm trong mt phm vi cho php ca robot, nu
ti trng ln hn ti trng nh mc th robot khng th m bo chnh xc dch
chuyn. Ti trng robot thng rt nh hn trng lng robot. V d, robot LR Mate ca
hng Fanuc c trng lng 40kg ch mang c ti trng 3kg, Robot M-16i c trng
lng 269kg mang c ti trng 15,8kg.
- Tm vi: l khong cch ln nht m robot c th vn ti trong phm vi lm vic.
tm vi l mt hm ph thuc vo cu trc ca robot.
- phn gii khng gian: l lng gia tng nh nht robot c th thc hin khi di
chuyn trong khng gian. phn gii ph thuc vo phn gii iu khin, chnh
3

Chng 1. Tng quan v robot cng nghip.

xc c kh v h thng phn hi: l t s ca phm vi di chuyn v s bc di chuyn ca
khp c a ch ha trong b iu khin.
(n l s bit ca b nh)
S bc di chuyn = 2n
chnh xc c khi trong c cu truyn ng cc khp v khu phn hi ca h
thng iu khin servo s nh hng n phn gii. Cc yu t lm gim chnh xc
c kh nh khe h trong hp truyn, r r ca h thng thy lc, ti trng trn tay robot,
tc di chuyn iu kin bo dng chnh xc c kh s gim theo phn gii.
- chnh xc: nh gi chnh xc v tr tay robot c th t c. chnh xc
c nh ngha theo phn gii ca c cu chp hnh. chnh xc di chuyn n v
tr mong mun s ph thuc vo dch chuyn nh nht ca khp.
- lp li: nh gi chnh xc khi robot di chuyn vi ti mt im trong nhiu
ln hot ng (100m). do mt s yu t m robot khng th vi ti cng mt im trong
nhiu ln hot ng, m cc im vi ca robot nm trong mt ng trn c tm l
im ch mong mun. bn knh ng trn l lp li. lp li c ngha quan
trng hn chnh xc. lp li c xc nh bng thc nghim vi cc hng i v
ti trng khc nhau. lp li ca robot cng nghip thng thng l 0,025mm.
- nhn: biu th s dch chuyn ca im cui c tay robot p ng li lc hoc
mmen tc dng. nhn ln c ngha l c tay robot s dch chuyn nhiu c khi lc
tc dng v ngc li. n lm gim chnh dch chuyn khi robot mang ti trng. Ti
trng nng s lm cho cnh tay robot b dch chuyn.
1.4. Cu to robot cng nghip
- Khp Robot: l khu lin kt 2 thanh ni c chc nng truyn ng thc hin di
chuyn thanh ni. khp gn vi thn robot gi l khp ni vo, thanh ni ra s to chuyn
ng so vi thanh ni vo. Khp gm 2 loi: khp tnh tin v khp quay.
- C tay robot: C nhim v nh hng chnh xc cho bn tay Robot. Bn tay cn
nh hng chnh xc so vi vt gp vt.
- Bn tay Robot: c gn ln c tay thc hin cc nhim v, cng vic khc nhau
ty theo yu cu cng vic. C 2 loi ch yu: c cu bn kp v c cu dng c.
Cc dng c cu hnh hc v khng gian lm vic ca robot.
- C cu robot ta DECAC (3P) gm 3 khp tnh tin chuyn ng theo phng
ca cc trc ca h ta DECAC. Khng gian lm vic ca bn tay robot l mt hnh
hp ch nht.
- C cu robot tr: gm 2 khp tinh tin ca tr v 1 khp quay l cnh tay quay quanh
tr robot (RRP). Khng gian lm vic l mt hnh tr.
- C cu robot dng khp ni: gm 3 khp quay (3R) tng t cnh tay con ngi. y
l mt cu hnh robot c s dng rng ri trong cng nghip.

Chng 1. Tng quan v robot cng nghip.

- C cu robot SCARA (Selective Complian Assembly Robot Arm) c 2 khp quay c
trc song song nhau cho php robot chuyn ng trong mt mt phng nm ngang, mt
khp tinh tin di chuyn theo phng thng ng v mt khp quay nh hng bn tay
robot.
1.5. H thng iu khin robot
Gm c 2 loi iu khin: iu khin th v iu khin tinh.
1. iu khin th:
S xc nh lut iu khin thch hp tc , v tr do qu trnh chuyn ng
ca khp s bm st qu o c thit k trong thi gian qu nh nht.
2. iu khin tinh:
Lin quan n qu trnh khi robot di chuyn trong khng gian tip xc vi chi tit, lp
rp mt chi tit vo mt thit b my. Qu trnh ny yu cu c iu khin lc v v tr.
-

Phn loi:
iu khin
robot

iu khin th

iu khin tinh

(iu khin qu o)

(iu khin lc)

iu khin ta
khp

iu khin ta
Decac

iu khin tr
khng

iu khin hn
hp

Hnh 1.1. Cc phng php iu khin robot.

-

Trng hp iu khin robot theo qu o.

Vi gi thit tay Robot di chuyn khng ti v khng tip xc vi mi trng lm
vic th b iu khin qu o c thit k sao cho Robot c th bm theo mt qu o
t trc (di chuyn ca Robot l t do trong khng gian).

V Tr

Thit k qu
o

B iu
khin

ROBOT
Phn hi

Hnh 1.2. V d h thng iu khin theo qu o.

5

Chng 2. Thit k ng hc thun v ngc cho robot hai bc t do.

CHNG 2
XY DNG PHNG TRNH NG HC THUN V NGC
CHO ROBOT HAI BC T DO
2.1. Cu to robot 2 thanh ni
Robot 2 thanh ni c cu to nh hnh 2.1.

Hnh 2.1. Cu to robot 2 thanh ni.

Robot gm 2 khp quay ghp vi nhau bi hai thanh ni c di l1, l2. Tay ca
robot nm cui thanh 2.
2.2. Phng php t khung ta D-H

Hnh 2.2. Thit k khung ta thanh ni.

Xt 3 trc khp t cho nhau trong khng gian.
-

ai : di php tuyn chung ca khp i+1 v i.

ai-1 : di php tuyn chung ca khp i v i-1.
i : gc gia trc ca khp i+1 v khp i.
6

Chng 2. Thit k ng hc thun v ngc cho robot hai bc t do.

i : gc gia php tuyn chung ca trc khp i+1 v i, vi php tuyn chung ca
trc khp i v i-1.
Phng php bin i D-H.
- Khung ta thanh i c t trn trc khp i+1(cui thanh i), trc Zi trng vi trc
i+1.
- Trc xi c t theo phng php tuyn t i-1 n i.
Mt s trng hp c bit.
- Khi 2 trc Z khc nhau s khng c php tuyn chung gia 2 khp. khi im gc
ca khung ta l giao im ca hai trc v trc x s c t dc theo ng vung
gc ca mt phng cha 2 trc .
- Hai trc song song: s c v s php tuyn chung. Khi chn c php tuyn
chung trng vi php tuyn ca khp trc. Gc KT s chn sao cho di l nh nht.
- i vi khp tnh tin: di l bin khp. Hng ca trc khp trng vi hng di
chuyn ca khp. Hng ca trc c xc nh, nhng v tr trong khng gian khng
xc inh. Khi chiu di ai khng c ngha. Nn t ai = 0, gc ta trng vi gc
thanh ni tip theo.
2.3. Xy dng phng trnh c hc thun v ngc

Hnh 2.2. Cc khung ta ca robot.

Theo phng php chuyn i D-H.
Ta t gc ca robot trng vi khp quay th 1.
Khung ta th nht: trc x1 l phng ca thanh ni th nht. gc t ti cui
thanh ni th nht. bin khp l 1.
Chiu di php tuyn a1 = l1. Trc z1 l trc khp th hai.
Khung ta th hai. trc x2 l phng ca thanh ni th hai. gc t ti cui thanh
ni th hai. Bin khp l 2.
7

Chng 2. Thit k ng hc thun v ngc cho robot hai bc t do.

Chiu di php tuyn a2 = l2. Trc z2 l trc khp th hai.
Do ta c bng D-H nh bng 1.1:

ai

Bng 1.1
di
i

l1

l2

1. Phng trnh ng hc thun.

Din t php ton t cc bin khp i ta s c c hng v v tr ca tay robot.
T bng D-H ta c:
-

Quan h gia khung ta 1 vi khung ta gc c m t bng ma trn 0 T1 .

C1 S1
S1 C1
0
T1
0
0

0
0

Quan h gia khung ta 2 vi khung ta 1 c m t bng ma trn 1 T2 .

C 2 S 2
S 2 C2
1
T2
0
0

0
0

0 l1C1
0 l1S1
1 0

0 1

0 l2 C 2
0 l2 S 2
1
0

0
1

Phng trnh ng lc hc ca robot c m t bng tch ca 2 ma trn 0 T1 v 1 T2 .

C12 S12
S12 C12
T 0 T1 .1T2
0
0

0
0

0 l1C1 l2C12
0 l1S1 l2 S12

1
0

0
1

Vy ma trn ng nht T m t phng trnh ng hc thun ca robot.

Trong 3 ct u tin ch hng ca tay robot, ct th 4 ch v tr ca tay robot.
2. ng hc ngc:
Bi ton ng hc thun v tr v hng ca tay c xc nh t cc bin khp tuy
nhin trong bi ton ng hc ngc t v tr ca tay robot trong khng gian ta cn xc
nh gi tr ca cc bin khp tng ng vi v tr mong mun ca tay robot.
Bi ton ny thng kh gii v khng c li gii tng qut cho mi robot.
8

Chng 2. Thit k ng hc thun v ngc cho robot hai bc t do.

3. Xy dng phng trnh ng hc ngc:
T phng trnh ng hc thun ta c v tr ca tay robot xc nh theo phng trnh
sau:
px l1C1 l2C12

(2 1)

py l1S1 l2 S12

(2 2)

T phng trnh (2-1) v (2-2) bnh phng v ly tng 2 v:

px 2 py 2 l12 l2 2 2l1l2C 2

(2 3)

Ta tnh c 2 .
px 2 py 2 l12 l2 2
2 arcos (
)
2l1l2

2 atan2( 4l12l2 2 ( px 2 py 2 l12 l2 2 ) 2 , px 2 py 2 l12 l2 2 )

(2 4)

T hnh 2.2 ta nhn thy:

1 arctg (
sin

py
)
px

l2 S 2
px 2 py 2

(2 5)

(2 6)

T (2-5) v (2-6):
arcos (

1 arctg(

px 2 py 2 l12 l2 2
2l1 px 2 py 2

(2 7)

px 2 py 2 l12 l2 2
py
) arcos(
)
px
2l1 px 2 py 2

1 atan2(py, px) atan2( 4l12 ( px 2 py 2 ) ( px 2 py 2 l12 l2 2 ) 2 , px 2 py 2 l12 l2 2 )

Ta c 2 gi tr ca bin khp i cho mt v tr ca tay robot.

Hnh 2.3. Hai v tr ca thanh ni cho mt v tr ca tay robot.

9

Chng 2. Thit k ng hc thun v ngc cho robot hai bc t do.

Vy phng trnh ng hc ngc ca robot l:

1 atan2( py, px) atan2( 4l12 ( px 2 py 2 ) ( px 2 py 2 l12 l2 2 )2 , px 2 py 2 l12 l2 2 )

2 = atan2( 4l12 l2 2 (px 2 py 2 l12 l2 2 )2 , px 2 + py 2 l12 l2 2 )

4. Code matlab cho phng trnh ng thun v ngc:
Trng hp 1:
% Dong Hoc Nguoc
%THETA1(t)
THETA1=atan2(py,px)-atan2(sqrt((4*l1^2*(px^2+py^2)-(px^2+py^2+l1^2l2^2)^2)),(px^2+py^2+l1^2-l2^2));
%THETA2(t)
THETA2=atan2(sqrt(4*l1^2*l2^2-(px^2+py^2-l1^2-l2^2)^2),px^2+py^2-l1^2-l2^2);
% Dong Hoc Thuan
qbx=l1*cos(THETA1);
qby=l1*sin(THETA1);
qcx=l1*cos(THETA1)+l2*cos(THETA1+THETA2);
qcy=l1*sin(THETA1)+l2*sin(THETA1+THETA2);

Trng hp 2:
% Dong Hoc Nguoc
%THETA1(t)
THETA1=atan2(py,px)+atan2(sqrt((4*l1^2*(px^2+py^2)-(px^2+py^2+l1^2l2^2)^2)),(px^2+py^2+l1^2-l2^2));
%THETA2(t)
THETA2=-atan2(sqrt(4*l1^2*l2^2-(px^2+py^2-l1^2-l2^2)^2),px^2+py^2-l1^2-l2^2);
% Dong Hoc Thuan
qbx=l1*cos(THETA1);
qby=l1*sin(THETA1);
qcx=l1*cos(THETA1)+l2*cos(THETA1+THETA2);
qcy=l1*sin(THETA1)+l2*sin(THETA1+THETA2);

10

Chng 3. Thit k qu o.

CHNG 3
THIT K QU O CHO ROBOT
3.1. Xy dng qu o
Qu o chuyn ng lin quan mt thit n bi ton iu khin robot di chuyn t
v tr ny n v tr khc trong khng gian lm vic. ng i v qu o c thit k l
cc gi tr t cho robot. Qu o c thit k s nh hng n cht lng di chuyn
ca robot trong khng gian lm vic.
1. Phng php thit k qu o dng 2-1-2 cho robot
Qu o chuyn ng cho robot cng nghip thng c dng 2-1-2. th khp c
dng hnh thang vi gi thit tc ban u bng khng, gia tc chuyn ng ca khp
l hng s giai on khi ng ln giai on hm. m bo tnh i xng qu o,
im trung bnh c chn:
qm

qc qo
t t
v tm c o
2
2

Qu o phi di chuyn t qo n qc trong thi gian tc.

Tc cui khp ca on parabol phi bng tc cui ca on tuyn tnh:
1
qt

qm q1
tm t1

( 3 1)

Phng trnh biu din giai on khi ng t to n t1 l:

1
q( t ) qo q1t 2
2

Do v tr khp ti t1 l:
1
q1( t1 ) qo q1t12
2

(3 2)

T (3-1) v (3-2) ta c phng trnh:

q1t12 q1tc t1 qc qo 0

(33)

Gii phng trnh (3-3) ta c t1: t1

tc 1 tc 2 q1 4( qc qo )

2 2
q1

(3 4)

Suy ra gia tc trong on khi ng phi tho mn iu kin:

q1

4 qc qo
tc 2

Mt khc gia tc chuyn ng ca khp phi nh hn gia tc cho php tc l:

4 qc qo
q1 qcf
tc 2

11

Chng 3. Thit k qu o.
Vi cc thng s xc nh ta c phng trnh chuyn ng ca robot theo cc giai
on nh sau:
1 2

qo 2 q1t

q( t ) qo q1t1( t 1 )
2

2
qc 2 q1( tc t )

( 0 t t1 )
( t1 t tc t1 )
( tc t1 t tc )

3.2. Xy dng qu o cho robot 2 thanh ni

1. Cc thng s ca robot:
- Chiu di thanh ni th nht l1 = 0.35m.
- Chiu di thanh ni th hai l2 = 0.35m.
- V tr ban u qo = [0,3;0].
- V tr cui cng qc = [0;0,3].
- Qu o l mt ng thng.
- th tc cho nh hnh 3.1.

v (m/s)

0.25

0.75 1

t (s)

Hnh 3.1. th vn tc.

2. Phng trnh qu o.
- T phng trnh (3-3) rt ra c q1 theo cng thc (3-5):
q1

qc qo
t1tc t12

(35)

Vi t1 = 0.25s, t2 = 0,75s, tc = 1s.

p dng cng (3-4) ta tnh c gia tc khi ng chiu ln trc x l:
qx

pxc pxo
0 0 ,3
8

( m / s2 )
2
2
t1tc t1
0 , 25.1 0 , 25
5

12

Chng 3. Thit k qu o.
Suy ra qu o chuyn ng chiu ln trc x l:
1 2

( 0 t t1 )
pxo 2 qx t

px( t ) pxo qx .t1( t 1 ) (t1 t t2 )

2

2
pxc 2 qx .( tc t ) (t2 t tc )

p dng cng thc tnh gia tc (3-5) ta c gia tc chiu ln trc y l:

py pyo
0 ,3 0
8

( m / s2 )
qy c
2
2
t1tc t1
0 , 25.1 0 , 25
5

Tng t trc x ta c qu o chuyn ng chiu ln trc y l:

1 2

( 0 t t1 )
pyo 2 qy t

py( t ) pyo qy .t1( t 1 ) (t1 t t2 )

2

2
pyc 2 qy .( tc t ) (t2 t tc )

Sau khi c c phng trnh qu o ta nhp vo matlab:

%Thiet Ke Quy Dao
for t = 0:0.1:1,
if t<0.25
px=qxo+1/2*ddqx*t^2;
py=qyo+1/2*ddqy*t^2;
elseif t<0.75
px=qxo+ddqx*t1*(t-t1/2);
py=qyo+ddqy*t1*(t-t1/2);
elseif t<=1
px=qxc-1/2*ddqx*(tc-t)^2;
py=qyc-1/2*ddqy*(tc-t)^2;
end

13

Chng 4. Biu din qu o trn th.

CHNG 4
BIU DIN QU O TRN TH
4.1. th qu o
- Mfile: T 2 phng trnh qu o chiu ln trc x v y ta c on m trn matlab nh
sau.
%thong so thoi gian, vi tri dau va cuoi
t1=0.25; tc=1; qxo=0.3; qxc=0; qyo = 0; qyc = 0.3;
%Chieu dai 2 thanh noi
l1 = 0.35; l2 = 0.35;
%gia toc
ddqx=-8/5;
ddqy=8/5;
hold on;
%Thiet Ke Quy Dao
for t = 0:0.1:1,
if t<0.25
px=qxo+1/2*ddqx*t^2;
py=qyo+1/2*ddqy*t^2;
elseif t<0.75
px=qxo+ddqx*t1*(t-t1/2);
py=qyo+ddqy*t1*(t-t1/2);
elseif t<=1
px=qxc-1/2*ddqx*(tc-t)^2;
py=qyc-1/2*ddqy*(tc-t)^2;
end
% Dong Hoc Nguoc
THETA1=atan2(py,px)+atan2(sqrt((4*l1^2*(px^2+py^2)-(px^2+py^2+l1^2l2^2)^2)),(px^2+py^2+l1^2-l2^2));
THETA2=-atan2(sqrt(4*l1^2*l2^2-(px^2+py^2-l1^2-l2^2)^2),px^2+py^2-l1^2-l2^2);
%THETA1=atan2(py,px)-atan2(sqrt((4*l1^2*(px^2+py^2)-(px^2+py^2+l1^2l2^2)^2)),(px^2+py^2+l1^2-l2^2));
%THETA2=atan2(sqrt(4*l1^2*l2^2-(px^2+py^2-l1^2-l2^2)^2),px^2+py^2-l1^2-l2^2);
% Dong Hoc Thuan
qbx=l1*cos(THETA1);
qby=l1*sin(THETA1);
qcx=l1*cos(THETA1)+l2*cos(THETA1+THETA2);
qcy=l1*sin(THETA1)+l2*sin(THETA1+THETA2);
% Ve Do Thi
line([0 qbx],[0 qby]);
line([qbx qcx],[qby qcy]);
end

4.2. Kt qu
1. Trng hp 1.
THETA1=atan2(py,px)+atan2(sqrt((4*l1^2*(px^2+py^2)-(px^2+py^2+l1^2l2^2)^2)),(px^2+py^2+l1^2-l2^2));
THETA2=-atan2(sqrt(4*l1^2*l2^2-(px^2+py^2-l1^2-l2^2)^2),px^2+py^2-l1^2-l2^2);

14

Chng 4. Biu din qu o trn th.

0.4
0.35
0.3
0.25
0.2
0.15
0.1
0.05
0
-0.4

-0.3

-0.2

-0.1

0.1

0.2

0.3

Hnh 4.1. ng biu din v tr khp v tay robot.

0.35

0.3

0.25

0.2

0.15

0.1

0.05

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

0.8

0.9

Hnh 4.2. th biu din px(t).

0.35

0.3

0.25

0.2

0.15

0.1

0.05

0.1

0.2

0.3

0.4

0.5

0.6

0.7

Hnh 4.3. th biu din py(t).

15

Chng 4. Biu din qu o trn th.

2.8
2.6
2.4
2.2
2
1.8
1.6
1.4
1.2
1

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

0.8

0.9

Hnh 4.4. th biu din 1(t).

-2.2

-2.25

-2.3

-2.35

-2.4

-2.45

-2.5

-2.55

0.1

0.2

0.3

0.4

0.5

0.6

0.7

Hnh 4.5. th biu din 2(t).

16

Chng 4. Biu din qu o trn th.

2. Trng hp 2.
THETA1=atan2(py,px)-atan2(sqrt((4*l1^2*(px^2+py^2)-(px^2+py^2+l1^2l2^2)^2)),(px^2+py^2+l1^2-l2^2));
THETA2=atan2(sqrt(4*l1^2*l2^2-(px^2+py^2-l1^2-l2^2)^2),px^2+py^2-l1^2-l2^2);
0.4

0.3

0.2

0.1

-0.1

-0.2

-0.3

-0.4

0.1

0.2

0.3

0.4

Hnh 4.6. ng biu din v tr khp v tay robot.

0.6
0.4
0.2
0
-0.2
-0.4
-0.6
-0.8
-1
-1.2

0.1

0.2

0.3

0.4

0.5

0.6

0.7

Hnh 4.7. th biu din 1(t).

17

0.8

0.9

Chng 4. Biu din qu o trn th.

2.55

2.5

2.45

2.4

2.35

2.3

2.25

0.1

0.2

0.3

0.4

0.5

0.6

0.7

Hnh 4.8. th biu din 2(t).

ng biu din px(t) v py(t) tng t trng hp 1.

18

0.8

0.9

Kt lun

KT LUN
Sau khi lm xong n em hiu ngha ca cc phng trnh ng hc thun v
ngc, tnh ton v xy dng qu o chuyn ng cho robot, s dng matlab xy dng
c mt chng trnh tnh ton qu o, v th chuyn ng ca robot theo mt qu
o .
Tuy nhin, do thi gian, kin thc gii hn nn n ca em ny mi ch phng
trnh ton hc m t v tr chuyn ng (cc gi tr t v tr cho robot) cha xt n nh
hng khi lng, lc tc dng ln robot cng nh cha thit k b iu khin cho robot.
Mt ln na em xin cm n TS. Nguyn Mnh Tin tn tnh hng dn em hon
thnh n ny.
Sinh vin thc hin

19

Ti liu hng dn

TI LIU THAM KHO

[1]. Nguyn Mnh Tin - iu khin robot cng nghip. Nh xut bn Khoa hc v
K thut, H Ni - 2007.
[2]. Nguyn Phng Quang - Matlab v Simulink dnh cho k s iu khin t ng.
Nh xut bn Khoa hc v K thut, H Ni - 2004.

20