Mô hình hóa động lực học
5
Hội nghị toàn quố c về Điều khiển và Tự động ho á - VCCA-2011 VCCA-2011 Mô hình hóa động lực học Quadrotor Modeling of Quadrotor dynamics Đào Văn Hiệp, Trần Xuân Diệu, Phùng Thế Kiên Trường HVKTQS e-Mail: [email protected] Tóm tắt Sử dụng cách tiếp cận Cơ hệ nhiều vật, bài báo trình bày kết quả xây dựng mô hình động lực của mô hình máy bay lên thẳng dạng Quadrotor , nghiên cứu các ứng xử của mô hình khi thay đổi các thông số đầu vào. Kết quả khảo sát sẽ là cơ sở cho việc thiết lập thuật toán và thiết kế hệ thống cơ khí và điều khiển máy bay. Abstract: In this paper, the authors used the aprroach with Dynamics of Body systems to estalish the mathematical model of a helicopter called Quadrotor and investigated the behaviours of the model by varying the input parameters. The results from modelling are the fundament to estalish the control algorithms and to design the mechanical parts. Ký hiệu Ký hiệu Đơn vị Ý nghĩa H, A t , J T , J R , I, M các ma trận của mô hình , véc-tơ vận tốc góc và toán tử sóng véc -tơ vận tốc góc T, động năng và thế năng F u , F, Q*, các véc-tơ lực và mô men Chữ viết tắt UAV thiết bị bay không người lái 1. Đặt vấn đề Trong những năm gần đây, các thiết bị bay không người lái (UAV) được đặc biệt quan tâm do nhu cầu thực tế trong hoạt động quân sự như do thám, kiểm tra địa hình địa vật...cũng như trong hoạt động dân sự như chụp ảnh, giám sát, quan trắc... Có rất nhiều loại UAV đã được phát triển như kinh khí cầu, máy bay cánh cố định, trực thăng rô -to đơn, quadrotor,... Mỗi loại UAV đều có những ưu điểm và những hạn chế của nó. Quadrotor là loại máy bay trực thăng có 4 cánh quạt với những ưu điểm nổi bật là cất cánh và hạ cánh thẳng đứng dễ dàng, kích thước nhỏ gọn, giữ ổn định ở một vị trí trong không gian tốt, có kết cấu cơ khí đơn giản và khả năng tải lớn. Nghiên cứu mô hình động lực học của quadrotor là bước quan trọng ban đầu để từ đó đưa ra thuật toán điều khiển cũng như thiết kế chế tạo. 2. Mô hình hóa động lực học 2.1 Các đặc tính Quadrotor Quadrotor có 2 cặp cánh quạt gắn với 2 cặp động cơ tương ứng được bố trí như ở hình 1. Cặp động cơ trước-sau được điều khiển quay theo chiều kim đồng hồ, trong khi đó cặp động cơ còn lại được điều khiển quay ngược chiều kim đồng hồ, cách bố trí này sẽ làm triệt tiêu ảnh hưởng của các mô men cánh quạt và các hiệu ứng khí động học. H. 1 Cấu trúc của hệ thống điều khiển số H. 2 (a) thay đổi góc pitch; (b) thay đổi góc roll; (c) thay đổi góc yaw Quadrotor hoàn toàn không cần đĩa lắc để điều chỉnh góc cánh như trực thăng rô -to đơn. Chỉ cần điều khiển tốc độ của 4 động cơ, ta hoàn toàn có thể có được các góc roll, pitch, yaw như mong muốn. Nguyên l ý điều khiển của quadrotor rất đơn giản, có thể trình bày ngắn gọn như sau: giả sử lúc đầu 4 rotor 306
description
Hội nghị toàn quốc về Điều khiển và Tự động hoá - VCCA-2011Mô hình hóa động lực học Quadrotor Modeling of Quadrotor dynamicsĐào Văn Hiệp, Trần Xuân Diệu, Phùng Thế Kiên Trường HVKTQS e-Mail: [email protected] Tóm tắtSử dụng cách tiếp cận Cơ hệ nhiều vật, bài báo trình bày kết quả xây dựng mô hình động lực của mô hình máy bay lên thẳng dạng Quadrotor, nghiên cứu các ứng xử của mô hình khi thay đổi các thông số đầu vào. Kết quả khảo sát sẽ là cơ sở cho việc thiết lập thuật toán và thiết kế
Transcript of Mô hình hóa động lực học
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011 VCCA-2011 M hnh ha ng lc hc Quadrotor Modeling of Quadrotor dynamics o Vn Hip, Trn Xun Diu, Phng Th Kin Trng HVKTQS e-Mail: [email protected] Tm tt S dng cch tip cn C hnhiu vt, bibo trnh by kt quxy dngm hnh ng lc cam hnh mybaylnthngdngQuadrotor,nghincucc ngxcamhnhkhithayiccthngsu vo.Ktqukhostslcschovicthitlp thuttonvthitkhthngckhviukhin my bay. Abstract:Inthispaper,theauthorsusedthe aprroachwithDynamicsofBodysystemstoestalish themathematicalmodelofahelicoptercalled Quadrotorandinvestigatedthebehavioursofthe modelbyvaryingtheinputparameters.Theresults frommodellingarethefundamenttoestalishthe control algorithms and to design the mechanical parts. K hiu K hiun v ngha H,At,JT, JR, I, M cc ma trn ca m hnh e,e vc-t vn tc gc v ton t sng vc-t vn tc gc T, Hng nng v th nng Fu,F,Q*, t cc vc-t lc v m men Ch vit tt UAVthit b bay khng ngi li 1.t vn Trongnhngnmgny,ccthitbbaykhng ngili(UAV)ccbitquantmdonhucu thc t trong hot ng qun s nh do thm, kim tra ahnhavt...cngnhtronghotngdns nh chp nh, gim st, quan trc... C rt nhiu loi UAVcphttrinnhkinhkhcu,mybay cnhcnh,trcthngr-ton,quadrotor,...Mi loiUAVucnhnguimvnhnghnch can.Quadrotorlloimybaytrcthngc4 cnh qut vi nhng u im ni bt l ct cnh v h cnh thng ng d dng, kch thc nh gn, gi n nhmtvtrtrongkhnggiantt,cktcuc kh n gin v kh nng ti ln. Nghin cu m hnh nglchccaquadrotorlbcquantrngban utarathuttoniukhincngnh thit k ch to. 2.M hnh ha ng lc hc 2.1 Cc c tnh Quadrotor Quadrotorc 2cpcnhqutgnvi2cpngc tngngcbtrnhhnh1.Cpngc trc-sauciukhinquaytheochiukimng h, trong khi cp ng ccn li c iu khin quay ngc chiu kim ng h, cch b tr ny s lm trit tiu nh hng ca cc m men cnh qut v cc hiu ng kh ng hc. H. 1Cu trc ca h thng iu khin s H. 2 (a) thay i gc pitch; (b) thay i gc roll; (c) thay i gc yaw Quadrotor hon ton khng cn alciu chnh gc cnh nh trc thng r-to n. Ch cn iu khin tc ca 4 ng c, ta hon ton c th c c cc gc roll, pitch, yaw nh mong mun.Nguynliukhincaquadrotorrtngin,c th trnh by ngn gn nh sau: gi s lc u 4 rotor 306Hi ngh ton quc v iu khin v T ng ho - VCCA-2011 VCCA-2011 ccngtc(ccnglngga),thayigc pitchtatng(gim)tccangctrcng thi gim (tng) tc ca ng c sau; thay i gc rollcnglmtngt;thayigcyawtatng (gim)tccacpngctrc-sauvgim (tng)tccacpngctri-phi.gin nhcao,tanngitnglnggakhngi.S quan h gia cc lc khi thay i cc gc c th hin hnh 2 [3]. 2.2 M hnh ng lc hc Ta coi quadrotor l mt vt rn chu mt lc (hp lc ca cc lc to ra bi cc ng c) v 3 m-men lm quadrotorquaytheoccgcroll,pitch,yaw. nghin cu ng lc hc ca m hnh quadrotor y ta s dng phng trnh Lagrange II. H. 3 Cc h quy chu ca quadrotor Chn h quy chiu qun tnh Fi v h quy chiu ng gn vi vt Fb c th hin hnh 3 [3]. Gc ta cahquychiuFbcgnvitrngtmca quadrotor. Cc ta suy rng i vi quadrotor c chn nh sau:6 5 4 3 2 1, , , , , q q q q z q y q x = | = u = = = =, hay 1 2 3 4 5 6[ ]Tq q q q q q = q. y, (x, y, z) l ta ca trng tm C ca m hnh quadrotortronghquychiuquntnhv,u,|l bagcyaw,pitchvrollthhinhngca quadrotor. Ta c th chuyn h ta Fi sang h ta Fb bng cc php bin i sau: -TnhtinFidctheotrcz0mtonz,ch ta hin thi F1; -Tnh tinF1 dctheo trcx1 mt on x, c ta hin thi F2; -Tnh tinF2 dctheo trcy2 mt ony, c ta hin thi F3; - Quay F3 quanh trc x3 mt gc |, ta c h ta hin thi F4; - Quay F4 quanh trc y4 mt gc u, ta c h ta hin thi F5; - Quay F5 quanh trc z5 mt gc, ta c h ta ng gn vi vt Fb. Ma trn ca php bin i t h quy chiu Fi sang h quy chiu Fb s l:
H0 0 0 1( (+ + (= ( + + ( c c s s s xc s s s c s s s c c c s yc s c s s s s c c s c c zu u uu| | u| | u|u| | u| | u|. (1) Theo [2], t ma trn chuyn i ta nhn c ma trn cosinchhngkhithchinphpchuynilin tip t h quy chiu Fi sang h quy chiu Fb l: tc c s c sc s s s c s s s c c c sc s c s s s s c c s c cu u uu| | u| | u|u| | u| | u|( (= + + ( ( + + A(2) v ta trng tm l: | |1 2 3 Tq q q =Cr (3) o hm biu thc trn trong h quy chiu qun tnh ta c vn tc ca khi tm | |1 2 3 TC Cq q q = = v r(4) Tmatrncosinchhng,theo[1]taddngxc nhctontsngcavc-tvntcgctrong h quy chiu qun tnh:
4 5 6 5 6 4 5 6 5 64 5 6 5 6 6 4 54 5 6 5 6 6 4 5000Tt tq c c q s q c s q cq c c q s q q sq c s q c q q s +( (= + ( ( + = A A e (5) Ttontsng(5)taxcnhcvntcgc ca quadrotor trong h quy chiu qun tnh:
| |6 4 5 4 5 6 5 6 4 5 6 5 6= + + +Tq q s q c s q c q c c q s e(6) PhngtrnhLagrangeloiIIivivtrn(m hnh quadrotor) l
Qq q q*| | | | | | c c c = + |||c c c\ . \ . \ .T T Td T TdtH (7) trong : T l ng nng ca vt rn H - l th nng trng lc ca vt rn Q* l vc-t lc suy rng. ngnngcaquadrotorcxcnhtheocng thc q M(q)q1T2=Ttrong : M(q)lmatrnkhilngsuyrng, T TT T R Rm M(q) = J J +JIJ. JT l Jacobi tnh tin, c xc nh theo cng thc 1 0 0 0 0 00 1 0 0 0 00 0 1 0 0 0T (c (= =(c ( CrJq(8) JR l Jacobi quay, c xc nh theo cng thc 56 5 66 5 60 0 0 0 10 0 0 00 0 0 0Rss c cc c s (c (= = (c ( Jqe(9) 307Hi ngh ton quc v iu khin v T ng ho - VCCA-2011 VCCA-2011 I l Ten-x qun tnh khi ca quadrotor i vi h quychiuquntnh,theo[1]ncxcnhtheo cng thc I AI A =Tb (10) Ib l Ten-x qun tnh khi ca quadrotor i vi h quychiung,doquadrotorchnhdngixng trcvcctrctacchntrngvitrci xng, nn theo [1] ta c
0 00 00 0xb yzIII ( (=( ( I (11) Thayccbiuthc(2)v(11)vobiuthc(10)ta c Ten-x qun tnh khi ca quadrotor i vi h quy chiu qun tnh. Th nng trng lc c xc nh theo cng thc 0Tm H = Cg r(12) trong : g0 l vc-t gia tc trng trng, 0[0 0 ]T Tg = gThay (3) v vc-t gia tc trng trng vo biu thc (12) ta c H = mgz(13) Vc-tlcsuyrnggm2thnhphnchnh,lcc lc v cc m men nh c a ra trn. Do Q F*[ ] =T T Tt(14) Lc to ra t 4 ng c xt trong h quy chu Fb l [0 0 ]Tuf = F. y,f l lc tng hp sinh ra t cc ng c1 2 3 4f f f f f = + + + ,fi = kfe2i, i = 1...4 vi kf l hng s dng ph thucvo tng loi cnh qut, eil vn tc gc ca ng c th i. Do,Fu tronghquychiuFicxcnhtheo cng thc F = AtFf = | |5 6 5 6 5Ts f s c f c c f (15) Vc-t m men t l [ ] ,T u |t t t = t
trong 1 2 3 4,M M M M t t t t t = + + + 2 4( ) , u u lut = 3 1( ) , u u l|t = y, Mit l cc m men c to ra t cc ng c Mi, l l khong cch t cc ng c ti trng tm ca quadrotor. Ch rng tMi = kMe2i, i = 1...4, kM l cc hng s dng. t k = kM/kf, t ta d dng ctMi = k fi. S dng phn mm Maple xy dng cc cng thc vlphphngtrnhviphn.Tphngtrnh LagrangeII (2.7) tac h 6 phng trnhvi phn sau: f s q m5 1 = (16) f c s q m5 6 2 = (17) mg f c c q m =5 6 3 (18) ) ( ) 2 2 (4 4 4 425 5 524 524 5 5 5 6 5 6 4 y x y z y x x z zI s c I s c q I c I c I c c I c c I c q q I s q I q + + + + + + + t = + ) (4 425 4 42526 y xI c s c I c s c q (19) ) 2 2 ( ) ( 2 ) (24 5 524 5 5 5 6 4 4 4 4 4 5 42424 5 y y x z x x y x y xI c c I c I c c I c I c q q I c s I c s q q I c c I I q + + + + + +
ut = + + + ) ( ) (4 4 5 4 4 5 6 5 524 5 5 5 5 524 526 y z y y xI c s c Ix s c c q I c s I c s c I s c I s c c q (20) x x y z x y y z zI c c q q I s c c I s c c q I s q I c c I c c I c I c I q24 5 5 4 4 4 5 4 4 5 5 5 4242524252525 62 ( ) ( ) ( + + + + + +
) 2 ( 2 ) 5 2 55 5 524 5 5 524 5 5 6 5 5 524 z x y y x z y yI c s I s c c I s c c I s c q q I c I c I c c I c + + + + +
|t = + + + ) ( ) ( 24 4 5 4 4 525 4 425 4 425 6 4 x y y xI s c s I s c s q I c s c I s c c q q (21) 2.3 M phng ng lc hc quadrotor Trongmcny,tassdngMatlab-Simulink giihphngtrnhgm6phngtrnhviphnt (16) n (23). Kt qu ca php gii m t ng x ca h khi chu cc tc ng.M phng cn phi c thc hin khng nh tnh ngnccgithitcachngtakhithitlpm hnhtoncaQuadrotor.Trncstnhngn camhnhton,khithayicctcngvoh (cc thng s ca ng c) ta s thu c cc kt qu ng x ca h thng. Thng sK hiu Gi trn v Khilngca Quadrotor m0,8kg M men qun tnh theo trc x Ix0,0115kg.m2 M men qun tnh theo trc y Iy0,015kg.m2 M men qun tnh theo trc z Iz0,0228kg.m2 Gia tc trng trngg9,81m/s2 Khongcchttm Quadrotorntmcc ng c l0,25m Hstlgiam menngcvlc ng c k0,001Nm/N Ccgitr u catt c cc thng strng thiu c gi tr bng khng. 308Hi ngh ton quc v iu khin v T ng ho - VCCA-2011 H. 4M hnh gii h phng trnh vi phn trong Simulink Mhnhhnh4mphngnguynliukhin ccthngsuvo(lcccngcfi)t c cc thay i ca cc thng s u ra (x, y, z, |, u, ) nh trnh by phn 2. Khi tnh ton lc ngchnh4thhinrngbucnhthay i cao, thay i cc gc roll, pitch, yaw..., khi ny c u ra l cc lc m cc ng c cn to ra. Khi tnh ton lc v m men cho h phng trnh viphntnhton,arahplcvmmen gmf,t,tu,t|.Khis-function(quadrotor)c lpgiihphngtrnhdatrnccthngs c cung cp t khi tnh ton lc v m men cho h phng trnh vi phn. u ra ca khi s-function l cc thng s ng hc quan tm ca quadrotor. Cc kt qu m phng gm: M phng 1: Khi cc ng c cha cung cp lc. H. 5 dch chuyn theo trc x, y, z theo thi gian v cc gc |, u, theo thi gian Ktqunychothykhiccngckhngcung cp lc th quadrotor ri t do. Kt qu ny th hin ng x ng n ca m hnh, kt qu ny cng c th kim nghim thng qua cng thc n gin h = 0,5gt2. gi c cao gc ta (z = 0) th mingccnphicungcpchohmtlcl m.g/4 = 1,962N. M phng 2: Quadrotor bay ln, mi ng c phi cung cp cho hmt lc ln hn 1,962N. y ta m phng ng x ca h khi cc ng c cung cp cng mt lc 3N. H. 6 dch chuyn theo trc x, y, z theo thi gian v cc gc |, u, theo thi gian Mphng3:Quadrotorbaylnngthiquay gc roll (|), ng c trc v sau cung cp mt lc nh nhau ( y l 3N), ng c bn tri cung cp mt lc ln hn (3,2N) trong khi ng c bn phi cung cp mt lc nh hn (2,8N). Ta thy khi iu khin ng thi cao v gc roll, nu lc ca cc ng c khng ln th quadrotor smtcao,slaoxungvcnggyradch chuyn theo phng y(hnh 7). H. 7 dch chuyn theo trc x, y, z theo thi gian v cc gc |, u, theo thi gian Mphng4:Quadrotorbaylnngthiquay gc pitch (u), ng c tri v phi cung cp mt lc nhnhau(yl3N),ngctrccungcp mt lc ln hn (3,2N) trong khi ng c sau cung cp mt lc nh hn (2,8N). 309Hi ngh ton quc v iu khin v T ng ho - VCCA-2011 VCCA-2011 H. 8 dch chuyn theo trc x, y, z theo thi gian v cc |, u, theo thi gian Tng t nh vi m phng 3, nhng y s xy ra dch chuyn theo phng x. Mphng5:Quadrotorbaylnngthiquay gc yaw (), ng c tri v ng c phi cung cp cng mt lc ln ( y l 3,2N), ng c trc v sau cung cp cng mt lc nh ( y l 2,8N). H. 9 dch chuyn theo trc x, y, z theo thi gian v cc |, u, theo thi gian ykhng thy cs nhhng caiukhin gcyawnthngsuracnli,nhnggc yawthayichmhnrtnhiusovithayi cagcroll,pitchtrongcngmtiukin,iu nychothythctiukhingcyawcng tng i kh. 3. Kt lun Vi cch tip cn c hc h nhiu vt, trong bi bo ny,nhmtcgitrnhbyktqunghincu m hnh ng lc hc ca quadrotor. Nhm tc gi s dng phng php Phng trnh Lagrange II lphphngtrnhnglchcquadrotorkhc hontonsoviccbibotrcysdng PhngphpNewton-Euler.Phngtrnh LagrangeIIlunaraslngphngtrnhti thiu (bng s bc t do ca h), cch thnh lp h phngtrnhcngnginhncbitphng phpnyrttngthchviccphnmmtnh ton nh Maple v Matlab-Simulink. Ccngxcamhnhnglchccm phng phn 4 chng t tnh ng n cam hnh ton v ph hp vi ng x trong thc t. M hnhnylcsthitkhiukhinvtnh ton kt cu c kh ca quadrotor - nhng ni dung nghin cu tip theo ca nhm tc gi. Ti liu tham kho [1]NguynVnKhang:nglchchnhiu vt.NXBKhoahcvKthut,HNi 2007. [2]oVnHip:Kthutrobot.NXBKhoa hc v K thut (in ln 2), H Ni 2004 [3]Pedro Castillo, Rogelio Lozano and Alejandro E.Dzul:ModellingandControlofMini-FlyingMachines,Springer,Compigne, France 2004. [4]JensWittenburg:DynamicsofBodysystems (Second Edition), Springer, Karlsruhe 2007. TrnXunDiusinhnm 1983.Anhttnghipihc tiHcvinkthutquns nm2007.Saukhiratrng anhlnghincuvinca VintnlaVinkthut quns.Hinnayanhang theo hc cao hc chuyn ngnh cinttiHcvinkthutquns.Hng nghincuchnhlnglchcviukhin robot, iu khin my li tn la. PhngThKinsinhnm 1982.Anhttnghipi hctiHcvinkthut qunsnm2007.Saukhi ratrnganhltrlk thutcngtythngtinM3- Tponvinthngqun iViettel.Hinnayanh angtheohccaohc chuynngnhcintti Hcvinkthutquns.Hngnghincu chnh l ng lc hc v iu khin robot. 310
