四旋翼飛行器程序

1旋翼自主飛行器（B題）摘要系統(tǒng)以R5F100LE作為四旋翼自主飛行器控制的核心，由電源模塊、電機調(diào)速控制模塊、傳感器檢測模塊、飛行器控制模塊等構(gòu)成。飛行控制模塊包括角度傳感器、陀螺儀，傳感器檢測模塊包括紅外障礙傳感器、超聲波測距模塊、TLS1401-LF模塊，瑞薩MCU綜合飛行器模塊和傳感器檢測模塊的信息，通過控制4個直流無刷電機轉(zhuǎn)速來實現(xiàn)飛行器的欠驅(qū)動系統(tǒng)飛行。在動力學(xué)模型的基礎(chǔ)上，將小型四旋翼飛行器實時控制算法分為兩個PID控制回路，即位置控制回路和姿態(tài)控制回路。測試結(jié)果表明系統(tǒng)可通過各個模塊的配合實現(xiàn)對電機的精確控制，具有平均速度快、定位誤差小、運行較為穩(wěn)定等特點。TOC\o"1-5"\h\z\o"CurrentDocument"1系統(tǒng)方案論證與控制方案的選擇 -2-\o"CurrentDocument"1.1地面黑線檢測傳感器 -2-\o"CurrentDocument"1.2電機的選擇與論證 -2-\o"CurrentDocument"1.3電機驅(qū)動方案的選擇與論證 -3-\o"CurrentDocument"2四旋翼自主飛行器控制算法設(shè)計 -3-四旋翼飛行器動力學(xué)模型 -3-PID控制算法結(jié)構(gòu)分析 -3-\o"CurrentDocument"3硬件電路設(shè)計與實現(xiàn) -5-飛行控制電路設(shè)計 -5-電源模塊 -6-電機驅(qū)動模塊 -6-傳感器檢測模塊 -7-\o"CurrentDocument"4系統(tǒng)的程序設(shè)計 -8-\o"CurrentDocument"5測試與結(jié)果分析 -9-5.1測試設(shè)備 -9-5.2測試結(jié)果 -9-\o"CurrentDocument"6總結(jié) -10-附錄A部分程序清單 -11-1系統(tǒng)方案論證與控制方案的選擇根據(jù)題目要求，對該系統(tǒng)的特點及其控制特性進(jìn)行了分析，進(jìn)行了幾種不同設(shè)計方案的比較。地面黑線檢測傳感器探測地面黑線的基本原理是：光線照射到路面并反射，由于黑線和白色地面對光的反射系數(shù)不同，所以可以根據(jù)接收到的反射光強弱來判斷黑線?？蓪崿F(xiàn)的方案有：方案一：采用普通發(fā)光二極管及光敏電阻組成的發(fā)射接收方案。該方案在實際使用時，容易受到外界光源的干擾，有時甚至檢測不到。主要是因為可見光的反射效果跟地表的平坦程度、地表材料的反射情況均對檢測效果產(chǎn)生宜接影響。雖然可采取超高高度發(fā)光二極管降低一定的干擾，但這乂增加額外的功率損耗。方案二：紅外避障傳感器E18-D80NK。這是一種集發(fā)射與接收于一體的光電傳感器，發(fā)射光經(jīng)過調(diào)制后發(fā)出，接收頭對反射光進(jìn)行解調(diào)輸出，有效的避免了可見光的干擾。透鏡的使用，也使得這款傳感器最遠(yuǎn)可以檢測80厘米距離。檢測障礙物的距離可以根據(jù)要求通過尾部的電位器旋鈕進(jìn)行調(diào)節(jié)。并且具有探測距離遠(yuǎn)、受可見光干擾小、價格便宜、易于裝配、使用方便等特點。比較以上二種方案，方案二占有很大優(yōu)勢，不但能準(zhǔn)確完成測量，而且能避免電路的復(fù)雜性，因此選擇方案二。電機的選擇與論證四旋翼無人飛行器是通過控制四個不同無刷直流電機的轉(zhuǎn)速，達(dá)到控制四旋翼無人飛行器的飛行姿態(tài)和位置，與傳統(tǒng)宜升機通過控制舵機來改變螺旋槳的槳距角，達(dá)到控制直升機的目的不同。在電機的選型上，主要有直流有刷電機和直流無刷電機兩種。方案一：直流有刷電機是當(dāng)前普遍使用的一種直流電機，它的驅(qū)動電路簡單、控制方法成熟，但是直流有刷電機使用電刷進(jìn)行換向，換向時電刷與線圈觸電存在機械接觸，電機長時間高速轉(zhuǎn)動使極易因磨損導(dǎo)致電氣接觸不良等問題，而且有刷電機效率低、力矩小、重量大，不適合對功率重量比敏感的電動小型飛行器。方案二：直流無刷電機能量密度高、力矩大、重量輕，采用非接觸式的電子換向方法，消除了電刷磨損，較好地解決了直流有刷電機的缺點，適用于對功率重量比敏感的用途，同時增強了電機的可靠性。-2-所以選擇宜流無刷電機作為動力源。電機驅(qū)動方案的選擇與論證方案一:采用電阻網(wǎng)絡(luò)或數(shù)字電位器調(diào)整電動機的分壓，從而達(dá)到調(diào)速的目的。但是電阻網(wǎng)絡(luò)只能實現(xiàn)有級調(diào)速，而數(shù)字電阻的元器件價格比較昂貴。更主要的問題在于一般電動機的電阻很小，但電流很大；分壓不僅會降低效率，而且實現(xiàn)很困難。方案二:采用繼電器對電動機的開或關(guān)進(jìn)行控制，通過開關(guān)的切換對小車的速度進(jìn)行調(diào)整。這個方案的優(yōu)點是電路較為簡單，缺點是繼電器的響應(yīng)時間慢、機械結(jié)構(gòu)易損壞、壽命較短、可靠性不高。方案三:采用全橋驅(qū)動PWM電路。這種驅(qū)動的優(yōu)點是使管子工作在占空比可調(diào)的開關(guān)狀態(tài)，提高使用效率實現(xiàn)電機轉(zhuǎn)速的微調(diào)。并且保證了可以簡單的方式實現(xiàn)方向控制?；谏鲜隼碚摲治?，選擇方案三。2四旋翼自主飛行器控制算法設(shè)計四旋翼飛行器動力學(xué)模型設(shè)計的小型四旋翼飛行器適用于室內(nèi)低速飛行，因此忽略空氣阻力的影響。因此，簡化后的飛行器動力學(xué)模型為x=(sui0cos。cos(p+sin^sin(p)/m

y=%(sin6cos。sin。一sill。cos(p)/mZ=(%cos0sin(p-nig)/mO=II^UIY(f>=/I'z式中次y為四旋翼飛行器在導(dǎo)航坐標(biāo)系下的線位移，［工y之了為運動加速度，m為飛行器質(zhì)量，儀夕。分別為機體的偏航角、俯仰角和橫滾角，/為旋翼面中心到四旋翼飛行器質(zhì)心的距離，IXJYJZ為軸向慣性主矩。該動力學(xué)模型對四旋翼飛行器的真實飛行狀態(tài)進(jìn)行了合理的簡化，忽略了空氣阻力等對系統(tǒng)運行影響較小的參數(shù)，使得飛行控制算法更加簡潔。PID控制算法結(jié)構(gòu)分析在動力學(xué)模型的基礎(chǔ)上，將小型四旋翼飛行器實時控制算法分為兩個控制回路，即位置-3-控制回路和姿態(tài)控制回路。算法結(jié)構(gòu)如圖B-1所示。圖2-1四旋翼飛行器控制算法結(jié)構(gòu)圖使用經(jīng)典PID控制算法實現(xiàn)位置控制回路和姿態(tài)控制回路。PID算法簡單可靠，理論體系完備，而且在長期的應(yīng)用過程中積攢了大量的使用經(jīng)驗，在飛行器位置和姿態(tài)控制應(yīng)用中具有良好的控制效果和較強的魯棒性，能提供控制量的較優(yōu)解?？刂苹芈钒?三個控制量，因此設(shè)計3個獨立的PLD控制器對位移進(jìn)行控制。根據(jù)PID控制器的原理，設(shè)砥分別為比例項、枳分項和微分項系數(shù)，有尤=kp(x-xd)+k^x-x^lt+kd(x-xd)4尸式1-2X=kp(z-Zd)+k.j(z-Zd)dt+kd^-td)其中，4,〉,d，Q為航姿參考系統(tǒng)測量到的加速度積分得到的位移量。姿態(tài)控制回路的作用是控制四旋翼飛行器的飛行姿態(tài)，使其實際姿態(tài)與設(shè)定的姿態(tài)一致。姿態(tài)控制回路有偏航角、俯仰角和橫滾角三個控制量，在此忽略三個通道之間的耦合效應(yīng)，設(shè)計3個獨立的PID控制器對每個量進(jìn)行獨立控制。,??1rA,iL=@,1丫/1 式1-3〃3=。乙根據(jù)PID控制器的原理，設(shè)女”心砥分別為比例項、積分項和微分項系數(shù)，有控制器方程如B-4。0=幺式0一%)+&J3一%十砥。(0-6)＜。=kpeg-a)+卜迫\(6-a)di+卜我(。-仇) 式1一4。=噎(。-?)+ & 4)其中比例項系數(shù)為3.3積分項系數(shù)為0.14微分項系數(shù)為3.2。-4-

3硬件電路設(shè)計與實現(xiàn)系統(tǒng)以R5F100LE單片機為核心，主要包括電源模塊、電機驅(qū)動模塊、飛行控制模塊、傳感器檢測等功能模塊，該系統(tǒng)硬件結(jié)構(gòu)框圖如圖2-2所示。陀螺儀 ?角度傳感器數(shù)據(jù)轉(zhuǎn)發(fā)控制器飛行控制模塊電源模塊鍵盤電子調(diào)速器電子調(diào)速器電子調(diào)速器—＞電子調(diào)速器陀螺儀 ?角度傳感器數(shù)據(jù)轉(zhuǎn)發(fā)控制器飛行控制模塊電源模塊鍵盤電子調(diào)速器電子調(diào)速器電子調(diào)速器—＞電子調(diào)速器圖2-2系統(tǒng)硬件結(jié)構(gòu)圖飛行控制電路設(shè)計飛行控制模塊是控制系統(tǒng)的核心部分。它在每個控制周期內(nèi)實時處理傳感器采集的數(shù)據(jù)和飛行器的姿態(tài)信息，完成PID控制的算法，得到四旋翼飛行器的姿態(tài)和位置信息，計算出控制量，轉(zhuǎn)化為相應(yīng)的控制信號經(jīng)驅(qū)動電路后驅(qū)動四個電機工作，保持四旋翼飛行器穩(wěn)定飛行。電路圖如圖2-3所示。-5-

:/上匚:三。=：K%s03VP?C4>Jq：3匚2：9kTSoTCOU)；：￡￡HTKF?03VP?10/R-CCha33S^l:二片廠二x：；:,二2加玉）eNJ毋K；Z3kTS3K；K3K；7SVP?2<SVFT?9S^s—.一■二；R”3二；二壯3NP?C44：7?工廠 -'%”匚;不泮」二；io苣T互正iTP120AXI：9P43P42TM4TO04P41T?-TOO-P40T00105t￡>ETP：24：<T2E：<C1XSP：23：<T：P：3-UCTMP122：CECCU：P121X15t￡GCvssIVSSOVDDEVDDO:/上匚:三。=：K%s03VP?C4>Jq：3匚2：9kTSoTCOU)；：￡￡HTKF?03VP?10/R-CCha33S^l:二片廠二x：；:,二2加玉）eNJ毋K；Z3kTS3K；K3K；7SVP?2<SVFT?9S^s—.一■二；R”3二；二壯3NP?C44：7?工廠 -'%”匚;不泮」二；io苣T互正iTP120AXI：9P43P42TM4TO04P41T?-TOO-P40T00105t￡>ETP：24：<T2E：<C1XSP：23：<T：P：3-UCTMP122：CECCU：P121X15t￡GCvssIVSSOVDDEVDDOUNWCE=nm=￡2NW6ENNMEnNWSslUNWZ￡yMniAW=NW-￡e-UMAWsNwof。二dormm號&In。'。記。KIHHsOSN-INWZQd08^-￡0SH0E￡INU_znm-xdsINHass:RL7&G13-LQFP65一23於三？MdlNlMd-850-SE-□星豆星SWOE:-2Q^S^O-￡-ODS-2WS￡oMcls--2s/&HNU2WKdOEINUZWE-EINUSWR□dsdOMMas-ud0M￡~2dP14-AXIlSP144P：05CKCCSCLCOPl：SXCTOOUtxDSDAOOPLSSOO。Txl?TOOLTxDP：3TxD2SO20P：4RxD2S^0SDA20P15SCK20SCU0P：tfT?：TOO：U^TP5Pl-T?2TO02P55必P53P52P51TCTP2SO11NTP：1311SDA1：-ffi?KiK^sni瑞薩MCU1:3-39S3_3?<II3t5?E?一：la:hui^B3jF-3■群圖2-3飛行控制器電路原理圖電源模塊電源由一塊11.lV2200ma的鋰電池（重量約為166克）供電，在由電調(diào)降壓給系統(tǒng)中的各個模塊供5V電壓并給電機提供電流，這樣可滿足可滿足各個小系統(tǒng)的電源要求。電機驅(qū)動模塊四電機驅(qū)動模塊根據(jù)中心控制模塊指令驅(qū)動各個電機到達(dá)指定轉(zhuǎn)速，將電機的速度通過測速反饋裝置反饋給飛行姿態(tài)控制模塊，控制無刷直流電機閉環(huán)控制轉(zhuǎn)速，從而控制飛行狀態(tài)，達(dá)到預(yù)期位置和姿態(tài)。通過電子調(diào)速器給電機提供電流，電子調(diào)速器模塊電路圖如圖2-4所示。-6-

二su—?■A?尸9HSB-H51啰一J二su—?■A?尸9HSB-H51啰一JMege?(L)pgs卷X3dG:aXXHadnQ￡Qdnd也D3a患ppGVGVpPR4M2o-zg計圖2-4電子調(diào)速器電流圖傳感器檢測模塊傳感器模塊是為四旋翼飛行器的飛行控制提供各種飛行參數(shù)的裝置，包括測量機身三軸角速率的陀螺儀、測量機身三軸線加速度的加速度傳感器、測量機身航向及姿態(tài)信息的羅盤,電機轉(zhuǎn)速檢測的測速傳感器、飛行高度傳感器和黑線檢測傳感器。其中角度傳感器和陀螺儀電路圖如圖2-5和2-6所示。-7-

4系統(tǒng)的程序設(shè)計本系統(tǒng)以瑞薩MCU為核心，采用C語言對單片機進(jìn)行編程。主程序主要起導(dǎo)向和決策的作用，系統(tǒng)的控制總流程圖如圖所示。系統(tǒng)包括延時子程序，電機轉(zhuǎn)速控制子程序，檢測子程序，副翼子程序。系統(tǒng)控制的總流程圖如圖2-7所示。部分程序清單見附錄A。

圖2-7系統(tǒng)主程序流程圖5測試與結(jié)果分析測試設(shè)備模擬區(qū)域：按題目要求設(shè)計。設(shè)備：秒表、卷尺。測試環(huán)境為室內(nèi)。測試結(jié)果使系統(tǒng)運行，參照題目的各項要求進(jìn)行多次實際定量測量，得到該系統(tǒng)的測量結(jié)果如表表B-1所示°-9-

表B-1測試功能、測試工具及實現(xiàn)的情況表具體功能要求實際測試結(jié)果基本四旋翼自主飛行器一鍵式啟動，從A區(qū)飛向B區(qū)，在B區(qū)降落并停機；飛行時間不大于45s可以滿足，飛行時間為15s部分四旋翼自主飛行器一鍵式啟動，從B區(qū)飛向A區(qū)，在A區(qū)降落并停機；飛行時間不大于45s可以滿足要求，飛行時間為17s發(fā)揮飛行器擺放在A區(qū)，飛行器下面擺放一薄鐵片，一鍵式啟動，飛行器拾取薄鐵片并起飛部分飛行器攜帶薄鐵片從示高線上方飛向B區(qū)，并在空中將薄鐵片投放到B區(qū)；飛行器從示高線上方返回A區(qū)，在A區(qū)降落并停機以上往返飛行時間不大于30s其他最為完成發(fā)揮部分但以實現(xiàn)其在一個水平高度前行經(jīng)測試，系統(tǒng)可以滿足基本要求和部分發(fā)揮部分，并具有穩(wěn)定前行的特點。6總結(jié)本次設(shè)計按照題目要求，采用模塊化的硬件和軟件設(shè)計方法，運用PID控制算法，成功地實現(xiàn)了飛行器尋黑帶自主飛行并定點降落的要求。具有飛行穩(wěn)定、運動迅速、接收信號靈敏等特點。完成了題目基本部分和部分發(fā)揮部分的要求。附錄A部分程序清單,，***********************************************************************************************************************DISCLAIMERThissoftwareissuppliedbyRenesasElectronicsCorporationandisonlyintendedforusewithRenesasproducts.Nootherusesareauthorized.TinssoftwareisownedbyRenesasElectronicsCorporationandisprotectedunderallapplicablelaws,includmgcopyrightlaws.THISSOFTWAREISPROVIDED"ASIS”ANDRENESASMAKESNOWARRANTIESREGARDINGTHISSOFTWARE,WHETHEREXPRESS,IMPLIEDORSTATUTORY,INCLUDINGBUTNOTLIMITEDTOWARRANTIESOFMERCHANTABILITY.FITNESSFORAPARTICULARPURPOSEANDNON-INFRINGEMENT.ALLSUCHWARRANTIESAREEXPRESSLYDISCLAIMED.TOTHEMAXIMUMEXTENTPERMITTEDNOTPROHIBITEDBYLAW,NEITHERRENESASELECTRONICSCORPORATIONNORANYOFITSAFFILIATEDCOMPANIESSHALLBELIABLEFORANYDIRECTINDIRECT.SPECIAL,INCIDENTALORCONSEQUENTIALDAMAGESFORANYREASONRELATEDTOTHISSOFTWARE,EVENIFRENESASORITSAFFILIATESHAVEBEENADVISEDOFTHEPOSSIBILITYOFSUCHDAMAGES.Renesasreseivestheright,withoutnotice,tomakechangestotillssofhvareandtodiscontinuetheavailabilityoftillssoftwaie.Byusingtinssoftware,youagreetotlieadditionaltermsandconditionsfoundbyaccessingthefollowinglink:http://www.renesas.coin/disclainiei**Copyright(C)2011,2013RenesasElectronicsCoiporation.Allrightsreseived.***********************************************************************************************************************/,，************************************************************************************************************************FileName*VersionDevice(s)Tool-Chain:r_cg_timer_user.c*FileName*VersionDevice(s)Tool-Chain:r_cg_timer_user.c:CodeGeneiatorfbrRL78/G13V2.00.00.07[22Feb2013]:R5F100LE:CA78K0RDescription:ThisfileunplementsdevicedriverfbrTAUmodule.DescriptionCreationDate:2013/9/7***********************************************************************************************************************,,，***********************************************************************************************************************Pragmaduective***********************************************************************************************************************/-11-#pragmamtenuptINTTMOOr_tauO_channelO_inteiTuptpiagmamtenuptINTTMO1i_tauO_chaimel1_intennptpiagmamtenuptINTTM02i_tau0_channel2_intemiptpiagmamtenuptINTTMO3i_tauO_channel3_intemiptpiagmamtenuptINTTM04i_tau0_channel4_intemiptpiagmamtenuptINTTMO5i_tauO_channel5_intemiptpragmainterruptINTTM06i_tauO_channel6_inteiTuptpiagmamtenuptINTTM07i_tau0_channel7_intemipt/*Startusercodeforpragma.Donoteditcommentgeneratedhere*//*Endusercode.Donoteditcommentgeneratedhere*/,，***********************************************************************************************************************Includes***********************************************************************************************************************/includenr_cg_macrodiiveEhnincludenr_cg_timei-.hH/*Startusercodeforinclude.Donoteditcommentgeneratedhere*//*Endusercode.Donoteditconunentgeneratedhere*/includenr_cg_userdefine.hH#includenr_cg_kongzhi.hH并includenr_cg_lcd.hnJ***********************************************************************************************************************Globalvariablesandfiinctions***********************************************************************************************************************//*ForTAUO_chOpulsemeasurement*/volatileumt32_tg_tauO_chO_width=OU;/*Startusercodeforglobal.Donoteditcommentgeneratedhere*/uiiit32_toko=0;/*Endusercode.Donoteditcommentgeneratedhere*/,，***********************************************************************************************************************FunctionName:i_tauO_chaniielO_inten-uptDescription:ThisfimctionisINTTMOOmtenuptsendeeloutine.Aiguments:NoneReturnValue:None***********************************************************************************************************************/—mtenuptstaticvoidi_tauO_chaiinelO_interi-upt(void)12if((TSROO&_0001_TAU_OVERFLOW_OCCURS)==1U) /*oveiilowoccurs*/g_tauO_chO_width=(uint32_t)(TDR00+1U)+OxlOOOOU;}else(g_tauO_chO_width=(uint32_t)(TDR00+1U);}/*Startusercode.Donoteditconunentgeneratedhere*//*Endusercode.Donoteditcoininentgeneratedhere*/},，***********************************************************************************************************************FunctionName:oauO-chamielCmtemiptDescription:ThisfiinctionisINTTM01mtenuptserviceroutine.Aiguments:NoneReturnValue:None***********************************************************************************************************************/—intenuptstaticvoidi_tauO_chamielIjnten-upt(void)/*Startusercode.Donoteditconunentgeneratedhere*/TDR05=2980;delayms(500);TDR05=3025;delayms(lOO);TDR05=2980;delayms(500);TDR05=3040;delayms(50);oko++;if(oko++>=10){R_TAUO_Chaimell_Stop();z(2040);}//xiaiisliiQ;/*Endusercode.Donoteditcommentgeneratedhere*/},，***********************************************************************************************************************FunctionName:i_tau0_channel2_inteiruptDescription:ThisfiinctionisINTTM02mtenuptserviceloutiiie.Aiguments:NoneReturnValue:None-13-***********************************************************************************************************************/—intenuptstaticvoidi_tau0_chaiinel2_inten-upt(void)(/*Startusercode.Donoteditconunentgeneratedhere*//*Endusercode.Donoteditcoimnenteeneratedhere*/J},，***********************************************************************************************************************FunctionName:i_tau0_channel3_intern】ptDescription:ThisfimctionisINTTM03iiitemiptserviceroutine.Aiguments:NoneReturnValue:None***********************************************************************************************************************/—intenuptstaticvoidi_tau0_chaiinel3_inten-upt(void)(/*Startusercode.Donoteditconunentgeneratedhere*//*Endusercode.Donoteditcoimnenteeneratedhere*/J},，***********************************************************************************************************************FunctionName:i_tau0_channel4_intem】ptDescription:ThisfimctionisINTTM04iiitemiptserviceroutine.Aiguments:NoneReturnValue:None***********************************************************************************************************************/—intenuptstaticvoidi_tau0_chaiinel4_inten-upt(void)(/*Startusercode.Donoteditconunentgeneratedhere*//*Endusercode.Donoteditcoimnenteeneratedhere*/J},，***********************************************************************************************************************FunctionName:i_tau0_chamiel5_iiitemiptDescription:ThisfimctionisINTTM05iiitemiptsendeeloutine.Aiguments:NoneReturnValue:None******************************************************************************************