Tgp Chi Khoa Hgc Gido Dgc Ky Thugt (33/2015) Tru&ng Dgi Hgc Su Phgm Ky Thugt TR Hd ChiMinh
MQT PHAN TICH DQNG LlTC HQC MOI DE DIEU KHIEN ROBOT CA DANG CARANGIFORM BAM THEO QUY DAO CHO TRlTdC THE NEW DYNAMIC ANALYSIS APPROACH AND TRAJECTORY FOL-
LOWING CONTROL OF A CARANGIFORM FISH ROBOT
Hoang Khac Anh, Pham Van Anh, Nguyin Tan Tien, VS Tudng Quan Tru&ng Dgi hgc Bdch Khoa TP.HCM Ng^ytda so^n nh^n bai 20/4/2015, ngay phan bi$n d^nh gi^ 22/4/2015, ngay chip nh$n dang 27/4/2015 TOM TAT
Bdi bdo giai thieu mgt phdn tich dgng luc hgc mai de diiu khien robot cd 3 kh&p dgng Ca- rangiform bdm theo 16 trinh xdc dinh vai di xudt su- dung kit hgp phuang trinh Newton-Euler vd Euler-Lagrange. Robot cd dugc thiit ki bao gom 2 phdn: phdn ddu vdphdn duoi. Viec dieu khiin phan duoi cua robot cd - dugc phdn tich bdng ly thuyet dgng l\rc hoc tay mdy robot - se giup rdbdt cd di chuyin theo lo trinh xdc dinh, chdng hgn nhu co the dieu khien robot cd di chuyin trong mat phdng ndm ngang. Trong bdi bdo ndy, cdc nghien cuu tap trung vdo viic dieu khiin robot cd bai theo quy dgo cho truac trong mat phdng ngang vd di chuyen den do sdu mong muon trong mat phdng Oyz chua dugc xem xet den. Mgt bg dieu khien Fuzzy vd Adaptive BackStepping da dugc phdt trien de dieu khiin phdn duoi giup robot cd bdm theo quy dgo cho truac.
Tie khoa: robot cd, dgng luc hgc tay mdy, bg dieu khiin backstepping, bg diiu khien Fuzzy ABSTRACT
This paper proposes a new dynamic analysis to control a 3-joint Carangiform fish robot swim to along a desired trajectory by using Newton-Euler and Euler-Lagrange concepts. The fish robot is designed into two parts: the head and the tail of the fish robot. The tail of the fish
robot will be analyzed quite similarly to the analysis of the dynamic model of the manipulator.
By controlling the tail with respect to the desired profiles, one can control the fish robot on the horizontal plane. To simplify, it is assumed that the motions of the fish robot include two main actions: the movement offish robot on the horizontal plane and movement offish robot to the desired depth. The movement of the fish robot to the desired depth is ignored. In this paper, the authors only focus on the control on the fish robot to swim following the desired trajectory on the horizontal plane. Fuzzy and Adaptive Back Stepping controllers are developed to control the tail for this purpose.
Key words: fish robot, dynamic model, backstepping controller, fuzzy controller.
1. GIOI THI|;U
Ngay nay, hudng nghien cftu v6 robot cd ca [1]. Mat khdc v i phdn logi robot ed, ta co dang phat trien rdt manh va da dat dugc nhiiu thi chia chftng lam hai dang chinh dya vao thanh tuu ddng ke. J. Edward Colgate vd Kev- hinh dang boi cfta chiing: BCF (Body and/or in M. Lynch da cdng bd mdt bdi bdo rdt cd y Caudal Fin) vd MPF (Median and/or Paired nghia trinh bay tdng quan lgi tdt ca v i thiit k i Fin) [2]. BCF dya tten nhftng thay ddi hinh CO khi cung nhu viec dieu khiin boi cho robot dang cua than, dudi va vdy dudi de tgo lyc
Tgp Chi Khoa Hgc Gido Due Ky Thugt (33/2015) Tru&ng Dgi Hgc SuPhgm Ky Thugt TR Hd ChiMinh ddy. Dang MPF thi dya vdo sy dao ddng cfta
vay hdng hoac vay k i t hgp. Trong thyc t i ta cd t h i n h | n dinh rdng, ca dang BCF phd biin hon dgng MPF va cd bdn dang boi chft yiu:
Anguilliform, Subcarangiform, Carangiform va Thunniform. Trong dd, dang Carangiform la dgng di chuyin vdi bien dg dao ddng cua than va dudi la tuong ddi nhd. Vi vdy, ttong cdc dgng cua BCF, hdu h i t robot ca deu phat trien dya theo dang Carangiform bdi tinh linh hoat va d l dieu khien cfta nd. Thdng thudng, robot cd dang Carangiform dugc thiet k i vdi dgng 3 khap hogc 4 khdp. Motomu Nakashi- ma vd cac cdng sy da md td phuong trinh sd hpc va kinh nghiem thiet ke robot ca 2 khdp
va sft dung "sefl-excitation" [3] va nhdn thdy rdng robot cd.thS bed vdi van tdc Idn nhdt la 0.42m/s. Trong mpt nghien cftu khac, QinYan cung cac cdng sy da phat trien robot cd vdi 4 khdp dudi va xac dinh dugc cac thdng sd anh hudng ddn robot ca nhu tdn sd, bien dO, chieu ddi sdng, dg lech pha va cdc he sd din van tdc di chuyen cfta robot ca [4]. Viec thiit kd vd dieu khien robot boi nhu cd thdt bdng cdch sft dyng cac can true Hen tuc thay vi rdi rgc la nhiing ddng gdp cd y nghia [5]. Feitian Zhang va cac cgng sy da phdt trien robot ca bang viec ket hgp the mgnh cfta tau lugn dudi nude va robot cd tao ra mgt robot cd kha nang di chuyen linh ddng [6].
Undultioiy
OtcfllBtoiy On motions
^ L s ^ \^=^\ L^^l
pectoral, dorsal, and anal I Ottraet^brm I
Hinh 1: Hai dgng di chuyin cua cd: (a) Dgng BCF; (b) Dgng MPF [1]
Md hinh hda vd md phdng la hai viec quan tipng ttong diiu khiin robot, chftng se giup ta cd dugc nhung kinh nghiem ttong viec thay ddi cdc thdng sd he thdng \ruac khi tiiiit k i robot cd tiiat. Trong bai bao nay, cdc tdc gia se trinh bay phuong phap md hinh ddng lyc hoc mdi cho robot vdi y tudng thiit k i la su k i t hop dpng lyc hgc cua AUVs (Autonomous Underwater Vehicle - thiit bi ty ddng di chuyin dudi nude) va ddng lyc hgc cdnh tay robot. Sau phdn thiet ke ben ngoai, cdc bO dieu khiin se giftp robot ca di chuydn theo qu$^ dao
mong mudn se dugc thdo luan. Nhu ta da bidt robot ca la he phi tuyin, do dd chung ta cd th6 sft dung bd dieu khien Ihdng minh hodc bd dieu khiin phi tuyin d i diiu khien robot cd theo quy dgo mong mudn. Trong bai bdo nay cdc tac gia sft dyng bg diiu khiin thdng mmh Fuzzy va bp diiu khiin phi tiiyin Adaptive Back Step- ping d i diiu khiin chuyen dgng dudi tft dd diiu khiin robot ca di chuyen ttong mgt phdng ngang. Cudi ciing, mdt vai ket qua md phdng se dugc gidi thi^u de chftng minh tinh hi^u qua va tinh khd thi ciia phuong phap dd de xuat.
Tgp Chi Khoa Hgc Gido Dgc Ky Thudt (33/2015) Tru&ng Dgi Hgc SuPhgm Ky Thugt TR Hd ChiMinh 2. MO HINH HOA ROBOT
Thiit k i cfta robot ca dugc d i xudt nhu ttong hinh 2: robot cd cd 3 khdp dudi va phdn ddu, dudi cfta robot dugc tao bdi 2 khdp chu ddng va 1 khdp hi ddng. Robot cd di chuyen
ttong mat phdng ngang bdng each sft dung dao ddng cua duoi de tgo ra lyc ddy giftp ro- bot cd di chuyen den phia trudc. Cac thdng sd dieu khien cho robot ca dugc trinh bay ttong bang I.
Bdng 1: Cdc thong so dieu khien cho robot cd Bac ty do
1 2 3 4 5 6
Chuyen dgng tinh tiin
& chuyin dong xoay surge sway heave roll pitch
yaw
Van toe dai
& van toe xoay u
V
w p q r
Vi tri & Goc Euler (H€ tga dg trdi ddt)
X
y z
^
0 V
Hinh 2: Cdu true cua robot c Carangiform
Y tudng cfta bd dgng lyc hpc dugc phdt triin dya tten phuong trinh Newton-Euler va dgng lyc hpc tay mdy. Phuong trinh chuyin ddng cua phan ddu robot ca la phuang trinh phi tuyen va dugc md ta ttong phuang trinh (1) [8]:
M ' . r + [C(v) + Div^lv + g{r}^ = F (1) Trong dd:
M': ma ttan khdi lugng
C(v): ma tran lyc Coriohs va lyc hudng tam D(v): ma ttdn gidm chdn
g(Ti): Ma ttdn kit qua cfta ttpng luc va luc Archimedes
Hinh 3: Hi tga do gdn trin robot vd hi toa do CO dinh gdn tren trdi ddt F: Ma ttan lyc vd moment tac ddng xet ttong he tpa do dia phuang
v: ma ttan van tdc tinh tiin va quay ttong he tga dp dia phuang
v=[u V w p q r ] '
Phuang ttinh (1) md ta phuong trinh ddng lyc hpc cho ddu robot ca ttong khdng gian 6 bac ty do vdi he tpa do dia phuang. Chftng ta cd mdi quan he gifta van tdc tinh tien ttong h?
tga dp dia phuang vd toan cue:
= ;i(n2)f''
liv (2)
Tgp Chi Khoa Hgc Gido Due Ky Thugt (33/2015) Tru&ng Dgi Hgc SuPhgm Ky Thugt TR Hd ChiMinh Trong do:
A ("2) =
COS^KOSB
sirupcosB . —sin9
—sinipcos(f> + cosif)sin9sin(p cosipcos(() + sinipsin9sintp
cos9sin(}}
sinipsincf} + cosx})sin9cos(j)
—cosijJsiTKp + sin^sinBcoscp cos9cos(()
--hinz}
W
Vdi:
/a ("2) -•
(3)
1 sin(f)tan0 0 cos<p 0 sin({)/cos9
coscptanO
—siiKp cos^/cos9.
Chiing ta can luu y la ma tran thi khdng xac dinh tgi mgt sd gid tti cfta gde pitch (do).
tuy nhien didu dd chung ta cd thi bd qua vi ro- bot chi di chuydn tiong mat phang ndm ngang.
Luc day dugc tao ra bdi dudi robot ca. Gia tri cfta lyc phu thupc vdo tdn sd va bien dp cfta dudi. F^ la lyc qudn tinh chdt long va Fj la lyc nang cua vdy dudi. Fp la lyc day tgi vdy dudi, F^ la thdnh phdn lyc ngang tai vdy vd F^ Id lyc cdn. Theo [3,7], phuong trinh cua cac lyc tdc ddng tai vdy dudi dugc md ta ttong cac phuong trinh (4) va (5).
i i > '
Direction of movemeni
Hinh 4: Mo hinh cdc luc tdc dgng len robot [7]
Hinh 5: Cdc luc tdc dgng len duoi
(a) (b)
Hinh 6: (a) Moi quan h$ giita U, u, v; (b) Sa do tinh todn goc tdn cong [7]
Tgp Chi Khoa Hgc Gido Due Ky Thudt (33/2015) Tru&ng Dgi Hgc Su Phgm Ky Thugt TRHd ChiMinh
Fv = npL^C^Osina + npLiC^dUcosa (4) Fj = Inpl-i^CU'^sinacosa
Pc = ^cv •" f^ci
•-CFy-Fj)s\nCq^)
= (Fv,-F;)cos((73) (5) (6) (7) Trong dd Lj Id chiiu dai quet cfta dudi, la ttpng lugng rieng cua nude, U la van tdc tuong ddi tgi ttgng tdm cua vay, la gde tan cdng va 2C Id chiSu dai tu phdn ddu dudi robot d i n phdn cudi dudi robot. Ta cd cdc bien tdng
qudt ((/=1,2,3)) (hinh 8), Id vdn tdc cfta nude theo phuong Ox, v la van tdc cfta nude theo phuong Oy va dugc tinh toan:
v = 9c3 = liqicosq^ + l^qzcosqi + l^q^cosq-^
U = -v = -ikqicosqi + Iz^z^osqz + Z3?3COS(73)
cosa =-cosq3 - 7:sinq3
-sinqs; sina =
Hinh 7: Mo hinh Dudi cua robot ca dugc mo hinh nhu he thdng tay may 3 khdp vd dugc trinh bay ttong hinh 7. Md hinh cd he sd Id xo k^ tgi khdp thft nhdt, k^ tai khdp thft hai va k^ tgi khdp cudi.
Chung ta cd the bd qua k^, k^ bdi vi chftng rdt nhd. Cdc gia tri c,, c^ vd Cj Id cdc he sd giam chdn tai tdm cfta cdc khdp. Chftng ta dp dyng phuang trinh Euler-Lagrange cho dudi cua robot ca. Lyc tdng quat bao gdm moment cua cdc dpng CO va moment tao ra bdi Fp and F^,, bdi vi robot chi di chuyen ttong mat phang Oxy. Ta cd:
phdn tich cua duoi cd dugc viet nhu ben dudi:
A^ii Mi2 Mi3-| mi-i rNii vT^l M21 M22 M23 k z = /V2 + 7-2 LM3, M32 Mssllqsi UV3J W
(8)
d /dL\
'dixd^J'
dl dqi Qi
Trong dd: L la ham ndng lugng, Qi (i=l, 2, 3) la cdc bien tdng quat vd Id lyc tdng qudt.
Phuang tiinh cua h? thdng robot ca 3 khdp
Chi t i i t cdc m a trdn M.. vd N d u o c
ij I •
t r i n h b a y t r o n g p h d n p h y luc (i, j = 1, 2, 3 ) . r . la m o m e n cfta d d n g co tai k h d c k h d p (/ = 1, 2, 3 ) . Bk giai p h u o n g t r i n h 8, chftng ta cdn cac bien t d n g q u a t (qi (i=l, 2 , 3 ) ) , sau do tinh F^vkF^.
T r o n g bdi b d o n a y ta chi tdp t r u n g v a o cdc di c h u y S n t r o n g m^t phdng O x y , d y a vdo p h u a n g t r i n h ( 1 ) , p h u a n g t r i n h c h u y e n d g n g cfta p h a n d a u r o b o t ca d u g c t r i n h b a y n h u t r o n g p h u o n g t r i n h ( 9 ) :
Tpp Chi Khoa Hpc Gido Buc Ky Thupt (33/2015) Tmdng Dpi Hpc SuPhpm Ky Thudt TR Hd ChiMinh
(
m[ix -vr-vwq- Xg(q^ + r ^ ) -I- Zg(pr + q)] = S^f, m{v - wp •¥ ur-i-Zg(.qr - f)-k- Xg(qp + ;=)] = IY^hzf + {lyy - Ixx)pi + m[xg(v -wp-t- ur)] = H /V, ext ext 'ext
( 9 )
Trong dd: bot ddi vdi nude theo true Ox va Oy;
m: khdi lugng cua robot C^^ he sd can; Sx,Sy la dien tich can ciia (x^,y^,z^ ): Tpa dp trpng tam cua robot trong •'"''Ot ca theo phuong Ox va Oy.(Sy»Sx) he tpa dp dia phuong
1^,1^1,^: Moment quan tinh theo cac true Oz ,OyvaOx
Y, X „ , Z Y „ i • ^ ™ 8 'VC dpc true Ox, Oy Tdng mpment quay quanh true Oz Chiing ta cd:
IJ l^ext — l^F L Yext = l^c •
(10) (11) 7 Hext = (XgW - Xt,B)cos9sin(l) + (ygW - ybBlsine - Fcia„ + kcosQi (12)
Trong do:
"*£ = Mci + hlc2 + Ma
"*£! = Ci?iao
Wc2 = C2q2™si)i(ao -I- /iCOSid) Mcs = C3%(ao + licosqi + l2COSq2)cosq2
Vdi la tpa dp diem dat cua luc day Archi-
medes trong he tpa dp dia phumig, W la trpng ^g ^;x^ khiSn'rieng.
luc cfta robot ca, B Id lyc Archimedes va la
ITinh 8: Cdc bien kh&p tong qudt trong he tga do dia phuang 3. T H I E T KE BO DIEU KHIEN
Trong phdn ndy, chftng ta thiet ke hai bd dieu khien cho robot ca. Sft dung bg dieu khien Fuzzy hogc bd didu khien Adaptive BackStep- ping, chftng ta dieu khien dudi cua robot ca di chuyen theo bien dang mong mudn. Vdi bien dgng mong mudn do, dudi cua robot tgo ra lyc va moment de dieu khien robot. Bdi vi dudi cua robot cd thiet ke gidng nhu he thdng cdnh tay robot cdng nghiep nen mdi khdp se cd mdt
cac moment gdy ra bdi lyc giam chdn tai cac khdp thi:
- Lyc cdn theo phuang true Ox (F^) dugc tinh toan:
Fax=lpy'xCDSx 0 3 ) Lyc cdn theo phuang true Oy (F^) dugc
tinh todn:
F,y--,PV'yC,Sy (14) Vdi p Id khdi lugng ridng cua nude;
V ,V la van tdc tuong ddi cua ro-
A. Bg diiu khiin Fuzzy
Ngay nay, bd dieu khien Fuzzy logic ngay cang dugc img dung nhieu de dieu khien cac he thdng tuyen ti'nh vd phi tuyen. Bg didu khien Fuzzy cd hai tin hieu dau vao (sai sd (e) vd dp bidn ddi sai sd (ec)) va mdt tin hidu ddu ra T (Moment cfta ddng co tai cdc khdp).
Ta chpn ham lien thudc dgng tam gidc cho cdc bien dau vao vd ddu ra. Phuong phdp giai md la phuang phdp ttpng tdm COG. Bd luat Fuzzy duge gidi thieu ttong bdng 2.
Tap Chi Khoa Hpc Gido Buc Ky Thupt (33/2015) Tnrdng Bai Hpc SuPhpm Ky Tliupt TP. Hd ChiMinh
Bang 2: Bd luat Fuzzy T
e
ec NB
NM NS ZE PS PM PB
NB PB PB PM PM PS PS PB
NM PB PB PM PM PS ZE PB
NS PM PM PM PS ZE NS PB
ZE PS PS PS NS NS NM NM
PS PS PS ZE NS NS NM NM
PM ZE ZE NS NM NM NM NB
PB ZE NS NS NM NM NB NB B. Bg diiu khiin Adaptive Back Stepping
Bd diiu khiin BackStepping la mgt bd diiu khiin he phi tuyin thudng dugc dimg cho cac ftng dyng bam theo quy dgo cho trudc.
Trong bai bao nay ta Idy y tudng thiet ke tu tai lidu tham khdo [8] de thidt ke bg dieu khien Adaptive Back Stepping:
(15)
Ta dinh nghia: H= - M ' N, S,=q; S^=q (16) Phuong trinh (16) ttd thanh:
Si = S2 ; M(52 + H) = T (17) Dgt:
62 = (Href ~ ^2 ''
^ref = % ^ ' l + Rref
M„
Mj, [MSI
M,2 M22 M32
M„-|
M23 M33J
r<ii]
<!?.
[q,!
rw.i
= U,
LN3J +
m
T?Lo
Thay phuong trinh (19) vao phuong trinh (18):
ii = -Cl - 62^2
4 - M - ' M ( ( 1 - £i2)ei + e2(£, + £ 2 ) + ?rer *^°' 4-/?)-!- ff
Cac ma tran udc lugng dugc tim bing each sii dung ham Lyapunov. Ta chpn:
I T I T l ' = Y ' ^ l ^1 +2*^2 ^2
M"*'^ 1 (21)
Dao ham cip mdt ciia ham Lyapunov dugc trinh bay trong phuong trinh (22):
/ rf
t«)-^l4«)
(22)Trong dd:
Cj: sai sd gde quay cdc khdp cfta robot e^: sai sd van tdc gde cac khdp cua robot
^2 = (^ref -S2 = (CiCi + tire/) " ^ " ^ 7 + H
= (-£j^e, + e^8, + q,,f) - M'^T + H (18) Dya vao phuong trinh (18) ta chpn tin hieu diiu khiin:
T = M( ( I - ^ I > ' + \ (19) V«2(£l + £2) + <iref + H)
Trong phuang ttinh (19), va la cac ma tran udc lupng ciia ma tran M v a H. Ta dinh nghia sai sd udc lugng:
Thay phuang trinh (20) vao (22):
V = -cit-i^ei - r2e2'i!2 + (M-'-Mf [f2 ((1 - H^y^ + C2(f, + E2) + iim + R) -J^M\
Muc dich cfta ta cdn tim Id ^ ^ ^ vi vgy ma trdn udc lugng cd thi dugc chpn:
Khi dd: V = -Sie-Je-i - e2e2 ei< 0.
Vgy he thdng dn dinh.
4. K E T QUA M 6 P H O N G
Dua vao phuang trinh chuyen ddng cfta ro- bot cd dupe gidi thieu d phan 2 vd dp dung hai bp diiu khiin Fuzzy va Adaptive Back Stepping dugc thiet k6 d phdn 3 ta tien hanh md phdng diiu khiin gde yaw cua robot. Tft dd ta cd thi diiu khiin robot cd ttong mat phdng Oxy vdi q u j dao mong mudn. Dau tidn.
Tgp Chi Khoa Hgc Gido Dgc Ky Thugt (33/2015) Tru&ng Dgi Hoc Su Phgm Ky Thugt TP.Hd ChiMinh ta se k i i m tta diiu khiin robot vdi gde yaw
mong mudn cho truoc. K i t qua md phdng se gidi thieu dap ftng goc yaw cfta robot cd va van tdc gde cfta gde yaw. Cudi cimg ta se dieu khien robot bam theo quy dao mong mudn.
A. Diiu khien goc yaw
Gde yaw mong mudn la 60". Bien dang mong mudn cfta dudi robot cd la dgng hinh sin vdi phuong trinh nhu sau:
Qire/ = i' + Asin(2nft + (p{):
92re/ = 0 + Asin{2nft + ^2) Trong dd: D la goc mong mudn.
Kp: He sd ty le duong.
error_yaw: Sai so goc yaw.
A: bien dg cua hdm sin.
/ : tan sd cfta hdm sin.
(p, va (p^: gde lech pha.
Bdng 3: Thong so mo hinh cua robot cd / , , = 0.01304 (m*)
lyy = 0.3722 (m*) 4 J = 0.3626 (m*) m„ = 1.54 (kg) p = 9 9 8 kg/m^
Co = 0.5
Sx = 0.021 m^
il = 0.813(m'') m i = 0.354 (kg)
il = 0.167 ( m ) Oi = 0.115 ( m ) q = 1 2 x 1 0 " ^
12 = 0.195(m'') m^ = 0.06 (kg)
I2 = 0.088 (m) k, = 15x10-2 02 = 0.035 (m)
Cj = 8 x 1 0 " ^
13 = 0.215 (m*) m^ = 0.0286 (kg)
(3 = 0.11 (m) 03 = 0.055 (m)
C 3 = 5
- Bg dieu khien Fuzzy Ta chgn:
Kp-1, A = omiSrad,
TV
f = 0.5 Hz,(pi = -TTT rad;
12
<P2 •• 12 -rad.
Ddp ung cfta robot ca dugc trinh bay ttong hinh 9. Ta nhgn thdy bg dieu khien mdt 10
gidy de dieu khien robot dgt dugc goc yaw mong mudn. Cdc gia tri bien tdng quat q^ (i=l, 2, 3) da dugc chuyin qua he tpa do toan cue.
Khi dgt dugc gia tri gde mong mudn, gde yaw cfta robot cd van tiep tuc dao ddng vdi bien dp nhd khoang +/-10°. Diiu nay la binh thudng gidng vdi cd that.
Dieu khien goc yaw mong mudn la 60 dp.
J r
iiif
t 111 (1)1 mi 1111 M mi mi
1)11
_ _ i — , ~
i m mi 1111 )m
11)1 mi
Kl)
70
30
10
J )l - r
Al P
V'(111
llll mi 111) im
m mi 1111 If llll
1 .LWl
mi 111 im llll mi
llll
Hinh 9: Ddp ung goc yaw v&i goc mong muon 60"
Hinh 10: Ddp ung goc q^
cua duoi robot
Tap Chi Khoa Hpc Gido Buc Ky Thupt (33/2015) Truing Bpi Hpc Sir Phpm Ky Thupt TP. Hd ChiMinh
aiiiiiH
:j
b p ^
T l ^
B S B U B
U
Hinh 11: Ddp ung goc q^ cua duoi robot Hinh 12: Ddp ung goc q^ cua duoi robot - Bg dieu khien Adaptive BackStepping
Ta chpn:
Kp=Q 51. A = 0.1745rad,/ = 1 Hz,(pi = - —rarf;([32 = —rad;s, = €-, = 0.8.
dn dinh d giay thft 10. Gde yaw ed dao dpng manh d nhftng giay dau nhung sau do da dgt dugc trang thdi dn dinh vdi goc yaw mong mudn. Thdi gian hoat dgng sau Ddp ftng goc yaw dugc trinh bdy trong ^° f ^P '^^^ ^d d^c diem gidng vdi bd dieu hinh 13. Dya vao day ta thdy he t h i n g dgt ^ ' ^ ^ Fuzzy.
Tru&ng hgp goc yaw mong muon Id 60 do.
!
I l l' r
—x,„i 1 1 L
1
M
qIAn
•
Hinh 13: Ddp irng goc yaw v&i goc mong mudn 6(f ^ " * 1^- 0°P ™ ? SO"^ 1, ™ ° <*"'' fobot
\\
•30
OiAntft 1
'"
flSAn^ 1
Hinh 15: Ddp ung goc q^ cua duoi robot Hinh 16: Ddp ung gde q^ cua duoi robot
Tgp Chi Khoa Hgc Gido Dgc Ky Thugt (33/2015) Tru&ng Dgi Hgc Su Phgm Ky Thugt TR Hd ChiMinh B. Diiu khiin robot theo quy dgo mong muon
Trong phan nay robot ca sg dugc dieu khien de bam theo quy dgo mong mudn cho trudc.
Dudmg Uen tuc la ddp ung vi tii cua robot cd, dudng dftt gach Id qu^ dgo mong mudn.
' ' ^ , 1 OWpUSysteml
y V ^ °-'^
t J A
''\- -t T-
\ J ^^y
Hinh 17: Ddp ung vi tri cua robot
1 "
|L
\ >
V \
>
kV \
^ \J
T rf / /}
' LJ
>
/ '
HXnh 18: Ddp img gde yaw ciia robot
1 ° 1
\ >
1V \
kV \
\J
Tr if /
I — .
J
f '
A
/ f
An«ll|
- Bg dieu khien Fuzzy
Dap ung cua robot khi dp dyng bd d i i u khien Fuzzy dieu khien di chuyin trong mat phang ndm ngang dugc the hien qua hinh 17.
1.
^ n
1 q2Al>Btt|
Hinh 20: Ddp img goc q^ cua duoi
I 1 •"
i L
V >
V V
k
V \ iJ
TFy r
f J
r
1
f /
Hinh 19: Ddp ung goc q, cua duoi
Hinh 21: Ddp ung goc q^ cua duoi.
Trong hinh 18, robot cd co thdi gian hoan thanh quy dao mong mudn la 700 gidy. Sai sd xudt hien theo phuong Ox khodng 0.4m.
- Bg diiu khiin Adaptive BackStepping Dap ftng cua robot khi sft dung bd dieu khiin Adaptive BackStepping de dieu khiSn robot cd boi theo quy dao mong mudn dugc trinh bay trong hinh 26. Hinh 22-25 t h i hien cdc dap ftng goc cua cac khdp dudi. Tdt ca cac gid tri cfta gde da dugc quy ddi sang he tga do toan cue.
Tgp Chi Khoa Hgc Gido Due Ky Thudt (33/2015)
^Tru&ng Dgi Hgc SuPhgm Ky Thugt TR Hd ChiMinh
I T 1 I I
Hinh 22: Ddp ung goc yaw cua robot Hinh 23: Ddp ung goc q^ cita duoi robot
' «
Hinh 24: Ddp ung goc q^ cua duoi
/ / /
/"
\
Vl
'.
tt^
1 Oulpul Syslem 1
,^
,/
ij
\ I
Hinh 26: Ddp img vi tri ciia robot trong he tga do todn cue.
Trong hinh 26, ta thay robot cdn khodng 600 gidy de hoan thanh quj' dao mong mudn.
Khoang each lech theo phuong Ox la khodng 0.3m.
5. K E T LUAN
Trong bai bao nay, phuang ttinh ddng hpc cua robot ca 3-khdp dang Carangiform dugc gidi thieu. Dya vao phuang ttinh ddng lyc hpc
Hinh 25. Ddp ung goc q^ ciia duoi ta thiit k i 2 bd dieu khien Id Fuzzy va Adap- tive BackStepping de dieu khien robot ttong mat phdng ndm ngang. Ket qua md phdng nhdn thdy bd dieu khien Adaptive BackStep- ping cho ddp ftng tdt ban bd dieu khien Fuzzy ve thdi gian ddp ung ciing nhu sai sd vi tri cudi hanh tiinh. Tuy nhien ban thdi gian dau hogt dgng bd dieu khien Fuzzy cho ket qua ddp ftng ve gde dn dinh hon. Ta thay sai sd cfta goc yaw sai lech khodng dp. Sai sd nay hoan toan cd the chdp nhdn dugc vi thyc td ca that khi di chuyen thi van tgo ra goc lech nhd. Qua qua trinh md phdng ta cung danh gia dugc mftc dp chinh xac cfta phuang trinh ddng lyc hgc mdi ma cdc tdc gid da xay dyng. Md hmh ddng luc hpc dugc xdy dyng theo phuang phap dd ttinh bdy d cac phdn tten la cd the chap nhgn dugc.
Cd thi sft dung de tien hanh md phdng danh gia cac thdng sd cfta robot.
Trong nhftng phan tiep theo, cac tdc gid se tiin hanh thyc nghiem de cd th6 danh gia tdt hon v i mdt thuc t i phuong phdp xdy dyng md
Tpp Chi Khoa Hpc Gido Bpc Ky Thudt (33/2015) Truimg Bpi Hpc SuPham Ky Thupt TP. Hd ChiMinh hinh njy va danh gia dugc cac bp didu khidn da LCJI CAM ON
dupe xay dung, bing each so sanh kdt qua ly Nghien ciiu nay dugc tai trg bdi Dai hgc thuyet md phdng dugc va kdt qua thuc nghiem. Quae gia Thanh phd Hd Chi Minh (VNU- Sau do cac tac gia cd thd nghien cuu giai quydt -HCM) trong khudn khd dd tai ma sd B-2013- bai toan dieu khien dp sau cho robot. 20-01.
TAI LIEU THAM KHAO
[1] J. Edward Colgate, Member, IEEE, and Kevin M. Lynch, Member, IEEE, Mechanics and Control of Swimming: A Review, IEEE loumal Of Oceanic Engineering, VoL 29, No. 3, July 2004.
[2] Michael Sfakiotakis, David M. Lane and J. Bruce C. Davies, Review of Fish Swimming Modes for Aquatic Locomotion (Translation Journals style), IEEE Joumal Of Oceanic Engmeering, Vol. 24, No. 2, April 1999.
[3] Motomu Nakashima, Norifiimi Ohgishi and Kyosuke Ono, A study on the Propulsive Mechanism of a Double Joint Fish Robot Utilizing Self-Excitation Control, JSME Inter- national Joumal, Series C, Vol. 46, No. 3, 2003.
[4] Qin Yan, Zhen Han, Shi-wu Zhang, Jie Yang, Parametric Research of Experiments on a Carangiform Robotic Fish, Journal of Bionic Engineering 5 (2008).
[5] Hadi EL Daou, Taavi Salumae, Asko Ristolainen, Gert Toming, Madis Listak, and Maar- ja Kruusmaa, A Bio-mimetic Design and Control of a Fish-like Robot using Compliant Structures, The 15th Intemational Conference on Advanced Robotics Tallinn University of Technology Tallmn, Estonia, June 20-23, 2011
[6] Feitian Zhang, John Thon, Cody Thon and Xiaobo Tan, Miniature Underwater Glider:
Design, Modeling, and Experimental Results, 2012 IEEE Intemational Conference on Robotics and Automation RiverCentre, Samt Paul, Miimesota, USA May 14-18, 2012 [7] Tuong Quan Vo, Hyoung Seok Kim, Byung Ryong Lee, Propulsive Velocity Optimization
of3-Joint Fish Robot Using Genetic-Hill Climbing Algorithm, Joumal of Bionic Engi- neering, Vol. 6, pp. 415-429, 2009.
[8] Ola-Erik Fjellstad, Control of Unmanned Underwater Vehicles in Six Degrees of Freedom A Quaternion Feedback Approach, Dr.lng Thesis, Department of Engineering Cybernet- ics, The Norwegian Institute of Technology, University of Trondheim, November 1994.
