畢業(yè)設(shè)計——四旋翼自主飛行器設(shè)計[共80頁]

分類號 V249 單位代碼 11395 密 級 學(xué) 號 1005270133 學(xué)生畢業(yè)設(shè)計（論文）題 目四旋翼自主飛行器設(shè)計作 者劉赟躍院 (系)能源工程學(xué)院專 業(yè)測控技術(shù)與儀器指導(dǎo)教師王晶答辯日期2014 年 5 月 24 日榆 林 學(xué) 院畢業(yè)設(shè)計（論文）誠信責(zé)任書本人鄭重聲明：所呈交的畢業(yè)設(shè)計（論文），是本人在導(dǎo)師的指導(dǎo)下獨立進行研究所取得的成果。畢業(yè)設(shè)計（論文）中凡引用他人已經(jīng)發(fā)表或未發(fā)表的成果、數(shù)據(jù)、觀點等，均已明確注明出處。盡我所知，除文中已經(jīng)注明引用的內(nèi)容外，本論文不包含任何其他個人或集體已經(jīng)公開發(fā)表或撰寫過的研究成果。對本文的研究做出重要貢獻的個人和集體，均已在文中以明確方式標(biāo)明。本人畢業(yè)設(shè)計（論文）與資料若有不實，愿意承擔(dān)一切相關(guān)的法律責(zé)任。 論文作者簽名: 年 月 日榆林學(xué)院本科畢業(yè)設(shè)計（論文）摘 要四旋翼飛行器是能夠垂直起降、多旋翼的飛行器。與傳統(tǒng)旋翼式飛行器不同的是其在飛行中四只旋翼可以相互抵消掉反扭力矩，而不需要反扭矩槳，使其結(jié)構(gòu)更緊湊、產(chǎn)生更大升力。同時因其飛行空間小、效率高、飛行穩(wěn)定、易控制等優(yōu)點，使之在軍事、民用方面有廣闊的前景。本設(shè)計以16位瑞薩(Renesas)單片機RL78/G13R5F100LEA作為控制核心，通過電子調(diào)速器控制、驅(qū)動飛行器的四個直流電機，利用三軸角速度傳感器、三軸加速度傳感器組成飛行器飛行狀態(tài)檢測、控制模塊，采用紅外收發(fā)管實現(xiàn)飛行器飛行壁障、超聲波傳感器測距來控制、檢測飛行高度，以C語言、卡爾曼濾波及PID算法實現(xiàn)控制軟件的編寫從而實現(xiàn)低空飛行的四旋翼自主飛行器驗證機的設(shè)計。設(shè)計首先對四旋翼飛行器進行概述，分析四旋翼飛行器特點和發(fā)展趨勢。其次，根據(jù)設(shè)計要求進行系統(tǒng)方案設(shè)計，并完成總體結(jié)構(gòu)設(shè)計、硬件設(shè)計、軟件設(shè)計。最后，進行調(diào)試、驗證試飛。本設(shè)計從低成本、低功耗、高性能方面入手，完成了飛控系統(tǒng)硬件設(shè)計、制作、電路調(diào)試等實現(xiàn)了四旋翼自主飛行器的基本功能。關(guān)鍵詞：四旋翼飛行器；電子調(diào)速器；加速度傳感器；卡爾曼濾波；PIDDesign of Four-rotor Autonomous AircraftABSTRACTFour-rotor aircraft is a multi rotor aircraft, which is capable of vertical take-off and landing. Different from the traditional rotary wing aircraft four-rotor aircraft can be mutually offset anti torsion moment, without the need of anti torque propeller, making it more compact and can generate greater lift. At the same time, because of its advantages of high efficiency, small space flight, flight stability, easy control, which make it have a broad prospect in military, civilian.The design use the 16 bit Renesas RL78/G13 MCU R5F100LEA as control core, through the electronic speed control, driving the aircrafts four DC motors, make use of the three axis angular velocity sensor, three axis acceleration sensor as flight state detection and control module, using the infrared receiving tube to achieve flight obstacle avoidance, ultrasonic sensor to control the flying height, distance detection, using C language, Calman filter and PID algorithm to achieve the control software to realize the test machine design of four-rotor autonomous aircraft which can fly at low altitude. This design first gives an overview of the four-rotor aircraft, flight characteristics and the development trends of the four-rotor aircraft. Secondly, according to the design requirements design the system, and complete the overall structure design, hardware design, software design. Finally, debugging, flight-test and verification.From the aspects of low cost, low power, high performance, the design completed the flight control system hardware design, manufacture, circuit debugging and realized the basic functions of the four-rotor autonomous aircraft.Key words: Four-rotor aircraft; Electronic speed control; Acceleration sensor; Calman filter;PID73目 錄摘 要IABSTRACTII目 錄III1 緒論11.1本設(shè)計的目的及意義11.2國內(nèi)外發(fā)展研究現(xiàn)狀11.2.1國外發(fā)展研究現(xiàn)狀11.2.2國內(nèi)發(fā)展研究現(xiàn)狀21.3本設(shè)計的主要內(nèi)容21.4本章小結(jié)22 四旋翼自主飛行器總體設(shè)計32.1四旋翼自主飛行器飛行平臺32.2四旋翼自主飛行器的結(jié)構(gòu)及控制原理32.3本章小結(jié)73 四旋翼自主飛行器硬件設(shè)計93.1系統(tǒng)結(jié)構(gòu)分析93.2微控制系統(tǒng)模塊93.3飛行姿態(tài)檢測模塊103.3.1飛行姿態(tài)檢測傳感器MPU6050113.3.2 MPU6050與主機的通信133.4電機驅(qū)動模塊153.5超聲波測距模塊173.6紅外避障模塊183.7電源模塊183.8飛行控制模塊193.9本章小結(jié)194 四旋翼自主飛行器軟件設(shè)計214.1軟件總體設(shè)計214.2軟件開發(fā)平臺簡介234.2.1 CubeSuite+編譯軟件的介紹及操作方法244.2.2 Renesas Flash Programmer燒寫軟件的介紹及操作方法274.3飛行姿態(tài)解算算法及程序294.3.1加速度傳感器測量數(shù)據(jù)解算294.3.2陀螺儀測量數(shù)據(jù)解算304.4數(shù)據(jù)處理及程序304.4.1卡爾曼濾波與平均值濾波314.4.2卡爾曼濾波、平均值濾波程序324.5PID控制算法344.5.1數(shù)字PID位置型控制算法354.5.2數(shù)字PID增量型控制算法354.6本章小結(jié)365 四旋翼自主飛行器系統(tǒng)調(diào)試與實現(xiàn)375.1電子調(diào)速器對電機控制的調(diào)試375.2 MPU6050傳感器測量調(diào)試385.3超聲波測距調(diào)試395.4紅外避障調(diào)試405.5四旋翼自主飛行器整體調(diào)試415.6本章小結(jié)426 總結(jié)與展望436.1總結(jié)436.2展望43參考文獻45致 謝47附錄A 四旋翼自主飛行器原理圖49附錄B 四旋翼自主飛行器實物圖51附錄C 程序代碼531 緒論1.1本設(shè)計的目的及意義四旋翼自主飛行器是一種能夠垂直起降、多旋翼式的飛行器，其通過自帶電源驅(qū)動電機來提供動力。它在總體布局上屬于非共軸式碟形飛行器，與常規(guī)旋翼式飛行器相比，因其四只旋翼可相互抵消反扭力矩的優(yōu)點，而不需要專門的反扭矩槳從而使其結(jié)構(gòu)更為緊湊，能夠產(chǎn)生更大的升力。同時又因其具有靈活性高、要求的飛行空間小、能源利用率高、隱蔽性強以及安全性能高等優(yōu)勢，特別適合在近地面環(huán)境（如室內(nèi)、城區(qū)和叢林等）中執(zhí)行監(jiān)視、偵查等任務(wù)，其在軍事（電子戰(zhàn)）和民用（通信、氣象、災(zāi)害監(jiān)測）方面都有很大的應(yīng)用前景。另外，新穎的外形、簡單的結(jié)構(gòu)、低廉的成本、卓越的性能及獨特的飛行控制方式（通過控制四只旋翼的轉(zhuǎn)速實現(xiàn)飛行控制）使其對廣大科研人員具有很強的吸引力，成為國際上新的研究熱點1。1.2國內(nèi)外發(fā)展研究現(xiàn)狀1.2.1國外發(fā)展研究現(xiàn)狀早在1907年，由Breguet-Richht發(fā)明的世界上第一架四旋翼飛行器“Gyroplane No.1”升上了天空。目前世界上存在的四旋翼飛行器基本上都屬于微小型無人飛行器。國外的四旋翼飛行器因擁有悠久的科學(xué)文化歷史和研發(fā)團體機構(gòu)，加快了多旋翼飛行器的發(fā)展。國外的很多高校和研究機構(gòu)如Stanford University（美國的斯坦福大學(xué)）、University of Technology of Compiegne（法國貢比涅技術(shù)大學(xué)）等已經(jīng)展開了相關(guān)的研究并取得了一定的成果。另外國外一些商業(yè)公司也加入了四旋翼無人飛行器研究的行列，如美國Draganfly公司，德國Microdrones GmbH公司等，他們的加入使得四旋翼飛行器的發(fā)展得到了迅速的發(fā)展。其中Draganfly公司的Draganflyer飛行器已經(jīng)被很多高校采用為飛行器本體，進行實驗。20世紀(jì)末19世紀(jì)初期，貝爾波音公司將固定四旋翼直升機概念和傾轉(zhuǎn)旋翼概念進行了融合，設(shè)計出了新型的軍用飛行器，成功地實現(xiàn)了四旋翼飛行器新型機在軍事上的應(yīng)用，從而極大地提高了飛行器的作戰(zhàn)能力。21世紀(jì)，網(wǎng)絡(luò)信息時代飛速發(fā)展。美國Parrot AR Drone和蘋果公司推出了通過iphone手機藍牙、WiFi控制四旋翼飛行器飛行的新產(chǎn)品。歐美發(fā)達國家四旋翼飛行器已投入了商業(yè)、軍事領(lǐng)域，獲得了顯著的效果。1.2.2國內(nèi)發(fā)展研究現(xiàn)狀我國的四旋翼飛行器的發(fā)展仍然處于初期發(fā)展階段，由于缺乏獨立的核心技術(shù)，能應(yīng)用于專業(yè)領(lǐng)域的相關(guān)產(chǎn)品仍未大批量生產(chǎn)、投入使用。但是在國內(nèi)，一些研究機構(gòu)及高校對四旋翼飛行器的研究也取得了卓有成效的進展，其中有國防科技大學(xué)、北京航空航天大學(xué)、清華大學(xué)、浙江大學(xué)等，尤其是浙江大學(xué)的“玉泉之翼”實現(xiàn)了自主空中懸停1分鐘，雖然懸停時間還沒有外國的長，但是因為是完全自主研發(fā)的，因此在國內(nèi)具有里程碑的意義，同時也是我國四旋翼飛行器的最高水平。1.3本設(shè)計的主要內(nèi)容本設(shè)計基于實現(xiàn)低成本、低功耗、高性能的四旋翼飛行器驗證機，本文設(shè)計、開發(fā)一種以瑞薩(Renesas)RL78/G13系列單片機為核心的四旋翼自主飛行器。具體分為飛行器的硬件結(jié)構(gòu)設(shè)計、電路制作測試以及飛行控制底層軟件程序的設(shè)計，利用單片機輸出PWM（Pulse Width Modulation）通過電子調(diào)速器控制四旋翼飛行器的四個電機的轉(zhuǎn)速使飛行器完成各種基本動作，并通過角度傳感器、加速度傳感器、卡爾曼濾波算法、PID算法編程實現(xiàn)飛行器自主調(diào)節(jié)平衡，從而實現(xiàn)四旋翼自主飛行器的自主起飛、爬升、前進、轉(zhuǎn)彎、避障、降落等四旋翼飛行器的基本動作，利用CubeSuite+、Renesas Flash Programmer軟件分別對單片機編程、下載。最終實現(xiàn)四通道能夠開發(fā)、拓展其他更多功能的四旋翼自主飛行器。1.4本章小結(jié)本章主要介紹了四旋翼自主飛行器的發(fā)展歷史、發(fā)展趨勢及本設(shè)計的內(nèi)容、目的。加深了對四旋翼飛行器的應(yīng)用、發(fā)展方向及前景的了解，明確了本設(shè)計的重點及難點，有利于作好設(shè)計、研究準(zhǔn)備，同時也使自己對飛行器有了更多的了解，為完成本設(shè)計打下了堅實的基礎(chǔ)。2 四旋翼自主飛行器總體設(shè)計2.1四旋翼自主飛行器飛行平臺四旋翼飛行器按照四只旋翼和機架布置的方式其飛行控制平臺（機架）可以分為十字模式和X模式。X模式比十字模式靈活，但是對于姿態(tài)測量和控制的算法編程來說，十字模式較X模式簡單，更容易實現(xiàn)。X模式通過同時控制兩對旋翼轉(zhuǎn)速的大小來實現(xiàn)飛行控制及姿態(tài)的調(diào)整，而十字模式只要同時控制一對旋翼的轉(zhuǎn)速就能實現(xiàn)相應(yīng)的飛行動作。十字模式容易操作，飛行平穩(wěn)，綜合考慮采用十字模式?？紤]到自己制作四旋翼飛行器平臺需要采購零部件、制作過程中瑣碎的機械結(jié)構(gòu)設(shè)計及評價制作出的飛行平臺的性價比。本設(shè)計決定采用購買的十字模式的飛行器機架，以便減少制作機架的費用、時間、降低成本，將大量的時間和精力放在本設(shè)計的重點部分上飛行器的硬件結(jié)構(gòu)和軟件的總體設(shè)計。2.2四旋翼自主飛行器的結(jié)構(gòu)及控制原理四旋翼自主飛行器是由安裝在十字型剛性結(jié)構(gòu)的四個電機作為驅(qū)動的飛行器?？刂破魍ㄟ^調(diào)節(jié)四個電機的轉(zhuǎn)速使四個旋翼間出現(xiàn)特定的轉(zhuǎn)速差從而實現(xiàn)飛行器的各種動作。由于四旋翼自主飛行器是通過增大或減小四只旋翼的轉(zhuǎn)速達到四個方向升力的變化進而控制飛行器的飛行姿態(tài)和位置的穩(wěn)定，相對于傳統(tǒng)的直升機少去了舵機調(diào)節(jié)平衡、控制方向，并且不用改變螺旋槳的槳距角，使得四旋翼自主飛行器更容易控制。但是四旋翼自主飛行器有六個狀態(tài)輸出，即是一種六自由度的飛行器，而它卻只有四個輸入，是一個欠驅(qū)動系統(tǒng)。也正是由于這個原因使得四旋翼自主飛行器非常適合在靜態(tài)及準(zhǔn)靜態(tài)的條件下飛行。四旋翼自主飛行器飛行控制系統(tǒng)由飛行控制器、各類測量傳感器裝置、驅(qū)動電機、被控對象（飛行器機體）等部分組成，如圖2-1。傳感器用來測量四旋翼自主飛行器的飛行姿態(tài)信息如俯仰角、傾斜角、偏航角等，這些信息經(jīng)過控制器處理后轉(zhuǎn)換成為能夠被控制系統(tǒng)識別和處理的有效、有用信息，飛行控制器依據(jù)飛行器姿態(tài)檢測傳感器反饋回來的飛行姿態(tài)信息及預(yù)先設(shè)定的飛行狀態(tài)進行計算、處理及控制，然后通過輸出PWM信號來控制電機的轉(zhuǎn)速進而調(diào)節(jié)或改變飛行姿態(tài)。圖2-1 四旋翼自主飛行器控制系統(tǒng)圖2-2 四旋翼自主飛行器模型俯視圖四旋翼自主飛行器的結(jié)構(gòu)如圖2-2，在螺旋槳1和螺旋槳3順時針旋轉(zhuǎn)的同時，螺旋槳2和螺旋槳4逆時針旋轉(zhuǎn)，當(dāng)飛行器平衡飛行時四個電機產(chǎn)生的反扭矩力大小相等方向兩兩相反進而兩兩相互抵消，相對于經(jīng)典的傳統(tǒng)直升機不需要專門的舵機來抵消反扭矩力。四旋翼自主飛行器有六個自由度（分別為沿3個軸作平移和旋轉(zhuǎn)動作），這六個自由度中的任意一個自由度的控制和調(diào)節(jié)都可以通過聯(lián)合調(diào)節(jié)四個電機的轉(zhuǎn)速進而改變四個螺旋槳的轉(zhuǎn)速來實現(xiàn)。其基本運動分別是：(1)垂直運動；(2)前后運動；(3)俯仰運動；(4)滾轉(zhuǎn)運動；(5)偏航運動；(6)側(cè)向運動。在圖2-3中電機1和電機3作順時針旋轉(zhuǎn)，電機2和電機4作逆時針旋轉(zhuǎn)，設(shè)飛行器沿X軸正方向運動為向前運動，箭頭向上表示電機轉(zhuǎn)速增大，箭頭向下表示電機轉(zhuǎn)速減小。圖2-3(a)表示垂直運動：在圖中兩對電機轉(zhuǎn)動方向相反，可以相互抵消掉旋轉(zhuǎn)產(chǎn)生的反扭矩力，當(dāng)同時增加四個電機的轉(zhuǎn)速使得旋翼的升力同時增大，當(dāng)升力足以克服整機的重力時，四旋翼自主飛行器便會離開地面垂直上升；反之，同時減小四個電機的轉(zhuǎn)速使旋翼的升力減小，四旋翼自主飛行器就會豎直下降，直到降落到地面，從而實現(xiàn)沿Z軸進行上下飛行的垂直運動。而當(dāng)外界擾動很小或為零時，四旋翼自主飛行器的四只旋翼產(chǎn)生的升力等于其自身重力時，飛行器便會保持一定高度不變，實現(xiàn)懸停。圖2-3(b)表示前后運動：欲使四旋翼自主飛行器能夠在水平面上前后、左右運動，必須對飛行器施加一個水平的力。在圖中電機3的轉(zhuǎn)速增大，電機1的轉(zhuǎn)速減小，電機2、4的轉(zhuǎn)速保持不變。為了使四旋翼自主飛行器不因旋翼轉(zhuǎn)速的改變而致使飛行器受力不均導(dǎo)致不平衡，要求電機1和電機3的轉(zhuǎn)速改變量大小相等，只有這樣才能使旋翼1和旋翼3產(chǎn)生的反扭矩力抵消，這樣四旋翼自主飛行器X軸稍微傾斜（旋翼1高度稍微低于旋翼3）旋翼1、3產(chǎn)生的升力便會產(chǎn)生一個指向X軸正方向的分力，使飛行器向X軸正方向運動實現(xiàn)前進。向后飛行與向前飛行正好相反。圖2-3(c)表示俯仰運動：在圖中增加電機1的轉(zhuǎn)速使旋翼提供的升力增大，相應(yīng)地減小電機3的轉(zhuǎn)速使旋翼的升力減小同樣的大小，同時保持其他兩個電機的轉(zhuǎn)速不變，只有這樣才能保持產(chǎn)生的反扭矩力相互抵消進而保持飛行器的平衡。由于旋翼1的升力增大旋翼3的升力減小，產(chǎn)生的飛行器上下不平衡的分力使飛行器繞Y軸旋轉(zhuǎn)（方向如圖），同理，電機1轉(zhuǎn)速減小電機3的轉(zhuǎn)速增大飛行器便繞著Y軸向另一個方向旋轉(zhuǎn)，實現(xiàn)俯仰運動。圖2-3(d)表示滾轉(zhuǎn)運動：與俯仰運動的原理相同，在圖d中相應(yīng)改變電機2、4的轉(zhuǎn)速而保持電機1、3的轉(zhuǎn)速不變則可以使飛行器繞X軸旋轉(zhuǎn)，從而實現(xiàn)四旋翼自主飛行器的滾轉(zhuǎn)運動。圖2-3(e)表示偏航運動：四旋翼自主飛行器的偏航運動可以通過四只旋翼之間產(chǎn)生的反扭矩力來實現(xiàn)，一般情況下四旋翼自主飛行器的為抵消掉產(chǎn)生的反扭矩力使兩對轉(zhuǎn)向相反的電機的轉(zhuǎn)速相同，此時，為了利用反扭矩力使電機1、3的轉(zhuǎn)速增大，電機2、4的轉(zhuǎn)速減小，此時旋翼1、3產(chǎn)生的對飛行器的反扭矩力大于旋翼2、4產(chǎn)生的反扭矩力，于是飛行器便在大的反扭矩力的作用下繞Z軸轉(zhuǎn)動，其轉(zhuǎn)向與電機1、3的轉(zhuǎn)向相反。圖2-3(f)表示側(cè)向運動：因為四旋翼自主飛行器的結(jié)構(gòu)完全對稱，所以側(cè)向運動的原理與前后運動的原理完全一樣。(a)垂直運動(b)前后運動(c)俯仰運動(d)滾轉(zhuǎn)運動(e)偏航運動(f)側(cè)向運動圖2-3 四旋翼自主飛行器的運動2.3本章小結(jié)本章主要介紹了四旋翼飛行器的飛行平臺、飛行器結(jié)構(gòu)、飛行控制原理。了解了四旋翼飛行器的飛行平臺、飛行方式、控制方式應(yīng)，明確了本設(shè)計需要怎樣著手、作哪些方面的研究、設(shè)計，為下面的設(shè)計打下理論基礎(chǔ)。3 四旋翼自主飛行器硬件設(shè)計3.1系統(tǒng)結(jié)構(gòu)分析四旋翼自主飛行器系統(tǒng)可分為微控制系統(tǒng)模塊、飛行姿態(tài)檢測模塊、電機驅(qū)動模塊、超聲波測距模塊、紅外避障模塊、電源模塊。其中微控制系統(tǒng)模塊采用RL78/G13系列單片機R5F100LEA作為主控芯片，該控制器是一款功能強大的16位處理器，具有運算速度快、擁有豐富的片上資源，非常適用于處理較復(fù)雜的任務(wù)。飛行姿態(tài)檢測模塊采用MPU6050芯片，此芯片集成了3軸MEMS陀螺儀，3軸MEMS加速度計，并通過三個16位的ADC將其測量到的模擬量轉(zhuǎn)化為可輸出且能夠被單片機直接處理的數(shù)字量，從而減輕了單片機的工作量。電機驅(qū)動模塊利用單片機輸出PWM波通過電子調(diào)速器驅(qū)動四個電機，并且通過調(diào)節(jié)PWM的占空比實現(xiàn)電機的調(diào)速。超聲波測距模塊利用超聲波的反射特性來測距離從而控制飛行器的高度。紅外避障模塊采用紅外光電開關(guān)，當(dāng)檢測到障礙物通過中斷使處理器響應(yīng)，從而實現(xiàn)避障。電源模塊采用7.4V的鋰電池供電。系統(tǒng)框圖如圖3-1。圖3-1系統(tǒng)框圖3.2微控制系統(tǒng)模塊微控制系統(tǒng)模塊是飛行控制系統(tǒng)的核心處理器，主要負責(zé)采集傳感器檢測到的姿態(tài)角速率（俯仰角速率、橫滾角速率和偏航角速率）、三軸線加速度和航向信息并實時解算、處理；根據(jù)檢測到的飛行姿態(tài)信息，結(jié)合預(yù)定要求的控制方案，計算輸出控制量控制電機轉(zhuǎn)速改變，進而保持或改變飛行狀態(tài)。四旋翼自主飛行器算法復(fù)雜且要求較高的處理速度，以便能及時處理飛行偏差、調(diào)節(jié)自身平衡，所以普通的單片機如51系列達不到要求。因此本設(shè)計使用瑞薩公司生產(chǎn)的RL78/G13系列單片機R5F100LEA，該芯片是功能強大的16位處理器，自帶10位12ch的ADC、強大的定時器，具有非常豐富的片上資源，能輸出多通道獨立占空比的PWM波，擁有高性能的外圍設(shè)備，采用引腳復(fù)用方式，既節(jié)省了引腳資源又拓展了功能，并且具有超低功耗，支持CSI 2ch、UART、IC 2ch串口通信，并且不需要EEPROM。能方便對電機進行控制，可以進行浮點型運算。并且還有精密的比較器，很大的RAM和ROM，能存儲比較大的程序段。該控制器采用64引腳LQFP封裝，單片機最小系統(tǒng)電路及引腳圖如圖3-2。圖3-2 RL78/G13-R5F100LEA單片機引腳圖3.3飛行姿態(tài)檢測模塊本設(shè)計的采用陀螺儀和加速度傳感器來檢測飛行姿態(tài)，為了避免陀螺儀和加速度傳感器安裝中可能出現(xiàn)的機械偏差，本設(shè)計采用集成兩者的MPU6050傳感器模塊。3.3.1飛行姿態(tài)檢測傳感器MPU6050飛行器正常飛行需要不斷地檢測自身的飛行狀態(tài)，然后根據(jù)自身姿態(tài)來調(diào)節(jié)自身平衡，從而實現(xiàn)穩(wěn)定飛行。四旋翼自主飛行器的姿態(tài)檢測主要就是檢測自身的傾斜度，目前比較成熟的就是慣性測量：利用陀螺儀或者加速度傳感器來測量傾角。大多數(shù)加速度傳感器是基于壓電效應(yīng)的原理來工作的。所謂壓電效應(yīng)，即某些電介質(zhì)在沿一定方向上受到外力的作用而變形時，其內(nèi)部會產(chǎn)生極化現(xiàn)象，同時在它的兩個相對表面上出現(xiàn)正負相反的電荷。通過測量這些電荷量，并將其轉(zhuǎn)化為電壓信號輸出，就完成了加速度傳感器的測量。加速度傳感器的工作原理就是把產(chǎn)生加速度的壓力轉(zhuǎn)化為電信號輸出，進而根據(jù)輸出的電信號利用數(shù)學(xué)方法計算出相應(yīng)的加速度。陀螺儀又稱為角速度傳感器，其工作原理是科里奧利力原理：科里奧利力來自于物體運動所具有的慣性，在旋轉(zhuǎn)體系中進行直線運動的質(zhì)點，由于慣性的作用，有沿著原有運動方向繼續(xù)運動的趨勢，但是由于體系本身是旋轉(zhuǎn)的，在經(jīng)歷了一段時間的運動之后，體系中質(zhì)點的位置會有所變化，而它原有的運動趨勢的方向，如果以旋轉(zhuǎn)體系的視角去觀察，就會發(fā)生一定程度的偏離。當(dāng)一個質(zhì)點相對于慣性系做直線運動時，相對于旋轉(zhuǎn)體系，其軌跡是一條曲線。立足于旋轉(zhuǎn)體系，我們認為存在一個力驅(qū)使質(zhì)點運動軌跡形成曲線，這個力就是科里奧利力。陀螺儀輸出三軸的角加速度然后通過對加速度積分便得到了角速度，然后再積分就得到了角度。陀螺儀的輸出不受飛行器震蕩影響，輸出比較精確，由于輸出角速度需要經(jīng)過積分才能得到角度，因此測量誤差也會通過積分不斷地積累最終使得小誤差積累成為較大的誤差，從而影響飛行器的可靠控制，并且陀螺儀只能感知飛行器姿態(tài)的改變，然后通過控制器阻止這種改變從而獲得平衡，它只能測量位置改變量而不能實現(xiàn)絕對定位自己的位置和姿態(tài)；加速度傳感器通過測量重力加速度并運用三軸間的加速度關(guān)系進而測量出傾斜角度，但是在飛行器飛行過程中電機轉(zhuǎn)動、飛行器震動都會使得測量到的加速度不是重力加速度而是合外力加速度，從而使測量誤差增大。綜合考慮陀螺儀和加速度傳感器的優(yōu)缺點決定采用兩者結(jié)合，形成互補，相互矯正。考慮到將陀螺儀和加速度傳感器更緊密地結(jié)合在一起進而盡可能減小誤差，本設(shè)計采用集陀螺儀和加速度傳感器于一體的MPU6050模塊。MPU6050是全球首例融合6軸運動處理的組件，整合了3軸陀螺儀、3軸加速器，相較于多組件方案，免除了組合陀螺儀與加速器時之軸間差的問題，減少了大量的包裝。MPU6050的角速度全格感測范圍為±250、±500、±1000與±2000°/sec (dps)，可準(zhǔn)確追蹤快速與慢速動作，并且，用戶可程式控制的加速器全格感測范圍為±2g、±4g±8g與±16g，輸出采用IC傳輸，內(nèi)部含內(nèi)建的溫度感測器、包含在運作環(huán)境中僅有±1%變動的振蕩器。其中陀螺儀和加速度傳感器分別用了三個16位的ADC，將測量的模擬量轉(zhuǎn)化為單片機能直接處理的數(shù)字量。其引腳圖、模型圖和電路圖分別如圖3-3、3-4所示。圖3-3 MPU6050引腳圖及模型圖圖3-4 MPU6050模塊電路圖3.3.2 MPU6050與主機的通信MPU6050通過IC與主機通信：（1）IC總線概述IC（Inter Integrated Circuit）總線是由Philips公司研制、開發(fā)的一種簡單的雙向兩線式串行總線，實現(xiàn)有效的IC之間的控制與通信。IC總線生產(chǎn)于20世紀(jì)80年代，最初為音頻和視頻設(shè)備開發(fā)的，如今主要在服務(wù)管理中使用，其中包括單個組件狀態(tài)的通信。（2）IC總線的特征1）IC總線只要求有兩條總線線路：一條串行數(shù)據(jù)線SDA（Serial Data Line）和一條串行時鐘線SCL（Serial Clock Line）。2）每個連接到總線的IC器件都可以通過唯一的地址和一直存在的主機/從機關(guān)系軟件設(shè)定地址，主機也可以作為主機發(fā)送器和主機接收器。3）IC總線是一個真正的多主機總線，如果有兩個或更多主機同時進行初始化，數(shù)據(jù)傳輸可以通過沖突檢查和仲裁來防止傳輸數(shù)據(jù)被破壞。4）IC串行的8位雙向傳輸數(shù)據(jù)位率在標(biāo)準(zhǔn)模式下可達到100Kbps，而在快速模式下可以達到400Kbps，高速模式下可達3.4Mbps。5）IC總線器件片上的濾波器可以濾去總線數(shù)據(jù)線SDA上可能出現(xiàn)的毛刺，能更好地保證數(shù)據(jù)完整。6）連接到同一總線上的IC數(shù)量僅只受到總線的最大電容400pF的限制。（3）開始（S）和結(jié)束（P）標(biāo)志IC接口包括串行數(shù)據(jù)線（SDA）和串行時鐘線（SCL）。連接到IC接口的設(shè)備可以作為主設(shè)備也可以作為從設(shè)備。主設(shè)備將從地址（Slave Address）傳到總線上，從設(shè)備通過與其匹配的地址來識別主設(shè)備。當(dāng)MPU6050與微處理器連接時，MPU6050作為從設(shè)備，微處理器作為主設(shè)備。其從地址為7位字長b110110X，最低有效位X由A0引腳的邏輯電平?jīng)Q定，本設(shè)計A0接地，所以設(shè)備地址為b1101100。當(dāng)時鐘線SCL為高電平時，數(shù)據(jù)線SDA由高到低時的下降沿，為傳輸開始標(biāo)志（S）。直到主設(shè)備發(fā)出結(jié)束信號（P），否則總線狀態(tài)一直處于忙狀態(tài)。結(jié)束標(biāo)志（P）規(guī)定為：當(dāng)時鐘線SCL為高電平時，數(shù)據(jù)線SDA由低到高時的上升沿。如圖3-5。圖3-5 開始停止條件（4）數(shù)據(jù)格式/應(yīng)答IC的傳輸數(shù)據(jù)字節(jié)為8bits長度，對每次傳送的總字節(jié)數(shù)量沒有限制。但是對每一次傳輸必須伴有一個應(yīng)答（ACK）信號，其時鐘信號由處理器提供，而真正的應(yīng)答信號則由MPU6050發(fā)出，在時鐘為高時，通過拉低并保持SDA線上的值來實現(xiàn)。如果從設(shè)備MPU6050處于忙狀態(tài)時，它可以使SCL線保持在低電平，這會強制使處理器進入等待狀態(tài)。當(dāng)MPU6050空閑后，同時釋放時鐘線時，原來的數(shù)據(jù)傳輸才會繼續(xù)進行。如圖3-6。圖3-6 MPU6050與主機握手方式（5）通信開始標(biāo)志（S）發(fā)出后處理器會傳送出一個7位的從地址，并在后面跟著一個第8位，該位被稱為Read/Write位。R/W位表示處理器是需要接收MPU6050的數(shù)據(jù)還是向其寫數(shù)據(jù)。之后，處理器釋放SDA線，等待MPU6050的應(yīng)答信號（ACK）。應(yīng)答信號每個字節(jié)的傳輸都要跟隨一個應(yīng)答位，應(yīng)答產(chǎn)生時，MPU6050將SDA線拉低并在SDA為高電平時保持為低。數(shù)據(jù)傳輸總是以停止標(biāo)志（P）終止，然后釋放通信線路。但是處理器也可以產(chǎn)生重復(fù)的開始信號去操作其他的IC設(shè)備，而不發(fā)出停止標(biāo)志。由此可知：除了開始和結(jié)束標(biāo)志，所有的SDA信號的變化都要在SCL時鐘線為低電平時進行。如圖3-7。圖3-7 MPU6050與主機數(shù)據(jù)傳輸時序圖如果要對MPU6050寄存器進行寫操作，處理器除了發(fā)出開始標(biāo)志（S）和地址位，還要加一個R/W讀寫位，0為寫，1為讀。在第9個時鐘周期（高電平）時，MPU6050產(chǎn)生一個應(yīng)答信號。然后處理器開始發(fā)送寄存器地址，接到MPU6050的應(yīng)答后開始傳送寄存器數(shù)據(jù)，之后仍需要有應(yīng)答信號，以此類推。如果要讀取MPU6050寄存器的值，首先要由處理器產(chǎn)生開始信號（S），然后再發(fā)送MPU6050的地址位和一個寫數(shù)據(jù)位，最后發(fā)送寄存器地址，只有這樣才能開始讀寄存器。接著，收到應(yīng)答信號后，處理器再發(fā)一個開始信號，然后再發(fā)送MPU6050地址位和一個讀數(shù)據(jù)位。然后，作為從設(shè)備的MPU6050產(chǎn)生應(yīng)答信號并開始發(fā)送其寄存器中的數(shù)據(jù)。它們之間的通信以處理器產(chǎn)生的拒絕應(yīng)答信號（NACK）和結(jié)束標(biāo)志（P）為結(jié)束標(biāo)志。拒絕應(yīng)答信號（NACK）的產(chǎn)生定義為SDA線上的數(shù)據(jù)在第9個時鐘周期中一直為高。3.4電機驅(qū)動模塊電機為四旋翼自主飛行器提供動力，通過控制電機轉(zhuǎn)速進而改變旋翼轉(zhuǎn)速從而實現(xiàn)飛行器的各種飛行運動。考慮到本設(shè)計的低成本特性及設(shè)計驗證機的目的，本設(shè)計采用價格低廉的直流有刷電機。直流電機在使用時需要在電動機的兩個接線端上加載上電壓，電壓的高低直接影響電動機的轉(zhuǎn)速，它們兩者之間的關(guān)系如下公式：式中 U加載在電動機兩端的直流電壓；I直流電動機的工作電流；R直流電動機線圈的等效內(nèi)阻；常數(shù)，與電機本身的結(jié)構(gòu)參數(shù)有關(guān)；每極總磁通。由于購買到直流電機后，電機的機械結(jié)構(gòu)已經(jīng)固定，勵磁部分為永久磁鐵，所以式中的R、和已經(jīng)固定，能夠改變的就只有加載在直流電機兩端的直流電壓。常見的電機電壓調(diào)節(jié)的方法有兩種：數(shù)模轉(zhuǎn)換器輸出法和PWM輸出法。由于數(shù)模轉(zhuǎn)換器大多是電流輸出型，需要外接運放才能將其轉(zhuǎn)換成電壓，另外由于運放輸出電流有限，如果直接連接到直流電動機會造成直流電動機的轉(zhuǎn)矩過小和運放過熱的現(xiàn)象對電機調(diào)速不利，所以采用PWM輸出法。PWM輸出法由單片機的I/O口作為PWM的輸出端，通過輸出信號控制大功率晶體管的開啟和關(guān)閉，以控制電動機的運轉(zhuǎn)和停止，當(dāng)PWM的頻率足夠高時，由于電動機的繞組是感性負載，具有儲能的作用，對PWM輸出的高低電平起到了平波的作用，在電動機的兩端可以得到近似直流的電壓值，因此PWM的占空比越高，電動機獲得的直流電壓越高，反之，PWM的占空比越低，電動機獲得的直流電壓將會越低。考慮到本設(shè)計是設(shè)計飛行器，需要電機有很高的轉(zhuǎn)速及簡便、低成本性，因此采用單片機輸出PWM信號通過專用的電子調(diào)速器來控制電動機。本設(shè)計采用好盈公司的HOBBYWING20A有刷電子調(diào)速器來驅(qū)動電機。HOBBYWING20A有刷電子調(diào)速器兼容鋰電池和鎳鎘或鎳氫兩類電池，通過其上面的跳帽可以選擇電池類型，并且它具有以下特點：（1）低電壓保護功能：鋰電池單體的低壓保護閾值為3V，因此對于2S 鋰電池組，低壓保護閾值為6V；對于3S 鋰電池組，低壓保護閾值為9V。鎳鎘/鎳氫電池組的保護電壓為4V。當(dāng)進入低壓保護后，電調(diào)將逐步降低輸出功率，來防止電池過度放電從而保護電池。（2）過溫保護：當(dāng)電調(diào)工作溫度超過 110 度時，電調(diào)會降低輸出功率進行保護，但不會將輸出功率全部關(guān)閉，最多只降到全功率的50%， 以保證電機仍有動力，避免摔機。溫度下降后，電調(diào)會逐漸恢復(fù)最大動力。 （3）油門信號丟失保護：當(dāng)檢測到油門遙控信號丟失2 秒后，電調(diào)開始降低輸出功率，如果信號始終無法恢復(fù)，則一直降到零輸出（降功率過程最長時間為2 秒）。如果在功率下降的過程中油門遙控信號重新恢復(fù)，則立即恢復(fù)油門控制，這樣做的好處是：在油門信號瞬間丟失的情況下（小于2 秒），電調(diào)并不會進行斷電保護；如果遙控信號確實長時間丟失，則進行保護，但也不是立即關(guān)閉輸出，而是有一個逐步降低輸出功率的過程，兼顧安全性和實用性。圖3-8為電子調(diào)速器的實物圖。（4）BEC輸出：該電子調(diào)速器擁有BEC輸出，其能夠?qū)⒏唠妷旱闹绷麟娹D(zhuǎn)換為5V/1A的直流電，能直接給其他模塊供電，從而省去了電壓轉(zhuǎn)換模塊簡化了電路，并且減小了飛行器的負載。圖3-8 電子調(diào)速器3.5超聲波測距模塊本設(shè)計要求飛行器近地飛行，且控制飛行一定的高度，由于超聲波到達物體表面能反射能被接收器檢測到，且能應(yīng)用于惡劣的環(huán)境，因此決定采用超聲波來測距。本設(shè)計的超聲波測距模塊采用HC-SR04超聲波測距傳感器，這是一種集發(fā)射與接收于一體的超聲波模塊，該模塊性能穩(wěn)定，測度距離精確，能和國外的SRF05,SRF02等超聲波測距模塊相媲美。該模塊高精度，盲區(qū)很近只有2cm，測量距離為2cm450cm非常適合近地飛行測距。該模塊采用IO觸發(fā)測距，只要向trig（控制端）輸入至少10us的高電平信號，該模塊就開始工作：自動發(fā)送8個40KHz的方波，自動檢測是否有信號返回，若有信號返回就在echo（接收端）輸出一高電平，而高電平持續(xù)的時間就是超聲波從發(fā)射到返回的時間。所以測量距離=(高電平時間*聲速(340m/s)/2。圖3-9為HC-SR04超聲波測距模塊實物圖。圖3-9 超聲波傳感器模塊3.6紅外避障模塊四旋翼自主飛行器避障要求檢測到障礙物時，使飛行器減速然后通過偏航運動實現(xiàn)轉(zhuǎn)彎從而避開障礙物。因此，利用中斷即可實現(xiàn)避障，當(dāng)檢測到障礙物時在中斷服務(wù)程序中實現(xiàn)避障，這樣可以使微處理器更能高效率地工作。鑒于此避障模塊采用紅外避障傳感器E18-D50NK，它是一種集發(fā)射與接收于一體的光電傳感器，發(fā)射光經(jīng)過調(diào)制后發(fā)出，接收頭對反射光進行解調(diào)輸入，有效地避免了外界可見光的干擾。同時本紅外避障傳感器通過使用透鏡，使該款傳感器最遠可以檢測到80厘米的障礙物，但由于紅外光的特性，對于不同顏色的物體，能檢測的最大距離也不同：白色物體檢測的最遠，黑色物體檢測的最近。另外該傳感器檢測障礙物的距離可以根據(jù)要求通過調(diào)節(jié)尾部的電位器旋鈕進行設(shè)置。該傳感器有三根引線：紅色接高電平VCC（5V）、綠色接地、黃色為數(shù)據(jù)輸出線。當(dāng)傳感器檢測到障礙物時，會通過黃色數(shù)據(jù)輸出線輸出低電平，將此線接在單片機的外部中斷觸發(fā)端口時就能觸發(fā)中斷進而進入中斷服務(wù)子程序完成避障的功能。紅外避障傳感器實物如圖3-10。圖3-10 紅外避障傳感器E18-D50NK3.7電源模塊電源模塊為四旋翼自主飛行器提供動力源，為各個模塊提供電源，尤其是電機驅(qū)動模塊，必須具有較高的電壓和電流，同時由于四旋翼自主飛行器的載重很有限，所以電源系統(tǒng)應(yīng)該盡可能輕，能量密度盡可能大，以便減輕飛行器的負載。因此本設(shè)計采用能量密度高的2S鋰電池供電，其電壓為7.4V足以提供驅(qū)動電機的電壓和電流。電子調(diào)速器可以直接從2S鋰電池獲取電源而單片機和其他的傳感器可以通過電子調(diào)速器的BEC輸出來獲取5V的電源。3.8飛行控制模塊將微控制系統(tǒng)、MPU6050姿態(tài)檢測模塊、超聲波模塊和紅外避障模塊進行整合，使之成為飛行控制模塊。如圖3-11為用萬用板搭建的飛行控制模塊。圖3-11 飛行控制模塊3.9本章小結(jié)本章對四旋翼自主飛行器硬件整體設(shè)計方案進行了介紹，對微控制系統(tǒng)模塊、飛行姿態(tài)檢測模塊、電機驅(qū)動模塊、超聲波測距模塊、紅外避障模塊、電源模塊的功能和核心器件進行了詳細說明，在硬件理論方面實現(xiàn)了對四旋翼自主飛行器的設(shè)計。4 四旋翼自主飛行器軟件設(shè)計4.1軟件總體設(shè)計四旋翼自主飛行器軟件設(shè)計的總體目標(biāo)是通過核心處理器初始化各個模塊并協(xié)調(diào)各個模塊正常工作從而使四旋翼自主飛行器能夠按照既定的方案穩(wěn)定飛行。但是由于四旋翼自主飛行器是六自由度、四輸入的欠驅(qū)動系統(tǒng)，所以被控量之間存在耦合關(guān)系，因此所設(shè)計的飛行控制算法必須能夠通過控制四個被控量來對六個量進行穩(wěn)定、有效的控制。本設(shè)計首先通過3軸陀螺儀和3軸加速度傳感器測量出3軸角速率和3軸加速度，然后通過傾斜角度與三軸角速率和三軸加速度的關(guān)系從而得到陀螺儀測量的傾斜度和加速度測量的傾斜度，由于兩者都存在較大的誤差所以再將陀螺儀和加速度測量的角度進行卡爾曼濾波及融合，進而得到較精確的傾斜角度即飛行器的姿態(tài)，最后應(yīng)用控制理論中的PID控制算法將飛行器的姿態(tài)變化（傾斜角度變化）轉(zhuǎn)化為控制器輸出PWM波的占空比的變化，從而控制相應(yīng)的電機加速或減速實現(xiàn)飛行器平衡的保持和各種姿態(tài)變換、運動。主程序先進行各模塊的初始化，檢測、記錄傳感器的偏移量，控制初始化，然后啟動電機并加速使飛行器垂直升高，當(dāng)超聲波傳感器檢測到飛行器升高到3m時執(zhí)行既定的任務(wù)（前進、旋轉(zhuǎn)等），否則電機繼續(xù)加速，飛行器繼續(xù)升高。任務(wù)完成后減速降落，其中紅外避障、姿態(tài)檢測及調(diào)整都在此過程中進行，程序主流程圖如圖4-1。飛行姿態(tài)檢測及解算是通過定時器中斷每隔0.5s進行一次檢測和調(diào)整：進入中斷后先保護現(xiàn)場，然后對加速度傳感器和陀螺儀的測量值進行平均值濾波，之后再通過濾波后的測量值解算出各個軸的傾斜角，再經(jīng)過卡爾曼濾波將各個相應(yīng)的軸的傾角進行融合，以便濾掉加速度傳感器和陀螺儀的各自的偏差得到更加精確的傾角值，然后再通過數(shù)字PID算法將設(shè)置值（傾角為0）與實際值作差，根據(jù)偏差來算出傾斜軸相應(yīng)兩個電機減小傾斜角應(yīng)該輸入的PWM的占空比，流程圖如4-2。四旋翼自主飛行器高度的測量通過超聲波收發(fā)測量高度，超聲波發(fā)出時單片機的定時器開始計時，當(dāng)收到超聲波是停止計時，通過定時器計時時間和超聲波傳輸速度即可測出飛行高度，流程圖如4-3。紅外避障通過紅外收發(fā)裝置檢測障礙物，若檢測到障礙物則該模塊輸出口輸出低電平，該輸出口接在單片機的外部中斷觸發(fā)口，當(dāng)檢測到障礙物時，中斷觸發(fā)進入中斷服務(wù)子程序，該程序通過控制器輸出控制信號增大飛行器前邊電機轉(zhuǎn)速，減小后邊的電機轉(zhuǎn)速從而實現(xiàn)后退避障，然后再通過偏航運動實現(xiàn)飛行方向的改變，流程圖如圖4-4。 圖4-1 主流程圖 圖4-2 姿態(tài)檢測及調(diào)整流程圖超聲波測距流程圖：圖4-3 超聲波測距流程圖紅外避障中斷流程圖：圖4-4 中斷服務(wù)子程序流程圖4.2軟件開發(fā)平臺簡介本設(shè)計利用瑞薩(Renesas)公司的單片機，其軟件開發(fā)環(huán)境、程序編輯、編譯、仿真運行的軟件為瑞薩公司開發(fā)的CubeSuite+，程序下載、燒寫軟件為Renesas Flash Programmer。4.2.1 CubeSuite+編譯軟件的介紹及操作方法（一）CubeSuite+的啟動1.在桌面上雙擊CubeSuite+圖標(biāo)打開CubeSuite+的開始界面（CubeSuite+Start）如圖4-5。圖4-5 CubeSuite+Start界面（二）創(chuàng)建新工程在CubeSuite+Start界面上單擊下的按鈕（如圖4-4中標(biāo)記處），打開如圖4-6。由于本設(shè)計使用的是RL78/G13-R5F100LEA的控制器，所以在單片機選型界面上“Microcontroller”后下拉選項中選擇“RL78”，在“Using microcontroller”下面選擇框中鍵入“100LE”然后Enter，即可搜索出，然后在Project name后輸入工程名字需要注意的是“工程名稱不能起中文名稱”，在Place后點擊選擇工程所保存的路徑其中不能存放于中文路徑及桌面，最后點擊按鈕完成工程創(chuàng)建。圖4-6 單片機選型界面（三）工程代碼編輯界面簡介圖4-7 代碼編輯界面CubeSuite+軟件中自帶有代碼生成工具，在這里可以設(shè)置單片機的時鐘（Clock Generator）、端口（Port）、中斷（Interrupt）、串口（Serial）、AD轉(zhuǎn)換器（A/D Converter）、定時器(Timer)、看門狗(Watchdog Timer)、時鐘輸出或蜂鳴器輸出(Clock Output/Buzzer Output)等，用戶可以直接通過設(shè)置相關(guān)的功能來讓軟件來自動生成相關(guān)的程序代碼，省去了用戶對基本程序的編寫，大大減少了程序開發(fā)的時間和難度。（四）程序調(diào)試仿真程序編輯完成后點擊菜單中的保存按鈕或點擊【File】【Save All】然后點擊菜單中的編譯按鈕或點擊【Build】【Build Project】編譯完成輸出窗口將顯示編譯結(jié)果，如果沒有錯誤程序就可以進行調(diào)試，若有錯誤則需要檢查程序編輯的錯誤。圖4-8 輸出窗口輸出的編譯結(jié)果信息本設(shè)計采用單片機配套的仿真器RL78 EZ Emulator，首先將仿真器連接到計算機，并安裝驅(qū)動，然后選擇調(diào)試工具：右鍵點擊選擇“RL78 EZ Emulator”圖4-9 仿真器的選擇點擊按鈕將程序下載到單片機中，下載成功將出現(xiàn)程序調(diào)試界面：圖4-10 程序調(diào)試界面點擊按鈕，仿真程序便開始運行，點擊按鈕仿真程序?qū)⑦M行單步運行，點擊按鈕將會使正在運行的程序復(fù)位，點擊能使程序進入函數(shù)內(nèi)部運行，點擊能使程序跳過函數(shù)運行，點擊按鈕終止仿真，點擊按鈕將會斷開仿真器于計算機的連接。4.2.2 Renesas Flash Programmer燒寫軟件的介紹及操作方法雙擊桌面上的Renesas Flash Programmer圖標(biāo)將出現(xiàn)Welcome！界面如圖：圖4-11 Welcome！界面創(chuàng)建工程如上圖點擊出現(xiàn)下圖：圖4-12 創(chuàng)建一個工程Microcontroller選擇“RL78”Filter鍵入“100LE”即可出現(xiàn)然后在Workspace Name鍵入工程名稱“1005270133”點擊選擇保存路徑，工程名稱不能起中文名稱，且不能存放于中文路徑及桌面。最后點擊完成工程創(chuàng)建。然后出現(xiàn)Communication Interface如下圖：圖4-13 選擇通訊接口在Tool右邊下拉選項中選擇單片機與計算機連接的COM端口（查看方式：我的電腦設(shè)備管理器端口）例如COM3。點擊出現(xiàn)Power Supply默認設(shè)置，點擊，出現(xiàn)Project Setting默認設(shè)置，點擊。圖4-14 工程設(shè)置之后在Renesas Flash Programmer中點擊選擇要下載的程序，命令使用自動命令“Autoprocedure(EP)”然后點擊Start按鈕，Renesas Flash Programmer軟件會自動進行“連接單片機、空白檢測、擦除原來程序、燒寫、斷開連接”，正常完成會顯示“PASS”， 斷開Emulator板與計算機的連接，結(jié)束程序的燒寫。如圖4-15。圖4-15 程序燒寫成功4.3飛行姿態(tài)解算算法及程序設(shè)計中飛行姿態(tài)檢測模塊采用的是MPU6050傳感器模塊，其中融合了加速度傳感器和陀螺儀，它們相互校準(zhǔn)從而減小各自測量的誤差。4.3.1加速度傳感器測量數(shù)據(jù)解算加速度傳感器通過測量各個軸上重力加速度的分量的大小，然后再利用重力加速度的分解及各軸與參考面軸的夾角關(guān)系來實現(xiàn)測量傾角。測量原理示意圖如圖4-16。圖4-16 加速度傳感器測量原理示意圖根據(jù)上圖可通過基本三角函數(shù)證明，可以利用下列三個等式計算傾角： （4-1） （4-2） （4-3）其中為X軸與水平面的夾角；為Y軸與水平面的夾角；為Z軸與豎直方向的夾角。4.3.2陀螺儀測量數(shù)據(jù)解算陀螺儀具有動態(tài)性能好的優(yōu)點。其直接輸出值是相對靈敏軸（陀螺儀自身的虛擬軸）的角速率，角速率對時間積分便可得到圍繞靈敏軸旋轉(zhuǎn)過的角度值。由于系統(tǒng)采用單片機采用循環(huán)采樣程序獲得陀螺儀角速率信息，即每隔一段很短的時間采樣一次，所以采用累加的方法來近似實現(xiàn)積分的功能來計算轉(zhuǎn)過的角度值。陀螺儀是用來測量角速度信號的，通過對角速度的積分，能得到再很短一段時間內(nèi)轉(zhuǎn)過的角度值。但由于溫度變化、摩擦力、不穩(wěn)定力矩等因素的影響，陀螺儀會產(chǎn)生漂移誤差。而無論多么小的常值漂移誤差通過積分都將會得到無限大的角度誤差，因此需要減小該誤差。綜上所述，對于飛行姿態(tài)檢測系統(tǒng)而言，單獨使用陀螺儀或者加速度傳感器，都不能提供有效且可靠的飛行狀態(tài)信息來提供給單片機保證飛行器平衡。雖然陀螺儀動態(tài)性能良好，能夠提供瞬間的動態(tài)角度變化且不受加速度變化、振動的影響，但是由于受其本身固有的特點及積分過程的影響，存在嚴(yán)重的累積漂移誤差，不適合長時間單獨工作；加速度傳感器表態(tài)響應(yīng)很好，能夠準(zhǔn)確提供表態(tài)的傾斜角度，但受動態(tài)加速度、振動的影響較大，不適合跟蹤動態(tài)物體的運動。為了克服兩者的缺點，決定采用濾波、融合的方法來融合陀螺儀和加速度傳感器的輸出信號，補償陀螺儀的漂移誤差、累積誤差和加速傳感器的動態(tài)誤差，使兩者能夠相互校正，避免各自缺點，發(fā)揮優(yōu)點，使測量得到一個更優(yōu)的傾角近似值，使姿態(tài)測量更加準(zhǔn)確。4.4數(shù)據(jù)處理及程序本設(shè)計采用卡爾曼濾波及平均值濾波對MPU6050傳感器輸出的數(shù)據(jù)進行濾波、融合進而得到誤差小的姿態(tài)信息。4.4.1卡爾曼濾波與平均值濾波卡爾曼濾波是一種高效率、可能最優(yōu)的遞歸濾波器(自回歸濾波器), 它能夠從一系列的不完全包含噪聲的測量中最優(yōu)地估計動態(tài)系統(tǒng)的狀態(tài)。假設(shè)一個離散控制過程系統(tǒng)可以用一個線性隨機微分方程來描述： （4-4）系統(tǒng)的測量值為： （4-5）其中是時刻系統(tǒng)的狀態(tài)，是時刻加在系統(tǒng)上的控制量。A、B是系統(tǒng)固有的參數(shù)，對于多模型系統(tǒng)而言，它們?yōu)榫仃?。是時刻的測量值，H是測量系統(tǒng)的參數(shù)，對于多測量系統(tǒng)，H為矩陣。和分別表示系統(tǒng)過程和系統(tǒng)測量的噪聲。假設(shè)噪聲為高斯白噪聲且不隨系統(tǒng)狀態(tài)變化而變化，他們的協(xié)方差分別是Q，R。對于滿足以上條件的過程，卡爾曼濾波是最優(yōu)的處理器。首先要利用系統(tǒng)過程模型來預(yù)測下一狀態(tài)： （4-6）其中是通過上一狀態(tài)預(yù)測的結(jié)果，是上一狀態(tài)最優(yōu)的結(jié)果，為現(xiàn)在狀態(tài)的控制量。對應(yīng)于的協(xié)方差為： （4-7）其中是對應(yīng)的協(xié)方差，是對應(yīng)的協(xié)方差，表示的轉(zhuǎn)置矩陣，是系統(tǒng)過程的協(xié)方差。以上是對系統(tǒng)的預(yù)測，然后收集對系統(tǒng)現(xiàn)在狀態(tài)的測量值，通過融合預(yù)測值和測量值就可以得到現(xiàn)在狀態(tài)（）的最優(yōu)估計值： （4-8）其中為卡爾曼增益（Kalman Gain）： （4-9）為了讓卡爾曼濾波器不斷地運行下去，就還要更新狀態(tài)下的協(xié)方差： （4-10）其中為單位陣，對于單模型單測量系統(tǒng)而言=1。當(dāng)系統(tǒng)進入（下一狀態(tài)）狀態(tài)時，就變?yōu)榇藭r的。如此，就可以自回歸地運算下去，實現(xiàn)最優(yōu)化估計。平均值濾波通過求n個值的平均值，將平均值作為測量值，這樣使測量值更接近實際值，能夠減小隨機誤差，減弱外界干擾對控制過程的影響。4.4.2卡爾曼濾波、平均值濾波程序（1）卡爾曼濾波程序vo