Do An CĐT

N THIT K H THNG C IN T GVHD:ThS Mc Th Thoa
Sinh vin thc hin: inh c Anh

Lp CT2- K55
1

MC LC

LI NI U .................................................................................................................................. 3
PHN I: GII THIU V ROBOT CNG NGHIP ..................................................................... 4
1.1. S LC QU TRNH PHT TRIN CA ROBOT CNG NGHIP (IR:
INDUSTRIAL ROBOT) ............................................................................................................... 4
1.2. NG DNG ROBOT CNG NGHIP TRONG SN XUT ........................................ 5
1.3. CC KHI NIM V NH NGHA V ROBOT CNG NGHIP ................................. 6
1.3.1. nh ngha robot cng nghip .......................................................................................... 6
1.3.2. Bc t do ca robot (DOF: Degrees Of Freedom) ........................................................... 7
1.3.3. H to (Coordinate frames) ......................................................................................... 7
1.3.4. Trng cng tc ca robot (Workspace or Range of motion) ......................................... 8
1.3. CU TRC C BN CA ROBOT CNG NGHIP .................................................... 9
1.4.1. Cc thnh phn chnh ca robot cng nghip ................................................................ 10
1.4.2. Kt cu ca tay my ....................................................................................................... 10
1.5. PHN LOI ROBOT CNG NGHIP .............................................................................. 11
1.5.1. Phn loi theo kt cu .................................................................................................... 11
1.5.2. Phn loi theo h thng truyn ng .............................................................................. 11
1.5.3. Phn loi theo ng dng................................................................................................. 11
1.5.4. Phn loi theo cch thc v c trng ca phng php iu khin ............................. 11
PHN II: THIT K M HNH 3D ROBOT RR ......................................................................... 13
2.1 M HNH 3D CA ROBOT PHNG HAI BC T DO ................................................. 13
PHN III: TNH TON NG HC THUN V NGC ROBOT ...................................... 16
3.1 T H TA ................................................................................................................ 16
3.2 CC XC NH CC THAM S NG HC CA BNG DH .................................... 17
3.3 THIT LP B THNG S NG HC DENAVIT HARTENBERG ....................... 17
3.4 THIT LP CC PHNG TRNH NG HC ROBOT ............................................. 19
3.5 BI TON NG HC THUN ...................................................................................... 21
3.5 BI TON NG HC NGC ....................................................................................... 27
PHN IV : BI TON TNH HC .............................................................................................. 31
4.1 TNH LC DN NG TI CC KHP M BO CN BNG TNH ..................... 31
4.2 XT KHU TC NG CUI.......................................................................................... 32
Lp CT2- K55
2

4.3 XT KHU TH 1: ........................................................................................................... 34
PHN V: TNH TON NG LC HC ................................................................................ 37
5.1 XY DNG CU TRC NG HC ............................................................................... 37
PHN VI: THIT K QU O CHUYN NG ................................................................... 41
6.1 THIT K QU O THEO KHNG GIAN KHP ........................................................ 41
6.2 THIT K QU O THEO KHNG GIAN LM VIC ................................................ 46
6.2.1 Qu o ca im tc ng cui theo ng thng t A n B trong t (s) ................... 46
6.2.2 Thit k qu o tc ng tc ng cui di chuyn theo ng trn t A n B trong
tc(s) ly AB lm ng knh .................................................................................................... 48
PHN VII : IU KHIN V M PHNG ROBOT .................................................................. 51
7.1 H THNG IU KHIN TRONG KHNG GIAN KHP .............................................. 51
7.1.1. H thng iu khin phn hi ........................................................................................ 51
7.1.2 Thit k cho robot RR .................................................................................................... 53
7.1.3 S dng phng trnh ng lc hc m phng iu khin trong simulink: .................. 54
7.2 S DNG M HNH XUT RA T SOLIDWORKS ...................................................... 58
PHN VIII M PHNG CHUYN NG CA ROBOT BNG ............................................ 63
PHN MM VISUAL STUDIO V TH VIN OPENGL ......................................................... 63
8.1.Thit k 3D ............................................................................................................................ 63
8.2 M phng hot ng ca robot. ............................................................................................. 63
PHN IX: KT LUN ................................................................................................................... 66
PHN X :CC CHNG TRNH IU KHIN V THIT K C KH CC BN V ...... 67
Phn XI : TI LIU THAM KHO .............................................................................................. 81

Lp CT2- K55
3

LI NI U
Trong s nghip cng nghip ha, hin i ha t nc vn t ng ha 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
ng..t ra l h thng sn xut phi c tnh linh hot cao.Robot cng nghip, c
bit l nhng tay my robot l b phn quan trng to ra nhng h thng .
Tay my Robot c mt trong sn xut t nhiu nm trc, ngy nay tay my
Robot dng nhiu lnh vc sn xut, xut pht t nhng u im m tay my
Robot v c kt li trong qu trnh sn xut lm vic, tay my c nhng tnh
nng m con ngi khng th c c, kh nng lm vic n nh, c th lm vic
trong mi trng c hi..Do vic u t nghic cu, ch to ra nhng loi tay
my Robot phc v cho cng cuc t ng ha sn xut l rt cn thit cho hin ti
v tng lai.
N THIT K H THNG C IN T gip chng em lm quen v tm
hiu k hn vi nhng vn ct li v c bn nht v robot v rt c ch cho chng
em sau ny. Qua chng em c th tm hiu su hn v tm hiu c cch tip
cn v gii quyt cc vn ca mn hc. ng thi qua lm n cng hnh thnh
thm cc k nng lm vic,lp k hoch,vit bo cort c ch cho sau ny.
Trong qu trnh lm n c rt nhiu vn t ra m trong phm vi kh nng
ca em cn hn ch c th cha gii quyt trit c, em rt mong c c ch
bo v hng dn thm.
Em xin chn thnh cm n !
Sinh vin thc hin:
inh c Anh

Lp CT2- K55
4

PHN I: GII THIU V ROBOT CNG NGHIP
1.1. S LC QU TRNH PHT TRIN CA ROBOT CNG NGHIP
(IR: INDUSTRIAL ROBOT)

Thut ng Robot xut pht t ting CH Sc (Czech) Robota c ngha
l cng vic tp dch trong v kch Rossums Universal Robots ca Karel
Capek, vo nm 1921. Trong v kch ny, Rossum v con trai ca ng ta
ch to ra nhng chic my gn ging vi con ngi phc v con ngi.
C l l mt gi ban u cho cc nh sng ch k thut v nhng c cu,
my mc bt chc cc hot ng c bp ca con ngi.
u thp k 60, cng ty M AMF (American Machine and Foundry
Company) qung co mt loi my t ng vn nng v gi l Ngi my
cng nghip (Industrial Robot). Ngy nay ngi ta t tn ngi my cng
nghip (hay robot cng nghip) cho nhng loi thit b c dng dp v mt
vi chc nng nh tay ngi c iu khin t ng thc hin mt s
thao tc sn xut.
V mt k thut, nhng robot cng nghip ngy nay, c ngun gc t hai
lnh vc k thut ra i sm hn l cc c cu iu khin t xa
(Teleoperators) v cc my cng c iu khin s (NC - Numerically
Controlled machine tool).
Cc c cu iu khin t xa (hay cc thit b kiu ch-t) pht trin
mnh trong chin tranh th gii ln th hai nhm nghin cu cc vt liu
phng x. Ngi thao tc c tch bit khi khu vc phng x bi mt bc
tng c mt hoc vi ca quan st c th nhn thy c cng vic bn
trong. Cc c cu iu khin t xa thay th cho cnh tay ca ngi thao tc;
n gm c mt b kp bn trong (t) v hai tay cm bn ngoi (ch). C
hai, tay cm v b kp, c ni vi nhau bng mt c cu su bc t do
to ra cc v tr v hng tu ca Tay cm v b kp. C cu dng iu
khin b kp theo chuyn ng ca tay cm.
Vo khong nm 1949, cc my cng c iu khin s ra i, nhm p
ng yu cu gia cng cc chi tit trong ngnh ch to my bay. Nhng robot
u tin thc cht l s ni kt gia cc khu c kh ca c cu iu khin t
xa vi kh nng lp trnh ca my cng c iu khin s.
Mt trong nhng Robot Cng nghip u tin c ch to l Robot
Versatran ca cng ty AMF, M. Cng vo khong thi gian ny M xut
hin loi robot Unimate (1900) c dng u tin trong k ngh t.
Tip theo M, cc nc khc bt u sn xut robot cng nghip: Anh
(1967), Thy in v Nht (1968) theo bn quyn ca M, CHLB c
(1971), Php (1972), (1973). . .
Tnh nng lm vic ca robot ngy cng c nng cao, nht l kh nng
Lp CT2- K55
5

nhn bit v x l. Nm 1967 trng i hc tng hp Stanford (M)
ch to ra mu robot hot ng theo m hnh mt-tay, c kh nng nhn
bit v nh hng bn kp theo v tr vt kp nh cc cm bin. Nm 1974
Cng ty M Cincinnati a ra loi robot c iu khin bng my vi tnh,
gi l robot T3 (The Tomorrow Tool: Cng c ca tng lai). Robot ny c
th nng c vt c khi lng n 40 KG.
C th ni, Robot l s t hp kh nng hot ng linh hot ca cc c cu
iu khin t xa vi mc tri thc ngy cng phong ph ca h thng
iu khin theo chng trnh s cng nh k thut ch to cc b cm bin,
cng ngh lp trnh v cc pht trin ca tr khn nhn to, h chuyn gia
Trong nhng nm sau ny, vic nng cao tnh nng hot ng ca robot
khng ngng pht trin. Cc robot c trang b thm cc loi cm bin khc
nhau nhn bit mi trng chung quanh, cng vi nhng thnh tu to ln
trong lnh vc Tin hc - in t to ra cc th h robot vi nhiu tnh nng
c bit, S lng robot ngy cng gia tng, gi thnh ngy cng gim. Nh
vy, robot cng nghip c v tr quan trng trong cc dy chuyn sn xut
hin i.
Mt vi s liu v s lng robot c sn xut mt vi nc cng
nghip pht trin nh sau:
Bng 1: s lng sn xut robot cc nc cng nghip

1.2. NG DNG ROBOT CNG NGHIP TRONG SN XUT

T khi mi ra i robot cng nghip c p dng trong nhiu lnh vc
di gc thay th sc ngi. Nh vy cc dy chuyn sn xut c t
chc li, nng sut v hiu qu sn xut tng ln r rt.
Nc SX
Nm
1990
Nm
1994
Nm
1998
Nht 66.118 29.756 67000
M 4.237 7.634 11000
c 5.845 5.125 8.600
2.500 2.408 4000
Php 1.448 1.197 2000
Anh 510 1086 1500
Hn Quc
1000 1200
Lp CT2- K55
6

Mc tiu ng dng robot cng nghip nhm gp phn nng cao nng sut
dy chuyn cng ngh, gim gi thnh, nng cao cht lng v kh nng cnh
tranh ca sn phm ng thi ci thin iu kin lao ng. t c cc mc
tiu trn l nh vo nhng kh nng to ln ca robot nh : lm vic khng
bit mt mi, rt d dng chuyn ngh mt cch thnh tho, chu c phng
x v cc mi trng lm vic c hi, nhit cao, cm thy c c t
trng v nghe c c siu m ... Robot c dng thay th con ngi
trong cc trng hp trn hoc thc hin cc cng vic tuy khng nng nhc
nhng n iu, d gy mt mi, nhm ln.
Trong ngnh c kh, robot c s dng nhiu trong cng ngh c, cng
ngh hn, ct kim loi, sn, phun ph kim loi, tho lp vn chuyn phi, lp
rp sn phm . . .
Ngy nay xut hin nhiu dy chuyn sn xut t ng gm cc my
CNC vi Robot cng nghip, cc dy chuyn t mc t ng ho cao,
mc linh hot cao . . . y cc my v robot c iu khin bng cng
mt h thng chng trnh.
Ngoi cc phn xng, nh my, k thut robot cng c s dng trong
vic khai thc thm lc a v i dng, trong y hc, s dng trong quc
phng, trong chinh phc v tr, trong cng nghip nguyn t, trong cc lnh
vc x hi . . .
R rng l kh nng lm vic ca robot trong mt s iu kin vt hn
kh nng ca con ngi; do n l phng tin hu hiu t ng ho,
nng cao nng sut lao ng, gim nh cho con ngi nhng cng vic nng
nhc v c hi. Nhc im ln nht ca robot l cha linh hot nh con
ngi, trong dy chuyn t ng, nu c mt robot b hng c th lm ngng
hot ng ca c dy chuyn, cho nn robot vn lun hot ng di s gim
st ca con ngi.
1.3. CC KHI NIM V NH NGHA V ROBOT CNG NGHIP
1.3.1. nh ngha robot cng nghip
nh ngha theo tiu chun AFNOR (Php):
Robot cng nghip l c cu chuyn ng t ng c th lp trnh, lp li
cc chng trnh, tng hp cc chng trnh t ra trn cc trc to ; c kh
nng nh v, nh hng, di chuyn cc i tng vt cht: chi tit, dao c,
g lp theo nhng hnh trnh thay i chng trnh ho nhm thc hin
cc nhim v cng ngh khc nhau.
nh ngha theo RIA (Robot institute of America):
Robot l mt tay my vn nng c th lp li cc chng trnh c thit
k di chuyn vt liu, chi tit, dng c hoc cc thit b chuyn dng thng
qua cc chng trnh chuyn ng c th thay i hon thnh cc nhim
v khc nhau.
Lp CT2- K55
7

nh ngha theo GOCT 25686-85 (Nga):
Robot cng nghip l mt my t ng, c t c nh hoc di ng
c, lin kt gia mt tay my v mt h thng iu khin theo chng
trnh, c th lp trnh li hon thnh cc chc nng vn ng v iu khin
trong qu trnh sn xut.
C th ni Robot cng nghip l mt my t ng linh hot thay th tng
phn hoc ton b cc hot ng c bp v hot ng tr tu ca con ngi
trong nhiu kh nng thch nghi khc nhau.
Robot cng nghip c kh nng chng trnh ho linh hot trn nhiu trc
chuyn ng, biu th cho s bc t do ca chng. Robot cng nghip c
trang b nhng bn tay my hoc cc c cu chp hnh, gii quyt nhng
nhim v xc nh trong cc qu trnh cng ngh : hoc trc tip tham gia
thc hin cc nguyn cng (sn, hn, phun ph, rt kim loi vo khun c,
lp rp my . . .) hoc phc v cc qu trnh cng ngh (tho lp chi tit gia
cng, dao c, g . . .) vi nhng thao tc cm nm, vn chuyn v trao i
cc i tng vi cc trm cng ngh, trong mt h thng my t ng linh
hot, c gi l H thng t ng linh hot robot ho cho php thch ng
nhanh v thao tc n gin khi nhim v sn xut thay i.
1.3.2. Bc t do ca robot (DOF: Degrees Of Freedom)
Bc t do l s kh nng chuyn ng ca mt c cu (chuyn ng quay
hoc tnh tin). dch chuyn c mt vt th trong khng gian, c cu
chp hnh ca robot phi t c mt s bc t do. Ni chung c h ca
robot l mt c cu h, do bc t do ca n c th tnh theo cng thc:

=
=
5
1
6
i
i
ip n w
y: n - S khu ng;
p
i
- S khp loi i (i = 1, 2,...,5 : S bc t do b hn ch).
i vi cc c cu c cc khu c ni vi nhau bng khp quay
hoc tnh tin (khp ng loi 5) th s bc t do bng vi s khu ng...
i vi c cu h, s bc t do bng tng s bc t do ca cc khp ng.
nh v v nh hng khu chp hnh cui mt cch tu trong
khng gian 3 chiu robot cn c 6 bc t do, trong 3 bc t do nh v
v 3 bc t do nh hng. Mt s cng vic n gin nng h, sp xp...
c th yu cu s bc t do t hn. Cc robot hn, sn... thng yu cu 6 bc
t do. Trong mt s trng hp cn s kho lo, linh hot hoc khi cn phi
ti u ho qu o... ngi ta dng robot vi s bc t do ln hn 6.
1.3.3. H to (Coordinate frames)
Mi robot thng bao gm nhiu khu (links) lin kt vi nhau qua cc
khp (joints), to thnh mt xch ng hc xut pht t mt khu c bn
Lp CT2- K55
8

(base) ng yn. H to gn vi khu c bn gi l h to c bn (hay
h to chun). Cc h to trung gian khc gn vi cc khu ng gi l
h to suy rng. Trong tng thi im hot ng, cc to suy rng xc
nh cu hnh ca robot bng cc chuyn dch di hoc cc chuyn dch gc
ca cc khp tnh tin hoc khp quay. Cc to suy rng cn c gi l
bin khp (Hnh 1.1)

Cc h to gn trn cc khu ca robot phi tun theo qui tc bn tay
phi: Dng tay phi, nm hai ngn tay t v p t vo lng bn tay, xo 3
ngn : ci, tr v gia theo 3 phng vung gc nhau, nu chn ngn ci l
phng v chiu ca trc z, th ngn tr ch phng, chiu ca trc x v ngn
gia s biu th phng, chiu ca trc y (hnh 1.2).
Trong robot ta thng dng ch O v ch s n ch h to gn trn
khu th n. Nh vy h to c bn (H to gn vi khu c nh) s
c k hiu l O
0
; h to gn trn cc khu trung gian tng ng s l O
1
,
O
2
,..., On-1, H to gn trn khu chp hnh cui k hiu l On.
1.3.4. Trng cng tc ca robot (Workspace or Range of motion)
Trng cng tc (hay vng lm vic, khng gian cng tc) ca robot l
ton b th tch c qut bi khu chp hnh cui khi robot thc hin tt c
cc chuyn ng c th. Trng cng tc b rng buc bi cc thng s hnh
hc ca robot cng nh cc rng buc c hc ca cc khp; v d, mt khp
Lp CT2- K55
9

quay c chuyn ng nh hn mt gc 360
0
. Ngi ta thng dng hai hnh
chiu m t trng cng tc ca mt robot (hnh 1.3).

1.3. CU TRC C BN CA ROBOT CNG NGHIP

Hnh 2. S khi ca Robot cng nghip
Lp CT2- K55
10

1.4.1. Cc thnh phn chnh ca robot cng nghip
Mt robot cng nghip thng bao gm cc thnh phn chnh nh : cnh
tay robot, ngun ng lc, dng c gn ln khu chp hnh cui, cc cm
bin, b iu khin , thit b dy hc, my tnh ... cc phn mm lp trnh
cng nn c coi l mt thnh phn ca h thng robot.
Cnh tay robot (tay my) l kt cu c kh gm cc khu lin kt vi nhau
bng cc khp ng c th to nn nhng chuyn ng c bn ca robot.
Ngun ng lc l cc ng c in (mt chiu hoc ng c bc), cc
h thng xy lanh kh nn, thu lc to ng lc cho tay my hot ng.
Dng c thao tc c gn trn khu cui ca robot, dng c ca robot c
th c nhiu kiu khc nhau nh: dng bn tay nm bt i tng hoc cc
cng c lm vic nh m hn, mi, u phun sn...
Thit b dy-hoc (Teach-Pendant) dng dy cho robot cc thao tc cn
thit theo yu cu ca qu trnh lm vic, sau robot t lp li cc ng tc
c dy lm vic (phng php lp trnh kiu dy hc).
Cc phn mm lp trnh v cc chng trnh iu khin robot c ci
t trn my tnh, dng iu khin robot thng qua b iu khin (Controller).
B iu khin cn c gi l Moun iu khin (hay Unit, Driver), n
thng c kt ni vi my tnh. Mt moun iu khin c th cn c cc
cng Vo - Ra (I/O port) lm vic vi nhiu thit b khc nhau nh cc
cm bin gip robot nhn bit trng thi ca bn thn, xc nh v tr ca i
tng lm vic hoc cc d tm khc; iu khin cc bng ti hoc c cu cp
phi hot ng phi hp vi robot...

1.4.2. Kt cu ca tay my
Nh ni trn, tay my l thnh phn quan trng, n quyt nh kh nng
Lp CT2- K55
11

lm vic ca robot. Cc kt cu ca nhiu tay my c phng theo cu to
v chc nng ca tay ngi; tuy nhin ngy nay, tay my c thit k rt a
dng, nhiu cnh tay robot c hnh dng rt khc xa cnh tay ngi. Trong
thit k v s dng tay my, chng ta cn quan tm n cc thng s hnh -
ng hc, l nhng thng s lin quan n kh nng lm vic ca robot nh:
tm vi (hay trng cng tc), s bc t do (th hin s kho lo linh hot ca
robot), cng vng, ti trng vt nng, lc kp . . .
Cc khu ca robot thng thc hin hai chuyn ng c bn:
Chuyn ng tnh tin theo hng x, y, z trong khng gian
Descarde, thng thng to nn cc hnh khi.
Chuyn ng xoay theo cc trc x, y, z trong khng gian.
Cc chuyn ng ny thng k hiu l T (Translation) hoc P
(Prismatic).

1.5. PHN LOI ROBOT CNG NGHIP
1.5.1. Phn loi theo kt cu
Theo kt cu ca tay my ngi ta phn thnh robot kiu to cc,
Kiu to tr, kiu to cu, kiu to gc, robot kiu SCARA nh
trnh by trn.
1.5.2. Phn loi theo h thng truyn ng
C cc dng truyn ng ph bin l:
H truyn ng in: Thng dng cc ng c in 1 chiu (DC: Direct
Current) hoc cc ng c bc (step motor). Loi truyn ng ny d iu
khin, kt cu gn.
H truyn ng thu lc: c th t c cng sut cao, p ng nhng
iu kin lm vic nng. Tuy nhin h thng thu lc thng c kt cu cng
knh, tn ti phi tuyn ln kh x l khi iu khin.
H truyn ng kh nn: c kt cu gn nh hn do khng cn dn ngc
nhng li phi gn lin vi trung tm to ra kh nn. H ny lm vic vi
cng sut trung bnh v nh, km chnh xc, thng ch thch hp vi cc
robot hot ng theo chng trnh nh sn vi cc thao tc n gin nhc
ln - t xung (Pick and Place or PTP: Point To Point).
1.5.3. Phn loi theo ng dng
Da vo ng dng ca robot trong sn xut c Robot sn, robot hn, robot
lp rp, robot chuyn phi .v.v...
1.5.4. Phn loi theo cch thc v c trng ca phng php iu khin
C robot iu khin h (mch iu khin khng c cc quan h phn hi),
Lp CT2- K55
12

Robot iu khin kn (hay iu khin servo): s dng cm bin, mch phn
hi tng chnh xc v mc linh hot khi iu khin.
Ngoi ra cn c th c cc cch phn loi khc tu theo quan im v mc
ch nghin cu.
S iu khin c bn :

Hnh 1.5 S iu khin h

Hnh 1.6 s iu khin kn

Lp CT2- K55
13

PHN II: THIT K M HNH 3D ROBOT RR
S dng phn mm thit k 3D Solidwork
2.1 M HNH 3D CA ROBOT PHNG HAI BC T DO
Thit k c kh ca khu v cc khu:
Khu :

Hnh 2.1 Khu
Khu 1: Khu 2

Hnh 2.2:khu 1 Hnh2 .3 Khu 2
Lp CT2- K55
14

Cc chi tit khc:

Hnh 2.4 Bnh rng khu 1,2 Hnh 2.5 Then

Hnh 2.6 Np hp bnh rng Hnh 2.7 Trc bnh rng
Lp CT2- K55
15

Hnh 2.8 Khp bnh rng 1 Hnh 2.9 Khp bnh rng 2
C cu phng hai bc t do RR c thit k nh sau:

Hnh 2.10 M hnh 3D ca ROBOT phng hai bc t do
(chi tit v cc thng s xem thm phn ph lc pha cui)
Lp CT2- K55
16

PHN III: TNH TON NG HC THUN V NGC
ROBOT
3.1 T H TA

Hnh 3.1 h trc ta ca robot

Theo Denavit Hartenberg (1955) quy c h ta Decard gn vo mi khu
ca mt tay my Robot nh sau:
* Trc
c chn dc theo trc ca khp th (i+1).

Hng ca php quay v php tnh tin c chn ty .
* Trc
c xc nh dc theo ng vung gc chung gia trc khp ng

th i v (i+ 1), hng t khp ng th i ti trc ( i+1).
* Trc
c xc nh theo quy tc bn tay phi.

T quy tc trn ta xy dng cc ta kho st (Hnh 2.1).
- H ta
0
0
:
Lp CT2- K55
17

Gc ta t ti tm ca khp ng th nht
Trc
0
0
dc theo hng ca trc khp ng th nht hng t trong ra
ngoi.
Trc
0
0
c phng vung gc vi
0
v ta chn v tr nm ngang.
Trc
0
0
xc nh theo quy tc bn tay phi.
- H ta
1
1
:
Gc ta t ti tm ca khp ng th 2
Trc
1
1
c phng hng theo trc khp ng th 2 vung gc vi mt
phng hnh v v c chiu hng t rong ra ngoi (nh hnh v)
Trc
1
1
c phng nm trn ng vung gc chung ca trc
0
0
v
1
do
0
0
//
1
1
nn ta chn
1
1
c hng dc theo khu 1 nh hnh v
Trc
1
1
- H ta
2
2
:
Gc ta t ti u khu th 2.
Trc
2
2
c phng // vi
1
1
v c chiu hng t trong ra ngoi
Trc
2
2
c phng nm trn ng vung gc chung ca trc
1
1
v
2
2
do
ta chn phng dc theo trc ca khu 2
Trc
2
2
3.2 CC XC NH CC THAM S NG HC CA BNG DH
V tr ca h ta khp ()
i vi h ta khp ()
1
c xc nh
bi 4 tham s
nh sau:
: gc quay quanh trc

1
trc
1
trng vi trc

(
//
: dch chuyn tnh ti dc trc

1
gc ta
1
chuy n

l giao
im ca trc
v trc
1

: dch chuyn dc trc
im
chuyn n im
: gc quay quanh trc
sao cho trc

1
(
1
//
1
) trng vi trc

3.3 THIT LP B THNG S NG HC DENAVIT HARTENBERG
Vi cch thnh lp h ta nh trn ta c th xc nh cc tham s ng hc ca
robot nh sau:

Lp CT2- K55
18

Bng 3.1 bng DH cc thng s ng hc ca robot RR
khu

1
1
0 a
1
0
2
2
0 a
2
0

Trong
1 2
, u u l cc bin khp , cn a
1
, a
2
l cc hng s
V X=[x
1
,x
2
,]
T
l vc t biu din v tr ca bn kp trong h c nh.
q=[q
1
,q
2
] vi q
1 =
u 1, q
2 =
u 2 l cc gc xoay cc bin khp
vi gii hn cc gc quay
1 2
7 2 2
: , :
6 6 3 3
q q
t t t t
= = th ta c vng khng gian
lm vic ca Robot nh sau:

Hnh 3.2 Khng gian lm vic ca Robot
- Dng tng qut ca ma trn Denavit-Hartenberg cho cc khu
Lp CT2- K55
19

i-1
A
i
=
i i i i i i i
i i i i i i i
i i i
cos sin cos sin sin a cos
sin cos cos sin cos a sin
0 sin cos d
0 0 0 1
(
(
(
(
(

Ma trn Denavit-Hartenbergca khu 1:

( )
1 1 1 1
1 1 1 1 0
1
cos sinq 0 .cos
sin cos 0 .sin
3.1
0 0 1 0
0 0 0 1
q a q
q q a q
A
(
(
(
=
(
(

Ma trn Denavit-Hartenbergca khu 2:

( )
2 2 2 2
2 2 2 2 1
2
cos sin 0 .cos
sin cos 0 .sin
3.2
0 0 1 0
0 0 0 1
q q a q
q q a q
A
(
(
(
=
(
(

3.4 THIT LP CC PHNG TRNH NG HC ROBOT
T cc ma trn (2.1) v (2.2) ta xc nh c ma trn bin i ta thun nht
ca khu 2 so vi trc h ta c nh
0
0
l :
1 2 1 2 1 1 2 1 2
1 2 1 2 1 1 2 1 2 0 0 1
2 1 2
cos( ) sin( ) 0 .cos .cos( )
sin( ) cos( ) 0 .sin .sin( )
. (3.3)
0 0 1 0
0 0 0 1
q q q q a q a q q
q q q q a q a q q
A A A
+ + + +
(
(
+ + + +
(
= =
(
(

- Ma trn
0
2
A
cho ta bit hng v v tr ca khu thao tc trong h ta c nh
hay ni cch khc l v tr ca im tc ng cui v hng ca h ta ng gn
vo khu ti im tc ng cui trong h ta c nh. V th n cn c biu
din qua thng s cc bin khp ta tm gi l q
i.
Trong bi ton c th th n l cc
khp xoay
i
, vi i=12.

Lp CT2- K55
20

- Khi , ma trn (2.3) c k hiu thnh
0
2
( ) A q

S dng cc gc Cardan xc nh hng ca vt rn. Ta gi | |
, , , , ,
E E E
x y z o | q
l gi
tr m t trc tip v tr v hng ca EX
2
Y
2
Z
2
so vi h ta O
0
Z
0
Y
0
Z
0
. Trong
: | |
, ,
E E E
x y z
l cc ta im E v [, , ] l cc gc quay Cardan ca
EX
2
Y
2
Z
2
so vi h ta O
0
Z
0
Y
0
Z
0
. Do cc ta thao tc u l hm ca thi
gian. Nn ta c th biu din:

0 0
0
( ) ( )
( ) (3.4)
0 1
n E
n
T
R t r t
A t
(
=
(

Vi:
0
E
A l ma trn Cardan m ta hng EX
2
Y
2
Z
2
so vi h ta O
0
Z
0
Y
0
Z
0
.

0
E
r l vect m t v tr ca im tc ng cui trong h ta
O
0
Z
0
Y
0
Z
0
.
| |
0
( ) (t), (t), ( ) (3.5)
T
E E E E
r t x y z t =
0
cos cos cos sin sin
sin sin cos cos sin sin sin sin cos cos sin cos (3.6)
cos sin cos sin sin cos sin sin sin cos cos s co
CD n
R R
| q | q |
o | q o q o | q o q o |
o | q o q o | q o q o |
(
(
= + +
(
( + +

=

- Do ma trn
0
2
( ) A q
biu din v tr v hng ca khu thao tc trong h ta
c nh thng qua bin khp q
i
(Ma trn trng thi khu thao tc theo cu trc ng
hc). Cn ma trn
0
( )
E
A t
cng m t v tr v hng ca khu thao tc thng qua
h ta khu thao tc. y ta chn cch biu din thng qua cc gc Cardan.
T ta c PT ng hc ROBOT c dng:
0 0
2
( ) ( )
n
A q A t =
. Vi
n = 2 v c cu ROBOT c 2 khu.
0 0
2 2
( ) ( ) (3.7) A q A t =

- T cc h thc (3.3), (3.4), (3.5),(3.6), (3.7) Ta xy dng c h 6 phng
trnh c lp nh sau:

Lp CT2- K55
21

0 0
1 2 2 1 1 2 1 2
0 0
2 2 2 1 1 2 1 2
0 0
3 2 2
0 0
4 2 2 1 2
0
5
( )[1, 4] ( )[1, 4] [ .cos .cos( )] 0
( )[2, 4] ( )[2, 4] [ .sin .sin( )] 0
( )[3, 4] ( )[3, 4] 0 0
( )[1,1] ( )[1,1] cos( ) (cos cos ) 0
E
E
E
f A q A t a q a q q x
f A q A t a q a q q y
f A q A t z
f A q A t q q
f A
| q
= = + + =
= = + + =
= = =
= = + =
=
0
2 2 1 2
0 0
6 2 2
(3.8)
( )[2, 2] ( )[2, 2] cos( ) ( sin sin sin cos cos ) 0
( )[3, 3] ( )[3, 3] 0 (cos cos ) 0
q A t q q
f A q A t
o | q o q
o |
= + + =
= = =

3.5 BI TON NG HC THUN

Bi ton ng hc thun: Cc thng s u vo: q
1
, q
2

Thng s cn xc nh: im tc ng cui E = (
E
x ,
E
y ,
E
z ), v hng ca khu tc
ng cui so vi h ta c s.
* Ta c ta ca im tc ng cui:
| |
| |
| |
0
3 1 1 2 1 2
0
3 1 1 2 1 2
0
3
1, 4 .cos .cos( )
2, 4 .sin .sin( ) (3.9)
3, 4 0
E
E
E
A a q a q q
A a q
x
y
z
a q q
A
= = + +
= = + +
= =

Ta chn quy lut chuyn ng cho robot trong thi gian 10s nh sau :
( )
.
1
1
2
2
( ) sin(2 )
( ) 2cos( t)
3
3.10
( ) sin( )
3 cos( )
3 3
q t t
q t
q t t
q t
t
t
t t
-

=

=

V cc thng s hnh hc:
a
1
= 65 cm, a
2
= 55cm ( VCK05-2_12)
thun tin cho vic tnh ton ta s dng phn mm tnh ton maple
Thay cc gi tr
1 2
( ) , ( ) q t q t v cc thng s hnh hc a
1
, a
2
vo biu thc ca
x
E
,y
E
,z
E
ta c ta c ta ca khu thao l :
( )
65cos(sin(2 )) 55cos(sin(2 ) sin( )) cm
3
y 65sin(sin(2 )) 55sin(sin(2 ) sin( )) cm 3.11
3
z 0
E
E
E
x t t t
t t t
t
t
= + +
= + +

Lp CT2- K55
22

Ta c th v th ca im thao tc cui qua cc ta nh sau:

Hnh 3.3 th xE,yE,zE theo t

Hnh 3.4 th qu o chuyn ng ca im tc ng cui
Lp CT2- K55
23

Tnh vn tc im tc ng cui E, vn tc gc khu thao tc
Vn tc im tc ng cui ca Robot
T phn trn ta xy dng c quy lut chuyn cng nh tm c ta ca
khu thao tc cui, cc bin khp v o hm cc cp theo t bit :
1 2
[ , ]
T
i
q q q =

Suy ra
( )
1 2
[ , ] 3.12
T
q q q =

Vn tc ca khu thao tc chnh l o hm v tr khu thao tc theo thi gian:
V
E
=r
E
=| | ( ) , , 3.13
T
E E E
x y z

T cc phng trnh v v tr ca im tc ng cui ca robot ta tnh c vn tc
ca im cui
t
55sin(sin(2t) + sin( )).(2cos(2t) + cos( ))-130.cos(2t).sin(sin(2.t)) cm
3 3 3
t
55cos(sin(2t) + sin( )).(2cos(2t) + cos( )) + 130.cos(2t).cos(sin(2.t)) cm
3 3 3
0
E
Ex
Ey E
E
Ez
t
v x
t
v y
v z
t t t
t t t
-
-
-
= =
= =
= =

th vn tc im E:

Hnh 3.5 th vn tc im E
Lp CT2- K55
24

o hm cc biu thc v
Ex
,v
Ey
,v
Ez
theo thi gian ta thu c cc biu thc gia tc
di ca im E
Ex
0
Ex
Ey
Ey
Ez
Ez
a v
a v
a v
= =

Khi ta c th gia tc im E:

Hnh 3.6 : th gia tc (di) im E
Bi ton xc nh vn tc gc cc khu :
Vn tc gc ca khu 1 :
1 1
1 1 1 1 1
1 1
0
. 0 (3.14)
0
z y
T
z x
y x
R R
e e
e e e
e e
(
(
= =
(
(

Vi R
1
l ma trn cosin ch hng ca khu 1 v c ly t (3.1):
Ta c :
Lp CT2- K55
25

( )
1 1
1 1 1
cos sin 0
sin cos 0 3.15
0 0 1
q q
R q q
(
(
=
(
(

( )
1 1
1 1 1 1
sin cos 0
cos sin 0 . 3.16
0 0 0
q q
R q q q
-

(
| | (
=
|
(
\ .
(

Cui cng nhn 2 ma trn ta c vn tc gc ca khu 1:
1 1 1 1
1 1 1 1 1 1 1 1
sin cos 0 cos sin 0
. cos sin 0 sin cos 0 . (3.17)
0 0 0 0 0 1
T
q q q q
R R q q q q q e
-

( (
| | ( (
= =
|
( (
\ .
( (

Suy ra c :

( )
1 1 1
1
0 1 0 0
1 0 0 . 0 3.18
0 0 0
q
q
e e
-
-
(
(
(
| | (
( = =
|
(
\ . (
(
(

Vn tc gc khu 2:
2 2
2 2 2 2 2
2 2
0
. 0 (3.19)
0
z y
T
z x
y x
R R
e e
e e e
e e
(
(
= =
(
(

Vi R
2
l ma trn cosin ch hng ca khu 2 v c ly t (3.2):
Ta c :
( )
2 2
2 2 2
cos sin 0
sin cos 0 3.20
0 0 1
q q
R q q
(
(
=
(
(

( )
2 2
2 2 2 2
sin cos 0
cos sin 0 . 3.21
0 0 0
q q
R q q q
-

(
| | (
=
|
(
\ .
(

Lp CT2- K55
26

Cui cng nhn 2 ma trn ta c vn tc gc ca khu 2:
2 2 2 2
2 2 2 2 2 2 2 2
sin cos 0 cos sin 0
. cos sin 0 sin cos 0 . (3.22)
0 0 0 0 0 1
T
q q q q
R R q q q q q e
-

( (
| | ( (
= =
|
( (
\ .
( (

Suy ra c :

( )
2 2 2
2
0 1 0 0
1 0 0 . 0 3.23
0 0 0
q
q
e e
-
-
(
(
(
| | (
( = =
|
(
\ . (
(
(

Vn tc gc khu thao tc cui E:
0
. 0 (3.24)
0
z y
T
E E E z x
y x
R R
e e
e e e
e e
(
(
= =
(
(

Vi RE l ma trn cosin ch hng ca khu cui v c ly t (3.3):
Ta c :
( )
1 2 1 2
1 2 1 2
cos( ) sin( ) 0
sin( ) cos( ) 0 3.25
0 0 1
E
q q q q
R q q q q
+ +
(
(
= + +
(
(

( )
1 2 1 2
1 2 1 2 1 2
sin( ) cos( ) 0
cos( ) sin( ) 0 . 3.26
0 0 0
E
q q q q
R q q q q q q
- -
+ +
(
| | (
= + + +
|
(
\ .
(

Cui cng nhn 2 ma trn ta c vn tc gc ca khu tc ng cui:
1 2 1 2 1 2 1 2
1 2 1 2 1 2 1 2 1 2
sin( ) cos( ) 0 cos( ) sin( ) 0
. cos( ) sin( ) 0 sin( ) cos( ) 0 . (3.27)
0 0 0 0 0 1
T
E E E
q q q q q q q q
R R q q q q q q q q q q e
- -
+ + + +
( (
| | ( (
= = + + + + +
|
( (
\ .
( (

Suy ra c :
Lp CT2- K55
27

( )
1 2
1 2
0 1 0 0
1 0 0 . 0 3.28
0 0 0
E E
q q
q q
e e
- -
- -
(
(
(
| | (
( = + =
|
(
\ . (
(
(
+

Khi ta c :

Hnh 3.7 : th vn tc gc khu 2
D thy rng i vi c cu robot phng th vector vn tc gc ch c thnh phn
theo phng z
3.5 BI TON NG HC NGC
T bi ton ng hc thun ta c c h phng trnh :
| |
| |
| |
0
3 1 1 2 1 2
0
3 1 1 2 1 2
0
3
1, 4 .cos( ) .cos( )
2, 4 .sin( ) .sin( ) (3.29)
3, 4 0
E
E
E
A a q a q q
A a q a
z
q y
A
x
q
= = + +
= = + +
= =

0 1 2 3 4 5 6 7 8 9 10
-3
-2
-1
0
1
2
3
4
Do thi wx,wy,wz
t [s]
w
x
,
w
y
,
w
z

[
r
a
d
/
s
]

wx
wy
wz
Lp CT2- K55
28

Bi ton t ra l : cho bit cc ta [ ]
E E E
x y z v nhim v ca ta l phi tm li
cc bin khp :
y do robot phng nn ta lun lun c z
E
= 0
v khi ta cn h phng trnh :
| |
| |
( )
0
3 1 1 2 1 2
0
3 1 1 2 1 2
1, 4 .cos( ) .cos( )
3.30
2, 4 .sin( ) .sin( )
E
E
A a q a q q
A
x
y a q a q q
= = + +
= = + +

Vi cc x
E
, y
E
, a
1
, a
2
bit.
Bnh phng tng v 2 phng trnh trn ta c :
( )
2 2 2 2 2
1 1 1 2 1 1 2 2 1 2
2 2 2 2 2
1 1 1 2 1 1 2 2 1 2
.cos ( ) 2a cos( )cos( ) .cos ( )
3.31
.sin ( ) 2a sin( )sin( ) .sin ( )
E
E
x a q a q q q a q q
y a q a q q q a q q
= + + + +
= + + + +

Cng hai phng trnh trn vi nhau ri sau rt gn ta c:

( )
2 2 2 2
1 2 1 2 1 1 2 1 1 2
2 2 2 2
1 2 1 2 2
2a (cos( ) cos( ) sin( )sin( ))
2a cos( ) 3.32
E E
E E
x y a a a q q q q q q
x y a a a q
+ = + + +
+ =

Suy ra :
( )
2 2 2 2
1 2
2
1 2
cos 3.33
2
E E
x y a a
q
a a
+
=
V
( )
2
2 2
sin 1 cos 3.34 q q =
Do ta c
( )
2 2 2
tan 2(sin , cos ) 3.35 q a q q =
Thay li q
2
vo phng trnh ca x
E
, yE v vit li h th ta thu c mt h
phng trnh lng gic bc nht mt n :
( )
1 2 2 1 2 2 1
2 2 1 1 2 2 1
( cos )cos sin sin
3.36
sin cos ( cos )sin
E
E
x a a q q a q q
y a q q a a q q
= +
= + +

Gii h trn ta c:
1 2 2 2 2 2 2 2 2
1 2 1 2 2
2 2 1 2 2
cos sin
2 cos
sin cos
E E
a a q a q
a a a a q x y
a q a a q
+
A = = + + = +
+

Lp CT2- K55
29

2 2
1 1 2 2 2
1 2 2
sin
( cos sin )
cos
E
E E E
E
x a q
a x a x q y q
y a a q
A = = + +
+

1 2 2
2 1 2 2 2
2 2
cos
(y cos sin )
sin
E
E E E
E
a a q x
a y a q x q
a q y
+
A = = +

T ta c :
( )
1 2 2 2
1 2 2
1 1 1
1 2 2 2
1 2 2
( cos sin )
cos
tan2(sin , cos ) 3.37
(y cos sin )
sin
E E E
E E
E E E
E E
a x a x q y q
q
x y
q a q q
a y a q x q
q
x y
+ +

Hay ta c :
( )
1 2 2 2
1
1 2 2 2
( cos sin )
tan 3.38
(y cos sin )
E E E
E E E
a x a x q y q
q
a y a q x q
+ +
=
+

Bi ton c th :
1. Qu o chuyn ng l mt ng thng :
Cho qu o chuyn ng ngc l ng thng AB, vi A(1039.23,600) v
B(50,1039.23) trong thi gian 5s ta thu c th bin khp q
1
q
2
nh sau :

Hnh 3.8 th cc bin khp q
1,
q
2
theo t
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
0
0.2
0.4
0.6
0.8
1
1.2
1.4
do thi q1,q2 theo t
t [s]
q
1
,
q
2

[
r
a
d
]

q1
q2
Lp CT2- K55
30

2. Cho qu o chuyn ng l mt ng trn :
Gi s cn cho im tc ng cui E di chuyn theo mt ng trn c phng
trnh :

2 2 2
( 450) ( 450) 300 x y + =

Hnh 3.9 th im tc ng cui cn iu khin
Khi bi ton ng hc ngc cho ta kt qu cc bin khp l :

Hnh 3.10 th cc bin khp q1,q2 thu c
Lp CT2- K55
31

PHN IV : BI TON TNH HC
4.1 TNH LC DN NG TI CC KHP M BO CN BNG TNH
C cu gn vo Robot bao gm cc khu ni vi nhau bi cc khp. Trng tm
c t vo trung im ca tng khu.

Hnh 4.1 : m hnh lc tc dng vo robot
Bi ton t ra l: Gi thit ngoi lc tc ng vo khu thao tc ti im E gm vc
t lc F
E,3
v momen M
E,3
. Tnh lc (v mmen) tc ng vo cc khp m
bo robot cn bng tnh.
Li gii chi tit nh sau:
- Ta c h phng trnh cn bng lc trong h ta c s:
( )
0 0 0
, 1 1,
0 0 0 0 0 0
, 1 , , 1
4.1
i i i i i
i i i i i i i ci i
F F P
M M r F r P
+
+
= +

Lp CT2- K55
32

Nu vit trong dng ma trn:
( )
0 0 0
, 1 1,
0 0 0 0 0 0
, 1 , , 1
4.2
i i i i i
i i i i i i i ci i
F F P
M M r F r P
+
+
=
= +

- H phng trnh cn bng lc trong h ta khu i:
( )
, 1 1,
, 1 , , 1
4.3
i i i
i i i i i
i i i i i i
i i i i i i i ci i
F F P
M M r F r P
+
+
= +

Nu vit trong dng ma trn:
( )
, 1 1,
, 1 , , 1
4.4
i i i
i i i i i
i i i i i i
i i i i i i i ci i
F F P
M M r F r P
+
+
=
= +

4.2 XT KHU TC NG CUI.
Khu 2:
Gi thit cc khu ca Robot ng nht hay trng tm ca cc khu nm chnh
gia. Bng cch t cc ngoi tc tc dng vo khu cui l Ta c: Ngoi lc tc
dng ti im E l:
E3
F v
E3
M :
( )
,2
,2
0
[ , ,0]
[0, 0, ]
[a , 0, 0] 4.5
[ , 0, 0]
2
[0, mg, 0]
T
E x y
T
E z
i T
i i
i T i
ci
T
i i
F F F
M M
r
a
r
P

Suy ra: PTCB

( )
0 0 0
2,1 3,2 2
0 0 0 0 0 0
2,1 3,2 2 2,1 2 2
4.6
C
F F P
M M r F r P
=
= +

Vi

( )
0
3,2
4.7
0
x
y
F
F F
(
(
=
(
(

v
( )
0
2 2
0
4.8
0
P m g
(
(
=
(
(

Lp CT2- K55
33

V vy:
( )
0
2,1 2 2
0 0 0 0
2,1 3,2 2 2 2 2
0
0 0 0
4.9
0
.
0 0
x x
y y
x
y C
F F
F F m g F m g
F
M M r F m g r m g
( ( (
( ( (
= = +
( ( (
( ( (

( (
( (
= + +
( (
( (

0
3,2
M
l momen cho ban u:
( )
0
3,2
0
0 4.10
z
M
M
(
(
=
(
(

Tnh ton
0 0
2 2
&
C
r r
nh sau:
1 2 1 2
0 0 1
2 1 2 1 2 1 2
cos( ) sin( ) 0
. sin( ) cos( ) 0 (4.11)
0 0 1
q q q q
R R R q q q q
+ +
(
(
= = + +
(
(

| |
( )
2
2 2
2 2
2
, 0, 0
, 0, 0 4.12
2
T
T
C
r a
a
r
=
(
=
(

2
1 2 1 2
0 0 2
2 2 2 1 2 1 2
2 2
1 2 1 2
0 0
2 2
2 1 2 2 1 2
2
1 2
cos( ) sin( ) 0
2
. sin( ) cos( ) 0 . 0 (4.13)
0 0 1 0
a
cos( ) 0 0 sin( )
2 2
a
sin( ) 0 0 cos( )
2 2
0 a
sin( )
2
C C
C C
a
q q q q
r R r q q q q
a
q q q q
a
r q q r q q
q q
(
(
+ +
(
(
(
= = + +
(
(
(
(

(

(
+ +
(
(
(
= + = +
(
(
(
+
(

2
1 2
a
cos( ) 0
2
q q
(
(
(
(
(
(
(
+
(

Lp CT2- K55
34

( )
1 2 1 2 2 2 1 2
0 0 2
2 2 2 1 2 1 2 2 1 2
2 1 2
0
2 2 1 2
2 1 2 2 1 2
cos( ) sin( ) 0 cos( )
. sin( ) cos( ) 0 . 0 sin( ) 4.14
0 0 1 0 0
0 0 a sin( )
0 0 cos( )
a sin( ) cos( ) 0
q q q q a a q q
r R r q q q q a q q
q q
r a q q
q q a q q
+ + +
( ( (
( ( (
= = + + = +
( ( (
( ( (

+
(
(
= +
(
( + +

Thay tt c vo (3.9) ta thu c :
0
2,1 2 2
2 1 2
0 0
2,1 2 1 2 2 2 2
2 1 2 2 1 2
0
0 0 0
4.1
0 0 0 a sin( ) 0
0 0 0 cos( ) .
a sin( ) cos( ) 0 0 0
x x
y y
x
y C
z
F F
F F m g F m g
q q F
M a q q F m g r m g
M q q a q q
( ( (
( ( (
= = +
( ( (
( ( (

+ ( ( ( (
( ( ( (
= + + +
( ( ( (
( ( ( ( + +

( ) 5

( )
0
2,1 2 2
0
2,1
2 1 2 2 1 2 2 2 1 2 2
0
0 0 0
4.16
0
0
1
- .sin( ). .cos( ).( ) .cos( ).m .
2
x x
y y
x y z
F F
F F m g F m g
M
a q q F a q q F m g a q q g M
( ( (
( ( (
= = +
( ( (
( ( (

=
(
+ + + + + +
(

4.3 XT KHU TH 1:

PTCB:
( )
0 0 0
1,0 2,1 1
0 0 0 0 0 0
1,0 2,1 1 1,0 1 1
4.17
C
F F P
M M r F r P
=
= +

Tnh ton cc gi tr nh sau: Theo (3.16)
Lp CT2- K55
35

0
2,1 2 2
0
0 0 0
x x
y y
F F
F F m g F m g
( ( (
( ( (
= = +
( ( (
( ( (

Theo bi :
0
1 1
0
0
P m g
(
(
=
(
(

( )
0
1,0 2 1 1 2
0
( ) 4.18
0 0 0
x x
y y
F F
F F m g m g F m m g
( ( (
( ( (
= + = + +
( ( (
( ( (

PTCB tng ng vi:
0
1,0 2 1 1 2
0 0 0 0
1,0 2,1 1 1 2 1 1
0
( )
0 0 0
(4.19)
0
. ( )
0 0
x x
y y
x
y C
F F
F F m g m g F m m g
F
M M r F m m g r m g
( ( (
( ( (
= + = + +
( ( (
( ( (

( (
( (
= + + +
( (
( (

0
2,1
M
c tnh t biu thc (3.16)
( )
1 1
0
1 1 1
cos sin 0
sin cos 0 4.20
0 0 1
q q
R q q
(
(
=
(
(

| |
( )
1
1 1
1 1
1
, 0, 0
, 0, 0 4.21
2
T
T
C
r a
a
r
=
(
=
(

( )
1 1 1 1 1 1 1
0 0 1 0
1 1 1 1 1 1 1 1 1 1
1 1 1 1
cos sin 0 .cos 0 0 a sin
. sin cos 0 . 0 a sin 0 0 .cos 4.22
0 0 1 0 0 a sin .cos 0
q q a a q q
r R r q q q r a q
q a q
( ( ( (
( ( ( (
= = = =
( ( ( (
( ( ( (

( )
1 1
1 1 1
1 1
0 0 1 0 1 1
1 1 1 1 1 1 1 1
1 1
1 1
cos 0 0 sin
2 2
cos sin 0
2
. sin cos 0 . 0 sin 0 0 cos 4.23
2 2
0 0 1 0
0
sin cos 0
2 2
C C C
a a
a q q
q q
a a
r R r q q q r q
a a
q q
( (
(
( (
(
(
( (
(
(
( (
= = = =
(
(
( (
(
(
( (

(
( (

( (

Lp CT2- K55
36

Thay tt c cc biu thc trn vo (3.19) ta c:
0
1,0 2 1 1 2
0
( )
0 0 0
x x
y y
F F
F F m g m g F m m g
( ( (
( ( (
= + = + +
( ( (
( ( (

0
1,0
2 1 2 2 1 2 2 2 1 2 2
1
1
1 1
1
1 1 1 2 1
1 1 1 1
1
1 1
1 1
0
0
1
- .sin( ). .cos( ).( ) .cos( ).m .
2
0 0 sin
2
0 0 a sin
0 0 .cos . ( ) 0 0 cos
2
a sin .cos 0 0
sin cos 0
2 2
x y z
x
y
M
a q q F a q q F m g a q q g M
a
q
q F
a
a q F m m g q
q a q
a a
q q
(
(
(
= +
(
(
+ + + + + +
(

( (
( (
+ +
( (
( (

1
0
.
0
m g
(
(
(
(
(
(

(
(
(
(

(
(

( )
0
1,0
2 1 2 2 1 2 2 2 1 2 2 1 1 1 1 1 2 1 1 1
0
0 4.24
1 1
- .sin( ). .cos( ).( ) .cos( ).m . .sin . .cos ( ( ). .cos .m .
2 2
x y x y z
M
a q q F a q q F m g a q q g a q F a q F m m g a q g M
=
+ + + + + + + + +
(
(
(
(
(
(

T cc cng thc trn ta c th tnh c cc lc v moment tc dng ln cc khu
v cc khp ca robot m bo cn bng tnh cho robot.

Lp CT2- K55
37

PHN V: TNH TON NG LC HC
5.1 XY DNG CU TRC NG HC

Hnh 5.1 : M hnh cu trc ng hc robot 2 DOF
Gi l
C1
l
C2
ln lt l khong cch t O
0
, O
1
n trng tm cc khu 1 v khu 2.
Coi cc khu ng cht tit din u trng tm t gia khu
Chn ta suy rng
1 2
[ ]
T
q q q = vi :
( )
2
1 1 1 1 1
2
2 2 2 2 2
1
0,
12
5.1
1
0,
12
x y z
x y z
I I I ma
I I I m a
= = =
= = =

T hnh v ta d dng tnh ra c ta cc khi tm v biu thc vn tc gc ca
tng khu l :
Lp CT2- K55
38

( )
1 1 1 1 2 1 2
1 1 1 2 1 1 2 1 2
1 1
cos cos cos( )
2 2
1 1
sin ; sin sin( ) 5.2
2 2
0 0
C C
a q a q a q q
r a q r a q a q q
( (
+ +
( (
( (
( (
= = + +
( (
( (
( (
( (

Biu thc vn tc gc cc khu:
( )
1 2
1 2
0 0
0 0 5.3
q q
e e
- -
( (
( (
( ( = =
( (
( (

T suy ra cc ma trn Jacobi tnh tin v ma trn Jacobi quay:
( )
1 1
1 1
1 1 1 1
1
sin
2
0 0 0
1
cos 0 ; 0 0 5.4
2
0 1 0
0
C
T R
a q
r
J a q J
q
q
e
-
(
(
(
(
c c
(
(
= = = =
(
(
c
c
(
(

(
(

( )
1 1 2 1 2 2 1 2
2 2
2 1 1 2 1 2 2 1 2 2
1 1
sin sin( ) sin( )
2 2
0 0
1 1
cos cos( ) cos( ) ; 0 0 5.5
2 2
1 1
0 0
C
T R
a q a q q a q q
r
J a q a q q a q q J
q
q
e
-
(
+ +
(
(
(
c c
(
(
= = + + + = =
(
(
c
c
(
(

(
(

Ma trn ten-x qun tnh c dng :
( )
1 1 2 2
1 2
0 0 0 0 0 0
0 0 ; 0 0 5.6
0 0 0 0
y y
z z
I I I I
I I
( (
( (
= =
( (
( (

Theo nh ngha ma trn khi lng suy rng ca robot c dng:
( )
2
11 12
1 21 22
( ) 5.7
T T
i Ti Ti Ri i Ri
i
m m
M m J J J I J
m m
=
(
= + =
(

Lp CT2- K55
39

Th cc biu thc (5.4),(5.5),(5.6) vo (5.7) Vi
2 2
1 1 1 1 2 2 2 2
1 1
,
12 12
y z y z
I I ma I I m a = = = = ta c :
( )
2 2 2
11 1 1 2 1 2 1 2 2 1 2
2 2 2
1 1 2 2 2 1 2 1 2 2
2 2 1 2
12 21 2 2 2 2 2 2 2 1 2 2
2
2 2
22 2 2 2 2
1 1
(a 2 cos )
4 4
1 1
cos
3 3
1 1 1
( cos ) cos 5.8
4 2 3 2
1
4 3
z z
z
z
m ma m a a a q I I
ma m a m a m a a q
a a
m m m a q I m a m a a q
a
m m I m a
= + + + + +
= + + +
= = + + = +
= + =

Th (5.8) vo biu thc ng nng ca robot:
( )
1
. ( ). 5.9
2
T
T q M q q
- -
=
Ta c :
( )
2 2 2 2 2 2
1 1 2 2 2 1 2 1 2 2 1 2 2 2
2
2 2 2 1 2 2 1 2
1 1 1 1
{[ cos ]. .
2 3 3 3
1 1
2[ cos ]. } 5.10
3 2
T ma m a m a m a a q q m a q
m a m a a q q q
- -
= + + + +
+ +

Biu thc th nng c h c dng:
( )
1 1 1 2 1 1 2 1 2
1
sin [ sin sin( )] 5.11
2
m ga q m g a q a q q H = + + +
Cng o ca cc lc suy rng khng th :
( )
1 1 2 2
5.12
x E y E
A q q F x F y o t o t o o o = +
Vi E l im tc ng cui cng trn khu 2, hay EO
2

T h thc :
1 1 2 1 2
1 1 2 1 2
.cos .cos( )
.sin .sin( )
E
E
x
y
a q a q q
a q a q q
= + +
= + +

Ta suy ra:
Lp CT2- K55
40

1 1 2 1 2 1 2 1 2 2
1 1 2 1 2 1 2 1 2 2
[ .sin .sin( )] .sin( )
[ .cos .cos( )] .cos( )
E
E
a q a q q q a q q q
a q a q q q a q q y q
x o o o
o o o
= + +
= + + + +

T ta c :
( )
( )
1 1 1 1 2 1 2 1 1 2 1 2
2 2 2 1 2 2 1 2
[ .sin .sin( )] [ .cos .cos( )] 5.13
sin( ) F cos( ) 5.14
np
x y
np
x y
Q F a q a q q F a q a q q
Q F a q q a q q
t
t
= + + + + +
= + + +

Th cc biu thc (5.10),(5.11),(5.12),(5.13) vo biu thc ca phng trnh
lagrange loi II ta c cc phng trnh vi phn chuyn ng ca robot phng hai
khu :
( )
2 2 2 2
1 1 1 2 2 2 1 2 1 2 2 1 2 2 2 1 2 2 2
2
2 1 2 2 1 2 2 1 2 2 2 1 2 1 1 2 2 1 2
1 1 2 1 2 1 1 2 1
1 1 1 1
( cos ) ( cos )
3 3 3 2
1 1 1
sin . sin . ( m ) a .g.cos cos( ) 5.15
2 2 2
[ .sin .sin( )] [ .cos .cos(
x y
ma m a m a m a a q q m a m a a q q
m a a q q q m a a q q m q m a g q q
F a q a q q F a q a q
t
-- --
- -
= + + + + +
+ + + +
+ + + + +
2
)] q

( )
2 2 2
2 2 2 2 1 2 2 1 2 2 2 2 1 2 1 2
2 2 1 2 2 1 2 2 1 2
1 1 1 1
( cos ) sin
3 2 3 2
1
cos( ) sin( ) F cos( ) 5.16
2
x y
m a m a a q q m a q m a a q q
m a g q q F a q q a q q
t
-- --
= + + +
+ + + + +

Lp CT2- K55
41

PHN VI: THIT K QU O CHUYN NG
6.1 THIT K QU O THEO KHNG GIAN KHP
L xc nh ng biu din ca v tr gc khp (gc quay ca khp vi khp quay
v di chuyn ca khp i vi khp tnh tin) theo thi gian khi di chuyn t v
tr ban u
0
(A) ti v tr cui
(B) trong thi gian
vi q l bin khp tng qut.

Qu o di chuyn ca khp gia hai v tr s tha mn 4 iu kin : v tr ban u
v v tr cui cng, tc ti v tr ban u v tc ti v tr cui cng. Do a
thc bc 3 s ph hp vi qu o ca khp robot:
2
1 2 3
( ) 2 3
i i i i
q t a a t a t = + + (6.1)
gi thit vn tc ti im u v im cui bng 0:

0
= 0 ;
= 0
Ta c h phng trnh:
( )
0 ( )
1
( ) ( )
2 2
( ) ( )
3 3
6.2
0
3( )
2( )
i i A
i
i B i A
i
i B i A
i
a q
a
q q
a
t
q q
a
t
=
=
=

=

Gii h ta c cc h s ca phng trnh qu o.
Bi ton c th:
cho im A(1039.2305; 600; 0) v im B(50; 1039.2305; 0).
n v l mm
Thi gian chuyn ng 5 t s =
2 3
0 1 2 3
0 0 ( )
( )
0
1 2 3
1 ( )
2
( )
( )
( )
( ) 0
3 ( ) 0 2
c c c
i i i A
c
i i B
i i i A
c
i i B
i i i i
c c c
i i i
q t q a
q t q
q t q a
a a t a t a t
a t a t a q t t q
= =
= =
= = =
+ + +
+ = + = =
Lp CT2- K55
42

Hnh 6.1 Minh ha v tr 2 im A , B trong khng gian lm vic
T bi ton ng hc ngc ta tnh c cc gc tng ng ban u :
1( )
1( )
6
3
A
B
q
q
t
t
v
2( )
2( )
0
3
A
B
q
q
t
=

Vi thi gian 5s nh trn thay vo (5.3) ta tnh c cc h s ca khp 1:
Lp CT2- K55
43

01 1( )
11
1( ) 1( )
21 2 2
1( ) 1( )
31 3 3
6
0
3( )
3( )
3 6
5 5
2( )
2( )
3 6
5 5
A
B A
B A
a q
a
q q
a
q q
a
t
t t
t t
= =
= =
= =
hay l ( )
01
11
21
31
6
0
6.3
50
375
a
a
a
a
t
t
t

Ta c
Phng trnh ng hc ca khu 1:
( )
2 3
1
2
1
1
6 50 375
6.4
25 125
2
25 125
( )
( )
( )
q t t t
q t t t
q t t
t t t
t t
t t
=
=
=

Hnh 6.2 ng biu din v tr, vn tc v gia tc ca khp 1 thao tc theo thi gian
Lp CT2- K55
44

Tng t th ta cng c cc h s ca khp 2
02 2( )
12
2( ) 2( )
22 2 2
2( ) 2( )
32 3 3
0
0
3( 0)
3( )
3
5 5
2( 0)
2( )
3
5 5
A
B A
B A
a q
a
q q
a
q q
a
t
t
= =
= =
= =
hay l : ( )
02
12
22
32
0
0
6.5
25
2
375
a
a
a
a
t
t
=

Ta c
Phng trnh ng hc ca khu 2:
( )
2 3
2
2
2
2
2
25 375
2 2
6.6
25 125
2 4
25 125
( )
( )
( )
q t t t
q t t t
q t t
t t
t t
t t
=
=
=

V ta cng c ng biu din

Hnh 6.3 : ng biu din v tr, vn tc, gia tc ca khu 2 theo thi gian.
Lp CT2- K55
45

Sau khi c cc bin khp ta c th v li qu o ca im E nh hnh v :

Hnh 6.4 th im E trong khng gian thao tc
V tr c cu khi :

Hnh 6.5 V tr khu ca c cu theo t
Ta thy hai im u mt ca qu o thc trng vi v tr ta cn a n nn ta
thy cc bin khp gii c l ng vi yu cu t ra.
0 200 400 600 800 1000 1200
500
600
700
800
900
1000
1100
xE [mm]
y
E

[
m
m
]
Do thi diem E trong khong gian thao tac
A
B
Duong thang AB
quy dao thuc diem E
0 200 400 600 800 1000 1200
0
200
400
600
800
1000
1200
Mo phong vi tri cua co cau theo t
xE
y
E
A
B
Lp CT2- K55
46

6.2 THIT K QU O THEO KHNG GIAN LM VIC
6.2.1 Qu o ca im tc ng cui theo ng thng t A n B trong t (s)
Tay robot di chuyn t vi tr ( )
A A A
A x y z trong khng gian lm vic ti v tr B
( )
B B B
x y z theo mt ng thng. ta c phng trnh ng thng AB c dng sau:

( ) 6.7
A A A
B A B A B A
x x y y z z
x x y y z z

= =

Cho robot chuyn ng trong thi gian 5(s) i t v tr A(1039.23; 600; 0) ti v tr
B(50; 1039.23; 0). Ti v tr A cc khp
1 2
, 0
6
q q
t
= =
D thy khi A,B nm trn mt phng z=0, nn ta vit phng trnh ng thng
i qua hai im A v B, ta c dng:

( ) 6.8 y ax b = +
T phng trnh (5.8) ta rt ra c quan h:

( ) 6.9
B A A B B A
B A B A
y y y x y x
y x
x x x x

= +

Cng nh cch thit lp qu o gc khp tha mn iu kin v vn tc u v
vn tc cui ta thit lp quan h x=x(t) l a thc bc 3.
( )
2 3
0 1 2 3
( ) 6.10 x t a a t a t a t = + + +
Cng cc iu kin

0
(0) ( , )
(0) 0
( ) ( , )
( ) 0
A A
B B
c
s A x y
v v
s t B x y
v t v
=
= =
= =
m quan h y=k.x+b => x=x(t) cng c cc iu kin nh

qu o , thay vo (5.11) ta rt ra c:
( )
0
1
2 2
3 3
6.11
0
3( )
2( )
A
B A
B A
a x
a
x x
a
t
x x
a
t
=
=
=

=
khi thit k x=x(t) ri thay vo (6.8)
ta c c y=y(t)
Lp CT2- K55
47

V d : vi iu kin ban u l : ti t=0 th v tr im E l ti A
v ti t=5s th v tr im E ti B t ta thay cc gi tr ban u vo (6.11) v ta c
kt qu phng trnh ng thng AB l :

0 1 2 3
2 3
1039.23 , 0, 118.7076, 15.82768
( ) 1039.23 118.7076 15.82768 0.444 1061.4306
A
a x a a a
x t t t y x
= = = = =
= + = +

V suy ra
2 3
( ) 600 52.7062 7.0275 y t t t = +
Khi ta c th v c th im E trong khong thi gian 5s nh hnh v

Hnh 6.6 th x(t),y(t) theo ng thng v qu o im E
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
0
200
400
600
800
1000
1200
do thi x,y theo t
t [s]

x
,
y

[
m
m
]

x(t)
y(t)
0 200 400 600 800 1000 1200
550
600
650
700
750
800
850
900
950
1000
1050
Duong thang quy dao diem E
x [mm]
y

[
m
m
]
A
B
Lp CT2- K55
48

Hnh 6.7 V tr c cu theo thi gian t
6.2.2 Thit k qu o tc ng tc ng cui di chuyn theo ng trn t A n B
trong tc(s) ly AB lm ng knh
Ta c phng trnh ng trn trong mt phng gia hai im A (x
A
,y
A
) , B(x
B
,y
B
)
ly AB lm ng knh l
2 2 2
( ) ( )
I I
x x y y R + = vi
( )
2 2
1
, , ( ) ( ) 6.12
2 2 2
A B A B
I I A B A B
x x y y
x y R x x y y
+ +
= = = +
Vit phng trnh ng trn di dng tham s nh sau :
cos( ( ))
sin( (t))
I
I
x x R a t
y y R a
= +
= +

cng tha mn iu kin v vn tc a(t) ta thit k cng phi l bc 3

2 3
0 1 2 3
( ) a t a a t a t a t = + + +
V phi tha mn iu kin:
( )
( )
0
(0) ( , )
0 0
6.13
( ) ( , )
( ) 0
A A
c B B
c c
s A x y
v v
s t B x y
v t v
=
= =
= =

T ta d dng tm c cc h s a
0
a
1
a
2
a
3
c dng:
0 1
cos( ) , 0
A I
x x
a ar a
R
= = t cos( ) cos( )
B I A I
x x x x
u ar ar
R R

=
0 200 400 600 800 1000 1200
0
200
400
600
800
1000
1200
xE
y
E
A
B
0 200 400 600 800 1000 1200
0
200
400
600
800
1000
1200
xE
y
E
A
B
Lp CT2- K55
49

Th ta suy ra c :
2 3 2 3
3 2
,
c c
u u
a a
t t
= =
V d cn thit k di chuyn theo ng trn t A (562.92,875) n
B(151.31,837,92) trong 5s vi AB l ng knh.
Da vo cng thc trn ta tnh c :
2 3
2 3
357.1150 206.6384cos(0.0898426 0.35543 0.0474 )
y 856.4600 206.6384sin(0.0898426 0.35543 0.0474 )
x t t
t t
= + +
= + +

Ta tin hnh v cc th minh ha nh hnh bn di :

Hnh 6.8 th x(t),y(t) theo cung trn
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
100
200
300
400
500
600
700
800
900
1000
1100
do thi x,y theo t
t [s]

x
,
y

[
m
m
]

x(t)
y(t)
Lp CT2- K55
50

Hnh 6.9 Qu o im E theo cung trn
Hnh 6.10 V tr c cu theo t
150 200 250 300 350 400 450 500 550 600
850
900
950
1000
1050
1100
quy dao diem E
x [mm]
y

[
m
m
]
0 100 200 300 400 500 600 700 800 900 1000
0
100
200
300
400
500
600
700
800
900
1000
1100
xE
y
E
A
B
Lp CT2- K55
51

PHN VII : IU KHIN V M PHNG ROBOT
Cc h thng iu khin nu di y u theo lut iu khin PD. Khi thit k h
thng iu khin ta b qua ng hc ca c cu chp hnh, qun tnh ng c. Nh
vy chc nng ca b iu khin l to ra mt momen cn thit truyn ng khp
robot m bo khp robot lun bm theo v tr t.
7.1 H THNG IU KHIN TRONG KHNG GIAN KHP
Tn hiu t l qu o bc 3 ca cc khp c tnh ton phn thit k
qu o.
7.1.1. H thng iu khin phn hi

a, Lut iu khin

Hnh 7.1.S cu trc h thng iu khin robot vi b iu khin PD

Tn hiu t v tr q
d
c so snh vi v tr thc ca khp q, sai lch c t vo
khu khuch i vi h s K
p
. Tn hiu ra ca khu t l c cng i s vi tn
hiu t l vi tc ca khp v t c cu chp hnh ca robot:

dk p D
K K t c c = +
Hay vit vi khp th i ta c:

dki pi i di i
K K t c c = +

Trong :
d
q q c = sai s v tr khp robot.

d
q q c = sai s tc khp robot.

1 2
( , ,..., )
p p p pn
K diag K K K = l ma trn ng cho cc h s khuch
i ca tng khp hp ring.
Lp CT2- K55
52

1 2
( , ,..., )
d d d dn
K diag K K K = l ma trn ng cho cc h s o hm
ca tng khp hp ring.
H thng iu kin vi cu trc b iu khin (3) n nh tuyt i ton
cc. tht vy, chn hm lyapunov c dng :

1
( . . )
2
T T
L p
V K q Hq c c = +
Hm V
L
biu th tng nng lng h thng robot: thnh phn
1
2
T
p
K c c t l
nng lng u vo v thnh phn
1
( . . )
2
T
q H q l ng nng ca robot. K
P
v H l
cc ma trn h s dng; nn hm V
L
> 0 vi
d
q q = .
Tnh o hm cp 1 hm V
L
ta c:

1 1 1 1 1
2 2 2 2 2
T T T T T
L p p
V K K q Hq q Hq q Hq c c c c = + + + +
Do tnh i xng ca cc thnh phn
T
p
K c c ,
T
q Hq nn (4.5) c rt gn li nh
sau:

1
2
T T T
L p
V K q Hq q Hq c c = + +
Thay phng trnh dng tng qut vo phng trnh trn vi gi thit khng c
thnh phn mmen trng lc G(p) ta c :

1
[ ( , ) ]
2
T T T
L p
V K q Hq q C q q q c c t = + +
S dng thuc tnh ca phng trnh ng lc hc v p dng lut iu khin
(4.1), Phng trnh (4.7) c bin i nhhuw sau:
1 1
( ) ( , ) [ ( ) ( , )]
2 2
T T T T T
L p p
V q K q H q q q C q q q q K q H q C q q q c c = + = +
Do ma trn
1
( ) ( , )
2
H q C q q l ma trn i xng ngc nn
1
( ) ( , )
2
H q C q q = 0
vi mi q nn t (4.7) ta nhn c :
0
T
L p
V q K q = s
Bt ng thc trn cho thy rng h thng n nh tuyt i. mc dng ca V
L

ph thuc vo K
p
v mc m ca V
L
ph thuc vo K
D
. Do tang tc hi t
bng cch tang gi tr ma trn K
D
.
Lp CT2- K55
53

Ta thy rng, mc dng ca V
L
ph thuc vo Kp; mc m ca
L
V
ph
thuc vo Kd . Do tng tc hi t bng tng gi tr Kd. Nng cao chnh xc
tinh ca h thng iu khin t c bng tng h s Kp ca khu khuch i. Tuy
nhin ,Kp v Kd qu ln s lm gim n nh v cht lng qu trnh qu nh
qu iu chnh , thi gian qu tng.
7.1.2 Thit k cho robot RR
Vi cc thng s u vo:
m1=3 kg, a1=0.650 m
m2=2.5 kg, a2=0.55 m.
H phng trnh ng lc hc c dng nh sau:
( ) ( , ) ( ) M H q q V q q G q = + +
Vi:
11 12
21 22
H H
H
H H
= ;
2
12 112 2 1 2
2
21 1
2 h q h q q
V
h q
- - -
-
+
= ;
1
2
g
G
g
=

2 2 2
11 1 1 2 1 1 2 1 2 2
2
12 21 2 1 2 1 2 2
2
22 2 1 2
122 112 211 2 1 1 2 2
1 1 1 1 2 1 1 1 2 1 2
2 2 1 2 1 2
( )( 2 os )
( )( os )
( )
( ) sin
os ( ) [a cos +a cos( )]
g ( ) os( )
H ma m m a a a a c q
H H m m a a a c q
H m m a
h h h m m a a q
g m ga c q m m g q q q
m m ga c q q
= + + + +
= = + +
= +
= = = +
= + + +
= + +

Phng trnh lut iu khin:
dk p D
M K K c c = +

Trong :
d
q q c = sai s v tr khp robot.

d
q q c = sai s tc khp robot.

1 2
( , ,..., )
p p p pn
K diag K K K = l ma trn ng cho cc h s khuch
i ca tng khp hp ring.

1 2
( , ,..., )
d d d dn
K diag K K K = l ma trn ng cho cc h s o hm
ca tng khp hp ring.
Qu o dt vo hai khp c dng bc ba c tnh ton m bo tay my di
chuyn t v tr ban u (0.2 0) n v tr cui cng (0 0.1) trong thi gian 2s.
Ma trn h s khuch i t l v o hm c la chn:
Lp CT2- K55
54

5000 0
0 5000
P
K = v
50 0
0 50
D
K =
7.1.3 S dng phng trnh ng lc hc m phng iu khin trong simulink:
Vi phng trnh ng lc hc r bt RR :
2 2 2 2
1 1 1 2 2 2 1 2 1 2 2 1 2 2 2 1 2 2 2
2
1 2
2 1 2 2 1 2 2 1 2 2 2 1 2 2 2 1 2
2
2 2 2 2 1 2 2 1
1 1 1 1
cos( ) cos( )
3 3 3 2
1 ( 2 ) 1
sin( ) sin( ) cos( ) cos( )
2 2 2
1 1 1
cos( )
3 2 3
M ma m a m a m a a q q m a m a a q q
m m
m a a q q q m a a q q ga q m ga q q
M m a m a a q q m
| | | |
= + + + + +
| |
\ . \ .
+
+ + +
| |
= + +
|
\ .
2 2
2 2 2 2 1 2 2 1 2 2 1 2
1 1
sin( ) cos( )
2 2
a q m a a q q m ga q q
+ + +
Vi lut iu khin: ( ) ( )
dk p d d d p d
M K q q K q q K K c c = + = +

m phng bng phng trnh ng lc hc th ta s dng lut iu khin PD
nh sau:

Hnh 7.2 S khi b iu khin PD
Trong cc khi con nh sau:
Khi t DAT
Lp CT2- K55
55

Hnh 7.3 s khi t DAT
Khi PD :

Hnh 7.4 s khi PD
S khi Robot

Hnh 7.5 s khi robot
Lp CT2- K55
56

Vi file m phng c thng s nh sau

Cc hm Fcn v Fcn1 nh sau:

% cac thong so mo phong
a1=0.65;
a2=0.55;
g=9.81;
m1=3;
m2=2.5;
tc=5;
s0=[1.03923;0.6];
q0=DHN(s0);
sc=[0.05;1.03923];
qc=DHN(sc);
a01=q0(1);
a11=0;
a21=3*(qc(1)-q0(1))/tc^2;
a31=-2*(qc(1)-q0(1))/tc^3;
a02=q0(2);
a12=0;
a22=3*(qc(2)-q0(2))/tc^2;
a32=-2*(qc(2)-q0(2))/tc^3;
% ham tinh DHN dong hoc nguoc
function [q] = DHN(s)
%thong so robot
a1=0.65;
a2=0.55;
%tinhq2
c2=(s(1)^2+s(2)^2-a1^2-a2^2)/2*a1*a2;
s2=sqrt(1-c2^2);
q2=atan2(s2,c2);
%tinh q1
c1=(a1*s(1)+a2*(s(1)*c2+s(2)*s2))/(s(1)^2+s(2)^2);
s1=((a1+a2*c2)*s(2)-a2*s2*s(1))/(s(1)^2+s(2)^2);
q1=atan2(s1,c1);
q=[q1;q2];
end

Hm Fcn:
(1/((1/3)*m1*a1^2+(1/3)*m2*a2^2+m2*a1^2+m2*a1*a2*cos(u(3
))))*(u(1)-
((1/3)*m2*a2^2+0.5*m2*a1*a2*cos(u(3)))*u(6)+m2*a1*a2*sin
(u(3))*u(4)*u(5)+0.5*m2*a1*a2*sin(u(3))*u(5)^2-
0.5*(m1+2*m2)*g*a1*cos(u(3))-0.5*m2*g*a2*cos(u(2)+u(3)))
Hm Fcn1:
(1/((1/3)*m2*a2^2))*(u(6)-
((1/3)*m2*a2^2+0.5*m2*a1*a2*cos(u(2)))*u(5)-
0.5*m2*a1*a2*sin(u(2))*u(3)^2-
0.5*m2*g*a2*cos(u(1)+u(2)))

Lp CT2- K55
57

Ta c kt qu m phng nh sau :
a,Sai s bin khp q t v q iu khin l :

Hnh 7.6 sai s cc bin khp iu khin
b gi tr qd v qdk

Hnh 7.7 qd v qdk
Lp CT2- K55
58

c gi tr vqd1,vq1,vqd2,vq2

Hnh 7.8 vqd1 vq1 vqd2,vq2
Ta thy vdq1 v vq1 bm st nhau, vdq2 v vdq2 cng bm st nhau, nn iu khin
theo phng php trn l chp nhn c.
7.2 S DNG M HNH XUT RA T SOLIDWORKS
m phng v iu khin robot th ta s dng phn mm matlab kt hp vi cc
toolbox v simulink. Trong n ny cho vic m phng trn matlab c n
gin th em s dng m hnh robot sau m phng chuyn ng:

Hnh 7.9 M hnh robot m phng chuyn ng
Lp CT2- K55
59

s khi m hnh trn matlab/simechanics:
Sau khi ci t xong phn Simmechanics Link, lin kt gia
Matlab/Simmechanics vi Solidworks, ta chy SolidWorks, trn thanh Task bar ca
SolidWorks,ta chn mc Add-ins,v tch vo Simmechanics Link. Lc ny, ta chn
Save as m hnh Assembly m ta va thit k, mc Save as ta lu file vi tn
banvelap chn ui .xml.
Mun to m hnh Robot trn Matlab/Simmechanics, ti dng lnh ca Matlab,
g:
>>Mech_import(banvelap.xml); nhn Enter
Ta s thu c m hnh nh hnh v:

Hnh 7.10 S m hnh xut t Solidwork sang matlab
Trong , cc khi Revolute th hin l khp quay 1 bc t do, cc khi Part m
t hnh hc ca cc khu, khi Weld th hin rng buc c nh gia khu vi h
ta gc. m t hot ng ca Robot, ta cn t vo cc khp Momen dn
ng hoc c th gn cc gi tr v v tr, vn tc, gia tc cho cc bin khp. y,
m phng th ta gn cho Robot cc gi tr bin khp gi tr ca v tr l hm theo
thi gian. Cc tn hiu ny c a t Simulink sang.

Cc khi cn ly: J oint Actuator , J oint sensor t v o tn hiu u vo
Khi Scope o tn hiu u ra ,
Khi Clock t cc gi tr hm thi gian
Lp CT2- K55
60

s khi m phng hot ng ca robot khi a cc khi tn hiu vo:
hnh 7.11 S m hnh simulink
vi b iu khin PD nh sau :

Hnh 7.12 S b iu khin PD
Vi cc ma trn cho
5000 0 50 0
0 5000 0 50
P I
K K
( (
= =
( (

Lp CT2- K55
61

Th ta thu c kt qu nh sau :
Momennt iu khin : M 1,M2

Hnh 7.13 s moment iu khin
Cc gi tr bin khp t v iu khin :

Hnh 7.14 so snh cc bin khp t v iu khin

Lp CT2- K55
62

Cc gi tr sai s ca bin khp :
Hnh 7.15 sai s cc gi tr t
Gi tr vn tc iu khin v vn tc t

Hnh 7.16 vn tc iu khin v vn tc t
Lp CT2- K55
63

PHN VIII M PHNG CHUYN NG CA ROBOT BNG
PHN MM VISUAL STUDIO V TH VIN OPENGL
8.1.Thit k 3D
S dng phn mm Solid Work thit k m hnh 3D ca Robot.
Khi thit k th chn h ta v nn trng vi khi ta chn h ta khi tnh
ton d dng cho qu trnh m phng bng OpenGL.

Hnh 8.1 M hnh m phng trong OpenGL
8.2 M phng hot ng ca robot.
M phng trn OpenGL :
V m hnh trn Solid Work ri xut ra file .stl . y ta cn v theo h ta
ng nh khi ta chn khi tnh ton
Mt vi kt qu m phng.
Lp CT2- K55
64

Hnh 8.3 ng thng

Hnh 8.4 ng trn
Lp CT2- K55
65

Hnh 8.5 ng astroid

Hnh 8.6 Hoa 4 cnh
Lp CT2- K55
66

PHN IX: KT LUN
Sau mt thi gian tm hiu nghin cu ng thi c s gip nhit tnh ca
thy hng dn, v c bn em hon thnh c n ny. Thng qua n ny
gip em h thng li nhng kin thc c hc, ng thi gip em tm hiu
thm mt s kin thc mi. c bit gip em tm hiu su hn v robot, mt lnh
vc khng cn mi nhng vn ang pht trin kh mnh nc ta.
Trong n ny c s gip ca bn b, thy c, v s tm hiu ca mnh em
lm c mt s phn nh sau: thit k m hnh robot bng solidworks theo
tng ca mnh, tnh ton ng hc thun ngc, thit lp phng trnh ng lc
hc robot, thit lp qu o chuyn ng trong khng gian khp v khng gian lm
vic, thit k b iu khin phn hi trong khng gian khp, m phng Simulink
trong khng gian khp. Tuy mi lm c nh vy nhng qua n cng gip em
hiu thm v robot, n l nn em c th pht trin sau ny
Nhng hn ch hay cha lm c trong n: tuy c gng nhng em vn
cn nhng hn ch nh: cha iu khin chnh xc c trong khng gian lm vic,
cc thng s hiu chnh cha c ti u qu o bm c tt nht.
Nhng hn ch ny , em s c gng tip tc hon thin hn vo nhng n tip
theo. Em xin chn thnh cm n!

Lp CT2- K55
67

PHN X :CC CHNG TRNH IU KHIN V THIT K
C KH CC BN V
Cc chng trnh MATLAB

clc; clear all; close all
% HAM TINH DONG HOC THUAN
% khai bao cac bien
syms alpha theta a d q1 q2 a1 a2 q3 a3 t
q1=sym('q1(t)');
q2=sym('q2(t)');
q=[q1;q2]; % Vector cc toa do suy rong q
dq1=diff(q1,t);
dq2=diff(q2,t);
dq =[dq1;dq2]; % Vector cc van toc dai
disp('TINH TOAN CHUONG 1')
disp ('MA TRAN DH TONG QUAT')
% ma tran DH
DH=[cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha),
a*cos(theta);...
sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha),
a*sin(theta);...
0, sin(alpha), cos(alpha), d;...
0, 0, 0, 1]
disp('ma tran khau thao tac 1 :A01')
A01=subs(DH,{theta,alpha,a,d},{q1,0,a1,0}) % ma tran khau thao tac 1
disp('ma tran khau thao tac 2 :A12')
A12=subs(DH,{theta,alpha,a,d},{q2,0,a2,0}) % ma tran khau thao tac 2
disp('ma tran cosin chi huong khau 1 :R01')
R01=A01(1:3,1:3) % ma tran cosin chi huong khau 1
disp('ma tran cosin chi huong khau 2 :R02')
R12=A12(1:3,1:3) % ma tran cosin chi huong khau 2
% tinh cac ma tran truyen
disp('ma tran truyen')
disp(' ma tran cosin chi huong khau 1 doi voi khau co dinh R01')
R01=A01(1:3,1:3)
% ma tran cosin chi huong khau 2 doi voi khau co dinh R02
A02=simplify(A01*A12)
disp(' ma tran cosin chi huong khau 2 doi voi khau co dinh R02')
R02=A02(1:3,1:3)
% Vector vi tri diem thao tac cuoi
disp('Vector vi tri khau thao tac cuoi')
rE=[A02(1,4);A02(2,4);A02(3,4)]
disp('Toa do xE')
xE=A02(1,4)
disp('Toa do yE')
yE=A02(2,4)
disp('Toa do zE')
zE=A02(3,4)
% tinh toan van toc dai diem E va van toc goc cac khau
vE = [diff(xE,t);diff(yE,t);diff(zE,t)] % Tnh vector van toc khau thao tac
cuoi
aE=[diff(diff(xE,t),t);diff(diff(yE,t),t);diff(diff(zE,t),t)]
R_0E = A02(1:3,1:3)
diff_R_0E=[diff(R_0E(1,1),t),diff(R_0E(1,2),t),diff(R_0E(1,3),t);...

Lp CT2- K55
68

diff(R_0E(2,1),t),diff(R_0E(2,2),t),diff(R_0E(2,3),t);...
diff(R_0E(3,1),t),diff(R_0E(3,2),t),diff(R_0E(3,3),t)] %Tinh dao ham cua R
omega_curve = diff_R_0E*R_0E'
omega_curve = simplify(omega_curve)
disp('Van toc goc')
omega = [omega_curve(3,2); omega_curve(1,3) ; omega_curve(2,1)]
%-----------
% cac lenh subs gia tri cua t vao bieu thuc
rEt=subs(rE,{a1,a2,q1,q2},{65,55,sin(2.*t),sin(pi/3.*t)})
vEt=subs(vE,{a1,a2,q1,q2},{65,55,sin(2.*t),sin(pi/3.*t)})
aEt=subs(aE,{a1,a2,q1,q2},{65,55,sin(2.*t),sin(pi/3.*t)})
%=============================================
omega_subs=subs(omega,{q1,q2},{sin(2.*t),sin(pi/3.*t)})
%====================================================
% thay t bang so vao cac bieu thuc do
t=0:0.1:10; % cho bien thoi gian la 20s
%===================================================
xE=subs(rEt(1,1),t); % thay tiep t vao lan nua de ra so
yE=subs(rEt(2,1),t);
zE=subs(rEt(3,1),t);
%=================================================
vEx=subs(vEt(1,1),t); % thay tiep t vao lan nua de ra so
vEy=subs(vEt(2,1),t);
vEz=subs(vEt(3,1),t);
%=================================================
aEx=subs(aEt(1,1),t); % thay tiep t vao lan nua de ra so
aEy=subs(aEt(2,1),t);
aEz=subs(aEt(3,1),t);
%============================
zE2=zE.*t; % vi cac bieu thuc nay =0 het
vEz2=vEz.*t; % nen phai nhan them t vao
aEz2=aEz.*t ; % thi cac vector moi cung so chieu
%======================================
wx=subs(omega_subs(1,1),t);
wy=subs(omega_subs(2,1),t);
wz=subs(omega_subs(3,1),t);
wx2=wx.*t;
wy2=wy.*t;
%======================================
disp('thay cac gia tri cua de bai vao ta duoc')
%===================================================
figure(1) % ve cac ham toa do
plot(t,xE,'r','linewidth',2)
hold on
plot(t,yE,'b','linewidth',2)
plot(t,zE2,'g','linewidth',2)
title('Do thi xE,yE,zE')
xlabel('t [s]'),ylabel('xE,yE,zE [cm]')
legend('xE','yE','zE')
hold off
grid on
%==================================================
% ve quy dao chuyen dong trong khong gian
figure(2)
plot3(xE,yE,zE2,'g','linewidth',2)
title('Do thi chuyen dong cua diem cuoi (xE,yE,zE) trong khong gian')
xlabel('xE [cm]'), ylabel('yE [cm]'),zlabel('zE [cm]')
grid on

Lp CT2- K55
69

%==================================================
% ve do thi vE trong mat phang va trong khong gian
figure(3)
plot(t,vEx,'r','linewidth',2)
hold on
plot(t,vEy,'b','linewidth',2)
plot(t,vEz2,'g','linewidth',2)
title('Do thi vEx,vEy,vEz')
xlabel('t [s]'),ylabel('vEx,vEy,vEz [cm/s]')
legend('vEx','vEy','vEz')
hold off
grid on
%=================================================
figure(4)
plot(t,aEx,'r','linewidth',2)
hold on
plot(t,aEy,'b','linewidth',2)
plot(t,aEz2,'g','linewidth',2)
title('Do thi aEx,aEy,aEz')
xlabel('t [s]'),ylabel('aEx,aEy,aEz [cm/s^2]')
legend('aEx','aEy','aEz')
hold off
grid on
%====================================================
figure(5)
plot(t,wx2,'r','linewidth',2)
hold on
plot(t,wy2,'b','linewidth',2)
plot(t,wz,'g','linewidth',2)
title('Do thi wx,wy,wz')
xlabel('t [s]'),ylabel('wx,wy,wz [rad/s]')
legend('wx','wy','wz')
hold off
grid on

% CHUONG TRINH TINH DONG HOC NGUOC DHN
function [q] = DHN(s)
%thong so robot
a1=0.65;
a2=0.55;
%tinhq2
c2=(s(1)^2+s(2)^2-a1^2-a2^2)/2*a1*a2;
s2=sqrt(1-c2^2);
q2=atan2(s2,c2);
%tinh q1
c1=(a1*s(1)+a2*(s(1)*c2+s(2)*s2))/(s(1)^2+s(2)^2);
s1=((a1+a2*c2)*s(2)-a2*s2*s(1))/(s(1)^2+s(2)^2);
q1=atan2(s1,c1);
q=[q1;q2];
end
% CHUONG TRINH TINH DONG HOC NGUOC DUONG THANG
global a1 a2 xE yE q1 q2 a b
xA=1039.23;
yA=600;
xB=50;
yB=1039.23;
a=(xB-xA)/5 % t=5s la thoi gian di chuyen tu A->B
b=(yB-yA)/5
t=0:0.01:5;
xE=a.*t+1039.23;

Lp CT2- K55
70

yE=b.*t+600;
a1=650;
a2=550;
c2=((xE.^2+yE.^2-a1^2-a2^2)/(2*a1*a2));
s2=sqrt(1-c2.^2);
q2=atan(s2./c2);
% code tinh q1
c1=(a1.*xE+a2*(xE.*cos(q2)+yE.*sin(q2)))./(xE.^2+yE.^2);
s1=sqrt(1-c1.^2);
%s1=(a1.*yE+a2*(yE.*cos(q2)-xE.sin(q2)))./(xE.^2+yE.^2);
q1=atan(s1./c1);
q1q2=[q1' q2'];
save q1q2thang.txt -ASCII -DOUBLE q1q2
%--------------------
figure(1)
plot(xE,yE,'m','linewidth',2)
title('Duong thang quy dao diem E')
xlabel('x [mm]'),ylabel('y [mm]')
legend('xE,yE')
grid on
%-------------------------
figure(2)
plot(t,q1,'r','linewidth',2)
hold on
plot(t,q2,'--b','linewidth',2)
title('do thi q1,q2 theo t')
xlabel('t [s]'), ylabel('q1,q2 [rad]')
legend('q1','q2')
grid on
hold off
% ve duong tron
t=0:0.01:2*pi;
x=R.*cos(t)+xR;
y=R.*sin(t)+yR;
a1=650;
a2=550;
circle=[x ; y]';
save cc.txt -ASCII -DOUBLE circle
%--------------------------------
c2=((x.^2+y.^2-a1^2-a2^2)/(2*a1*a2));
s2=sqrt(1-c2.^2);
q2=atan(s2./c2);
% code tinh q1
c1=(a1.*x+a2*(x.*cos(q2)+y.*sin(q2)))./(x.^2+y.^2);
s1=sqrt(1-c1.^2);
q1=atan(s1./c1);
q1q2tron=[q1' q2'];
save q1q2tron.txt -ASCII -DOUBLE q1q2tron
%=====================
figure(1)
plot(x,y,'r','linewidth',2)
title('duong tron duong kinh AB')
axis([0 1000 0 1050])
grid on
%==============================
figure(2)
hold on

Lp CT2- K55
71

plot([xA xB],[yA yB],'b','linewidth',2)
plot([0 640.125],[0 128.871355],'m','linewidth',3 )
plot([640.125 640.125],[128.871355 662.87],'g','linewidth',3)
plot([0 497.92888],[0 417.8119463],'m','linewidth',3 )
plot([497.92888 xB],[417.8119463 yB],'g','linewidth',3)
title('vi tri co cau tai A va B')
axis([0 1000 0 1050])
grid on
text(650,670,'A')
text(110,850,'B')
text(350,50,'khau 1')
text(650,400,'khau 2')
text(220,300,'khau 1')
text(400,600,'khau 2')
% ve q1 va q2
figure(3)
plot(t,q1,'r','linewidth',2)
hold on
plot(t,q2,'--b','linewidth',2)
title('Do thi q1 va q2 theo t')
xlabel('t [s]'),ylabel('q1 q2 [mm]')
legend('q1','q2')
grid on
% chuong trinh ve duong tron mo phong
t=0:0.01:10;
a1=650;
a2=550;
x=450+300*cos(2.*t);
y=450+300*sin(2.*t);
figure(1)
xlabel('xE')
ylabel('yE')
title('quy dao can chuyen dong theo la duong tron (x-450)^2+(y-
(450)^2=300^2')
grid on
% chuong trinh ve khong gian lam viec
global a1 a2 q1 q2 x y
a1=650; a2=550;
q1=-pi/6:0.01:7*pi/6;
q2=-2*pi/3:0.01:2*pi/3; N=length(q1);
x=a1.*cos(q1)+a2.*cos(q1+q2);
y=a1.*sin(q1)+a2.*sin(q1+q2);
x0=0;y0=0;
x0=x0.*(q1./q1);
y0=y0.*(q2./q2);
x1=a1.*cos(q1);
y1=a1.*sin(q1);
for i=1:2:N
plot([x1(i);x(i)],[y1(i);y(i)],'bo-')
hold on
grid on
end
%============
title('khong gian lam viec robot 2 DOF')
xlabel('xE'), ylabel('yE')
text (20,20,'O')

Lp CT2- K55
72

% chuong trinh tinh ve xE yE khi thiet ke quy dao ra q1 q2
global a1 a2 xE yE q1 q2
t=0:0.01:5;
a1=650;
a2=550;
q1=pi/6+(pi/50).*(t.^2)-(pi/375).*(t.^3); % vi tri khau 1
q2=(pi/25).*(t.^2)-(2*pi/375).*(t.^3); % vi tri khau 2
xE=a1*cos(q1)+a2*cos(q1+q2);
yE=a1*sin(q1)+a2*sin(q1+q2);
xA=1039.23
yA=600
xB=50
yB=1039.23
plot(xE,yE,'r','linewidth',2)
hold on
plot([xA,xB],[yA,yB],'b','linewidth',2)
xlabel('xE [mm]'),ylabel('yE [mm]')
title('Do thi diem E trong khong gian thao tac')
text(1050,620,'A')
text(30,1050,'B')
text(200,850,'Duong thang AB')
text(600,1010,'quy dao thuc diem E')
grid on
hold off

% chuong trinh mo phong vi tri co cau theo t
% khi da tim ra q1 va q2 theo bai toan thiet ke quy dao
global a1 a2 q1 q2 xA xB yA yB x0 y0
%................................................
% nhap tham so
a1=650;
a2=550;
x0=0;
y0=0;
t=0:0.01:5; N=length(t);
q1=pi/6+pi/50.*t.^2-pi/375.*t.^3;
q2=pi/25.*t.^2-2*pi/375.*t.^3;
xB=a1*cos(q1)+a2*cos(q1+q2);
yB=a1*sin(q1)+a2*sin(q1+q2);
xA=a1.*cos(q1);
yA=a1.*sin(q1);
x0=x0.*(t./t);
y0=y0.*(t./t);
for i=1:10:N
plot([x0(i);xA(i)],[y0(i);yA(i)],'ro-',...
[xA(i);xB(i)],[yA(i);yB(i)],'bo-')
hold on
end
title('Mo phong vi tri cua co cau theo t')
text(1060,630,'A')
text(45,970,'B')
grid on
% chuong trinh mo phong vi tri co cau theo t
% quy dao la nua duong tron
global a1 a2 x y
xA= 562.92
yA= 875

Lp CT2- K55
73

xB= 151.31
yB= 837.92
x0=0;
y0=0;
a1=650;a2=550;
%==============
% tinh toa do trung diem va ban kinh
xI=0.5*(xA+xB)
yI=0.5*(yA+yB)
R=0.5*sqrt((xA-xB)^2+(yA-yB)^2)
% tinh cac gia tri a0,a1,a2,a3
a0=acos((xA-xI)/R)
a11=0
u=acos((xB-xI)/R)-acos((xA-xI)/R);
a22=3*u/25
a3=(-2)*u/125
t=0:0.01:5; N=length(t);
x=xI+R.*cos(a0+a11.*t+a22.*t.^2+a3.*t.^3);
y=yI+R.*sin(a0+a11.*t+a22.*t.^2+a3.*t.^3);
% code tinh q1 q2
c2=((x.^2+y.^2-a1^2-a2^2)/(2*a1*a2));
s2=sqrt(1-c2.^2);
q2=atan(s2./c2);
% code tinh q1
s1=sqrt(1-c1.^2);
q1=atan(s1./c1);
xA1=a1.*cos(q1); yA1=a1.*sin(q1);
x0=x0.*(t./t);
y0=y0.*(t./t);
for i=1:15:N
plot([x0(i);xA1(i)],[y0(i);yA1(i)],'ro-',...
[xA1(i);x(i)],[yA1(i);y(i)],'bo-')
hold on
axis([0 1000 0 1100])
end
text(580,900,'A')
text(140,840,'B')
grid on
% hinh ve mo phong vi tri co cau theo t
% theo quy dao diem thao tac cuoi duong thang
global a1 a2 x y a b
xA=1039.23;
yA=600;
xB=50;
yB=1039.23;
x0=0; y0=0;
a1=650;
a2=550;
a=(yB-yA)/(xB-xA) % t=5s la thoi gian di chuyen tu A->B
b=(yA*xB-yB*xA)/(xB-xA)
a0=xA
a11=0
a22=3*(xB-xA)/25
a3=(-2)*(xB-xA)/125
t=0:0.01:5; N=length(t);
x=a0+a11.*t+a22.*t.^2+a3.*t.^3;
y=a.*x+b;

Lp CT2- K55
74

% tinh q1 q2
c2=((x.^2+y.^2-a1^2-a2^2)./(2*a1*a2));
s2=sqrt(1-c2.^2);
q2=atan(s2./c2);
%======================================
% code tinh q1
s1=sqrt(1-c1.^2);
q1=atan(s1./c1);
% ve do thi ..........
xA1=a1.*cos(q1);
yA1=a1.*sin(q1);
x0=x0.*(t./t);
y0=y0.*(t./t);
for i=1:10:N
plot([x0(i);xA1(i)],[y0(i);yA1(i)],'r*-',...
[xA1(i);x(i)],[yA1(i);y(i)],'bo-')
hold on
end
text(1060,630,'A')
text(45,970,'B')
grid on
% chuong trinh ve x(t),y(t) trong bai toan thiet ke quy dao thang
global a1 a2 x y a b
xA=1039.23;
yA=600;
xB=50;
yB=1039.23;
a=(yB-yA)/(xB-xA) % t=5s la thoi gian di chuyen tu A->B
b=(yA*xB-yB*xA)/(xB-xA)
a0=xA;
a1=0;
a2=3*(xB-xA)/25;
a3=(-2)*(xB-xA)/125;
t=0:0.01:5;
x=a0+a1+a2.*t.^2+a3.*t.^3;
y=a.*x+b;
% ve do thi
%--------------------
figure(1)
plot(x,y,'m','linewidth',2)
title('Duong thang quy dao diem E')
text(1050,620,'A')
text(45,1020,'B')
grid on
%-------------------------
figure(2)
plot(t,x,'r','linewidth',2)
hold on
plot(t,y,'--b','linewidth',2)
title('do thi x,y theo t')
xlabel('t [s]'), ylabel(' x,y [mm]')
legend('x(t)','y(t)')
grid on
hold off

Lp CT2- K55
75

% chuong trinh tinh ve do thi x y trong bai toan thiet ke
% quy dao theo duong tron
global a1 a2 x y
xA=562.92;
yA=875;
xB=151.31;
yB=837.92;
%==============
% tinh toa do trung diem va ban kinh
xI=0.5*(xA+xB)
yI=0.5*(yA+yB)
R=0.5*sqrt((xA-xB)^2+(yA-yB)^2)
% tinh cac gia tri a0,a1,a2,a3
a0=acos((xA-xI)/R)
a1=0
u=acos((xB-xI)/R)-acos((xA-xI)/R);
a2=3*u/25
a3=(-2)*u/125
t=0:0.01:5;
x=xI+R.*cos(a0+a1.*t+a2.*t.^2+a3.*t.^3);
y=yI+R.*sin(a0+a1.*t+a2.*t.^2+a3.*t.^3);
% ve do thi
%--------------------
figure(1)
plot(x,y,'m','linewidth',2)
title('quy dao diem E ')
%text(1050,620,'A')
%text(45,1020,'B')
grid on
%-------------------------
figure(2)
plot(t,x,'r','linewidth',2)
hold on
plot(t,y,'--b','linewidth',2)
title('do thi x,y theo t')
xlabel('t [s]'), ylabel(' x,y [mm]')
legend('x(t)','y(t)')
grid on
hold off
% CHUONG TRINH TINH CHUONG TINH HOC VA DONG LUC HOC
% khai bao cac bien
syms alpha theta a d q1 q2 a1 a2 q3 a3 t m1 m2 g Fx Fy Mz
q1=sym('q1(t)');
q2=sym('q2(t)');
q=[q1;q2]; % Vector cc toa do suy rong q
dq1=diff(q1,t);
dq2=diff(q2,t);
dq =[dq1;dq2]; % Vector cc van toc dai
% xet khau tac dong cuoi
FE2=[Fx;Fy;0]; ME2=[0;0;Mz]; F032=FE2 ;M032=ME2;
r11=[a1;0;0]; r22=[a2;0;0];
rc11=[-a1/2;0;0]; rc22=[-a2/2;0;0];
P01=[0;-m1*g;0]; P02=[0;-m2*g;0];
r0c2s=[0 0 -a2/2*sin(q1+q2);0 0 a2/2*cos(q1+q2);a2/2*sin(q1+q2) -
a2/2*cos(q1+q2) 0];
r02s=[0 0 a2*sin(q1+q2);0 0 -a2*cos(q1+q2);-a2*sin(q1+q2) a2*cos(q1+q2)
0];
% r0c2s va r02s la cac ma tran song cua khau 2

Lp CT2- K55
76

disp('luc tac dung len khau 2 : khau cuoi ')
F21=FE2-P02 % luc tac dong khau 2
disp('momoent tac dung len khau 2 : khau cuoi ')
M021=M032+r02s*F21-r0c2s*P02 % moment tac dong vao khau 2
% tinh toan cho khau 1
r0c1s=[0 0 -a1/2*sin(q1);0 0 a1/2*cos(q1);a1/2*sin(q1) -a1/2*cos(q1) 0];
r01s=[0 0 a1*sin(q1);0 0 -a1*cos(q1);-a1*sin(q1) a1*cos(q1) 0];
disp('luc tac dung len khau 1 : khau de')
F10=F21-P01
disp('moment tac dung len khau 1 : khau de ')
M010=M021+r01s*F10-r0c1s*P01
% TINH TOAN CHO CHUONG 5
I1x=0; I1y=1/12*m1*a1^2; I1z=I1y;
I2x=0; I2y=1/12*m2*a2^2;I2z=I2y;
rc1=[1/2*a1*cos(q1);1/2*a1*sin(q1);0]; % toa do khoi tam C1
rc2=[a1*cos(q1)+1/2*a2*cos(q1+q2);a1*sin(q1)+1/2*a2*sin(q1+q2);0];
disp('bieu thuc van toc goc cac khau')
w1=[0;0;dq1]
w2=[0;0;dq2]
disp('cac ma tran jacobi tinh tien va quay')
rc=[rc1;rc2];

JT1=[-1/2*a1*sin(q1) 0;1/2*a1*cos(q1) 0;0 0]
JR1=[0 0;0 0;1 0]
JT2=[-a1*sin(q1)-1/2*a2*sin(q1+q2) -
1/2*a2*sin(q1+q2);a1*cos(q1)+1/2*a2*sin(q1+q2) 1/2*a2*cos(q1+q2)]
JR2=[0 0;0 0;1 1]
disp('cac ma tran tenxo quan tinh')
I1=[0 0 0;0 I1y 0;0 0 I1z]
I2=[0 0 0;0 I2y 0;0 0 I2z]
disp('ma tran khoi luong suy rong')
M=m1*JT1'*JT1+JR1'*I1*JR1+m2*JT2'*JT2+JR2'*I2*JR2
m11=M(1,1)
m12=M(1,2)
m21=M(2,1)
m22=M(2,2)
disp('bieu thuc dong nang robot')
T=1/2*dq'*M*dq
disp('bieu thuc the nang co dang')
TL=m1*g*a1*sin(q1)+m2*g*(a1*sin(q1)+1/2*a2*sin(q1+q2))
% cac thong so mo phong
a1=0.65;
a2=0.55;
g=9.81;
m1=3;
m2=2.5;
tc=5;
s0=[1.03923;0.6];
q0=DHN(s0);
sc=[0.05;1.03923];
qc=DHN(sc);
a01=q0(1);
a11=0;
a21=3*(qc(1)-q0(1))/tc^2;
a31=-2*(qc(1)-q0(1))/tc^3;
a02=q0(2);
a12=0;
a22=3*(qc(2)-q0(2))/tc^2;
a32=-2*(qc(2)-q0(2))/tc^3;

Lp CT2- K55
77

Lp CT2- K55
78

Lp CT2- K55
79

Lp CT2- K55
80

Lp CT2- K55
81

Phn XI : TI LIU THAM KHO
1.Bi ging Robotics-PGS.TS.Phan Bi Khi.
2.ng lc hc h nhiu vt-GS.TSKH.Nguyn Vn Khang.
3. C s Robot cng nghip -GS.TSKH Nguyn Vn Khang
4. iu khin Robot cng nghip TS. Nguyn Mnh Tin.
5. Robot cng nghip GS. TSKH. Nguyn Thin Phc .
6 .Phn help ca phn mm Maple.
7. Phn help ca phn mm Matlab.

Do An CĐT

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Do An CĐT

Uploaded by

Copyright:

Available Formats

N THIT K H THNG C IN T GVHD:ThS Mc Th Thoa

Sinh vin thc hin: inh c Anh

c chn dc theo trc ca khp th (i+1).

c xc nh dc theo ng vung gc chung gia trc khp ng

c xc nh theo quy tc bn tay phi.

: gc quay quanh trc

: dch chuyn tnh ti dc trc

: dch chuyn dc trc

: gc quay quanh trc

sao cho trc

(B) trong thi gian

vi q l bin khp tng qut.

m quan h y=k.x+b => x=x(t) cng c cc iu kin nh

You might also like