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

《畢業(yè)設計——四旋翼自主飛行器設計[共80頁]》由會員分享，可在線閱讀，更多相關《畢業(yè)設計——四旋翼自主飛行器設計[共80頁]（82頁珍藏版）》請在裝配圖網(wǎng)上搜索。

1、分類號 V249 單位代碼 11395 密 級 學 號 1005270133 學生畢業(yè)設計（論文）題 目四旋翼自主飛行器設計作 者劉赟躍院 (系)能源工程學院專 業(yè)測控技術與儀器指導教師王晶答辯日期2014 年 5 月 24 日榆 林 學 院畢業(yè)設計（論文）誠信責任書本人鄭重聲明：所呈交的畢業(yè)設計（論文），是本人在導師的指導下獨立進行研究所取得的成果。畢業(yè)設計（論文）中凡引用他人已經(jīng)發(fā)表或未發(fā)表的成果、數(shù)據(jù)、觀點等，均已明確注明出處。盡我所知，除文中已經(jīng)注明引用的內(nèi)容外，本論文不包含任何其他個人或集體已經(jīng)公開發(fā)表或撰寫過的研究成果。對本文的研究做出重要貢獻的個人和集體，均已在文中以明確方式標明

2、。本人畢業(yè)設計（論文）與資料若有不實，愿意承擔一切相關的法律責任。 論文作者簽名: 年 月 日榆林學院本科畢業(yè)設計（論文）摘 要四旋翼飛行器是能夠垂直起降、多旋翼的飛行器。與傳統(tǒng)旋翼式飛行器不同的是其在飛行中四只旋翼可以相互抵消掉反扭力矩，而不需要反扭矩槳，使其結構更緊湊、產(chǎn)生更大升力。同時因其飛行空間小、效率高、飛行穩(wěn)定、易控制等優(yōu)點，使之在軍事、民用方面有廣闊的前景。本設計以16位瑞薩(Renesas)單片機RL78/G13R5F100LEA作為控制核心，通過電子調(diào)速器控制、驅(qū)動飛行器的四個直流電機，利用三軸角速度傳感器、三軸加速度傳感器組成飛行器飛行狀態(tài)檢測、控制模塊，采用紅外收發(fā)管實現(xiàn)

3、飛行器飛行壁障、超聲波傳感器測距來控制、檢測飛行高度，以C語言、卡爾曼濾波及PID算法實現(xiàn)控制軟件的編寫從而實現(xiàn)低空飛行的四旋翼自主飛行器驗證機的設計。設計首先對四旋翼飛行器進行概述，分析四旋翼飛行器特點和發(fā)展趨勢。其次，根據(jù)設計要求進行系統(tǒng)方案設計，并完成總體結構設計、硬件設計、軟件設計。最后，進行調(diào)試、驗證試飛。本設計從低成本、低功耗、高性能方面入手，完成了飛控系統(tǒng)硬件設計、制作、電路調(diào)試等實現(xiàn)了四旋翼自主飛行器的基本功能。關鍵詞：四旋翼飛行器；電子調(diào)速器；加速度傳感器；卡爾曼濾波；PIDDesign of Four-rotor Autonomous AircraftABSTRACTFou

4、r-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 greate

5、r 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, drivin

6、g 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,

7、 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

8、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

9、 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本設計的目的及意義11.2國內(nèi)外發(fā)展研究現(xiàn)狀11.2.1國

10、外發(fā)展研究現(xiàn)狀11.2.2國內(nèi)發(fā)展研究現(xiàn)狀21.3本設計的主要內(nèi)容21.4本章小結22 四旋翼自主飛行器總體設計32.1四旋翼自主飛行器飛行平臺32.2四旋翼自主飛行器的結構及控制原理32.3本章小結73 四旋翼自主飛行器硬件設計93.1系統(tǒng)結構分析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本章小結194 四旋翼自主飛行器軟件設計214.1軟件總體設計214.2軟件開發(fā)平臺簡介234.2

11、.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本章小結365 四旋翼自主飛行器系統(tǒng)調(diào)試與實現(xiàn)375.1電子調(diào)速器對電機控制的調(diào)試375.2 MPU6050傳感器測量調(diào)試385.3超聲波測距調(diào)試39

12、5.4紅外避障調(diào)試405.5四旋翼自主飛行器整體調(diào)試415.6本章小結426 總結與展望436.1總結436.2展望43參考文獻45致 謝47附錄A 四旋翼自主飛行器原理圖49附錄B 四旋翼自主飛行器實物圖51附錄C 程序代碼531 緒論1.1本設計的目的及意義四旋翼自主飛行器是一種能夠垂直起降、多旋翼式的飛行器，其通過自帶電源驅(qū)動電機來提供動力。它在總體布局上屬于非共軸式碟形飛行器，與常規(guī)旋翼式飛行器相比，因其四只旋翼可相互抵消反扭力矩的優(yōu)點，而不需要專門的反扭矩槳從而使其結構更為緊湊，能夠產(chǎn)生更大的升力。同時又因其具有靈活性高、要求的飛行空間小、能源利用率高、隱蔽性強以及安全性能高等優(yōu)勢，

13、特別適合在近地面環(huán)境（如室內(nèi)、城區(qū)和叢林等）中執(zhí)行監(jiān)視、偵查等任務，其在軍事（電子戰(zhàn)）和民用（通信、氣象、災害監(jiān)測）方面都有很大的應用前景。另外，新穎的外形、簡單的結構、低廉的成本、卓越的性能及獨特的飛行控制方式（通過控制四只旋翼的轉速實現(xiàn)飛行控制）使其對廣大科研人員具有很強的吸引力，成為國際上新的研究熱點1。1.2國內(nèi)外發(fā)展研究現(xiàn)狀1.2.1國外發(fā)展研究現(xiàn)狀早在1907年，由Breguet-Richht發(fā)明的世界上第一架四旋翼飛行器“Gyroplane No.1”升上了天空。目前世界上存在的四旋翼飛行器基本上都屬于微小型無人飛行器。國外的四旋翼飛行器因擁有悠久的科學文化歷史和研發(fā)團體機構，加

14、快了多旋翼飛行器的發(fā)展。國外的很多高校和研究機構如Stanford University（美國的斯坦福大學）、University of Technology of Compiegne（法國貢比涅技術大學）等已經(jīng)展開了相關的研究并取得了一定的成果。另外國外一些商業(yè)公司也加入了四旋翼無人飛行器研究的行列，如美國Draganfly公司，德國Microdrones GmbH公司等，他們的加入使得四旋翼飛行器的發(fā)展得到了迅速的發(fā)展。其中Draganfly公司的Draganflyer飛行器已經(jīng)被很多高校采用為飛行器本體，進行實驗。20世紀末19世紀初期，貝爾波音公司將固定四旋翼直升機概念和傾轉旋翼概念進

15、行了融合，設計出了新型的軍用飛行器，成功地實現(xiàn)了四旋翼飛行器新型機在軍事上的應用，從而極大地提高了飛行器的作戰(zhàn)能力。21世紀，網(wǎng)絡信息時代飛速發(fā)展。美國Parrot AR Drone和蘋果公司推出了通過iphone手機藍牙、WiFi控制四旋翼飛行器飛行的新產(chǎn)品。歐美發(fā)達國家四旋翼飛行器已投入了商業(yè)、軍事領域，獲得了顯著的效果。1.2.2國內(nèi)發(fā)展研究現(xiàn)狀我國的四旋翼飛行器的發(fā)展仍然處于初期發(fā)展階段，由于缺乏獨立的核心技術，能應用于專業(yè)領域的相關產(chǎn)品仍未大批量生產(chǎn)、投入使用。但是在國內(nèi)，一些研究機構及高校對四旋翼飛行器的研究也取得了卓有成效的進展，其中有國防科技大學、北京航空航天大學、清華大學、浙

16、江大學等，尤其是浙江大學的“玉泉之翼”實現(xiàn)了自主空中懸停1分鐘，雖然懸停時間還沒有外國的長，但是因為是完全自主研發(fā)的，因此在國內(nèi)具有里程碑的意義，同時也是我國四旋翼飛行器的最高水平。1.3本設計的主要內(nèi)容本設計基于實現(xiàn)低成本、低功耗、高性能的四旋翼飛行器驗證機，本文設計、開發(fā)一種以瑞薩(Renesas)RL78/G13系列單片機為核心的四旋翼自主飛行器。具體分為飛行器的硬件結構設計、電路制作測試以及飛行控制底層軟件程序的設計，利用單片機輸出PWM（Pulse Width Modulation）通過電子調(diào)速器控制四旋翼飛行器的四個電機的轉速使飛行器完成各種基本動作，并通過角度傳感器、加速度傳感器

17、、卡爾曼濾波算法、PID算法編程實現(xiàn)飛行器自主調(diào)節(jié)平衡，從而實現(xiàn)四旋翼自主飛行器的自主起飛、爬升、前進、轉彎、避障、降落等四旋翼飛行器的基本動作，利用CubeSuite+、Renesas Flash Programmer軟件分別對單片機編程、下載。最終實現(xiàn)四通道能夠開發(fā)、拓展其他更多功能的四旋翼自主飛行器。1.4本章小結本章主要介紹了四旋翼自主飛行器的發(fā)展歷史、發(fā)展趨勢及本設計的內(nèi)容、目的。加深了對四旋翼飛行器的應用、發(fā)展方向及前景的了解，明確了本設計的重點及難點，有利于作好設計、研究準備，同時也使自己對飛行器有了更多的了解，為完成本設計打下了堅實的基礎。2 四旋翼自主飛行器總體設計2.1四旋

18、翼自主飛行器飛行平臺四旋翼飛行器按照四只旋翼和機架布置的方式其飛行控制平臺（機架）可以分為十字模式和X模式。X模式比十字模式靈活，但是對于姿態(tài)測量和控制的算法編程來說，十字模式較X模式簡單，更容易實現(xiàn)。X模式通過同時控制兩對旋翼轉速的大小來實現(xiàn)飛行控制及姿態(tài)的調(diào)整，而十字模式只要同時控制一對旋翼的轉速就能實現(xiàn)相應的飛行動作。十字模式容易操作，飛行平穩(wěn)，綜合考慮采用十字模式?？紤]到自己制作四旋翼飛行器平臺需要采購零部件、制作過程中瑣碎的機械結構設計及評價制作出的飛行平臺的性價比。本設計決定采用購買的十字模式的飛行器機架，以便減少制作機架的費用、時間、降低成本，將大量的時間和精力放在本設計的重點部

19、分上飛行器的硬件結構和軟件的總體設計。2.2四旋翼自主飛行器的結構及控制原理四旋翼自主飛行器是由安裝在十字型剛性結構的四個電機作為驅(qū)動的飛行器。控制器通過調(diào)節(jié)四個電機的轉速使四個旋翼間出現(xiàn)特定的轉速差從而實現(xiàn)飛行器的各種動作。由于四旋翼自主飛行器是通過增大或減小四只旋翼的轉速達到四個方向升力的變化進而控制飛行器的飛行姿態(tài)和位置的穩(wěn)定，相對于傳統(tǒng)的直升機少去了舵機調(diào)節(jié)平衡、控制方向，并且不用改變螺旋槳的槳距角，使得四旋翼自主飛行器更容易控制。但是四旋翼自主飛行器有六個狀態(tài)輸出，即是一種六自由度的飛行器，而它卻只有四個輸入，是一個欠驅(qū)動系統(tǒng)。也正是由于這個原因使得四旋翼自主飛行器非常適合在靜態(tài)及準

20、靜態(tài)的條件下飛行。四旋翼自主飛行器飛行控制系統(tǒng)由飛行控制器、各類測量傳感器裝置、驅(qū)動電機、被控對象（飛行器機體）等部分組成，如圖2-1。傳感器用來測量四旋翼自主飛行器的飛行姿態(tài)信息如俯仰角、傾斜角、偏航角等，這些信息經(jīng)過控制器處理后轉換成為能夠被控制系統(tǒng)識別和處理的有效、有用信息，飛行控制器依據(jù)飛行器姿態(tài)檢測傳感器反饋回來的飛行姿態(tài)信息及預先設定的飛行狀態(tài)進行計算、處理及控制，然后通過輸出PWM信號來控制電機的轉速進而調(diào)節(jié)或改變飛行姿態(tài)。圖2-1 四旋翼自主飛行器控制系統(tǒng)圖2-2 四旋翼自主飛行器模型俯視圖四旋翼自主飛行器的結構如圖2-2，在螺旋槳1和螺旋槳3順時針旋轉的同時，螺旋槳2和螺旋槳

21、4逆時針旋轉，當飛行器平衡飛行時四個電機產(chǎn)生的反扭矩力大小相等方向兩兩相反進而兩兩相互抵消，相對于經(jīng)典的傳統(tǒng)直升機不需要專門的舵機來抵消反扭矩力。四旋翼自主飛行器有六個自由度（分別為沿3個軸作平移和旋轉動作），這六個自由度中的任意一個自由度的控制和調(diào)節(jié)都可以通過聯(lián)合調(diào)節(jié)四個電機的轉速進而改變四個螺旋槳的轉速來實現(xiàn)。其基本運動分別是：(1)垂直運動；(2)前后運動；(3)俯仰運動；(4)滾轉運動；(5)偏航運動；(6)側向運動。在圖2-3中電機1和電機3作順時針旋轉，電機2和電機4作逆時針旋轉，設飛行器沿X軸正方向運動為向前運動，箭頭向上表示電機轉速增大，箭頭向下表示電機轉速減小。圖2-3(a)

22、表示垂直運動：在圖中兩對電機轉動方向相反，可以相互抵消掉旋轉產(chǎn)生的反扭矩力，當同時增加四個電機的轉速使得旋翼的升力同時增大，當升力足以克服整機的重力時，四旋翼自主飛行器便會離開地面垂直上升；反之，同時減小四個電機的轉速使旋翼的升力減小，四旋翼自主飛行器就會豎直下降，直到降落到地面，從而實現(xiàn)沿Z軸進行上下飛行的垂直運動。而當外界擾動很小或為零時，四旋翼自主飛行器的四只旋翼產(chǎn)生的升力等于其自身重力時，飛行器便會保持一定高度不變，實現(xiàn)懸停。圖2-3(b)表示前后運動：欲使四旋翼自主飛行器能夠在水平面上前后、左右運動，必須對飛行器施加一個水平的力。在圖中電機3的轉速增大，電機1的轉速減小，電機2、4的

23、轉速保持不變。為了使四旋翼自主飛行器不因旋翼轉速的改變而致使飛行器受力不均導致不平衡，要求電機1和電機3的轉速改變量大小相等，只有這樣才能使旋翼1和旋翼3產(chǎn)生的反扭矩力抵消，這樣四旋翼自主飛行器X軸稍微傾斜（旋翼1高度稍微低于旋翼3）旋翼1、3產(chǎn)生的升力便會產(chǎn)生一個指向X軸正方向的分力，使飛行器向X軸正方向運動實現(xiàn)前進。向后飛行與向前飛行正好相反。圖2-3(c)表示俯仰運動：在圖中增加電機1的轉速使旋翼提供的升力增大，相應地減小電機3的轉速使旋翼的升力減小同樣的大小，同時保持其他兩個電機的轉速不變，只有這樣才能保持產(chǎn)生的反扭矩力相互抵消進而保持飛行器的平衡。由于旋翼1的升力增大旋翼3的升力減小

24、，產(chǎn)生的飛行器上下不平衡的分力使飛行器繞Y軸旋轉（方向如圖），同理，電機1轉速減小電機3的轉速增大飛行器便繞著Y軸向另一個方向旋轉，實現(xiàn)俯仰運動。圖2-3(d)表示滾轉運動：與俯仰運動的原理相同，在圖d中相應改變電機2、4的轉速而保持電機1、3的轉速不變則可以使飛行器繞X軸旋轉，從而實現(xiàn)四旋翼自主飛行器的滾轉運動。圖2-3(e)表示偏航運動：四旋翼自主飛行器的偏航運動可以通過四只旋翼之間產(chǎn)生的反扭矩力來實現(xiàn)，一般情況下四旋翼自主飛行器的為抵消掉產(chǎn)生的反扭矩力使兩對轉向相反的電機的轉速相同，此時，為了利用反扭矩力使電機1、3的轉速增大，電機2、4的轉速減小，此時旋翼1、3產(chǎn)生的對飛行器的反扭矩力

25、大于旋翼2、4產(chǎn)生的反扭矩力，于是飛行器便在大的反扭矩力的作用下繞Z軸轉動，其轉向與電機1、3的轉向相反。圖2-3(f)表示側向運動：因為四旋翼自主飛行器的結構完全對稱，所以側向運動的原理與前后運動的原理完全一樣。(a)垂直運動(b)前后運動(c)俯仰運動(d)滾轉運動(e)偏航運動(f)側向運動圖2-3 四旋翼自主飛行器的運動2.3本章小結本章主要介紹了四旋翼飛行器的飛行平臺、飛行器結構、飛行控制原理。了解了四旋翼飛行器的飛行平臺、飛行方式、控制方式應，明確了本設計需要怎樣著手、作哪些方面的研究、設計，為下面的設計打下理論基礎。3 四旋翼自主飛行器硬件設計3.1系統(tǒng)結構分析四旋翼自主飛行器系

26、統(tǒng)可分為微控制系統(tǒng)模塊、飛行姿態(tài)檢測模塊、電機驅(qū)動模塊、超聲波測距模塊、紅外避障模塊、電源模塊。其中微控制系統(tǒng)模塊采用RL78/G13系列單片機R5F100LEA作為主控芯片，該控制器是一款功能強大的16位處理器，具有運算速度快、擁有豐富的片上資源，非常適用于處理較復雜的任務。飛行姿態(tài)檢測模塊采用MPU6050芯片，此芯片集成了3軸MEMS陀螺儀，3軸MEMS加速度計，并通過三個16位的ADC將其測量到的模擬量轉化為可輸出且能夠被單片機直接處理的數(shù)字量，從而減輕了單片機的工作量。電機驅(qū)動模塊利用單片機輸出PWM波通過電子調(diào)速器驅(qū)動四個電機，并且通過調(diào)節(jié)PWM的占空比實現(xiàn)電機的調(diào)速。超聲波測距模

27、塊利用超聲波的反射特性來測距離從而控制飛行器的高度。紅外避障模塊采用紅外光電開關，當檢測到障礙物通過中斷使處理器響應，從而實現(xiàn)避障。電源模塊采用7.4V的鋰電池供電。系統(tǒng)框圖如圖3-1。圖3-1系統(tǒng)框圖3.2微控制系統(tǒng)模塊微控制系統(tǒng)模塊是飛行控制系統(tǒng)的核心處理器，主要負責采集傳感器檢測到的姿態(tài)角速率（俯仰角速率、橫滾角速率和偏航角速率）、三軸線加速度和航向信息并實時解算、處理；根據(jù)檢測到的飛行姿態(tài)信息，結合預定要求的控制方案，計算輸出控制量控制電機轉速改變，進而保持或改變飛行狀態(tài)。四旋翼自主飛行器算法復雜且要求較高的處理速度，以便能及時處理飛行偏差、調(diào)節(jié)自身平衡，所以普通的單片機如51系列達不

28、到要求。因此本設計使用瑞薩公司生產(chǎn)的RL78/G13系列單片機R5F100LEA，該芯片是功能強大的16位處理器，自帶10位12ch的ADC、強大的定時器，具有非常豐富的片上資源，能輸出多通道獨立占空比的PWM波，擁有高性能的外圍設備，采用引腳復用方式，既節(jié)省了引腳資源又拓展了功能，并且具有超低功耗，支持CSI 2ch、UART、IC 2ch串口通信，并且不需要EEPROM。能方便對電機進行控制，可以進行浮點型運算。并且還有精密的比較器，很大的RAM和ROM，能存儲比較大的程序段。該控制器采用64引腳LQFP封裝，單片機最小系統(tǒng)電路及引腳圖如圖3-2。圖3-2 RL78/G13-R5F100L

29、EA單片機引腳圖3.3飛行姿態(tài)檢測模塊本設計的采用陀螺儀和加速度傳感器來檢測飛行姿態(tài)，為了避免陀螺儀和加速度傳感器安裝中可能出現(xiàn)的機械偏差，本設計采用集成兩者的MPU6050傳感器模塊。3.3.1飛行姿態(tài)檢測傳感器MPU6050飛行器正常飛行需要不斷地檢測自身的飛行狀態(tài)，然后根據(jù)自身姿態(tài)來調(diào)節(jié)自身平衡，從而實現(xiàn)穩(wěn)定飛行。四旋翼自主飛行器的姿態(tài)檢測主要就是檢測自身的傾斜度，目前比較成熟的就是慣性測量：利用陀螺儀或者加速度傳感器來測量傾角。大多數(shù)加速度傳感器是基于壓電效應的原理來工作的。所謂壓電效應，即某些電介質(zhì)在沿一定方向上受到外力的作用而變形時，其內(nèi)部會產(chǎn)生極化現(xiàn)象，同時在它的兩個相對表面上出

30、現(xiàn)正負相反的電荷。通過測量這些電荷量，并將其轉化為電壓信號輸出，就完成了加速度傳感器的測量。加速度傳感器的工作原理就是把產(chǎn)生加速度的壓力轉化為電信號輸出，進而根據(jù)輸出的電信號利用數(shù)學方法計算出相應的加速度。陀螺儀又稱為角速度傳感器，其工作原理是科里奧利力原理：科里奧利力來自于物體運動所具有的慣性，在旋轉體系中進行直線運動的質(zhì)點，由于慣性的作用，有沿著原有運動方向繼續(xù)運動的趨勢，但是由于體系本身是旋轉的，在經(jīng)歷了一段時間的運動之后，體系中質(zhì)點的位置會有所變化，而它原有的運動趨勢的方向，如果以旋轉體系的視角去觀察，就會發(fā)生一定程度的偏離。當一個質(zhì)點相對于慣性系做直線運動時，相對于旋轉體系，其軌跡是

31、一條曲線。立足于旋轉體系，我們認為存在一個力驅(qū)使質(zhì)點運動軌跡形成曲線，這個力就是科里奧利力。陀螺儀輸出三軸的角加速度然后通過對加速度積分便得到了角速度，然后再積分就得到了角度。陀螺儀的輸出不受飛行器震蕩影響，輸出比較精確，由于輸出角速度需要經(jīng)過積分才能得到角度，因此測量誤差也會通過積分不斷地積累最終使得小誤差積累成為較大的誤差，從而影響飛行器的可靠控制，并且陀螺儀只能感知飛行器姿態(tài)的改變，然后通過控制器阻止這種改變從而獲得平衡，它只能測量位置改變量而不能實現(xiàn)絕對定位自己的位置和姿態(tài)；加速度傳感器通過測量重力加速度并運用三軸間的加速度關系進而測量出傾斜角度，但是在飛行器飛行過程中電機轉動、飛行器

32、震動都會使得測量到的加速度不是重力加速度而是合外力加速度，從而使測量誤差增大。綜合考慮陀螺儀和加速度傳感器的優(yōu)缺點決定采用兩者結合，形成互補，相互矯正?？紤]到將陀螺儀和加速度傳感器更緊密地結合在一起進而盡可能減小誤差，本設計采用集陀螺儀和加速度傳感器于一體的MPU6050模塊。MPU6050是全球首例融合6軸運動處理的組件，整合了3軸陀螺儀、3軸加速器，相較于多組件方案，免除了組合陀螺儀與加速器時之軸間差的問題，減少了大量的包裝。MPU6050的角速度全格感測范圍為250、500、1000與2000/sec (dps)，可準確追蹤快速與慢速動作，并且，用戶可程式控制的加速器全格感測范圍為2g、

33、4g8g與16g，輸出采用IC傳輸，內(nèi)部含內(nèi)建的溫度感測器、包含在運作環(huán)境中僅有1%變動的振蕩器。其中陀螺儀和加速度傳感器分別用了三個16位的ADC，將測量的模擬量轉化為單片機能直接處理的數(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世紀80年代，最

34、初為音頻和視頻設備開發(fā)的，如今主要在服務管理中使用，其中包括單個組件狀態(tài)的通信。（2）IC總線的特征1）IC總線只要求有兩條總線線路：一條串行數(shù)據(jù)線SDA（Serial Data Line）和一條串行時鐘線SCL（Serial Clock Line）。2）每個連接到總線的IC器件都可以通過唯一的地址和一直存在的主機/從機關系軟件設定地址，主機也可以作為主機發(fā)送器和主機接收器。3）IC總線是一個真正的多主機總線，如果有兩個或更多主機同時進行初始化，數(shù)據(jù)傳輸可以通過沖突檢查和仲裁來防止傳輸數(shù)據(jù)被破壞。4）IC串行的8位雙向傳輸數(shù)據(jù)位率在標準模式下可達到100Kbps，而在快速模式下可以達到400K

35、bps，高速模式下可達3.4Mbps。5）IC總線器件片上的濾波器可以濾去總線數(shù)據(jù)線SDA上可能出現(xiàn)的毛刺，能更好地保證數(shù)據(jù)完整。6）連接到同一總線上的IC數(shù)量僅只受到總線的最大電容400pF的限制。（3）開始（S）和結束（P）標志IC接口包括串行數(shù)據(jù)線（SDA）和串行時鐘線（SCL）。連接到IC接口的設備可以作為主設備也可以作為從設備。主設備將從地址（Slave Address）傳到總線上，從設備通過與其匹配的地址來識別主設備。當MPU6050與微處理器連接時，MPU6050作為從設備，微處理器作為主設備。其從地址為7位字長b110110X，最低有效位X由A0引腳的邏輯電平?jīng)Q定，本設計A0接

36、地，所以設備地址為b1101100。當時鐘線SCL為高電平時，數(shù)據(jù)線SDA由高到低時的下降沿，為傳輸開始標志（S）。直到主設備發(fā)出結束信號（P），否則總線狀態(tài)一直處于忙狀態(tài)。結束標志（P）規(guī)定為：當時鐘線SCL為高電平時，數(shù)據(jù)線SDA由低到高時的上升沿。如圖3-5。圖3-5 開始停止條件（4）數(shù)據(jù)格式/應答IC的傳輸數(shù)據(jù)字節(jié)為8bits長度，對每次傳送的總字節(jié)數(shù)量沒有限制。但是對每一次傳輸必須伴有一個應答（ACK）信號，其時鐘信號由處理器提供，而真正的應答信號則由MPU6050發(fā)出，在時鐘為高時，通過拉低并保持SDA線上的值來實現(xiàn)。如果從設備MPU6050處于忙狀態(tài)時，它可以使SCL線保持在低

37、電平，這會強制使處理器進入等待狀態(tài)。當MPU6050空閑后，同時釋放時鐘線時，原來的數(shù)據(jù)傳輸才會繼續(xù)進行。如圖3-6。圖3-6 MPU6050與主機握手方式（5）通信開始標志（S）發(fā)出后處理器會傳送出一個7位的從地址，并在后面跟著一個第8位，該位被稱為Read/Write位。R/W位表示處理器是需要接收MPU6050的數(shù)據(jù)還是向其寫數(shù)據(jù)。之后，處理器釋放SDA線，等待MPU6050的應答信號（ACK）。應答信號每個字節(jié)的傳輸都要跟隨一個應答位，應答產(chǎn)生時，MPU6050將SDA線拉低并在SDA為高電平時保持為低。數(shù)據(jù)傳輸總是以停止標志（P）終止，然后釋放通信線路。但是處理器也可以產(chǎn)生重復的開始

38、信號去操作其他的IC設備，而不發(fā)出停止標志。由此可知：除了開始和結束標志，所有的SDA信號的變化都要在SCL時鐘線為低電平時進行。如圖3-7。圖3-7 MPU6050與主機數(shù)據(jù)傳輸時序圖如果要對MPU6050寄存器進行寫操作，處理器除了發(fā)出開始標志（S）和地址位，還要加一個R/W讀寫位，0為寫，1為讀。在第9個時鐘周期（高電平）時，MPU6050產(chǎn)生一個應答信號。然后處理器開始發(fā)送寄存器地址，接到MPU6050的應答后開始傳送寄存器數(shù)據(jù)，之后仍需要有應答信號，以此類推。如果要讀取MPU6050寄存器的值，首先要由處理器產(chǎn)生開始信號（S），然后再發(fā)送MPU6050的地址位和一個寫數(shù)據(jù)位，最后發(fā)送

39、寄存器地址，只有這樣才能開始讀寄存器。接著，收到應答信號后，處理器再發(fā)一個開始信號，然后再發(fā)送MPU6050地址位和一個讀數(shù)據(jù)位。然后，作為從設備的MPU6050產(chǎn)生應答信號并開始發(fā)送其寄存器中的數(shù)據(jù)。它們之間的通信以處理器產(chǎn)生的拒絕應答信號（NACK）和結束標志（P）為結束標志。拒絕應答信號（NACK）的產(chǎn)生定義為SDA線上的數(shù)據(jù)在第9個時鐘周期中一直為高。3.4電機驅(qū)動模塊電機為四旋翼自主飛行器提供動力，通過控制電機轉速進而改變旋翼轉速從而實現(xiàn)飛行器的各種飛行運動?？紤]到本設計的低成本特性及設計驗證機的目的，本設計采用價格低廉的直流有刷電機。直流電機在使用時需要在電動機的兩個接線端上加載上

40、電壓，電壓的高低直接影響電動機的轉速，它們兩者之間的關系如下公式：式中 U加載在電動機兩端的直流電壓；I直流電動機的工作電流；R直流電動機線圈的等效內(nèi)阻；常數(shù)，與電機本身的結構參數(shù)有關；每極總磁通。由于購買到直流電機后，電機的機械結構已經(jīng)固定，勵磁部分為永久磁鐵，所以式中的R、和已經(jīng)固定，能夠改變的就只有加載在直流電機兩端的直流電壓。常見的電機電壓調(diào)節(jié)的方法有兩種：數(shù)模轉換器輸出法和PWM輸出法。由于數(shù)模轉換器大多是電流輸出型，需要外接運放才能將其轉換成電壓，另外由于運放輸出電流有限，如果直接連接到直流電動機會造成直流電動機的轉矩過小和運放過熱的現(xiàn)象對電機調(diào)速不利，所以采用PWM輸出法。PWM

41、輸出法由單片機的I/O口作為PWM的輸出端，通過輸出信號控制大功率晶體管的開啟和關閉，以控制電動機的運轉和停止，當PWM的頻率足夠高時，由于電動機的繞組是感性負載，具有儲能的作用，對PWM輸出的高低電平起到了平波的作用，在電動機的兩端可以得到近似直流的電壓值，因此PWM的占空比越高，電動機獲得的直流電壓越高，反之，PWM的占空比越低，電動機獲得的直流電壓將會越低。考慮到本設計是設計飛行器，需要電機有很高的轉速及簡便、低成本性，因此采用單片機輸出PWM信號通過專用的電子調(diào)速器來控制電動機。本設計采用好盈公司的HOBBYWING20A有刷電子調(diào)速器來驅(qū)動電機。HOBBYWING20A有刷電子調(diào)速器

42、兼容鋰電池和鎳鎘或鎳氫兩類電池，通過其上面的跳帽可以選擇電池類型，并且它具有以下特點：（1）低電壓保護功能：鋰電池單體的低壓保護閾值為3V，因此對于2S 鋰電池組，低壓保護閾值為6V；對于3S 鋰電池組，低壓保護閾值為9V。鎳鎘/鎳氫電池組的保護電壓為4V。當進入低壓保護后，電調(diào)將逐步降低輸出功率，來防止電池過度放電從而保護電池。（2）過溫保護：當電調(diào)工作溫度超過 110 度時，電調(diào)會降低輸出功率進行保護，但不會將輸出功率全部關閉，最多只降到全功率的50%， 以保證電機仍有動力，避免摔機。溫度下降后，電調(diào)會逐漸恢復最大動力。 （3）油門信號丟失保護：當檢測到油門遙控信號丟失2 秒后，電調(diào)開始降

43、低輸出功率，如果信號始終無法恢復，則一直降到零輸出（降功率過程最長時間為2 秒）。如果在功率下降的過程中油門遙控信號重新恢復，則立即恢復油門控制，這樣做的好處是：在油門信號瞬間丟失的情況下（小于2 秒），電調(diào)并不會進行斷電保護；如果遙控信號確實長時間丟失，則進行保護，但也不是立即關閉輸出，而是有一個逐步降低輸出功率的過程，兼顧安全性和實用性。圖3-8為電子調(diào)速器的實物圖。（4）BEC輸出：該電子調(diào)速器擁有BEC輸出，其能夠?qū)⒏唠妷旱闹绷麟娹D換為5V/1A的直流電，能直接給其他模塊供電，從而省去了電壓轉換模塊簡化了電路，并且減小了飛行器的負載。圖3-8 電子調(diào)速器3.5超聲波測距模塊本設計要求飛

44、行器近地飛行，且控制飛行一定的高度，由于超聲波到達物體表面能反射能被接收器檢測到，且能應用于惡劣的環(huán)境，因此決定采用超聲波來測距。本設計的超聲波測距模塊采用HC-SR04超聲波測距傳感器，這是一種集發(fā)射與接收于一體的超聲波模塊，該模塊性能穩(wěn)定，測度距離精確，能和國外的SRF05,SRF02等超聲波測距模塊相媲美。該模塊高精度，盲區(qū)很近只有2cm，測量距離為2cm450cm非常適合近地飛行測距。該模塊采用IO觸發(fā)測距，只要向trig（控制端）輸入至少10us的高電平信號，該模塊就開始工作：自動發(fā)送8個40KHz的方波，自動檢測是否有信號返回，若有信號返回就在echo（接收端）輸出一高電平，而高電

45、平持續(xù)的時間就是超聲波從發(fā)射到返回的時間。所以測量距離=(高電平時間*聲速(340m/s)/2。圖3-9為HC-SR04超聲波測距模塊實物圖。圖3-9 超聲波傳感器模塊3.6紅外避障模塊四旋翼自主飛行器避障要求檢測到障礙物時，使飛行器減速然后通過偏航運動實現(xiàn)轉彎從而避開障礙物。因此，利用中斷即可實現(xiàn)避障，當檢測到障礙物時在中斷服務程序中實現(xiàn)避障，這樣可以使微處理器更能高效率地工作。鑒于此避障模塊采用紅外避障傳感器E18-D50NK，它是一種集發(fā)射與接收于一體的光電傳感器，發(fā)射光經(jīng)過調(diào)制后發(fā)出，接收頭對反射光進行解調(diào)輸入，有效地避免了外界可見光的干擾。同時本紅外避障傳感器通過使用透鏡，使該款傳感

46、器最遠可以檢測到80厘米的障礙物，但由于紅外光的特性，對于不同顏色的物體，能檢測的最大距離也不同：白色物體檢測的最遠，黑色物體檢測的最近。另外該傳感器檢測障礙物的距離可以根據(jù)要求通過調(diào)節(jié)尾部的電位器旋鈕進行設置。該傳感器有三根引線：紅色接高電平VCC（5V）、綠色接地、黃色為數(shù)據(jù)輸出線。當傳感器檢測到障礙物時，會通過黃色數(shù)據(jù)輸出線輸出低電平，將此線接在單片機的外部中斷觸發(fā)端口時就能觸發(fā)中斷進而進入中斷服務子程序完成避障的功能。紅外避障傳感器實物如圖3-10。圖3-10 紅外避障傳感器E18-D50NK3.7電源模塊電源模塊為四旋翼自主飛行器提供動力源，為各個模塊提供電源，尤其是電機驅(qū)動模塊，必

47、須具有較高的電壓和電流，同時由于四旋翼自主飛行器的載重很有限，所以電源系統(tǒng)應該盡可能輕，能量密度盡可能大，以便減輕飛行器的負載。因此本設計采用能量密度高的2S鋰電池供電，其電壓為7.4V足以提供驅(qū)動電機的電壓和電流。電子調(diào)速器可以直接從2S鋰電池獲取電源而單片機和其他的傳感器可以通過電子調(diào)速器的BEC輸出來獲取5V的電源。3.8飛行控制模塊將微控制系統(tǒng)、MPU6050姿態(tài)檢測模塊、超聲波模塊和紅外避障模塊進行整合，使之成為飛行控制模塊。如圖3-11為用萬用板搭建的飛行控制模塊。圖3-11 飛行控制模塊3.9本章小結本章對四旋翼自主飛行器硬件整體設計方案進行了介紹，對微控制系統(tǒng)模塊、飛行姿態(tài)檢測

48、模塊、電機驅(qū)動模塊、超聲波測距模塊、紅外避障模塊、電源模塊的功能和核心器件進行了詳細說明，在硬件理論方面實現(xiàn)了對四旋翼自主飛行器的設計。4 四旋翼自主飛行器軟件設計4.1軟件總體設計四旋翼自主飛行器軟件設計的總體目標是通過核心處理器初始化各個模塊并協(xié)調(diào)各個模塊正常工作從而使四旋翼自主飛行器能夠按照既定的方案穩(wěn)定飛行。但是由于四旋翼自主飛行器是六自由度、四輸入的欠驅(qū)動系統(tǒng)，所以被控量之間存在耦合關系，因此所設計的飛行控制算法必須能夠通過控制四個被控量來對六個量進行穩(wěn)定、有效的控制。本設計首先通過3軸陀螺儀和3軸加速度傳感器測量出3軸角速率和3軸加速度，然后通過傾斜角度與三軸角速率和三軸加速度的關

49、系從而得到陀螺儀測量的傾斜度和加速度測量的傾斜度，由于兩者都存在較大的誤差所以再將陀螺儀和加速度測量的角度進行卡爾曼濾波及融合，進而得到較精確的傾斜角度即飛行器的姿態(tài)，最后應用控制理論中的PID控制算法將飛行器的姿態(tài)變化（傾斜角度變化）轉化為控制器輸出PWM波的占空比的變化，從而控制相應的電機加速或減速實現(xiàn)飛行器平衡的保持和各種姿態(tài)變換、運動。主程序先進行各模塊的初始化，檢測、記錄傳感器的偏移量，控制初始化，然后啟動電機并加速使飛行器垂直升高，當超聲波傳感器檢測到飛行器升高到3m時執(zhí)行既定的任務（前進、旋轉等），否則電機繼續(xù)加速，飛行器繼續(xù)升高。任務完成后減速降落，其中紅外避障、姿態(tài)檢測及調(diào)整

50、都在此過程中進行，程序主流程圖如圖4-1。飛行姿態(tài)檢測及解算是通過定時器中斷每隔0.5s進行一次檢測和調(diào)整：進入中斷后先保護現(xiàn)場，然后對加速度傳感器和陀螺儀的測量值進行平均值濾波，之后再通過濾波后的測量值解算出各個軸的傾斜角，再經(jīng)過卡爾曼濾波將各個相應的軸的傾角進行融合，以便濾掉加速度傳感器和陀螺儀的各自的偏差得到更加精確的傾角值，然后再通過數(shù)字PID算法將設置值（傾角為0）與實際值作差，根據(jù)偏差來算出傾斜軸相應兩個電機減小傾斜角應該輸入的PWM的占空比，流程圖如4-2。四旋翼自主飛行器高度的測量通過超聲波收發(fā)測量高度，超聲波發(fā)出時單片機的定時器開始計時，當收到超聲波是停止計時，通過定時器計時

51、時間和超聲波傳輸速度即可測出飛行高度，流程圖如4-3。紅外避障通過紅外收發(fā)裝置檢測障礙物，若檢測到障礙物則該模塊輸出口輸出低電平，該輸出口接在單片機的外部中斷觸發(fā)口，當檢測到障礙物時，中斷觸發(fā)進入中斷服務子程序，該程序通過控制器輸出控制信號增大飛行器前邊電機轉速，減小后邊的電機轉速從而實現(xiàn)后退避障，然后再通過偏航運動實現(xiàn)飛行方向的改變，流程圖如圖4-4。 圖4-1 主流程圖 圖4-2 姿態(tài)檢測及調(diào)整流程圖超聲波測距流程圖：圖4-3 超聲波測距流程圖紅外避障中斷流程圖：圖4-4 中斷服務子程序流程圖4.2軟件開發(fā)平臺簡介本設計利用瑞薩(Renesas)公司的單片機，其軟件開發(fā)環(huán)境、程序編輯、編譯

52、、仿真運行的軟件為瑞薩公司開發(fā)的CubeSuite+，程序下載、燒寫軟件為Renesas Flash Programmer。4.2.1 CubeSuite+編譯軟件的介紹及操作方法（一）CubeSuite+的啟動1.在桌面上雙擊CubeSuite+圖標打開CubeSuite+的開始界面（CubeSuite+Start）如圖4-5。圖4-5 CubeSuite+Start界面（二）創(chuàng)建新工程在CubeSuite+Start界面上單擊下的按鈕（如圖4-4中標記處），打開如圖4-6。由于本設計使用的是RL78/G13-R5F100LEA的控制器，所以在單片機選型界面上“Microcontroller

53、”后下拉選項中選擇“RL78”，在“Using microcontroller”下面選擇框中鍵入“100LE”然后Enter，即可搜索出，然后在Project name后輸入工程名字需要注意的是“工程名稱不能起中文名稱”，在Place后點擊選擇工程所保存的路徑其中不能存放于中文路徑及桌面，最后點擊按鈕完成工程創(chuàng)建。圖4-6 單片機選型界面（三）工程代碼編輯界面簡介圖4-7 代碼編輯界面CubeSuite+軟件中自帶有代碼生成工具，在這里可以設置單片機的時鐘（Clock Generator）、端口（Port）、中斷（Interrupt）、串口（Serial）、AD轉換器（A/D Converte

54、r）、定時器(Timer)、看門狗(Watchdog Timer)、時鐘輸出或蜂鳴器輸出(Clock Output/Buzzer Output)等，用戶可以直接通過設置相關的功能來讓軟件來自動生成相關的程序代碼，省去了用戶對基本程序的編寫，大大減少了程序開發(fā)的時間和難度。（四）程序調(diào)試仿真程序編輯完成后點擊菜單中的保存按鈕或點擊【File】【Save All】然后點擊菜單中的編譯按鈕或點擊【Build】【Build Project】編譯完成輸出窗口將顯示編譯結果，如果沒有錯誤程序就可以進行調(diào)試，若有錯誤則需要檢查程序編輯的錯誤。圖4-8 輸出窗口輸出的編譯結果信息本設計采用單片機配套的仿真器R

55、L78 EZ Emulator，首先將仿真器連接到計算機，并安裝驅(qū)動，然后選擇調(diào)試工具：右鍵點擊選擇“RL78 EZ Emulator”圖4-9 仿真器的選擇點擊按鈕將程序下載到單片機中，下載成功將出現(xiàn)程序調(diào)試界面：圖4-10 程序調(diào)試界面點擊按鈕，仿真程序便開始運行，點擊按鈕仿真程序?qū)⑦M行單步運行，點擊按鈕將會使正在運行的程序復位，點擊能使程序進入函數(shù)內(nèi)部運行，點擊能使程序跳過函數(shù)運行，點擊按鈕終止仿真，點擊按鈕將會斷開仿真器于計算機的連接。4.2.2 Renesas Flash Programmer燒寫軟件的介紹及操作方法雙擊桌面上的Renesas Flash Programmer圖標將出

56、現(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端口（查看方式：我的電腦設備管理器端口）例如COM3。點擊出現(xiàn)Power Supply默認設置，點擊，

57、出現(xiàn)Project Setting默認設置，點擊。圖4-14 工程設置之后在Renesas Flash Programmer中點擊選擇要下載的程序，命令使用自動命令“Autoprocedure(EP)”然后點擊Start按鈕，Renesas Flash Programmer軟件會自動進行“連接單片機、空白檢測、擦除原來程序、燒寫、斷開連接”，正常完成會顯示“PASS”， 斷開Emulator板與計算機的連接，結束程序的燒寫。如圖4-15。圖4-15 程序燒寫成功4.3飛行姿態(tài)解算算法及程序設計中飛行姿態(tài)檢測模塊采用的是MPU6050傳感器模塊，其中融合了加速度傳感器和陀螺儀，它們相互校準從而減

58、小各自測量的誤差。4.3.1加速度傳感器測量數(shù)據(jù)解算加速度傳感器通過測量各個軸上重力加速度的分量的大小，然后再利用重力加速度的分解及各軸與參考面軸的夾角關系來實現(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)點。其直接輸出值是相對靈敏軸（陀螺儀自身的虛擬軸）的角速率，角速率對時間積分便可得到圍繞靈敏軸旋轉過的角度值。由于系統(tǒng)采用單片機采用循環(huán)采樣

59、程序獲得陀螺儀角速率信息，即每隔一段很短的時間采樣一次，所以采用累加的方法來近似實現(xiàn)積分的功能來計算轉過的角度值。陀螺儀是用來測量角速度信號的，通過對角速度的積分，能得到再很短一段時間內(nèi)轉過的角度值。但由于溫度變化、摩擦力、不穩(wěn)定力矩等因素的影響，陀螺儀會產(chǎn)生漂移誤差。而無論多么小的常值漂移誤差通過積分都將會得到無限大的角度誤差，因此需要減小該誤差。綜上所述，對于飛行姿態(tài)檢測系統(tǒng)而言，單獨使用陀螺儀或者加速度傳感器，都不能提供有效且可靠的飛行狀態(tài)信息來提供給單片機保證飛行器平衡。雖然陀螺儀動態(tài)性能良好，能夠提供瞬間的動態(tài)角度變化且不受加速度變化、振動的影響，但是由于受其本身固有的特點及積分過程

60、的影響，存在嚴重的累積漂移誤差，不適合長時間單獨工作；加速度傳感器表態(tài)響應很好，能夠準確提供表態(tài)的傾斜角度，但受動態(tài)加速度、振動的影響較大，不適合跟蹤動態(tài)物體的運動。為了克服兩者的缺點，決定采用濾波、融合的方法來融合陀螺儀和加速度傳感器的輸出信號，補償陀螺儀的漂移誤差、累積誤差和加速傳感器的動態(tài)誤差，使兩者能夠相互校正，避免各自缺點，發(fā)揮優(yōu)點，使測量得到一個更優(yōu)的傾角近似值，使姿態(tài)測量更加準確。4.4數(shù)據(jù)處理及程序本設計采用卡爾曼濾波及平均值濾波對MPU6050傳感器輸出的數(shù)據(jù)進行濾波、融合進而得到誤差小的姿態(tài)信息。4.4.1卡爾曼濾波與平均值濾波卡爾曼濾波是一種高效率、可能最優(yōu)的遞歸濾波器(

61、自回歸濾波器), 它能夠從一系列的不完全包含噪聲的測量中最優(yōu)地估計動態(tài)系統(tǒng)的狀態(tài)。假設一個離散控制過程系統(tǒng)可以用一個線性隨機微分方程來描述： （4-4）系統(tǒng)的測量值為： （4-5）其中是時刻系統(tǒng)的狀態(tài)，是時刻加在系統(tǒng)上的控制量。A、B是系統(tǒng)固有的參數(shù)，對于多模型系統(tǒng)而言，它們?yōu)榫仃嚒Ｊ菚r刻的測量值，H是測量系統(tǒng)的參數(shù)，對于多測量系統(tǒng)，H為矩陣。和分別表示系統(tǒng)過程和系統(tǒng)測量的噪聲。假設噪聲為高斯白噪聲且不隨系統(tǒng)狀態(tài)變化而變化，他們的協(xié)方差分別是Q，R。對于滿足以上條件的過程，卡爾曼濾波是最優(yōu)的處理器。首先要利用系統(tǒng)過程模型來預測下一狀態(tài)： （4-6）其中是通過上一狀態(tài)預測的結果，是上一狀態(tài)最優(yōu)的

62、結果，為現(xiàn)在狀態(tài)的控制量。對應于的協(xié)方差為： （4-7）其中是對應的協(xié)方差，是對應的協(xié)方差，表示的轉置矩陣，是系統(tǒng)過程的協(xié)方差。以上是對系統(tǒng)的預測，然后收集對系統(tǒng)現(xiàn)在狀態(tài)的測量值，通過融合預測值和測量值就可以得到現(xiàn)在狀態(tài)（）的最優(yōu)估計值： （4-8）其中為卡爾曼增益（Kalman Gain）： （4-9）為了讓卡爾曼濾波器不斷地運行下去，就還要更新狀態(tài)下的協(xié)方差： （4-10）其中為單位陣，對于單模型單測量系統(tǒng)而言=1。當系統(tǒng)進入（下一狀態(tài)）狀態(tài)時，就變?yōu)榇藭r的。如此，就可以自回歸地運算下去，實現(xiàn)最優(yōu)化估計。平均值濾波通過求n個值的平均值，將平均值作為測量值，這樣使測量值更接近實際值，能夠減小隨機誤差，減弱外界干擾對控制過程的影響。4.4.2卡爾曼濾波、平均值濾波程序（1）卡爾曼濾波程序vo