四旋翼飛行器程序

1、1旋翼自主飛行器（B題）摘要系統(tǒng)以R5F100LE作為四旋翼自主飛行器控制的核心，由電源模塊、電機(jī)調(diào)速控制模塊、傳感器檢測(cè)模塊、飛行器控制模塊等構(gòu)成。飛行控制模塊包括角度傳感器、陀螺儀，傳感器檢測(cè)模塊包括紅外障礙傳感器、超聲波測(cè)距模塊、TLS1401-LF模塊，瑞薩MCU綜合飛行器模塊和傳感器檢測(cè)模塊的信息，通過控制4個(gè)直流無刷電機(jī)轉(zhuǎn)速來實(shí)現(xiàn)飛行器的欠驅(qū)動(dòng)系統(tǒng)飛行。在動(dòng)力學(xué)模型的基礎(chǔ)上，將小型四旋翼飛行器實(shí)時(shí)控制算法分為兩個(gè)PID控制回路，即位置控制回路和姿態(tài)控制回路。測(cè)試結(jié)果表明系統(tǒng)可通過各個(gè)模塊的配合實(shí)現(xiàn)對(duì)電機(jī)的精確控制，具有平均速度快、定位誤差小、運(yùn)行較為穩(wěn)定等特點(diǎn)。- TOC o 1-

2、5 h z1系統(tǒng)方案論證與控制方案的選擇-2-1.1地面黑線檢測(cè)傳感器-2-1.2電機(jī)的選擇與論證-2-1.3電機(jī)驅(qū)動(dòng)方案的選擇與論證-3-2四旋翼自主飛行器控制算法設(shè)計(jì)-3-2.1四旋翼飛行器動(dòng)力學(xué)模型-3-2.2PID控制算法結(jié)構(gòu)分析-3-3硬件電路設(shè)計(jì)與實(shí)現(xiàn)-5-3.1飛行控制電路設(shè)計(jì)-5-3.2電源模塊-6-3.3電機(jī)驅(qū)動(dòng)模塊-6-3.4傳感器檢測(cè)模塊-7-4系統(tǒng)的程序設(shè)計(jì)-8-5測(cè)試與結(jié)果分析-9-5.1測(cè)試設(shè)備-9-5.2測(cè)試結(jié)果-9-6總結(jié)-10-附錄A部分程序清單-11- -1系統(tǒng)方案論證與控制方案的選擇根據(jù)題目要求，對(duì)該系統(tǒng)的特點(diǎn)及其控制特性進(jìn)行了分析，進(jìn)行了兒種不同設(shè)計(jì)方案

3、的比較。1.1地面黑線檢測(cè)傳感器探測(cè)地面黑線的基本原理是：光線照射到路面并反射，由于黑線和白色地面對(duì)光的反射系數(shù)不同，所以可以根據(jù)接收到的反射光強(qiáng)弱來判斷黑線。可實(shí)現(xiàn)的方案有：方案一：釆用普通發(fā)光二極管及光敏電阻組成的發(fā)射接收方案。該方案在實(shí)際使用時(shí)，容易受到外界光源的干擾，有時(shí)共至檢測(cè)不到。主要是因?yàn)榭梢姽獾姆瓷湫Ч乇淼钠教钩潭?、地表材料的反射情況均對(duì)檢測(cè)效果產(chǎn)生直接影響。雖然可采取超高高度發(fā)光二極管降低一定的干擾，但這乂增加額外的功率損耗。方案二：紅外避障傳感器E18-D80NKo這是一種集發(fā)射與接收于一體的光電傳感器，發(fā)射光經(jīng)過調(diào)制后發(fā)出，接收頭對(duì)反射光進(jìn)行解調(diào)輸出，有效的避免了可見

4、光的干擾。透鏡的使用，也使得這款傳感器最遠(yuǎn)可以檢測(cè)80厘米距離。檢測(cè)障礙物的距離可以根據(jù)要求通過尾部的電位器旋鈕進(jìn)行調(diào)節(jié)。并且具有探測(cè)距離遠(yuǎn)、受可見光干擾小、價(jià)格便宜、易于裝配、使用方便等特點(diǎn)。比較以上二種方案，方案二占有很大優(yōu)勢(shì)，不但能準(zhǔn)確完成測(cè)量，而且能避免電路的復(fù)雜性，因此選擇方案二。1.2電機(jī)的選擇與論證四旋翼無人飛行器是通過控制四個(gè)不同無刷直流電機(jī)的轉(zhuǎn)速，達(dá)到控制四旋翼無人飛行器的飛行姿態(tài)和位置，與傳統(tǒng)直升機(jī)通過控制舵機(jī)來改變螺旋槳的槳距角，達(dá)到控制直升機(jī)的目的不同。在電機(jī)的選型上，主要有直流有刷電機(jī)和直流無刷電機(jī)兩種。方案一：直流有刷電機(jī)是當(dāng)前普遍使用的一種直流電機(jī)，它的驅(qū)動(dòng)電路簡(jiǎn)

5、單、控制方法成熟，但是直流有刷電機(jī)使用電刷進(jìn)行換向，換向時(shí)電刷與線圈觸電存在機(jī)械接觸，電機(jī)長(zhǎng)時(shí)間高速轉(zhuǎn)動(dòng)使極易因磨損導(dǎo)致電氣接觸不良等問題，而且有刷電機(jī)效率低、力矩小、重量大，不適合對(duì)功率重量比敏感的電動(dòng)小型飛行器。方案二：直流無刷電機(jī)能量密度高、力矩大、重量輕，采用非接觸式的電子換向方法，消除了電刷磨損，較好地解決了直流有刷電機(jī)的缺點(diǎn)，適用于對(duì)功率重量比敏感的用途，同時(shí)增強(qiáng)了電機(jī)的可靠性。所以選擇直流無刷電機(jī)作為動(dòng)力源。1.3電機(jī)驅(qū)動(dòng)方案的選擇與論證方案一:采用電阻網(wǎng)絡(luò)或數(shù)字電位器調(diào)整電動(dòng)機(jī)的分壓，從而達(dá)到調(diào)速的目的。但是電阻網(wǎng)絡(luò)只能實(shí)現(xiàn)有級(jí)調(diào)速，而數(shù)字電阻的元器件價(jià)格比較昂貴。更主要的問題

6、在于一般電動(dòng)機(jī)的電阻很小，但電流很大；分壓不僅會(huì)降低效率，而且實(shí)現(xiàn)很困難。方案二:采用繼電器對(duì)電動(dòng)機(jī)的開或關(guān)進(jìn)行控制，通過開關(guān)的切換對(duì)小車的速度進(jìn)行調(diào)整。這個(gè)方案的優(yōu)點(diǎn)是電路較為簡(jiǎn)單，缺點(diǎn)是繼電器的響應(yīng)時(shí)間慢、機(jī)械結(jié)構(gòu)易損壞、壽命較短、可靠性不高。方案三:采用全橋驅(qū)動(dòng)PWM電路。這種驅(qū)動(dòng)的優(yōu)點(diǎn)是使管子工作在占空比可調(diào)的開關(guān)狀態(tài)，提高使用效率實(shí)現(xiàn)電機(jī)轉(zhuǎn)速的微調(diào)。并且保證了可以簡(jiǎn)單的方式實(shí)現(xiàn)方向控制?；谏鲜隼碚摲治?，選擇方案三。2四旋翼自主飛行器控制算法設(shè)計(jì)2.1四旋翼飛行器動(dòng)力學(xué)模型設(shè)計(jì)的小型四旋翼飛行器適用于室內(nèi)低速飛行，因此忽略空氣阻力的影響。因此，簡(jiǎn)化后的飛行器動(dòng)力學(xué)模型為x=u4(sm

7、0cos0cos(p+sin0sm(p)/my=u4(sin6cos0sin0-sill0cos(p)/m式1-1Z=(4cos0sin(p-nig)/mO=u2-l/IY(p=w3/；式中xy可丁為四旋翼飛行器在導(dǎo)航坐標(biāo)系下的線位移，戊y殳丁為運(yùn)動(dòng)加速度，m為飛行器質(zhì)量，久分別為機(jī)體的偏航角、俯仰角和橫滾角，/為旋翼面中心到四旋翼飛行器質(zhì)心的距離，IXJYJZ為軸向慣性主矩。該動(dòng)力學(xué)模型對(duì)四旋翼飛行器的真實(shí)飛行狀態(tài)進(jìn)行了合理的簡(jiǎn)化，忽略了空氣阻力等對(duì)系統(tǒng)運(yùn)行影響較小的參數(shù)，使得飛行控制算法更加簡(jiǎn)潔。2.2PID控制算法結(jié)構(gòu)分析在動(dòng)力學(xué)模型的基礎(chǔ)上，將小型四旋翼飛行器實(shí)時(shí)控制算法分為兩個(gè)控制回

8、路，即位置控制回路和姿態(tài)控制回路。算法結(jié)構(gòu)如圖B-1所示。圖2-1四旋翼飛行器控制算法結(jié)構(gòu)圖使用經(jīng)典PID控制算法實(shí)現(xiàn)位置控制回路和姿態(tài)控制回路。PID算法簡(jiǎn)單可靠，理論體系完備，而且在長(zhǎng)期的應(yīng)用過程中積攢了大量的使用經(jīng)驗(yàn)，在飛行器位置和姿態(tài)控制應(yīng)用中具有良好的控制效果和較強(qiáng)的魯棒性，能提供控制量的較優(yōu)解?？刂苹芈钒巳齻€(gè)控制量，因此設(shè)計(jì)3個(gè)獨(dú)立的PLD控制器對(duì)位移進(jìn)行控制。根據(jù)PID控制器的原理，設(shè)kp,kj,kd分別為比例項(xiàng)、枳分項(xiàng)和微分項(xiàng)系數(shù)，有X=kp(x-xd)+RJ(x-xdW+kd(x-xd)歹=心0-兒)+燈0-兒)+力(丿一九)式1-2X=kp(z-Zd)+k.j(z-Zd

9、)dt+kd-td)其中，兒，Zd為航姿參考系統(tǒng)測(cè)量到的加速度積分得到的位移量。姿態(tài)控制回路的作用是控制四旋翼飛行器的飛行姿態(tài)，使其實(shí)際姿態(tài)與設(shè)定的姿態(tài)一致。姿態(tài)控制回路有偏航角、俯仰角和橫滾角三個(gè)控制量，在此忽略三個(gè)通道之間的耦合效應(yīng)，設(shè)計(jì)3個(gè)獨(dú)立的PID控制器對(duì)每個(gè)量進(jìn)行獨(dú)立控制。坷=0IY/1式13根據(jù)PID控制器的原理，設(shè)kp,kkd分別為比例項(xiàng)、積分項(xiàng)和微分項(xiàng)系數(shù)，有控制器方程如E4皆kwW-(Pd)+kw-(pd)dt+kg(0_)ETP：24：T2E：C1XSP：23：CT1：_g.z39N7EGNP40Z：Z2NF43：3*I曲d3*IXTMGCvssEVSSOVDDEVD&3

10、丄TFTT.；-：c732、:-S2Hn7TUNWCE二ws!mFMNWRdCZ5EaNwrEmniAW=NW-e-DMAWsNWOEsKIXHsOTUlNWZQd08-OSUOEEINWZfunadMxd-dINUuM0zdRL7&G13-LQFP65-850-SE-2QSO-ovclm2H/&HNU2Kd-EsiEdsdovcmdows壬2dP14-AXIiaP144P：05CKCCSCLCOP：SXCTOOUtxDSDAOOPLSSOOOTxlTOOLTxDP：3TxD2SO20P：4RxD2S0SDA20P15SCK20SCU0P：tfT：TOO：UTP5Pl*T2TO02P55P54

11、P53P52P51UCTP2SO11UTP：UllSDAll3男31raws*EBHSSfl3：-3:二V:3：3:二T：5帯FMCU:二二；圖2-3飛行控制器電路原理圖3.2電源模塊電源由一塊11.lV2200ma的鋰電池（重量約為166克）供電，在由電調(diào)降壓給系統(tǒng)中的各個(gè)模塊供5v電壓并給電機(jī)提供電流，這樣可滿足可滿足各個(gè)小系統(tǒng)的電源要求。3.3電機(jī)驅(qū)動(dòng)模塊四電機(jī)驅(qū)動(dòng)模塊根據(jù)中心控制模塊指令驅(qū)動(dòng)各個(gè)電機(jī)到達(dá)指定轉(zhuǎn)速，將電機(jī)的速度通過測(cè)速反饋裝置反饋給飛行姿態(tài)控制模塊，控制無刷直流電機(jī)閉環(huán)控制轉(zhuǎn)速，從而控制飛行狀態(tài)，達(dá)到預(yù)期位置和姿態(tài)。通過電子調(diào)速器給電機(jī)提供電流，電子調(diào)速器模塊電路圖如圖2

12、-4所示。3.4傳感器檢測(cè)模塊傳感器模塊是為四旋翼飛行器的飛行控制提供各種飛行參數(shù)的裝置，包括測(cè)量機(jī)身三軸角速率的陀螺儀、測(cè)量機(jī)身三軸線加速度的加速度傳感器、測(cè)量機(jī)身航向及姿態(tài)信息的羅盤,電機(jī)轉(zhuǎn)速檢測(cè)的測(cè)速傳感器、飛行高度傳感器和黑線檢測(cè)傳感器。其中角度傳感器和陀螺儀電路圖如圖2-5和2-6所示。4系統(tǒng)的程序設(shè)計(jì)本系統(tǒng)以瑞薩MCU為核心，釆用C語言對(duì)單片機(jī)進(jìn)行編程。主程序主要起導(dǎo)向和決策的作用，系統(tǒng)的控制總流程圖如圖所示。系統(tǒng)包括延時(shí)子程序，電機(jī)轉(zhuǎn)速控制子程序，檢測(cè)子程序，副翼子程序。系統(tǒng)控制的總流程圖如圖2-7所示。部分程序清單見附錄A。- -圖2-7系統(tǒng)主程序流程圖5測(cè)試與結(jié)果分析5.1測(cè)

13、試設(shè)備模擬區(qū)域：按題目要求設(shè)計(jì)。設(shè)備：秒表、卷尺。測(cè)試環(huán)境為室內(nèi)。5.2測(cè)試結(jié)果使系統(tǒng)運(yùn)行，參照題目的各項(xiàng)要求進(jìn)行多次實(shí)際定量測(cè)量，得到該系統(tǒng)的測(cè)量結(jié)果如表表B-1所示。表B-1測(cè)試功能、測(cè)試工具及實(shí)現(xiàn)的情況表具體功能要求實(shí)際測(cè)試結(jié)果基本部分四旋翼自主飛行器一鍵式啟動(dòng)，從A區(qū)飛向B區(qū)，在B區(qū)降落并停機(jī)；飛行時(shí)間不大于45s可以滿足，飛行時(shí)間為15s四旋翼自主飛行器一鍵式啟動(dòng)，從B區(qū)飛向A區(qū)，在A區(qū)降落并停機(jī)；飛行時(shí)間不大于45s可以滿足要求，飛行時(shí)間為17s發(fā)揮部分飛行器擺放在A區(qū)，飛行器下面擺放一薄鐵片，一鍵式啟動(dòng)，飛行器拾取薄鐵片并起飛飛行器攜帶薄鐵片從示高線上方飛向B區(qū)，并在空中將薄鐵片

14、投放到B區(qū)；飛行器從示高線上方返回A區(qū)，在A區(qū)降落并停機(jī)以上往返飛行時(shí)間不大于30s其他最為完成發(fā)揮部分但以實(shí)現(xiàn)其在一個(gè)水平高度前行經(jīng)測(cè)試，系統(tǒng)可以滿足基本要求和部分發(fā)揮部分，并具有穩(wěn)定前行的特點(diǎn)。6總結(jié)本次設(shè)計(jì)按照題目要求，采用模塊化的硬件和軟件設(shè)計(jì)方法，運(yùn)用PID控制算法，成功地實(shí)現(xiàn)了飛行器尋黑帶自主飛行并定點(diǎn)降落的要求。具有飛行穩(wěn)定、運(yùn)動(dòng)迅速、接收信號(hào)靈敏等特點(diǎn)。完成了題目基本部分和部分發(fā)揮部分的要求。 - 附錄A部分程序清單嚴(yán)*DISCLAIMERThissoftwareissuppliedbyRenesasElectioiiicsCorporationandisonlyintende

15、dforusewithRenesasproducts.Nootherusesareauthorized.TinssoftwareisownedbyRenesasElectronicsCorporationandisprotectedunderallapplicablelaws,includmgcopyrightlaws.THISSOFTWAREISPROVIDEDASIS”ANDRENESASMAKESNOWARRANTIESREGARDINGTHISSOFTWARE,WHETHEREXPRESS,IMPLIEDORSTATUTORY,INCLUDINGBUTNOTLIMITEDTOWARRA

16、NTIESOFMERCHANTABILITY.FITNESSFORAPARTICULARPURPOSEANDNON-INFRINGEMENT.ALLSUCHWARRANTIESAREEXPRESSLYDISCLAIMED.TOTHEMAXIMUMEXTENTPERMITTEDNOTPROHIBITEDBYLAWNEITHERRENESASELECTRONICSCORPORATIONNORANYOFITSAFFILIATEDCOMPANIESSHALLBELIABLEFORANYDIRECTINDIRECT.SPECIAL,INCIDENTALORCONSEQUENTIALDAMAGESFORA

17、NYREASONRELATEDTOTHISSOFTWARE,EVENIFRENESASORITSAFFILIATESHAVEBEENADVISEDOFTHEPOSSIBILITYOFSUCHDAMAGES.Renesasreseivestheright,withoutnotice,tomakechangestotliissofhvareandtodiscontinuetheavailabilityoftliissoftwaie.Byusingtinssoftware、youagreetotlieadditionaltermsandconditionsfoundbyaccessingthefol

18、lowinglink: HYPERLINK http:/www.Tenesas.coin/disclaiin%e4%ba%a9 http:/www.Tenesas.coin/disclaiin畝*Copyright(C)2011,2013RenesasElectronicsCoiporation.Allrightsreseived.*/J*FileName*VersionDevice(s)Tool-Chain:r_cg_timer_user.c:CodeGeneiatorforRL78/G13V2.00.00.0722Feb2013:R5F100LE:CA78K0RDescriptionCre

19、ationDate:2013/9/7:ThisfileunplementsdevicedriverforTAUmodule.*,J*Pragmaduective*,#piagmamtenuptINTTMOOi_tauO_chann亡10_intemipt#piagmamtenuptINTTMO1i_tauOchaimel1_intennpt#piagmamtenuptINTTM02i_tauO_chann亡12_int“Tupt#piagmamtermptINTTMO3OauOchaimeBmtenupt#piagmamtenuptINTTM04i_tauOchaimeUinteniipt#p

20、iagmamtenuptINTTM05OauOchaimebinteniipt#piagmamtenuptINTTM06i_tauO_chann亡16_intemipt#piagmamtenuptINTTM07i_tauO_chann亡17_int“Tupt/*StartusercodeforpragmaDonoteditcommentgeneratedhere*/*EndusercodeDonoteditcommentgeneratedhere*/嚴(yán)*Includes*/#iiicludenr_cg_macrodiiveEhn#iiicludenr_cg_timeEhn/*Startuser

21、codeforincludeDonoteditcommentgeneratedhere*/*EndusercodeDonoteditconunentgeneratedhere*/#iiicludenr_cg_userdefine.hH#iiicludenr_cg_kongzhi.hH幷inclucknr_cg_lcd.hnJ*Globalvariablesandfiinctions*/*ForTAUO_chOpulsemeasurement*/volatileumt32_tjtauO_chO_width=OU;/*Startusercodeforglobal.Donoteditcommentg

22、eneratedhere*/uiiit32_toko=0;/*EndusercodeDonoteditcommentgeneratedhere*/嚴(yán)*FunctionName:i_tauO_chamielO_interRiptDescriptionAiguments:ThisfiinctionisINTTMOOmtermptsendeeloutine.:None*ReturnValue:None*/lntenuptstaticvoidi_tauO_chaiinelO_interi-upt(void)- -if(TSROO&_0001_TAU_OVERFLOW_OCCURS)=1U)/*ovei

23、ilowoccurs*/g_tauO_chO_width=(umt32_t)(TDR00+1U)+OxlOOOOU;elseg_tauO_chO_width=(uint32_t)(TDR00+1U);/*StartusercodeDonoteditconunentgeneratedhere*/*EndusercodeDonoteditcoininentgeneratedhere*/*FunctionName:i_tauO_chamiell_iiitemiptDescriptionAiguments:ThisfxmctionisINTTM01iiitemiptsendeeloutine.:Non

24、e*ReturnValue:None*/intenuptstaticvoidi_tauO_chamiell_interi-upt(void)/*StartusercodeDonoteditconunentgeneratedhere*/TDR05=2980;delayms(500);TDR05=3025;delayms(lOO);TDR05=2980;delayms(500);TDR05=3040;delayms(50);oko+;if(oko+=10)R_TAU0_ChaiiMll_Stop();z(2040);/xiaiisliiQ;/*EndusercodeDonoteditcomment

25、generatedhere*/*FunctionName:i_tau0_chamiel2_mtemiptDescription:ThisfiinctionisINTTM02intenuptsendeeloutiiie.Aiguments:NoneReturnValue:None*/mteniiptstaticvoidi_tau0_chaiinel2_inten-upt(void)/*StartusercodeDonoteditconunentgeneratedhere*/*EndusercodeDonoteditcoimnenteeneiatedhere*/*FunctionName:i_ta

26、u0_chaniiel3_inteniiptDescriptionAiguments:ThisfimctionisINTTM03iiitemiptsendeeroutine.:None*ReturnValue:None*/mteniiptstaticvoidi_tau0_chaiinel3_inten-upt(void)/*StartusercodeDonoteditconunentgeneratedhere*/*EndusercodeDonoteditcoimnenteeneiatedhere*/*FunctionName:i_tau0_chamiel4_iiitemiptDescriptionAiguments:ThisfijnetionisINTTM04mteniiptsendeeroutine.:None*ReturnValue:None*/mteniiptstaticvoidi_tau0_chaiinel4_inten-upt(void)/*StartusercodeDonoteditconunentgeneratedhere*/*EndusercodeDonoteditcoimnenteeneiatedhere*/*FunctionN