Design and Simulation of the Control Algorithm for Vehicles of Geophysical Exploration

Transcription

1 th Intrnational Confrnc on Mchanical Matrials an Manufacturing Enginring (MMME 06 Dsign an Siulation of th Control Algorith for Vhicls of Gophysical Exploration Ji Liu & Yang Lv & Zkun Liu Collg of Instruntation & Elctrical Enginring, Jilin Univrsity, Changchun, China Zhiyan Dong* Collg of Mchanical Scinc an Enginring, Jilin Univrsity, Changchun, China *Corrsponing author: Zhiyan Dong ABSRAC: In this papr, w slct th quarotor as th airon gophysical xploration platfor. By coparing th currnt quarotor control algorith, th classical clos-loop PID thory is slct. An thn th platfor flight control syst is ivi into thr control loops, that is, position control loop, attitu control loop, an rotation sp control loop, which ar rspctivly sign PID controllr. h rsults that th controllr siulat in th MALAB / Siulink platfor vrify th ffctivnss of th sign controllr. KEYWORD: gophysical xploration; quarotor; platfor oling; algorith siulation INRODUCION rat y flight attitu (MD Schit, 0, th whol platfor flight control syst is ivi into thr control loops (Do Nascinto D A t al, 0 (zkan B. t al, 0, naly rotation sp control loop, attitu control loop, an position control loop. h rotation sp control loop is to control th four rotation sps of th platfor i ( i,, 3,.h attitu control loop is to control th translational ovnt of th platfor ( X Y Z.h position control loop is to control th rotational isplacnt of th platfor (.h whol flying control syst of th platfor is show in g.. Vhicls hav co or an or wily us in th gophysical xploration work of harsh nvironnt, an this tchnology has n a short oar to our country (Air M Y t al, 008. Of th tchnology, th control algorith is th ost iportant on. For this rason, w took a p stuy aout th flight control algorith in this articl. Bcaus of th ig tail ont influnc, th unann hlicoptr is not suital for low-flying, an with th rason of poor oility, fix wing UAV is largly influnc y th fil liiting whn taking-off an laning. So consiring all aov rasons, this papr chooss quarotor as airon gophysical xploration platfor. Bas on th classical PID control thory, this papr signs a control algorith suital for th syst. h PID control algorith has a copltly thortical syst, (Mian A A t al, 008 an th control structur is sipl, goo staility, an asy to ajust. hanks to th strong antijaing an roustnss in practical application, it has n wily us in th fil of chanical control. DESIGN OF CONROL ALGORIHM g. Flying control syst of th platfor. Analysis of control syst. Mol of th platfor h platfor syst is controll y changing th four rotors sp to chang th lift in four iffrnt irctions, an thn changing th otion platfor so that th platfor rachs th targt position. In orr to ruc th influnc of th utual coupling gn 06. h authors - Pulish y Atlantis Prss In orr to scri th kinatic rlationship of quarotor, firstly, w n to fin two coorinat systs, rspctivly groun (inrtial coorinat 50

2 syst E-fra an oy coorinat syst B- fra, as shown in g.. g. Coorinat systs of th platfor Accoring to th finition of coorinat systs, whry th introuction of six grs of fro of th aircraft, thr position X=[x y z] (rprsnts th cntroi of th aircraft with rspct to th spatial position of th inrtial coorinat syst an thr angls η=[φ θ ψ] (shows th rlationship twn th oy coorinat syst an th inrtial coorinat syst, usually Eulr angls: φ is th roll angl; θ is th pitch angl; ψ is th yaw angl. h rotational atrix R B E fro th oy coorinat syst B-fra to th inrtial coorinat syst E-fra can xprss as follows: c c ssccs sccss R B E c c ssccs scssc ( s sc cc Whr w us sϕ, sθ, sψ for sinϕ, sinθ, sinψ an cϕ, cθ, cψ for cosϕ, cosθ, cosψ. h ynaical ol of th platfor is as on th Nwton-Eulr quations as follows: F V W V ( M IW W IW Whr nots th ass of th platfor; F ots th total oy forc; th oy's linar acclration; lin sp, V u v w total oy ont; acclration; ; W V nots V ots th oy M ots th ots th oy angular W ots th oy angular vlocity, W p q r ; I ots th oy's inrtia ont atrix, I I XX IYY I ZZ For th ol st up in th inoor flight stat, ignor th influnc of air rsistanc. h forc of th platfor syst analysis shows that th ain forc of th syst inclus lift forc gnrat y th propllors rotation an gravity forc.h lift forc is F 0 0 (3 i Whr F (i=,, 3, for thrust gnrat y i propllr, i ( for propllr lift factor, ω for propllr rotation sp. FG 0 0 g ( ransfor th thrust F an gravity F G y th rotation atrix, it follows that F RF R 0 0 i (5 sin sin cos sin cos F i sin cos cos sin sin i cos cos G G F R F R 0 0 g sin (6 g cos sin cos cos Whr R R. Carrying forula (3 an forula (6 into th Nwtonian quations of th forula (, it follows that u gsin qw vr gcos sin wr pw v (7 g cos cos pwqu w i Accoring to Nwton's scon law V F, rwrit forula ( an forula t (5, it follows that sinsin cossincos X i Y sin cos cossinsin (8 i Z coscos g i Whr V is for th oy lin sp in E-fra, V X Y Z. h ain sourcs of th outsi ont of th platfor co fro rag torqu, aroynaic torqu, an gyroscopic ont. Bcaus of th sytry axis of th oy of th platfor, th rag on th oy os not prouc any torqu. Bas on 5

3 th spcial flight status of th platfor, low-attitu an low-sp, th gyroscopic ffct of th oy is not ovious, so hr w ignor th ipact of gyroscopic ffct. As a rsult, th ont of th platfor is ainly u to th aroynaic torqu gnrat y th propllr rotation. h aroynaic M torqu can xprss as lf F M lf F3 (9 M M M 3 M Whr Mi i (i=,, 3, for th propllr rsistanc ont, for th propllr rsistanc factor. Rwrit th quation (9 with (, it follows that l F F qr I I p I XX I XX lf F3 pr I XX IZZ q IYY I YY r M M M 3 M pq IYY I IZZ IZZ Accoring to th rotation thory, thr is ZZ YY XX (0 p q r ( Whr is for th transition atrix, tan sin tan cos 0 cos sin. 0 sin cos cos cos h Kintic ol quation of th platfor can xprss as follows: U X sin sin cos sin cos U Y sin cos cos sin sin U Z cos cos g ( U qr IZZ IYY p I XX I XX U 3 pr I XX I ZZ q IYY IYY U pq IYY I XX r IZZ IZZ U 3 i U lf Fl (3 U3 lff3l U MM M3 M ( 3 Whrin, to siplify th ol scription, finition four varials aout th rotors spu, U, U 3, U for th ol input. Consiring th flight status of th platfor, ostly in th low attitu, low sp or hovr stat, sall angl chang, so th chang rat of th angl can approxiatly zro an forula (0 can sn as a unit atrix. An thn th kintic ol of th platfor syst, forula ( can r-fin as: U X sin sin cos sin cos U Y sin cos cos sin sin U Z cos cos g ( U I XX U 3 IYY U I ZZ.3 Dsign of control algorith Position control loop h position PID controllr oul aopts th PID control algorith for th thr input sir isplacnt ( X Y Z, an th spcific for is as follows: X kpx ( X X kix ( X X tkdx ( X X (5 Y kpy ( Y Y kiy ( Y Y tkdy ( Y Y Z kpz ( Z Z kiz ( Z Z tkdz ( Z Z Whr kpx kpy kpz not th proportional cofficint, kix kiy kiz not th intgral cofficint, kdx kdy kdz not th iffrntial cofficint. In th acclration invrs solution oul, it taks th output of position PID controllr oul ( X Y Z as input, accoring to th forula ( th high acclration quation, U can calculat as follows: U ( Z g cos cos (6 Accoring to th forula ( in th horizontal acclration quation, can calculat as follows: arcsin[ ( X cos Y sin ] (7 U Whr onats th rfrnc valu for a givn sir yaw angl. ak th first quation in forula ( with (7, can calculat as follows: 5

4 X U sin sin arcsin( (8 cos cos Attitu control loop As th cor of th control syst of th platfor, th attitu control loop nsurs th position control loop staility, an trins th platfor flight status. Its control rsult is irctly affct th quality of flight platfor. h attitu PID controllr aopts th PID control algorith for th thr input sir angular isplacnt as follows: (, an th spcific for is U kp( X ki ( t kd( (9 U3 kp( ki ( t kd( U kp ( ki ( t kd ( Whr kp kp kp not th proportional cofficint, ki ki ki not th intgral cofficint, kd kd kd not th iffrntial cofficint. In th sp of invrs solution oul, it taks U, U, U 3, U as input, accoring to forula (3, it follows that: ( U U 3 U l ( U U U (0 l 3 ( U U 3 U l ( U U U l 3 Rotation sp control loop By changing th rat of rotation of th rotor, th platfor can achiv various flights. In orr to tak a flxil rspons, th rotation sp control loop of th platfor ust hav a quick ajusting aility. Du to th short rspons ti of th otor, th rspons ti is uch lss than th ovnt cycl of th syst platfor, so insta of using th gain factor of th transfr function of th otor. P control algorith is takn in th sp controllr oul, th spcific for of control of th sir angular vlocity i is as follows: i k P i i ( Whr k P i nots th proportional cofficint. 3 SIMULAION OF CONROL ALGORIHM W choos th MALAB/SIMULINK softwar as th flight control algorith of th airon gophysical xploration platfor siulation platfor. Bas on th siplifi ol of th platfor an th sign clos-loop PID controllrs, siuliat th platfor control syst on SIMULINK platfor, th platfor siulation oul shown in g.3. g.3 Siulation Moul of th Platfor Syst a. Paratrs al of th Platfor 0.75 kg J X kg/s l 0.5 JY kg/s g 9.8 /s JZ kg/s N*s N*s Bfor siulation, firstly, w ust st initial stat an sir stination path of th platfor. h initial position is (0 0 0; th initial attitu angl is ( ; th sir targt position is ( ; th sir yaw angl is 0 ra. h constant paratrs using in siulation shown in a.. Aftr svral siulation an ugging tsts, th cofficints of th PID controllrs wr trin, th spcific valus shown in a.. a. Paratrs al of th PID Controllrs k PX kix 0.8 kdx. kpy kiy 0.8 kdy. kpz.5 kiz kdz 3 kp 5 ki 0. k D. kp 5 ki 0. k D. kp.5 ki 0.9 kd 0. 53

5 h attitu rspons curv of th platfor controllr is shown in g.. g.5 Position Rspons Curv of PID Controllr g. 5 shows th position siulation rsults. It is shown that th sign of th position control loop has ttr autoatic control aility. X, Y, Z rspons curv can quickly rach th sir targt position, an to aintain a stay stat. Whn in 6s Z position of th stat can achiv th sir targt hight, so th quick rspons fathr of th position control loop t th sign rquirnts. h controllr works wll. g. Attitu Rspons Curv of PID Controllr gur shows th attitu siulation rsults. It is shown that th sign of attitu control loop has ttr control ffct. Roll, pitch, yaw thr control channls can quickly rach a stay stat, an can aintain in a stal stat. i rspons of th control loop is fastr, ttr staility, an to t th syst's asic sign rquirnts. h controllr works wll. h position rspons curv of th platfor controllr is shown in g.. CONCLUSION Accoring to th using ackgroun of th airon gophysical xploration platfor, w sign th flight controllr of th platfor. rstly, th ntir platfor flight control syst is ivi into thr control loops: position control loop, attitu control loop an sp control loop, an as on th classical clos-loop PID control thory o th flying controllr sign. hn th MALAB/SIMULINK siulation platfor is us to siulat th platfor control syst, Accoring to th siulation rsults, th airon gophysical xploration platfor syst can staly fulfill its ission unr th sign PID controllr. REFERENCES Air M Y, Aass V. Moling of Quarotor Hlicoptr Dynaics[C]// Sart Manufacturing Application, 008. ICS- MA 008. Intrnational Confrnc on. 008: Do Nascinto D A, Rufino C Z, Olivira S C, t al. Moling an Dsign of an Autonoous Unann Arial Vhicl Quarotor Prototyp for Applications in Swar of Roots[C]// Rootics Syposiu an Latin Arican Rootics Syposiu (SBR-LARS, 0 Brazilian. IEEE, 0:

6 MD Schit. Siulation an Control of a Quarotor Unann Arial Vhivl[D]. Univrsity of Kntucky, 0. Mian A A, Daoo W. Nonlinar Flight Control Stratgy for an Unractuat Quarotor Arial Root[C]// Ntworking, Snsing an Control, 008. ICNSC 008. IEEE Intrnational Confrnc on. IEEE, 008: zkan B., Stoll J.B., Brgrs R., an Groach H.. Unann aircraft syst provs itslf as a gophysical asuring platfor for aroagntic survys [J]. rst Brak, 0, 9: