robot rắn vn

Lun Vn Tt Nghip GVHD : TS.V Tng Qun

I

LI CM N

t nc ta ang pht trin do khoa hc k thut ng mt vai tr ht sc quan trng i

vi i sng con ngi.Vic p dng khoa hc k thut chnh l lm tng nng sut lao ng

ng thi n cng gp phn khng nh trong vic thay th sc lao ng ca ngi lao ng mt

cch c hiu qu nht, m bo an ton cho h trong qu trnh lm vic. Cc robot chnh l s

thay th tuyt vi cho sc ngi trong cc cng vic nguy him v cc cng vic cc m trng

lm vic khc nghit.

Lun vn tt nghip l mt mn hc gip cho sinh vin ngnh C in T ng dng cc kin

thc c hc ti nh trng ng dng vo thc t v kim tra nhng l thuyt hc so vi

thc t c ci nhn c th hn vi cc vn hc.T rt ra cc kinh nghim lm cc

ti v cc cng vic trong nh my, x nghip, cng ty khi sinh vin ra trng

Trong phm vi , cc kin thc t cc mn c s nh Nguyn L My, Chi Tit My, Thit

K v V Bng My Tnh,Vi X L, Giao Tip My Tnh, K Thut Ngi Myc p dng.

y l cc mn hc m b mn C in T trang b cho sinh vin trc khi lm Lun Vn

Tt Nghip. thc s quan trng v cn thit i vi sinh vin trong qu trnh thc hin ti

Em xin chn thnh cm n TS. V Tng Qun tn tm hng dn em hon thnh Lun Vn

Tt Nghip. Em cng xin chn thnh cm n cc thy c hng dn trong hc k ny v nhng

bui phn bin gip em c thm kin thc, kinh nghim khi thc hin bn v. ng thi em

cng xin cm n tp th lp CK07CD, cc bn t nhiu gip em hon thin n ny.

y l Ti Lun Vn Tt Nghip u tin nn s khng trnh c nhng thiu st v

thiu kinh nghim trong vic tnh ton, chn la cc chi tit,trong m hnh v iu khin.Em knh

mong c s ch dn thm ca qu thy c em c cng c kin thc v c kt thm

nhng kinh nghim qu bu phc v cho cng vic sau ny.

HCM, Ngy ThngNm 2011

Bi Thanh Vinh


II

TM TT LUN VN

Robot rn l ti c nghin cu t rt lu v thu ht cc nh khoa hc,cc sinh vin.

Trong nhng nm gn y ,nhng nghin cu trong lnh vc ny c nhiu kt qu xut sc.

Lun vn ny c thc hin vi mc tiu nghin cu, m phng chuyn ng ca robot rn

trong mi trng mt phng ngang v cc cc phng php iu khin lin quan.

ti c th xem l tng hp kin thc trong chng trnh C in t bao gm m hnh ha,

m phng, thit k c kh, thit k mch in, thit k cc thut ton iu khin

L thuyt tnh ton nglc hc c ng dng t cc nghin cu trn th gii cho m hnh

robot rn chuyn ng trn mt phng .

Qu trnh tnh ton, m phng v p dng cc lut iu khin c thc hin trn Matlab

kim chng cc chuyn ng ca robot rn. Trong gii hn thi gian ca lun vn, lun vn ch

m phng p dng thut ton Fuzzy vo m hnh thc t iu khin nh hng cho robot rn


III

MC LC

LI CM N .............................................................................................................................. I

TM TT LUN VN ............................................................................................................. II

MC LC ................................................................................................................................ III

DANH SCH HNH V .......................................................................................................... VI

1. CHNG I - TNG QUAN ............................................................................................ 1

1.1 GII THIU ................................................................................................................. 1

1.2 TNH HNH NGHIN CU TRONG V NGOI NC .................................... 1

1.2.1 Nc Ngoi ............................................................................................................ 1

1.2.2 Trong nc ............................................................................................................. 2

1.3 C IM SINH HC CA RN............................................................................. 2

1.3.1 Cu to ca rn ...................................................................................................... 2

1.3.2 Cc dng chuyn ng ca rn : ............................................................................ 3

1.4 CC PHNG PHP TNH NG HC CA ROBOT RN ............................... 5

1.4.1 Phng php quy c Denavit- Hartenberg .......................................................... 5

1.4.2 Cc lin kt nonholonomic .................................................................................... 6

1.4.3 Cc ng xng sng v cc robot mi trng lin tc ...................................... 7

1.5 CC PHNG PHP NG LC HC .................................................................. 7

1.6 MC TIU CA TI ............................................................................................ 8

1.7 TM TT NI DUNG CA TI V QU TRNH THC HIN ..................... 9

1.7.1 Ni dung thc hin................................................................................................. 9

1.7.2 Cc bc thc hin ................................................................................................ 9

2. CHNG II TNH TON NG HC - NG LC HC Robot........................ 10

2.1 LA CHN M HNH .............................................................................................. 10

2.2 NH NGHA V K HIU ..................................................................................... 10


IV

2.3 PHNG PHP SVD ................................................................................................ 12

2.4 DNG PHNG PHP LAGRANGE TNH NG LC HC robot ............ 12

2.4.1 Tnh moment qun tnh. ....................................................................................... 13

2.4.2 Tnh lc lin kt gia cc khu ............................................................................ 13

2.4.3 Tnh vn tc gc ca tng t ca robot.............................................................. 15

2.4.4 Tnh vn tc ca robot rn ................................................................................... 23

3. CHNG III TNH TON THIT K C KH ....................................................... 26

3.1 GII THIU V PHN TCH MT S H THNG C KH .............................. 26

3.1.1 Hnh thc di chuyn nh ma st .......................................................................... 26

3.1.2 Mt hnh thc chuyn ng ma st theo hai trc ................................................. 26

3.1.3 M hnh chuyn ng nh bnh xe ...................................................................... 28

3.2 QU TRNH THIT K C KH ............................................................................. 29

3.2.1 t vn : .......................................................................................................... 29

3.2.2 Gii thiu ng c RC servo ............................................................................... 29

3.2.3 Cu to ng c RC servo chun ......................................................................... 29

3.2.4 Phn loi ng c RC servo ................................................................................. 30

3.2.5 Nguyn tc hot ng .......................................................................................... 31

3.2.6 Cc thng s k thut ca RC .............................................................................. 31

3.2.7 Tnh ton moment xon ng c ......................................................................... 32

3.2.8 La chn ng c ................................................................................................ 32

3.2.9 Thit k h thng c kh : .................................................................................... 33

4. CHNG IV - IU KHIN V THIT K MCH IU KHIN ........................ 37

4.1 IU KHIN .............................................................................................................. 37

4.1.1 Phng n iu khin .......................................................................................... 37

4.1.2 iu khin ng c RC servo .............................................................................. 39


V

4.2 IU KHIN NH HNG ................................................................................... 41

4.2.1 B iu khin Fuzzy ............................................................................................ 41

4.3 THIT K MCH IN CHO ROBOT RN .......................................................... 48

4.3.1 Mch Ngun cho vi iu khin trn robot ........................................................... 48

4.3.2 Mch cm bin xc nh hng ........................................................................... 49

4.3.3 Mch vi iu khin .............................................................................................. 50

4.3.4 Moduel cc ng c v Led bo tn hiu ca ng c......................................... 51

4.4 LU IU KHIN ROBOT RN ...................................................................... 52

5. CHNG V : M PHNG V THC NGHIM ....................................................... 54

5.1 CC THNG S M PHNG ................................................................................. 54

5.2 M PHNG VI SIM MECHANICS ....................................................................... 54

5.2.1 Xy dng m hnh m phng .............................................................................. 54

5.2.2 Xy dng m hnh mc tiu ................................................................................. 55

5.2.3 B iu khin ca robot ....................................................................................... 56

5.2.4 Kt qu m phng ................................................................................................ 58

5.3 THC NGHIM ......................................................................................................... 60

5.3.1 Xy dng m hnh ............................................................................................... 60

5.3.2 Kt Qu ................................................................................................................ 63

6. CHNG VI : KT LUN ........................................................................................... 64

6.1 KT QU T C .............................................................................................. 64

6.2 KT QU CHA T C ................................................................................. 64

6.3 HNG PHT TRIN .............................................................................................. 64

TI LIU THAM KHO ..................................................................................................... 65

PH LC A ............................................................................................................................. A1

PH LC B : ............................................................................................................................ B1


VI

DANH SCH HNH V

Hnh 1.1 : Robot rn OmniTread(OT-8)

Hnh 1.2 robot rn ACM-R5

Hnh 1.3 : Qu trnh chuyn ng ca rn

Hnh 1.4 : Chuyn ng sng ngang

Hnh 1.5 : Chuyn ng ln mt bn

Hnh 1.6 cc bc t do ca robot rn

Hnh 1.7: 3 khu trong lin kt ACM III

Hnh 2.1 : n - t ca robot rn

Hnh 2.2 : M hnh ha cc ng c

Hnh 2.3 : M hnh lc v moment ca robot rn

Hnh 2.4 : Th gc v vn tc gc ca khu 1 3




Hnh 2.8 : Khong cch di chuyn ca robot theo thi gian


Hnh 2.10 : Vn tc di chuyn ca robot theo thi gian


Hnh 3.1 : kt cu robot rn chuyn ng nh ma st

Hnh 3.2 : Cu to khp ni 1 bc t do

Hnh 3.3 : m hnh chuyn ng ma st theo 2 trc

Hnh 3.4 : ACM III ca Tin s Shigeo Hirose Nht Bn

Hnh 3.5 : Robot ACM 5 chuyn ng nh 6 bnh xe


VII

Hnh 3.6: Cu to ng c RC servo

Hnh 3.7 nh ng c thc t dng trong m hnh

Hnh 3.8 : M hnh 3D ca RC servo

Hnh 3.9 : M hnh 3D ca gi ng c

Hnh 3.10 : M hnh 3D ca ng lm 1 t ca robot

Hnh 3.11 : M hnh 3D 1 t ca robot

Hnh 3.12 : M hnh 3D ng ni cc t

Hnh 3.13 : Mt t thc t ca robot rn

Hnh 3.14: Robot rn 6 t

Hnh 4.1 : P1 chuyn ng theo khu u , P2 l chuyn ng hnh sin

Hnh 4.2 : Gin xung iu khin ng c RC servo

Hnh 4.3 : Lu thut ton to xung iu khin ng c

Hnh 4.4 : M hnh xc nh hng ca robot

Hnh 4.5 : Mch quang in tr

Hnh 4.6 : S bin thin ca quang in tr

Hnh 4.7 : B tr cm bin nh hng

Hnh 4.8 : S b iu khin Fuzzy

Hnh 4.9 : S Membership Function input

Hnh 4.10 : S Membership Function output

Hnh 4.11: Phng php gii m theo ta trng tm

Hnh 4.12 : S h lut ca b Fuzzy

Hnh 4.13 : Pin si cho m hnh robot

Hnh 4.14: Mch ngun cho vi iu khin trn robot

Hnh 4.15 : S mch nh hng LDR


VIII

Hnh 4.16 : Mch vi iu khin trn robot

Hnh 4.17: Module ni ti cc ng c

Hnh 4.18 : Cc n Led bo tn hiu iu khin ng c

Hnh 4.19 : Lu gii thut iu khin robot rn

Hnh 5.1 : M hnh m phng robot

Hnh 5.2 : M hnh lc ma st

Hnh 5.3 : M hnh ng c Servo

Hnh 5.4 : M hnh 2 mc tiu

Hnh 5.5 : B iu khin Fuzzy bm mc tiu

Hnh 5.6 : Membership Function input

Hnh 5.7 : Membership Function output

Hnh 5.8 : B iu khin Fuzzy trung tm v cc tn hiu iu khin

Hnh 5.9 : th ta trong tm ca robot

Hnh 5.10 : Qu trnh di chuyn qua 2 mc tiu ca robot

Hnh 5.11 : M hnh 3D di chuyn n mc tiu

Hnh 5.12 : Mch iu khin robot


Hnh 5.14 : M hnh tht t ca robot rn

Hnh 5.15 : M hnh thc t ca robot rn



Chng I : Tng Quan

Trang 1

1. CHNG I - TNG QUAN

1.1 GII THIU

Trc y hu ht cc thit b c chuyn ng u dng n cc loi bnh xe,nhng thit b

ny c th duy chuyn vi tc cao trn cc b mt phng tuy nhiu di chuyn trn cc bn mt

g gh th gp nhiu kh khn.V vn c c ra l tm cc dng di chuyn khc hot ng

d dng trn nhng b mt g gh,mt trong s nhng loi di chuyn c tm ra chnh l di

chuyn ca loi rn.Chng di chuyn nh s ma st gia thn v b mt.Chnh nh iu ny m

rn di chuyn rt nhiu a hnh khc nhau,ngoi ra rn cn c th di chuyn di nc , leo

cy .. V th ng dng chuyn ng ca rn ln robot c nhiu ngi quan tm v nghin

cu.V Nhiu robot rn ra i vi nhiu ng dng khc nhau phc v cho cng tc nghin cu

v cuc sng

Chuyn ng ca robot rn c n nh , linh hot , t h hng v c cu . Nhng li mang

ti trng km . Ha hn ng dng trong nhiu ngnh nh : gii phu , cu ha , cu h ngi

mc kt , thm him ..

1.2 TNH HNH NGHIN CU TRONG V NGOI NC

1.2.1 Nc Ngoi

Nhng nc i u v cng ngh nghin cu v ch to nhiu loi robot rn khc nhau v

cho ra i nhiu th h robot rn c kh nng linh hot rt cao nh:

Ti i hc Michigan c robot rn OmniTread(OT-8) c kh nng di chuyn trn nhiu

a hnh phc tp, c kh nng di chuyn trong ng ng v di chuyn qua cc hang

nh...

Hnh 1.1 : Robot rn OmniTread(OT-8) [3]

Chng I : Tng Quan

Trang 2

Ti Nht c robot rn ACM-R5 ca tin s Shigeo Hirose c kh nng di chuyn c di

nc v trn cn mt cch linh hot

Hnh 1.2 robot rn ACM-R5 [9]

V ti nhiu trng i hc ln khc trn th gii cng c nhng sn phm l robot rn

nh CMU ca i hc Carnegie Mellon,JL-I ca i hc Hamburg...

1.2.2 Trong nc

Ti Vit Nam ang bt u nghin cu v ch to mt s robot rn nhng rt t.Ch yu vn

dng li qu trnh tnh ton l chnh.V cha c sn phm robot rn no c kh nng di chuyn

linh hot c

Ti cc trng i hc Bch Khoa TP HCM th cng c nhng ti lun vn tt

nghip v robot rn nhng nhng m hnh lm c kt cu c kh khng p ng

c th di chuyn linh hot c

1.3 C IM SINH HC CA RN

1.3.1 Cu to ca rn

Cu to xng sng

Mt b xng rn thng c t 100 400 t. Khong chuyn ng ca mi khp thng t

100-20

0 di chuyn qua li, khong vi di chuyn theo phng ln xung. Ngoi ra cc

khp xng cn c th quay mt gc rt nh dc theo trc xng sng thun li cho cc dng

chuyn ng phc tp.

Cu to da :

Chng I : Tng Quan

Trang 3

Da rn c ph kn vy . Hu ht rn di chuyn da vo lp vy ny . Da rn kh nhn hoc

c ht . Mi mt rn trong sut v thng xuyn ng kn , c gi l vy mt . Mc ch c bn

lt da l trng thnh , lt da cng khin rn loi b k sinh trng trong sut qu trnh di

chuyn . Da rn ng 1 vai tr quan trng trong gip rn di chuyn nhanh nhiu a hnh khc

nhau .

Di chuyn :

Ton thn rn c bao bc mt lp vy . Nhng chic vy ny v cng cng rn , khng ln

ln tng ng theo s trng thnh ca thn th rn . V vy c 2-3 thng rn phi thay da 1 ln .

Nhng chic vy ny khng ch gip rn bo v m cn c chc nng nh bn chn rn trng

b. Khi di chuyn, thn di v nh n un thnh hnh ch S , pha di thn th theo st b phn

pha trn b ln cng v tr y . Khi b , cc vy trn theo b phn li ra , rn dng u nhn

ca cc chic vy tro ln nhng m c hoc m t g gh .

1.3.2 Cc dng chuyn ng ca rn :

1.3.2.1 S di chuyn iu ha kiu n concertina

Vi dng chuyn ng ny , rn xp v gin c th di chuyn ti . Phn xp s gi v tr c

nh v phn cn li s c y hoc ko ti .

Hnh 1.3 : Qu trnh chuyn ng ca rn [7]

Sau , hai phn s i vai tr vi nhau . Chuyn ng dng ny t c khi lc y c nh

ln hn lc m st tc dng ln phn di chuyn .

Chng I : Tng Quan

Trang 4

Chuyn ng ny c thc hin khi di chuyn theo ng hp , trong ng , trn cnh cy .

1.3.2.2 Chuyn ng theo sng ngang

y l kiu chuyn ng lin tc ca ton b c th rn trn mt phng. Chuyn ng c

thc hin nh truyn dng sng di chuyn t trc ra sau ng thi da vo nhm ca mt

t. Mi phn ca c th s i qua v tr ca phn trc nh dng mt hnh sin.

Hnh 1.4 : Chuyn ng sng ngang

trnh hin tng trt sang hai bn khi ang di chuyn ti, rn bm vo mt t nh cc

vy. Ngoi ra chng c th da vo dng ca b mt nh ta vo to lc y ti. Mi im

tip xc vi mt t u tr thnh im ta cho rn. Mt con rn cn t nht 3 im ta to

chuyn ng i ti, hai im cn sinh lc v im th 3 cn bng cc lc di chuyn.

c im quan trng ca dng chuyn ng ny l s chnh lch gia cc h s ma st theo

phng vung gc v phng tip tuyn vi c th. Lc ma st theo phng vung gc phi ln

hn nhiu sao vi lc ma st tip tuyn, nhm trnh s trt ngang.

Hiu sut ca dng chuyn ng ny ph thuc vo 2 yu t chnh: (1) Hnh dng ca b mt.

Trn b mt g g, c cc cc lm im ta th s tng hiu sut di chuyn. (2) T l gia

chiu di v chu vi. Nhng con rn nhanh nht c chiu di khng qu 10 - 13 ln chu vi ca n.

Tc ny c th t ti 11km/h khi di chuyn trn b mt g g.

Dng chuyn ng ny khng thch hp vi b mt nhn, ma st thp v trong cc hnh lang

hp. N cng khng thch hp cho nhng con rn qu ngn hoc qu nng v chng khng th t

c bin dng cn thit di chuyn hoc hiu sut chuyn ng gim ng k do c th qu

nng.

Chng I : Tng Quan

Trang 5

1.3.2.3 Chuyn ng kiu ln mt bn

Hnh 1.5 : Chuyn ng ln mt bn

y l dng chuyn ng c thu ht nhiu nht ca rn v c s dng ch yu bi cc

loi rn sng trn sa mc . Rn nhc v un c th thnh cc vt ngn , song song trn mt t

ng thi di chuyn theo mt gc nghin . Khc vi dng sng ngang , dng ny c mt on

tip xc ngn gia c th rn v b mt .

S pht trin dng chuyn ng ny c l l do nhu cu v lc ko trn cc b mt trt nh

ct v s cn thit phi trnh nhit qu cao trn b mt sa mc . Rn c th t vn tc 3km/h

vi dng chuyn ng ny .

1.3.2.4 Cc dng chuyn ng khc

Rn cn c cc dng chuyn ng him gp hn ty vo mi trng sng. V d nh b theo

ng thng (rectilinear crawling), o bi (burrowing), nhy (jumping), dng sin (sinus-lifting),

trt ngang (skidding), leo v bi,

1.4 CC PHNG PHP TNH NG HC CA ROBOT RN

1.4.1 Phng php quy c Denavit- Hartenberg

Phng php D-H l mt phng php tt c thit lp m t v tr v s nh hng ca

mi khu trong cnh tay robot i vi h ta nn . Cc nghin cu chia robot rn thnh cc

module gm cc khp c ni vi nhau bi cc khu c chiu di bng nhau . H ta nn

c ngh t vo khu khng chuyn ng ( tng phn robot chuyn ng ng thi ) , hoc

t trn cc khp o c khi lng bng khng , s dng cu trc nh hng v nh v o .

Chng I : Tng Quan

Trang 6

Hnh 1.6 cc bc t do ca robot rn [6]

1.4.2 Cc lin kt nonholonomic

im ct yu trong chuyn ng ca robot rn l thay i hnh dng robot lin tc bng cch

quay hoc gin di hnh dng ca robot . Cc nghin cu a ra cc m hnh ng hc s dng

cc lin kt nonholonomic ( dn n vic gn bnh xe cho robot ) v cc yu t hnh hc khc

nh cc khp ni .

Hnh 1.7: 3 khu trong lin kt ACM III [9]

Cc lin kt ng hc nonholonomic c thc hin bng cch gn cc bnh xe b ng vo

robot c th hin di dng :

Chng I : Tng Quan

Trang 7

. .

sin( ) os( ) 0i ix y c (1.1)

Vi

. .

( , )x y l cc vn tc khi tm ca khu i l gc khp . Bnh xe c gi s khng

trt ngang v t tnh cht ma st l tng ca da rn . Kt hp vi cc m hnh t hnh hc ti

khp ni , cc nghin cu gii thch c lm th no t vic thay i hnh dng c th sinh ra s

chuyn ng . Cc nghin cu minh ha robot ACM-III dng 3 on u tin xc nh dng

chuyn ng ca c h , cn cc on cn li s bm theo dng ny nh vo cc lin kt

nonholonomic ca cc bnh xe .

1.4.3 Cc ng xng sng v cc robot mi trng lin tc

Thay v bt u tm v tr v hng ca mi khp mt cch trc tip bng quy c D-H , cc

nghin cc khc tip cn vi mt ng cong m t c hnh dng ca robot rn . Cch tip cn

ny c hn ch l cc h ta gn dc ng cong khng c xc nh cho cc khu thng v

cc hm vector m t ng khng gian i hi mt php gii s phc tp . Dng ng xng

sng c a ra khc phc hn ch ny . ng xng sng c nh ngha l mt ng

lin tc tng khc chy qua cc on ca robot rn . Mt tp hp cc h quy chiu trc giao t

dc ng ny ti cc im thch hp s ch ra hnh dng tht s ca robot rn .

Vn xc nh cc gc khp i vi cnh tay robot t v tr ca khu tc ng cui ( ng

hc ngc ) to ra s li gii v hn i vi cc cnh tay c d tha cao. Thut ton nh v

khu tc ng cui tng ng vi dng ng xng sng s gim bt hn ch ny .

Cc robot lin tc l dng c bit ca cnh tay robot khng c cc khu cng v cha cc

khp quay khng nh ngha c ( nh vi voi , tay chn bch tut ) . Cc nghin cu theo

hng ny gn cc khi nim cnh tay robot c khu cng vo trong ng xng sng lin tc

v chuyn cc tham s ng hc thnh cc tham s D-H . T , quy c D-H c dng xc

nh v tr v hng ca u robot .

1.5 CC PHNG PHP NG LC HC

Vi robot khng bnh xe , lc ma st ng vai tr quan trng trong chuyn ng , vic m

hnh ng lc hc l cn thit do chuyn ng dng sng ngang . Vi robot c bnh xe , bnh xe

s gim ma st dc trc v c th ch dng m hnh ng hc km iu kin khng trt ngang

Chng I : Tng Quan

Trang 8

ca bnh xe ( iu kin ny dn n nhng kh khn khi iu khin moment ti khp ). Ngoi ra,

cc m hnh ng lc hc ca robot khng bnh xe c th dng p dng cho robot c bnh xe

Cc phng php ng lc hc cho robot rn c thit lp t nhiu k thut khc nhau nh :

cch tip cn Newton - Euler , cch tip cn Lagrange , c hc hnh hc

Cch tip cn m hnh c chia lm hai hng :

Cch tip cn Newton-Euler ( phn tch v lc ) :

cch tip cn ny , mi khu c phn tch c lp bng cch m t chuyn ng tuyn

tnh v quay . Sau , cc khu cng c ni vi nhau bi cc khp v cc lc lin kt hai

chiu . y l phng php thch hp tm moment xon cn t vo khp t c chuyn

ng mong mun .

Cch tip cn lagrange ( phn tch v nng lng )

Cch tip cn Lagrange xem ton b robot l mt h ngay t u v s dng hm Lagrange

phn tch di dng nng lng . Cch ny vt tri khi dng phn tch ton b h theo thi

gian . Mt s nghin cu kt hp cch tip cn ny vi l thuyt c hnh hc iu khin

moment xon ca robot rn . Tuy nhin , iu ny i hi c mt nn tng v c hnh hc

Cc m hnh ha ton hc c nghin cu theo hai hng :

iu khin da theo hnh dng :

Hng i ny a ra m hnh di dng iu kin bit trc gc , vn tc v gia tc gc . T

nhng d liu cho , ta xc nh c gia tc tnh tin v gia tc . Hn na , moment cn thit

robot di chuyn theo hnh dng cng xc nh c .

iu khin da theo moment

Hng tip cn ny l hng thng dng nht trong cc m hnh robot rn . Vn c

nghin cu l robot rn di chuyn th no vi nhng moment khp c cho . T cc moment

ny , ta tm c gia tc tnh tin v quay ca khu u v t tm ra vn tc v v tr .

1.6 MC TIU CA TI

Tnh ton ng hc ca robot rn theo phng php Lagrange ng dng vo iu khin

Tm gii thut di chuyn hp l cho robot rn

M phng qu trnh di chuyn t phng trnh ng lc hc v gii thut di chuyn tm c

Chng I : Tng Quan

Trang 9

Thit k thc t robot rn p dng l thuyt tnh ton c

T nhng tnh ton v m phng s to ra mt robot rn c kh nng di chuyn linh hot trn

cn

1.7 TM TT NI DUNG CA TI V QU TRNH THC HIN

1.7.1 Ni dung thc hin

Tnh ton ng hc ca robot rn

Tm gii thut di chuyn linh hat cho robot rn

M phng qu trnh di chuyn ca robot rn

Thit k m hnh robot rn

1.7.2 Cc bc thc hin

Bc 1 : Thit k kt cu c kh ca robot rn

Bc 2 : Tnh ton ng hc

Bc 3 : Tnh ton v tm gii tht di chuyn cho robot rn

Bc 4 : M phng qu trnh di chuyn

Bc 5 : Ch to m hnh robot rn

Bc 6 : Thit k mch iu khin

Bc 7 : Th nghim v iu khin

Bc 8 : Hon chnh v ti u cc phn trc khi kt thc

Chng II : Tnh Ton ng Lc Hc Robot

Trang 10

2. CHNG II TNH TON NG HC - NG LC HC ROBOT

2.1 LA CHN M HNH

Mi phng php nghin cu trnh by trong chng 1 u c u nhc im ring. Trong

gii hn ca ti, em s dng m hnh ng hc, ng lc hc nh hnh di vi cch trnh by

n gin hn. M hnh ny da trn cch tip cn Lagrange, s dng m hnh ma st Coulomb.

V mt iu khin chuyn ng, em thc hin theo hng ch yu l tm gc tham chiu khp

to ra cc dng chuyn ng khc nhau. Vic m phng v thc hin cc thut ton iu khin

da trn Matlab/Simulink.

2.2 NH NGHA V K HIU

Hnh 2.1 : n - t ca robot rn

Mt trong nhng u im ca robot rn l cc module u ging nhau. m hnh ha ta xem

xt robot rn tng quan gm n khu c cng chiu di l v khi lng m v moment qun tnh I


Trang 11

c ni vi nhau bng n-1 khp. Khi lng ca mi khu c phn b xem nh u sao cho

trng tm mi khu nm chnh gia khu. H ta gc ca mi khu c gn cng vo trng

tm v c cc trc xi (dc trc) v yi (vung gc). Cc trc ny trng vi trc khi gc khu l

0.

Robot rn trong mt phng 2D nm ngang c n+2 bc t do. Gc tuyt i i ca mi khu

c so vi trc x theo chiu lng gic.

t s 1 Moment qun tnh Khi lng Chiu di Khong cch n trng tm cng tng ng gim chn tng ng

2

3

i ii

m lI [kgm

2]

m1= 0.4[kg]

l1= 0,21[m]

d1=0,105[m]

k1=0,5 [Nm/rad]

c1= 10-3

[Nms/rad]


2

3

i ii

m lI [kgm

2]

m2=0,4 [kg]

l2= 0,24 [m]

d2=0,12 [m]

k2= 0,5 [Nm/rad]

c2= 10-3

[Nms/rad]


2

3

i ii

m lI [kgm2]

m3=0,4 [kg]

l3= 0,24[m]

d3= 0,12 [m]

k3= 0,5 [Nm/rad]

c3= 10-3

[Nms/rad]


2

3

i ii

m lI [kgm2]

m4=0,4 [kg]

l4=0,24 [m]

d4=0,12 [m]

k4= [Nm/rad]

c4= 10-3

[Nms/rad]


2

3

i ii

m lI [kgm2]

m5= 0,4[kg]

l5=0,24 [m]

d5=0,12 [m]


Trang 12

k5= 0,5 [Nm/rad]

c5= 10-3

[Nms/rad]

t s 6 Moment qun tnh Khi lng Chiu di Khong cch n trng tm

2

3

i ii

m lI [kgm2]

m6=0,3 [kg]

l6= 0,18 [m]

d6=0,09 [m]

Bng 2.1: Bng thng s ban u ca robot rn

2.3 PHNG PHP SVD

Phng php SVD (Singular value decomposition) c ng dng trong ti ny l mt

phng php cho ha ma trn cho trc nhm lm gim s chiu ca ma trn,dung tnh

ton gi tr ma trn ngc c d dng v thun tin hn khi ma trn b suy bin.

SVD c da trn mt nh l ca i s tuyn tnh, t mt ma trn A c th c phn tch

thnh tch ca 3 ma trn: mt ma trn trc giao U, mt ma trn ng cho S, ma trn chuyn v

ca ma trn trc giao V. N c dng nh sau:

. . Tmn mm mn nnA U S V (2.1)

2.4 DNG PHNG PHP LAGRANGE TNH NG LC HC ROBOT

gii phng trnh dao ng ta c rt nhiu phng php gii nh: s dng nh lut 2

Newton, nh l bin thin ng nng, nh l bin thin ng lng, nh l bin thin mmen

ng lng, nguyn l DAlamber, phng trnh Lagrange, nguyn l cng o, nguyn l

HamiltonThng thng:

Nu c h l mt vt rn tuyt i th c th dng nh lut II Newton v cc nh l chng

minh t nh lut ny.

Nu h gm nhiu vt rn tuyt i to thnh h mt bc t do th nn s dng nh l ng

nng.

Nu h gm cc vt rn tuyt i c s bc t do hu hn ta nn s dng phng trnh

Lagrange.

i vi cc h s c s bc t do v hn thng s dng nguyn l Hamilton.


Trang 13

i vi m hnh robot rn dng ny, gm cc vt rn tuyt i gn li v s bc t do l hu

hn (6 bc), do phng php gii c chn l phng php Lagrange.

2.4.1 Tnh moment qun tnh.

Xem cc t ging nhau do ta c

1 2 3 4 .............. nI I I I I vi iI l moment qun tnh ca khu i

Xem mi t nh hnh tr thng c chiu di li v khi lng mi

Moment c tnh theo cng thc sau

21

3i i iI m l

(2.2)

2.4.2 Tnh lc lin kt gia cc khu

Cc khu lin kt vi nhau c th th hin nh m hnh di y

Hnh 2.2 : M hnh ha cc ng c

Theo nh lut 2 Newton ta c: ma F mx F (2.3)

Xt khi m1

1 1 1 2 1 1 2 1 1( ) ( ) ( )m x k x x c x x f t (2.1)

(2.4)

1 1 1 1 1 2 1 1 2 1( ) ( )m x c x c x k x x f t

Xt khi 2m :

2 2 2 3 2 2 3 2 1 2 1 1 2 1 2( ) ( ) ( ) ( ) ( )m x k x x c x x k x x c x x f t (2.5)

2 2 1 1 1 2 2 2 3 1 2 2 1 1 2 3 2( ) ( ) ( )m x c x c c x c x k k x k x k x f t

Tng t vi cc khi khc ta c kt qu


Trang 14

1 1 1 1 1 2 1 1 2 1

2 2 1 1 1 2 2 2 3 1 2 2 1 1 2 3 2

3 3 2 2 2 3 3 3 4 2 3 3 2 2 3 4 3

4 4 3 3 3 4 4 4 5 3 4 4 3 3 4 5 4

( ) ( )

( ) ( ) ( )

( ) ( ) ( )

( ) ( ) ( )

m x c x c x k x x f t

m x c x c c x c x k k x k x k x f t



m

5 5 4 4 4 5 5 5 6 4 5 5 4 4 5 6 5

6 6 5 5 5 6 5 6 5 5 6

( ) ( ) ( )

( )

x c x c c x c x k k x k x k x f t

m x c x c x k x k x f t

T h phng trnh trn suy ra c h phng trnh nh sau

1 1 1 1 1 2 1 1 2 1

2 2 1 1 1 2 2 2 3 1 2 2 1 1 2 3 2

3 3 2 2 2 3 3 3 4 2 3 3 2 2 3 4 3

4 4 3 3 3 4 4 4 5 3 4 4 3 3 4 5 4

( ) ( )

( ) ( ) ( )

( ) ( ) ( )

( ) ( ) ( )

m c c k f t

m c c c c k k k k f t



m

5 5 4 4 4 5 5 5 6 4 5 5 4 4 5 6 5

6 6 5 5 5 6 5 6 5 5 6

( ) ( ) ( )

( )

c c c c k k k k f t

m c c k k f t

t 1( ) ( )i i i i iT t l f t l m ta c

1 1 1 1 1 2 1 1 2

2 2 1 1 1 2 2 2 3 1 2 2 1 1 2 3

3 3 2 2 2 3 3 3 4 2 3 3 2 2 3 4

4 4 3 3 3 4 4 4 5 3 4 4 3 3 4 5

5 5 4

( ) [ ( )]

( ) [ ( ) ( ) ]

( ) [ ( ) ( ) ]

( ) [ ( ) ( ) ]

( ) [

T t l c c k

T t l c c c c k k k k



T t l c

4 4 5 5 5 6 4 5 5 4 4 5 6

6 6 5 5 5 6 5 6 5 5

( ) ( ) ]

( ) [ ]

c c c k k k k

T t l c c k k


Trang 15

2.4.3 Tnh vn tc gc ca tng t ca robot

Hnh 2.2 : M hnh ha robot n khp

Ta c phng trnh tng qut ca Lagrange

i it i i

dLd LQ

d d

(2.6)

L K P

Trong :

K : ng nng ca h thng

P : Th nng ca h thng

Qi : Lc tng qut lin kt vi cc ta i

Ta trong tm ca mi khu di dng tng qut


Trang 16

1

1

1

1

os os

os sin

i

i j j i i

j

i

i j j i i

j

x l c d c

y l c d

(2.7)

Vn tc ca mi khu theo phng x v y

1

1

1

1

sin sin

os os

i

i j j j i i

j

i

i j j j i i

j

x l d

y l c d c

(2.8)

1

1

1

1

sin sin

os os

i

i i i i i i i

j

i

i i i i i i i

j

x l d

y l c d c

Do ta c

2 2 2

iv x y (2.9)

ng nng ca cc mi khu

2 2 2 2 21 1 1 1 ( )2 2 2 2

i i i i i i i i i iK I m v I m x y (2.10)

Th nng P =0 nn ta c

L=K P = K 0 = K

2 2 2

1 12 2 2

1 1

1 1( )

2 2

1 1( sin sin ) ( os os )

2 2

i i i i i i

i i

i i i i i i i i i i i i i i i i

j j

L I m x y

L I m l d l c d c

(2.11)

Suy ra

1 12

1 1

sin sin os osi i

ii i i i i j i j j i j i j j i

j ji

dLI m d l d l d c c

d

(2.12)


Trang 17

1 1

1 1

1 12 2

1 1

1 12

1 1

sin sin os os

os sin sin os

sin os os sin

i i

j j j i j j j i

j j

i ii

i i i i i i i j j j i j j i j i

j jt i

i i

j j j i j j i j i

j j

l l c c

dLdI m d m d l c l c

d d

l c l c

(2.13)

1 1

1 1

sin os os sini i

ii i j j i j i j j i j i

j ji

Lm d l c l c

(2.14)

1 1

1 1

1 12 2

1 1

1 12

1 1

sin sin os os

os sin sin os

sin os os sin

i i

j j j i j j j i

j j

i ii i

i i i i i i i j j j i j j i j i

j jt i i

i i

j j j i j j i j i

j j

l l c c

dL LdI m d m d l c l c

d d

l c l c

1 1

1 1

sin os os sini i

i i j j i j i j j i j i

j j

m d l c l c

(2.15)

1 1

1 12

1 12 2

1 1

sin sin os os

os sin sin os

i i

j j j i j j j i

j ji ii i i i i i i i i

t i i

j j j i j j j i

j j

l l c cdL Ld

I m d m dd d

l c l c

V phi ca phng trnh Lagrange ta c m hnh lc v moment nh hnh di


Trang 18

Hnh 2.3 : M hnh lc v moment ca robot rn

Trong phm vi lun vn ny ch xt ti lc ma st coulomb

Phng trnh lc ma st coulomb tc dng ln robot rn:

Theo phng tip tuyn

tti i t if m g sign v vi i:1,2,....n

ti i t if m g sign

Theo phng php tuyn

nni i n if m g sign v vi i:1,2,....n

ni i n if m g sign


Trang 19

Trong :

, :t n l h s ma st nht theo phng tip tuyn v phng php tuyn.

H s nht gia b mt robot v mi trng l 0,05

0,56

t

n

, :ti nif f l lc ma st theo phng tip tuyn v phng php tuyn tc dng robot rn.

Phng trnh ca moment xon l:

sin 2i i i iT A f t k

Tng cc lc v phi ca phng trnh Lagrange l

( 1) ( 1) 1

1 1

( sin os ) os ( ) sin ( ) ( )n n

i i y i i x i i i i yj i yj i i i i

j i j i

Q d f f c l c f f T T l f t

(2.16)

Vi , f : 1

: 1 1

xi yi

i

f i n

T i n

xi ni

yi ti

f f

f f

Do ta s suy ra c phng trnh Lagrange nh sau

i ij

t i i

dL LdQ

d d

(2.17)

1 1

1 12

1 12 2

1 1

( 1) ( 1)

1 1

sin sin os os

os sin sin os

( sin os ) os ( ) sin ( )

i i

j j j i j j j i

j j

i i i i i i i i i

j j j i j j j i

j j

n n

i y i i x i i i i yj i yj

j i j i

l l c c

I m d m d

l c l c

d f f c l c f f

1 ( )i i i iT T l f t

Chuyn h phng trnh trn v dng


Trang 20

11 12 1 1

21 2 2

1 2

..... ..... .....

..... ..... ..... .....

..... ..... ..... ..... ..... ..... ...

..... ..... ..... ..... ..... ..... ...

...... ..... ..... ..... ..... ..... .

..... ..... .....

n

n

n n nn

m m m

m m

m m m

1

2

...

...

.....

nn

N

N

N

(2.18)

Vi robot rn 6 t nn n = 6 ta s c phng trnh

11 12 13 14 15 16 11

21 22 23 24 25 26 22

31 32 33 34 35 36 33

41 42 43 44 45 46 44

51 52 53 54 55 56 55

61 62 63 64 65 66 66

m m m m m m N

m m m m m m N

m m m m m m N

m m m m m m N

m m m m m m N

m m m m m m N

2

11 1 1 1 1 1

12

13

14

15

16

0

0

0

0

0

m I m d m l

m

m

m

m

m

21 2 2 1 2 1

2

22 2 2 2 2 2

23

24

25

26

cos( )

0

0

0

0

m m d l

m I m d m l

m

m

m

m

31 3 3 1 3 1

32 3 3 2 3 2

2

33 3 3 3 3 3

34

35

36

cos( )

cos( )

0

0

0

m m d l

m m d l

m I m d m l

m

m

m

41 4 4 1 4 1

42 4 4 2 4 2

43 4 4 3 4 3

2

44 4 4 4 5 5

45

46

cos( )

cos( )

cos( )

0

0

m m d l

m m d l

m m d l

m I m d m l

m

m

51 5 5 1 5 1

52 5 5 2 5 2

53 5 5 3 5 3

54 5 5 4 5 4

2

55 5 5 5 5 5

56

cos( )

cos( )

cos( )

cos( )

0

m m d l

m m d l

m m d l

m m d l

m I m d m l

m

61 6 6 1 6 1

62 6 6 2 6 2

63 6 6 3 6 3

64 6 6 4 6 4

65 6 6 5 6 5

2

66 6 6 6 6 6

cos( )

cos( )

cos( )

cos( )

cos( )

m m d l

m m d l

m m d l

m m d l

m m d l

m I m d m l


Trang 21

Thng s v phi ca phng trnh

1 1

12

1

1

( os sin ) os ( ) sin ( )

( ( ) ) sin( )

n n

i i yi i xi i i i yj i yj

j i j i

i

i i i i i i i i i j j i j

j

N d f c f l c f f

T T l f t m l m d l

(2.19)

Thay cc gi tr i ta c

6 6

1 1 1 1 1 1 1 1 1 1

2 2

( os sin ) os ( ) sin ( ) ( )y x i yj yjj j

N d f c f l c f f T T t

6 6 12

2 2 2 2 2 2 2 2 2 2 2 2 1 2 2

3 3 1

( os sin ) os ( ) sin ( ) sin( ) ( )y x yj yj j j jj j j

N d f c f l c f f m d l T T T t

6 6 22

3 3 3 3 3 3 3 3 3 3 3 3 2 3 3

4 4 1



6 6 32

4 4 4 4 4 4 4 4 4 4 4 4 3 4 4

5 4 1



6 6 42

5 5 5 5 5 5 5 5 5 5 5 5 4 5 5

5 5 1



6 6 42

6 6 6 6 6 6 6 6 6 6 6 6 5 6 6

6 6 1



S dng hm ode45 trong matlab gii h phng trnh trn ta c cc gi tr ca

Vn tc gc tng khu v gc ca tng khu theo tng thi im t.

Vi cc thng s u vo nh bng 2.1 v thi gian tnh ton chn l 120s ta xc nh c

cc gi tr gc v vn tc gc ca cc khu nh cc hnh v 2.4 v hnh 2.5

Thay i cc h s ma st kho st chuyn ng ca robot trong nhng trng hp chuyn

ng ti nhng mi trng c h s ma st khc.

Gi tr cc gc ca tng khu theo hnh sin vi cc khu lch pha nhau mt gc beta = 600

chn trc .V vn tc ca cc khu thay i lin tc


Trang 22


Vi h s ma st 0,05; 0,56t n


Vi h s ma st 0,05; 0,56t n


Trang 23


Vi h s ma st 0,1; 0,5t n



2.4.4 Tnh vn tc ca robot rn

Theo nh lut II Newton ta c phng trnh ng lc hc ca robot nh sau


Trang 24

mx F

Trong

m : khi lng ca robot rn

:x gia tc ca robot rn

:F Tng cc lc ma mt tc dng ln robot rn

Dng ode45 gii phng trnh ny ta c c chuyn v,vn tc ca robot rn


Vi h s ma st 0,05; 0,56t n




Trang 25


Vi h s ma st 0,05; 0,56t n



Chng III: Tnh Ton Thit K C Kh

Trang 26

3. CHNG III TNH TON THIT K C KH

3.1 GII THIU V PHN TCH MT S H THNG C KH

3.1.1 Hnh thc di chuyn nh ma st

Hnh 3.1 : kt cu robot rn chuyn ng nh ma st [7]

Mi mt khp ca robot c trang b 2 ng c RC v cc trc ng c c gn cng vi

tay quay v kt ni vi nhau thng qua 1 khp ni nh hnh trn .

Do mt khp ni c gn cng vi hai ng c RC nn gc xoay ca mt khp c th xoay

1 gc 00 1800 tng s uyn chuyn trong chuyn ng ca robot .

Nhng c im ca m hnh ny l ch c th chuyn ng ngang trn mt t , nhng a

hnh bng phng v khng c kh nng leo tro

Cu to khp ni :

Hnh 3.2 : Cu to khp ni 1 bc t do [7]

3.1.2 Mt hnh thc chuyn ng ma st theo hai trc


Trang 27

Hnh 3.3 : m hnh chuyn ng ma st theo 2 trc [9]

y l dng cu trc c hai trc c th xoay ngang v xoay dc. C mi khp ngang c ni

dnh vi 1 khp dc to thnh 1 khu .

Cc khu lin kt theo kiu khp bn l . Chnh v th m h thng ny th robot rn c th

chuyn ng theo phng ngang nhng ng thi cng c th chuyn ng theo phng thng

ng vt cc vt th c cao .

M hnh ny th gc hot ng ca robot khng rng bng m hnh trn nhng robot m

hnh ny c kh nng hot ng c theo c 2 phng gip rn c th b c nhng a hnh

nhp nh v c chng ngi vt


Trang 28

3.1.3 M hnh chuyn ng nh bnh xe

Hnh 3.4 : ACM III ca Tin s Shigeo Hirose Nht Bn [7]

Hnh 3.5 : Robot ACM 5 chuyn ng nh 6 bnh xe [9]

Nh 6 bnh xe trn mi khp m robot c th t do di chuyn v pha trc vi tc kh cao

.


Trang 29

Ngoi ra do thit k mi khu c bt kn trong 1 khi tru di nn robot c th chuyn ng

mi trng nc ..

Kt cu c kh mi khp c th lm cho robot di chuyn theo phng ngang hay di chuyn

theo phng thng ng , v th robot c th di chuyn 3D .

3.2 QU TRNH THIT K C KH

3.2.1 t vn :

Sau khi tm hiu cc kt cu v cc cch di chuyn ca mt s robot rn . Em xy dng mt

thit k c kh chuyn ng trn mt phng nh lc ma st.V s c bc mt lp chng thm

nc cho m hnh.

M hnh gm c 6 t s gip robot di chuyn. Mi t c ng knh l 75 mm v di

120mm, lin kt gia cc t l mt ng ni. Mi t gm c mt ng c RC dng truyn

ng.

3.2.2 Gii thiu ng c RC servo

RC servo l mt ng c DC c gn vi bnh rng gim tc , mt mch iu khin hi tip.

RC servo c kh nng quay n mt v tr chnh xc v gi ti v tr vi mt moment ln. Di

gc quay chun ca u trc ra thng l -900 n +900

3.2.3 Cu to ng c RC servo chun

Hnh 3.6: Cu to ng c RC servo


Trang 30

1 . ng c DC

2. Mch iu khin

3,4,5. Dy ni

6 . Chit p

7. B bnh rng v trc

8. Cn servo

9. V ng c

10. Chip iu khin

c th hot ng c th servo cn 3 dy : mt dy ngun , mt dy ni t , mt dy tn

hiu . C hai chun u ni : kiu S ( Hitec) , kiu J(Futaba) . Th t ca cc dy l nh nhau

nhng mu sc c th khc nhau . Dy l dy cp ngun , dy ni t l dy mu en ( hay

mu nu ) v dy tnh hiu mu vng ( kiu S) hoc mu trng ( kiu J) . Vi cc servo khc mu

sc c th thay i .

C cu chp hnh , thng l c gi l cn servo , c cc rnh then hoa trn n khp vi

cc rnh then hoa trn u trc . Cc rnh ny gip khng b trt khi c moment xon . C mt

c vt gn cn cht vo trc , Cn c th c rt nhiu dng : cnh tay , thanh , a

3.2.4 Phn loi ng c RC servo

a. Phn loi v ngun cp:

C servo 1 chiu, servo xoay chiu 1 pha, servo xoay chiu 3 pha.

Trong ti s dng servo 1 chiu cng sut nh dng lm cc chi m hnh. Trn th

gii, dng sn phm ny c 2 hng sn xut ln, cht lng cao l: Hitec (M) v Futaba (Nht).

b. Phn loi v vt liu lm hp gim tc: composit, kim loi v hp kim

c. Phn loi v phng php iu khin : servo analog v digital.

B ngoi th khng c g khc nhau v v c bn, cc phn bn trong cng khng phn bit

nhiu ngoi tr mt vi phn in t v ng c digital servo c mt b vi x l.


Trang 31

Servo digital c tn s hot ng ln n 300Hz. Mt s servo digital c th lp trnh c cc

tham s nh tc , chiu quay, quay v di cht. V servo digital c th nhn lnh u vo

nhanh hn servo analog, chng c th cp nht v tr ng c nhanh, p ng nhanh hn, moment

mnh hn v di cht hp hn. Nhc im l chng l tiu th nhiu dng ln.

3.2.5 Nguyn tc hot ng

quay ng c , tn hiu s c gi ti mch iu khin . Tn hiu ny khi ng ng c

in , thng qua chui bnh rng , ni vi trc ra . V tr yu cu l tn hiu vo, chit p trn trc

s a ra tn hiu in th phn hi v v tr trc ra ca servo . u im ca servo l bin tr

c ni trc tip vi trc ra nn ti mi v tr ca trc ra u c th kim sot c bng gi tr

trn bin tr

Mch iu khin trong servo so snh in th ny vi di cc xung s a vo v pht tn

hiu sai s nu in th khng ng . Tn hiu sai s ny t l vi lch gia v tr ca chit p

v di ca tn hiu vo . Mch iu khin s kt hp tn hiu sai s ny quay ng c . Khi

in th ca chit p v di cc xung s bng nhau , tn hiu sai s c loi b v ng c

ngng .

3.2.6 Cc thng s k thut ca RC

Moment xon : tng ngu lc m ng c sinh ra . n v chun ca moment xon trong RC

l ounce.inch

Thi gian chuyn tip transit time ( cn gi l t l quay ) : thi gian trc RC quay 1 gc

X ( 0.2 s/600 ) . Thi gian chuyn tip cng nhanh th RC hot ng cng nhanh . T thi gian

chuyn tip ta c th tnh c vn tc quay ca trc ng c .

Moment v thi gian chuyn tip c gn cho mt in p nht nh, thng l 4,8V n 6V

Di cht ca RC l thi gian sai lch ln nht gia tn hiu iu khin ng vo v tn hiu

tham chiu ni sinh ra bi v tr ca chit p . Nu khng c di cht , RC phi lin tc d tm

im tng thch chnh xc gia tn hiu vo v tn hiu tham chiu ni . Di cht cho php RC

gim thiu s d tm ny v s ly im ln cn im cn tm . Nu thi sai lch nh hn di

cht, RC khng cn chnh ng c sa sai lch .


Trang 32

Di cht thay i tuy theo RC , in hnh l 5us . Nu b x l c phn gii 2us v nu RC

c di cht 5us th s thay i 1 hay 2 gi tr - tc l 2 hay 4us trong b rng xung , s khng nh

hng ln RC . Ta chn RC c di cht hp nu cn chnh xc v mch iu khin hay mi

trng lp trnh c phn gi ln . Ngc li ra khng cn lu n di cht .

3.2.7 Tnh ton moment xon ng c

Mi khu robot c khi lng 350g, c gia trng trng tm nm chnh gia khu. H

s masat gia khu v mi trng theo phng php tuyn 0,56n

Moment xon cn thit cho ng c

1 10,35 0,56*25,0 2,45 .

2 2i i n iT m l x x kg cm

(3.1)

3.2.8 La chn ng c

ng c RC servo c chn s dng l loi Towerpro MG946R

Hnh 3.7 nh ng c thc t dng trong m hnh

Thng s ng c :

Khi lng : 55g

ng knh : 40.7*19.7*42.9mm

Moment xon : 10,5kg.cm ( 4.8V) =1.02 Nm, 13kg.cm(6V)


Trang 33

Vn tc : 0.2 s/60degree(4.8V) , 0.17 s/60degree(6V)

Vol : 4.8 7.2V

Hp s l bnh rng kim loi

Di cht 5us

Vi cc thng s trn , th ng c c th thng lc cn v gip cho robot c th chuyn ng

c.

3.2.9 Thit k h thng c kh :

3.2.9.1 Thit k m hnh 3D

ng C

Hnh 3.8 : M hnh 3D ca RC servo

ng c RC c v trn nhng kch thc tht ca chi tit

Gi ng C


Trang 34

Hnh 3.9 : M hnh 3D ca gi ng c

ng lm 1 t ca robot rn

Hnh 3.10 : M hnh 3D ca ng lm 1 t ca robot

1 t ca robot rn


Trang 35

Hnh 3.11 : M hnh 3D 1 t ca robot

ng ni cc t

Hnh 3.12 : M hnh 3D ng ni cc t


Trang 36

Hnh 3.13 : Robot rn 6 t

Hnh 3.16 : Robot rn 6 t

Chng IV : iu Khin V Thit K Mch iu Khin

Trang 37

4. CHNG IV - IU KHIN V THIT K MCH IU KHIN

4.1 IU KHIN

4.1.1 Phng n iu khin

Trong khun kh lun vn ny, chuyn ng ca robot rn l sng ngang (lateral undulation)

bm mc tiu. iu khin c thc hin cc module vi nhim v l iu khin ng c thc

hin dng chuyn ng .

4.1.1.1 Chuyn ng hnh sin

Cch n gin nht to ra di chuyn dng sng trong mt chui ni tip ca n khu l do

cc gc khc nhau ca tng khu c 1 tn s chung v 1 hng s lch pha gia cc khu vi

nhau .

Ta bit gc tng i gia cc khu theo dng chuyn ng dng sng ngang nh sau:

, sin( ( 1) ) ( 1 )i d t i i n

Vi l bin dao ng, l lch pha gia cc khu k tip, quyt nh hng ca

robot ( =0 th robot chy thng), v l tn s gc ca dao ng. Tham s v quyt nh tc

ca dng sng truyn dc theo robot v qua xc nh tc robot.

Ti u Pareto

Cc nghin cu theo l thuyt ti u Pareto v nng lng xc nh cc tnh cht:

l hm tuyn tnh ca vn tc di chuyn ti

l hm t l vi t l /t nc c ca cc h s ma st

l hm t l vi s khu n , ta thng chn 2n

Kt qu ny s c s dng trc tip iu khin robot ch khng chng minh li

ng c RC Servo c iu khin trong khong -90 900. Chn phng trnh chuyn ng

ca robot nh sau :

,

230sin( ( 1) ) ( 1 6)

6i d t i i


Trang 38

Vi trong khong [-4545 ] c iu khin nh hng theo thut ton iu khin Fuzzy.

4.1.1.2 Chuyn ng hnh sin vi s nh hng ca khu u

Khi rn chuyn ng ,rn s di chuyn chnh xc khi c nh hng bi khu u. Qu trnh

ny c thc hin thng qua vic nhn tn hiu t cc cm bin gn trn u m hnh .

Khi robot rn di chuyn, nh hng ca khu u cng thay i theo hnh sin v do tn

hiu cm bin cng thay i lin tc trong sut qu trnh di chuyn. iu ny lm cho vic iu

khin robot tr nn kh khn hn , v th ta c th cn thm nhiu cm bin gip rn hot

ng chnh xc hn .

Hnh 4.1 : P1 chuyn ng theo khu u , P2 l chuyn ng hnh sin

gim thiu ti a nhng thay i nh hng ca khu u trong qu trnh chuyn ng

trong khi cc khu lin kt khc tip tc i theo chuyn ng hnh rn. Ta c phng trnh quan

h ca cc gc tng i ca cc khu lin k nhau

sin( ( 1) ) ( 1 )i t i i n

sin( )h h h ht

h l gc tng i ca khu u . h v h l gc lnh pha ln nht ca khu u . Hai

tham s ny m bo rng u robot vn cn khong cch song song vi mc tiu .


Trang 39

T trn ta thy , nhim v ca ta l s dng lin kt u thit lp cc hng chuyn ng

thng qua tham s h , iu khin cc lin kt cn li to ra chuyn ng sng ngang v

thng qua chuyn ng ca c th robot rn y rn v pha trc .

4.1.2 iu khin ng c RC servo

4.1.2.1 Nguyn l iu khin ng c RC Servo

ng c RC servo c iu khin da vo xung PWM a vo dy tn hiu. ng c RC

Servo analog c tn s l 50Hz, mi chu k xung l 20ms. Khi rng xung khong 1.5ms th

ng c quay v v tr gia. Khi rng xung trong khong 1.5 2.0ms th ng c t cc v tr

ngc chiu kim ng h v khi rng xung trong khong 0.7 1.0ms th ng c t cc v tr

cng chiu kim ng h. ng vi mi rng xung, ng c s t cc v tr chnh xc khc

nhau. V tr ny c sai s ty thuc loi ng c.

Hnh 4.2 : Gin xung iu khin ng c RC servo

4.1.2.2 iu khin ng c RC servo trong robot rn

Vi iu khin c s dng trong m hnh l pic 18F4431 . Tn s PWM trong phn cng ca

vi iu khin l :

PWM period = [ (PR2+1)] x 4 x Tosc x ( TMR2 prescaler value )

Thanh ghi PR2 l thanh ghi 8 bit c gi tr ti a l 0xFF(255) , ta c :


Trang 40

Chu k PWM = ( 255+1) x 4 x (1/4000000) x 16 = 0.0041 (s)

Tn s PWM = 1/chu k PWM = 1/0.0041 = 244.1 (Hz)

Tn s 244.1 Hz l qu cao so vi tn s hot ng ca RC servo ( 50Hz) , ta iu khin ng

c servo bng PWM to bi phn mm . Ta s dng b nh th TIMER0 to ngt xut xung .

Thi gian timer m ln 1 n v c tnh bng cng thc

4

Focs

prescalerT

Hnh 4.3 : Lu thut ton to xung iu khin ng c


Trang 41

4.2 IU KHIN NH HNG

4.2.1 B iu khin Fuzzy

Chuyn ng ca robot rn theo phng trnh

sin( ( 1) ) ( 1 )i t i i n (4.1)

C tham s iu khin hng l . Khi =0, robot chy thng. Khi >0, robot r tri theo

ng trn v ngc li


Trang 42

Hnh 4.4 : M hnh xc nh hng ca robot

Hng ca robot c nh ngha bi cng th 1

1 n

i

in

(4.2)

Hng mong mun * l hng t trng tm robot n mc tiu

lch hng gia hng mong v hng ca robot l *

thc hin gii thut iu khin iu khin nh hng cho robot rn, ta coi mc tiu s

c pht mt ngun sng v pha robot. Cm bin nh hng c s dng l quang in tr.

Dng xc nh gc lch ca robot vi mc tiu. Quang in tr (LDR) l mt cm bin in

tr nhy sng lm bng vt liu bn dn Cadmium Sulfide (CdS). Bnh thng, tr khng ca

LDR l rt cao khong 1.000.000 , khi c chiu sng vi nh sng th tr khng gim rt

nhiu.


Trang 43

Hnh 4.5 : Mch quang in tr

Khi mc nh sng thp in tr ca LDR l cao. iu ny lm cc lm cc transistors

khng dn. Do LED khng sng.

Tuy nhin, khi c nh sng chiu ln b mt LDR n phn cc cho transistror u tin dn, v

do transistor th 2 dn, n LED s pht sng.

Mt bin tr t sn c th iu chnh tng hoc gim tr khng ca mch (LDR), trong

cch ny, n c th lm cho nhy ca mch thay i

Chn ngun sng l mt bng n Rng ng Compact 40W t cch robot 2 m. ri sng

ca bng n vo khong 500Lux. Khi robot hot ng trong iu kin bnh thng, vi cc b

tr cm bin nm ngang, ri sng vo hai cm bin do nh sng mi trng to ra vo

khong 50Lux. Nh vy, in tr ca LDR s bin thin trong khong 110 k ( 1 lux l ri

c c ca mt b mt c din tch 1 mt vung c thng lng chiu sang 1 lumen )

Hnh 4.6 : S bin thin ca quang in tr


Trang 44

Mt cp LDR s ng vai tr cm bin nh sng nh hng cho robot. t 2 LDRs nh v

tr chng 60 theo hng ca khu u xc nh gc tng i ca khu vi ngun sang thng

qua tnh hiu ca cng sng. Cp LDR s cung cp v tr ngun sng v mch iu khin v

to thnh mt vng hi tip. im quan trng y l ta s dng gi tr sai lch gia LDR tri v

LDR phi, v gim c nhiu do nh sng bn ngoi.

Hnh 4.7 : B tr cm bin nh hng

4.2.1.1 M Ha

Gi tr u vo ca tp m l gc lch ca robot rn vi mc tiu. Gi tr u ra l h s l h

s nh hng trong phng trnh chuyn ng ca robot rn. S dng b ADC 10 bit ca vi iu

khin, vi cm bin LDR 12mm c chn, in p chn Analog t c trong khong

15V. Tn hiu cm bin c a v s nm trong khong [100 1000] v tn hiu sai lch (LDR

tri LDR phi) nm trong khong [-900 900].

4.2.1.2 H lut m

S dng lut If Then thnh lp h lut ngn ng cho h


Trang 45

Nu 0 th 0 (mc tiu lch bn tri robot r tri)

Nu 0 th 0 (mc tiu khng lch robot i thng)

Nu 0 th 0 (mc tiu lch bn phi robot r phi)

Tn hiu vo bin thin trong khong [-900 900 ], ta cho tn hiu ra bin thin trong

khong [- 600 600 ] .

S dng phng php suy din MAX MIN to thnh quan h m v phng php

MAMDANI kt hp cc lut hp thnh.

Hnh 4.8 : S b iu khin Fuzzy

Hnh 4.9 : S Membership Function input


Trang 46

Hnh 4.10 : S Membership Function output

Trng s ca tng lut c tnh theo suy din Min Max

1 min( ( ) & ( ))w Ne Ne (4.3)

2 min( ( ) & ( ))w Ze Ze (4.4)

3 min( ( ) & ( ))w Po Po (4.5)

Kt qu l u ra c tnh theo cng thc

1 2 3( ) max( , , )w w w (4.6)

4.2.1.3 Gii m

S dng phng php trng tm (centroid) gii m, tch phn s c tnh gn ng bng

cng thc Simpson


Trang 47

*( ).

( )

B

B

y ydyy

y dy

(4.7)

Hnh 4.11: Phng php gii m theo ta trng tm

Hnh 4.12 : S h lut ca b Fuzzy


Trang 48

B ADC ca vi x l s xc nh sai lch gia 2 cm bin LDR tri v LDR phi.Gc

c xc nh theo thut ton Fuzzy v tnh gc quay ca mi module theo phng trnh chuyn

ng. T chuyn thnh xung kch cho cc ng c servo quay theo mong mun

4.3 THIT K MCH IN CHO ROBOT RN

4.3.1 Mch Ngun cho vi iu khin trn robot

S dng loi pin sc 860mA ngun 7.4V

Hnh 4.13 : Pin si cho m hnh robot

Tt c cc module ca robot iu s dng 1 ngun chung t pin . Ring vi iu khin dng IC

LM7805 chuyn thnh in p 5V cho mch t pin .


Trang 49

Hnh 4.14: Mch ngun cho vi iu khin trn robot

4.3.2 Mch cm bin xc nh hng

Cc cm bin s c mc ni tip vi mt bin tr 5K lm cu chia p cho chn Analog

AN0 v AN1 ca vi iu khin. S bin thin cng sng lm bin thin in p vo chn

Analog. Gi tr in tr ca LDR bin thin t 110K nn ta t gi tr bin tr khong 5K.

in p vo chn Analog s bin thin t 15V


Trang 50

Hnh 4.15 : S mch nh hng LDR

4.3.3 Mch vi iu khin

S dng vi iu khin PIC18F4431 iu khin robot rn.Vi cc chn RB0-RB4 c ni

vi chn tnh hiu ca ng c RC servo. V hai chn Analog AN0 v AN1 c ni vi cm

bin quang (LDR).

Hnh 4.16 : Mch vi iu khin trn robot


Trang 51

4.3.4 Moduel cc ng c v Led bo tn hiu ca ng c

Hnh 4.17: Module ni ti cc ng c

Hnh 4.18 : Cc n Led bo tn hiu iu khin ng c


Trang 52

4.4 LU IU KHIN ROBOT RN

Vi iu khin s c tn hiu Analog t hai knh AN0 v AN1 t y tnh c gc lch gia

robot v mc tiu.

Dng Fuzzy tnh cc gc v tnh cc gc cho tng ng c.Sau chuyn cc gi tr gc

thnh cc gi tr xung v xut ra iu khin cc ng c iu khin robot chuyn ng ti mc

tiu


Trang 53

Hnh 4.19 : Lu gii thut iu khin robot rn

Chng V : M Phng V Thc Nghim

Trang 54

5. CHNG V : M PHNG V THC NGHIM

5.1 CC THNG S M PHNG

S khu n=6 Khi lng ca khu m=0,4 (kg)

Chiu di khu l=0,24m H s Kp ca servo Kp= 100

H s =/6 (rad) H s Ki ca servo Ki = 10

Gc lch pha =/3(rad) H s Kd ca servo Kd = 10

Vn tc gc =1(rad/s) Moment qun tnh I=ml2/3 (kg.m2)

5.2 M PHNG VI SIM MECHANICS

5.2.1 Xy dng m hnh m phng

Mi khu c ni vi nhau bi mt khp quay c gn ng c servo, robot c ba bc t do

chuyn ng so vi t gm hai bc tnh tin theo x, y v mt bc quay theo z.

Hnh 5.1 : M hnh m phng robot


Trang 55

Mi khu s chu lc tc ng ca lc ma st (khi Friction) . M hnh lc ma st nht nh

sau

Hnh 5.2 : M hnh lc ma st

Hnh 5.3 : M hnh ng c Servo

5.2.2 Xy dng m hnh mc tiu

Ta xy dng 2 mc tiu c ta khc nhau lm mc tiu cho robot rn di chuyn.u tin

robot s di chuyn n mc tiu th nht v sau s tip n mc tiu th hai


Trang 56

Hnh 5.4 : M hnh 2 mc tiu

5.2.3 B iu khin ca robot

B iu khin trung tm s quyt nh hnh dng ca robot ty theo mc ch iu khin (iu

khin hng bm mc tiu, hoc iu khin tc ).Ty vo mc ch iu khin b iu

khin s cho ra cc gi tr ng c servo thc thi. ng c servo s nhn tn hiu gc tham

chiu t b iu khin cp moment vo cc khp quay. Do hnh dng robot s chy theo

mong mun ca b iu khin.

Ta xy dng b iu khin Fuzzy robot di chuyn bm theo mc tiu

Tn hiu a vo b iu khin s l sai lch gia hng di chuyn ca robot v hng

mong mun n mc tiu .

Tn hiu vo bin thin trong khong [-/2 /2 ], ta cho tn hiu bin thin trong khong

[- 1 1 ] .

Hnh 5.5 : B iu khin Fuzzy bm mc tiu


Trang 57

Hnh 5.6 : Membership Function input

Hnh 5.7 : Membership Function output


Trang 58

Hnh 5.8 : B iu khin Fuzzy trung tm v cc tn hiu iu khin

5.2.4 Kt qu m phng

Cho robot di chuyn n mc tiu th nht c ta [17 17] v mc tiu th 2 c ta [20 25]

Kt qu robot i qua c 2 mc tiu

Hnh 5.9 : th ta trong tm ca robot


Trang 59

Chun b qua mc tiu th nht

Qua mc tiu th nht

Chun b qua mc tiu th 2

Qua mc tiu th 2

Hnh 5.10 : Qu trnh di chuyn qua 2 mc tiu ca robot

Hnh 5.11 : M hnh 3D di chuyn n mc tiu


Trang 60

Vic iu khin nh hng bng Fuzzy Logic thc hin c. Tuy nhin,ta trng tm

khi kt thc m phng vn cn lch so vi v tr ca mc tiu. Cn ci thin robot di chuyn

chnh xc hn

5.3 THC NGHIM

5.3.1 Xy dng m hnh

Mch iu khin robot

Hnh 5.12 : Mch iu khin robot

Mt t ca robot



Trang 61


Ton b robot

Hnh 5.15 : M hnh tht t ca robot rn


Trang 62




Trang 63

5.3.2 Kt Qu

M hnh thc nghim kim tra c thut ton iu khin ng c v iu khin robot theo

dng hnh sin

Chng VI : Kt Lun

Trang 64

6. CHNG VI : KT LUN

6.1 KT QU T C

- Tnh ton ng hc v ng lc hc cho robot rn dng tng qut

- Vit chng trnh tnh ton ng lc hc cho robot rn

- M phng chuyn ng ca robot

- iu khin robot bm mc tiu vi b iu khin Fuzzy

- Thit k robot rn chuyn ng

- Thit k robot rn chuyn ng bm mc tiu

- Xy dng m hnh robot rn

- Thit k mch iu khin cho robot rn

6.2 KT QU CHA T C

- Phn cng chnh xc vi thit k

- Gii thut Fuzzy di vi iu khin cha chnh xc nh m phng

- Cm bin nh hng gc cn hn ch [-60 60]

6.3 HNG PHT TRIN

- M phng v thit k robot rn c th di chuyn di nc

- M phng v thit k robot rn c th chuyn ng trong khng gian 3D

- S dng cc cm bin khc nh GPS, Camera nh v, gim xc robot v xc nh mc

tiu .

- Tng tc p ng ca b iu khin bng cc vi x l mnh hn

Ti Liu Tham Kho

Trang 65

TI LIU THAM KHO

[1] S.A. Fjerdingen et al, Adaptive Snake Robot Locomotion: A Benchmarking Facility for

Experiments, H. Bruyninckx et al. (Eds.): European Robotics Symposium 2008, STAR 44, pp.

1322, 2008.

[2] The MathWorks, Inc, Fuzzy Logic Toolbox Users Guide

[3] William R. Hutchison, Betsy J. Constantine, Johann Borenstein, and Jerry Pratt

Development of Control for a Serpentine Robot

[4] Toshio TAKAYAMA, Shigeo HIROSE Amphibious 3D Active Cord Mechanism HELIX

with Helical Swimming Motion

[5] ENDO, Keiji TOGAWA and Shigeo HIROSE Study on Gen Self-contained and Terrain

Active Cord Mechanism Adaptive

[6] Brad Oraw & Jeremy Tinder Proposal for a Modular Snake Robot

[7] Kevin J. Dowling; Advised by William L. Whittaker Limbless Locomotion: Learning to

Crawl with a Snake Robot

[8] P. Liljebck, . Stavdahl and K. Y. Pettersen, Modular Pneumatic Snake Robot: 3D

Modelling, Implementation and Control, Proceedings of 16th IFAC World Congress,

Prague,Czech Republic (Jul. 2005). M. Saito, M. Fukaya and T. Iwasaki, Serpentine locomotion

with robotic snakes, IEEE Contr. Syst. Mag. 22(1), 6481 (Feb. 2002).

[9] SHIGEO HIROSE AND HIROYA YAMADA Snake-Like Robots - Machine Design of

Biologically Inspired Robots

[10] Bi ging My v h thng thng minh PGS.TS T Dip Cng Thnh - HBK TP

HCM

Ph Lc A : Chng trnh tnh ng lc hc

Trang A1

PH LC A

CHNG TRNH MATLAB TNH NG LC HC CHO ROBOT

clc;

clear all;

echo off;

format long;

global invM;

global N1;

global N2;

global N3;

global N4;

global N5;

global N6;

global Q1;

global Q2;

global Q3;

global Q4;

global Q5;

global Q6;

global f1;

global f2;

global f3;

global f4;

global f5;

global f6;

global FF;

global Ff;

global FF_AV

delta_t = 0.01;

i_end_2 = 12000; % total time

%Thong so tinh luc ma sat

g=9.98;

mit= 0.1; % he so muy(u) theo phuong tiep tuyen t

min= 0.5;

% mit= 0.05; % he so muy(u) theo phuong tiep tuyen t

% min= 0.56;

% Khai bao cac thong so lien quan den hinh dang hinh hoc cua ca

i1 = 7.68*10^3;

m1 = 0.4;

l1 = 0.21;

a1 = 0.105;


Trang A2

k1 = 0.5;

c1 = 10^-3;

i2 = 7.68*10^3;

m2 = 0.4;

l2 = 0.24;

a2 = 0.12;

k2 = 0.5;

c2 = 10^-3;

i3 = 7.68*10^3;

m3 = 0.4;

l3 = 0.24;

a3 = 0.12;

k3 = 0.5;

c3 = 10^-3;

i4 = 7.68*10^3;

m4 = 0.4;

l4 = 0.24;

a4 = 0.12;

k4 = 0.5;

c4 = 10^-3;

i5 = 7.68*10^3;

m5 = 0.2;

l5 = 0.24;

a5 = 0.12;

k5 = 0.5;

c5 = 10^-3;

i6 = 7.68*10^3;

m6 = 0.24;

l6 = 0.24;

a6 = 0.12;

sampling = 0.01;%thoi gian lay mau

DtoR = pi/180;% he so chuyen sang radian

% Input functions data

%thong so tin hieu dieu khien dong co

A1 = pi/2;

A2 = A1;

A3 = A1;

A4 = A1;

A5 = A1;

A6 = A1;


Trang A3

f1 = 1.5;

f2 = f1;

f3 = f1;

f4 = f1;

f5 = f1;

f6 = f1;

beta=60;

beta1=60;

beta2=60;

beta3=60 ;

beta4=60;

beta5=60;

beta6=60;

% he so de tinh trong SVD

SVD_Max = 0.0355;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% khai bao cac luc Ma Sat ban dau,gia su van toc ban dau duong nen

% Theo Phuong x

Fx1 = 0;

Fx2 = 0;

Fx3 = 0;

Fx4 = 0;

Fx5 = 0;

Fx6 = 0;

% Theo Phuong y

Fy1 = 0;

Fy2 = 0;

Fy3 = 0;

Fy4 = 0;

Fy5 = 0;

Fy6 = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

NN = [];

FF = [];

P = [];

DD = [];

tor1 = [];

FF =0; % Khai bao vector cua luc day FF de cong don gia tri tai tung thoi diem

P = 0;

tor_01 = 0;

invm = zeros(6,6);

x0 = [0 0 0 0 0 0 0 0 0 0 0 0]; % Gia tri ban dau cho cac goc: x0(1): theta1, x0(2): theta1_dot,

x0(3): theta2, x0(4): theta2_dot, x0(5): theta3, x0(6): theta3_dot,x0(7):theta4,x0(8):theta4_dot,

x0(9):theta5, x0(10):theta5_dot, x0(11):theta6, x0(12):theta6_dot

% Calculate the begining time of theta2 base on beta


Trang A4

theta2_begin = (beta*DtoR)/(2*pi*f2);

theta2_begin_step = theta2_begin/delta_t;

if theta2_begin_step = 0.5

theta2_begin_real = 1;

else

theta2_begin_real = 0;

end

theta2_begin_time = (theta2_begin_integer + theta2_begin_real);

end

for i=1:i_end_2

i

s_t = (i-1)*delta_t;

e_t = i*delta_t;

tspan = [s_t e_t];%s_t start time va e_t la end time

x_tmp = size(x0,1);

x = x0(x_tmp,:);

% Khai bai cac tin hieu (moment xoan)dieu khien dau vao cho dong co RC -

x(1) = A1*(sin(2*pi*f1*s_t*DtoR));

x(3) = A2*(sin((2*pi*f2*s_t*DtoR - beta2*DtoR)));

x(5) = A3*(sin((2*pi*f3*s_t*DtoR - 2*beta3*DtoR)));




T1 = 0.1;

T2 = T1;

T3 = T1;

T4 = T1;

T5 = T1;

T6 = T1;

% KHai bao bo dong hoc cua robot ca

m11 = i1+m1*a1^2;

m12 = 0;

m13 = 0;

m14 = 0;

m15 = 0;

m16 = 0;

m21 = m2*a2*l1*cos(x(3)-x(1));

m22 = i2+m2*a2^2;

m23 = 0;


Trang A5

m24 = 0;

m25 = 0;

m26 = 0;

m31 = m3*a3*l1*cos(x(5)-x(1));

m32 = m3*a3*l2*cos(x(5)-x(3));

m33 = i3+m3*a3^2;

m34 = 0;

m35 = 0;

m36 = 0;

m41 = m4*a4*l1*cos(x(7)-x(1));

m42 = m4*a4*l2*cos(x(7)-x(3));

m43 = m4*a4*l3*cos(x(7)-x(5));

m44 = i4+m4*a4^2;

m45 = 0;

m46 = 0;

m51 = m5*a5*l1*cos(x(9)-x(1));

m52 = m5*a5*l2*cos(x(9)-x(3));

m53 = m5*a5*l3*cos(x(9)-x(5));

m54 = m5*a5*l4*cos(x(9)-x(7));

m55 = i5+m5*a5^2;

m56 = 0;

m61 = m6*a6*l1*cos(x(11)-x(1));

m62 = m6*a6*l2*cos(x(11)-x(3));

m63 = m6*a6*l3*cos(x(11)-x(5));

m64 = m6*a6*l4*cos(x(11)-x(7));

m65 = m6*a6*l5*cos(x(11)-x(9));

m66 = i6+m6*a6^2;

% Update the value of tt value

tt1=l1*[c1*x(2)-c1*x(4)+k1*(x(1)-x(3))];

tt2=l2*[c1*x(2)+(c1+c2)*x(4)-c2*x(6)+(k1+k2)*x(3)-k1*x(1)-k2*x(5)];




tt6=l6*[c5*x(10)+c5*x(12)+k5*x(12)-k5*x(10)];

Q1=a1*[Fy2*cos(x(1))-Fx2*sin(x(1))]+l1*[cos(x(1))*(Fy2+Fy3+Fy4+Fy5+Fy6)-

sin(x(1))*(Fx2+Fx3+Fx4+Fx5+Fx6)]-T1+tt1;

Q2=a2*[Fy3*cos(x(3))-Fx3*sin(x(3))]+l2*[cos(x(3))*(Fy3+Fy4+Fy5+Fy6)-

sin(x(3))*(Fx3+Fx4+Fx5+Fx6)]+T1-T2+tt2;

Q3=a3*[Fy4*cos(x(5))-Fx4*sin(x(5))]+l3*[cos(x(5))*(Fy4+Fy5+Fy6)-

sin(x(5))*(Fx4+Fx5+Fx6)]+T2-T3+tt3;

Q4=a4*[Fy5*cos(x(7))-Fx5*sin(x(7))]+l4*[cos(x(7))*(Fy5+Fy6)-sin(x(7))*(Fx5+Fx6)]+T3-

T4+tt4;

Q5=a5*[Fy6*cos(x(9))-Fx6*sin(x(9))]+l5*[cos(x(9))*(Fy6)-sin(x(9))*(Fx6)]+T4-T5+tt5;

Q6=T5-T6+tt6;


Trang A6

N1=Q1;

N2=Q2-[m2*a2*l1*x(2)^2*sin(x(3)-x(1))];

N3=Q3-[m3*a3*l1*x(2)^2*sin(x(5)-x(1))+m3*a3*l2*x(4)^2*sin(x(5)-x(3))];

N4=Q4-[m4*a4*l1*x(2)^2*sin(x(7)-x(1))+m4*a4*l2*x(4)^2*sin(x(7)-

x(3))+m4*a4*l3*x(6)^2*sin(x(7)-x(5))];


x(3))+m5*a5*l3*x(6)^2*sin(x(9)-x(5))+m5*a5*l4*x(8)^2*sin(x(9)-x(7))];


x(3))+m6*a6*l3*x(6)^2*sin(x(11)-x(5))+m6*a6*l4*x(8)^2*sin(x(11)-

x(7))+m6*a6*l5*x(10)^2*sin(x(11)-x(9))];

% Khai bao ma tran M tong quat

M = [m11 m12 m13 m14 m15 m16 ;m21 m22 m23 m24 m25 m26 ;m31 m32 m33 m34 m35

m36 ;m41 m42 m43 m44 m45 m46 ;m51 m52 m53 m54 m55 m56 ;m61 m62 m63 m64 m65

m66];

% Su dung giai thuat SVD de tinh ma tran nghich dao (dung de tranh truong hop M suy bien)

% Tinh ma tran nghich dao

% tinh svd

[UU, D, V] = svd(M);

DD = [DD; D];

[m n] = find(D < SVD_Max & D > 0); %0.025

size_m = size(m);

if(size_m(1) == 1)

invm(m(1),m(1)) = 0;

for i1=1:6

if i1~=m(1)

invm(i1,i1) = 1/D(i1,i1);

end

end

invM = V*invm*UU';

elseif(size_m(1) == 2)

invm(m(1),m(1)) = 0;

invm(m(2),m(2)) = 0;

for i1=1:6

if (i1~=m(1))& (i1~=m(2))

invm(i1,i1) = 1/D(i1,i1);

end

end

invM = V*invm*UU';


invm(m(1),m(1)) = 0;

invm(m(2),m(2)) = 0;

invm(m(3),m(3)) = 0;

for i1=1:6


Trang A7

if [(i1~=m(1))&(i1~=m(2))]&(i1~=m(3))

invm(i1,i1) = 1/D(i1,i1);

end

end

invM = V*invm*UU';


invm(m(1),m(1)) = 0;

invm(m(2),m(2)) = 0;

invm(m(3),m(3)) = 0;

invm(m(4),m(4)) = 0;

for i1=1:6

if [[(i1~=m(1))&(i1~=m(2))]&(i1~=m(3))]&(i1~=m(4))

invm(i1,i1) = 1/D(i1,i1);

end

end

invM = V*invm*UU';


invm(m(1),m(1)) = 0;

invm(m(2),m(2)) = 0;

invm(m(3),m(3)) = 0;

invm(m(4),m(4)) = 0;

invm(m(5),m(5)) = 0;

for i1=1:6

if [[[(i1~=m(1))&(i1~=m(2))]&(i1~=m(3))]&(i1~=m(4))]&(i1~=m(5))

invm(i1,i1) = 1/D(i1,i1);

end

end

invM = V*invm*UU';


invm(m(1),m(1)) = 0;

invm(m(2),m(2)) = 0;

invm(m(3),m(3)) = 0;

invm(m(4),m(4)) = 0;

invm(m(5),m(5)) = 0;

invm(m(5),m(5)) = 0;

invM = V*invm*UU';

else

invM = V*inv(D)*UU';

end

% Su dung phuong trinh Ode45 de giai ra theta1, theta2, theta3, theta4, theta5,

theta6,theta1_dot, theta2_dot, theta3_dot, theta4_dot, theta5_dot, theta6_dot

% Ham 'fishrobotfunc_Input_Torque' khai bao giong nhu trong bai vi du mau minh copy cho

em hom truoc

[t,y]= ode45('snakerobot',tspan,x); % ham fishrobot trong file snakerobot.m x la gia tri ban dau

y0


Trang A8

y_tmp = size(y,1);

x0(x_tmp+1,:) = y(y_tmp,:);

%Tinh luc ma sat

Ft1=m1*g*mit*sign(x(2));






Fn1=m1*g*min*sign(x(2));






%luc theo phuong x y

Fx1= sin(x(1))*Ft1-cos(x(1))*Fn1;






Fy1= cos(x(1))*Ft1+sin(x(1))*Fn1;






% Tinh tong cac luc

Fm1=[c1*x(2)-c1*x(4)+k1*(x(1)-x(3))];

Fm2=[c1*x(2)+(c1+c2)*x(4)-c2*x(6)+(k1+k2)*x(3)-k1*x(1)-k2*x(5)];




Fm6=[c5*x(10)+c5*x(12)+k5*x(12)-k5*x(10)];

FT1=[Fy2*cos(x(1))-Fx2*sin(x(1))]+[cos(x(1))*(Fy2+Fy3+Fy4+Fy5+Fy6)-

sin(x(1))*(Fx2+Fx3+Fx4+Fx5+Fx6)]+Fm1;

FT2=[Fy3*cos(x(3))-Fx3*sin(x(3))]+[cos(x(3))*(Fy3+Fy4+Fy5+Fy6)-

sin(x(3))*(Fx3+Fx4+Fx5+Fx6)]+Fm2;

FT3=[Fy4*cos(x(5))-Fx4*sin(x(5))]+[cos(x(5))*(Fy4+Fy5+Fy6)-

sin(x(5))*(Fx4+Fx5+Fx6)]+Fm3;

FT4=[Fy5*cos(x(7))-Fx5*sin(x(7))]+[cos(x(7))*(Fy5+Fy6)-sin(x(7))*(Fx5+Fx6)]+Fm4;

FT5=[Fy6*cos(x(9))-Fx6*sin(x(9))]+[cos(x(9))*(Fy6)-sin(x(9))*(Fx6)]+Fm5;


Trang A9

FT6=Fm6;

Ff=FT1+FT2+FT3+FT4+FT5+FT6;

FF= [FF Ff];

end

theta1 = x0(:,1);

ans1 = (180*x0(:,1)/pi); % theta1

ans2 = (18000*x0(:,2)/pi); % theta1_dot

theta2 = x0(:,3);

ans3 = 180*x0(:,3)/pi; % theta2

ans4 = (18000*x0(:,4)/pi); % theta2_dot

theta3 = x0(:,5);

ans5 = 180*x0(:,5)/pi; % theta3

ans6 = (18000*x0(:,6)/pi); % theta3_dot

theta4 = x0(:,7);

ans7 = (180*x0(:,7)/pi); % theta4

ans8 = (18000*x0(:,8)/pi); % theta4_dot

theta5 = x0(:,9);

ans9 = 180*x0(:,9)/pi; % theta5

ans10 = (18000*x0(:,10)/pi); % theta5_dot

theta6 = x0(:,11);

ans11 = 180*x0(:,11)/pi; % theta6

ans12 = (18000*x0(:,12)/pi); % theta6_dot

tt = 0:0.01:i_end_2/100;

time_theta2_zero = 0:0.01:(theta2_begin_time/100)-0.01;

time_theta2 = (theta2_begin_time/100):0.01:(i_end_2/100 + theta2_begin_time/100);

for i = 1:theta2_begin_time

theta2(i,:) = 0;

end

for i = theta2_begin_time+1:i_end_2

theta2(i,:) = ans3(i,:);

end

xG_initial = [0 0];

for i = 1:i_end_2

i

s_t = (i-1)*delta_t;

e_t = i*delta_t;

tspan_AV = [s_t e_t];


Trang A10

xG_tmp = size(xG_initial,1);

xG = xG_initial(xG_tmp,:);

FF_AV = FF(:,i);

[t,yG] = ode45('Velocity_Function',tspan_AV,xG);

yG_tmp = size(yG,1);

xG_initial(xG_tmp+1,:) = yG(yG_tmp,:);

end

Moving_Distance = (xG_initial(:,1));

Real_Velocity = (xG_initial(:,2));

figure(1);

subplot(321);

plot(tt,ans1,'b','LineWidth',2);

title('Link 1');

ylabel('Displacement(Degree)');

xlabel('Time (s)');

grid on;

subplot(322);

plot(tt,ans2,'r','LineWidth',2);

title('Link 1');

ylabel('Angular velocity(Degree/s)');

xlabel('Time (s)');

xlim([0 i_end_2/100]);

grid on;

subplot(323);


title('Link 2');


xlabel('Time (s)');

grid on;

subplot(324);


title('Link 2');


xlabel('Time (s)');

grid on;

subplot(325);


title('Link 3');

ylabel('Displacement (Degree)');

xlabel('Time (s)');

grid on;


Trang A11

subplot(326);


title('Link 3');


xlabel('Time (s)');

grid on;

figure(2);

subplot(321);


title('Link 4');

ylabel('Displacement (Degree)');

xlabel('Time (s)');

grid on;

subplot(322);


title('Link 4');


xlabel('Time (s)');

grid on;

subplot(323);


title('Link 5');


xlabel('Time (s)');

grid on;

subplot(324);


title('Link 5');


xlabel('Time (s)');

grid on;

subplot(325);


title('Link 6');


xlabel('Time (s)');

grid on;

subplot(326);



Trang A12

title('Link 6');


xlabel('Time (s)');

grid on;

figure(3);

time = 0:0.01:i_end_2/100;

subplot(211);

plot(time,Moving_Distance,'-k','LineWidth',2);

title('The relation between moving distance and time');

xlabel('Time (s)');

ylabel('Moving distance (mm)');

grid on;

time1 = 0:0.01:i_end_2/100;

subplot(212);

plot(time1,Real_Velocity,'-k','LineWidth',2);

title('The relationship between real velocity and time');

xlabel('Time (s)');

ylabel('Velocity of snake robot (mm/s)');

grid on;

hm Snakerobot

function xp = snakerobot(t,x)

global invM;

global N1;

global N2;

global N3;

global N4;

global N5;

global N6;

% Ha bac cua phuong trinh: M x Theta_2dot = N

% --> Theta_2dot = inv(M)*N

xp = zeros(12,1);

xp(1)=x(2);

xp(2) = invM(1,1)*N1 + invM(1,2)*N2 + invM(1,3)*N3 + invM(1,4)*N4 + invM(1,5)*N5 +

invM(1,6)*N6;

xp(3)=x(4);


invM(2,6)*N6;

xp(5)=x(6);


invM(3,6)*N6;

xp(7)=x(8);


invM(4,6)*N6;


Trang A13

xp(9)=x(10);


invM(5,6)*N6;

xp(11)=x(12);


invM(6,6)*N6;

Hm vn tc function xG_dot =Velocity_Function(t,xG)

global FF_AV;

m = 2.4;

xG_dot = zeros(2,1);

xG_dot(1) = xG(2);

xG_dot(2) = (1/m)*FF_AV;

Ph Lc B : Chng trnh cho vi iu khin

Trang B1

PH LC B :

CHNG TRNH CHO VI IU KHIN

#include

#include

#fuses HS,NOPROTECT,NOLVP,NOWDT

#use delay(clock=20000000)

int gamma;

unsigned int16 value[5];

unsigned int16 counter0=0;





// Ham tim gia tri minimum

float min(float x,float y){

if (xy) return(x);

else return(y);

}


Trang B2

// Ham khoi tao ADC

void ADC_Init(){

setup_adc_ports(AN0_TO_AN1);

setup_adc(ADC_CLOCK_INTERNAL);

}

// Ham khoi tao Timer0

void Timer0_Init(){

setup_timer_0(RTCC_INTERNAL|RTCC_DIV_2);

set_timer0(231);

enable_interrupts(int_timer0);

enable_interrupts(global);

}

// Ngat Timer0 xuat xung PWM

#int_timer0

void isr_timer0(void) {

set_timer0(231);

if (counter0 == 0) {

output_high(PIN_B0);

}

else if (counter0 == value[1]) {

output_low(PIN_B0);

}

else if (counter0 == 2000) {

counter0=0;


Trang B3

}

Counter0++;



}


output_low(PIN_B1);

}


counter1=0;

}

Counter1++;



}


output_low(PIN_B2);

}


counter2=0;

}

Counter2++;




Trang B4

}


output_low(PIN_B3);

}


counter3=0;

}

Counter3++;



}


output_low(PIN_B4);

}


counter4=0;

}

Counter4++;

}

// Ham de tinh Fuzzy

void Fuzzy(int16 del_xi){

float sum_moment=0;

float sum_mass=0;

int step= 1;


Trang B5

int z[90];

float fz[90];

float Ne,Ze,Po;

for(i=0;i


Trang B6

else if (del_xi


Trang B7

gamma = sum_moment/sum_mass;

}

void PWM_value() {

for(i=1;i


Trang B8

Fuzzy(ldr);

}

delay(1ms) ;

}

}

robot rắn vn

Documents