室內(nèi)小型四旋翼無人機的姿態(tài)與位置控制-畢業(yè)論文

1、<p><b>　　碩士畢業(yè)論文</b></p><p><b>　?。?0 屆）</b></p><p>　　室內(nèi)小型四旋翼無人機的姿態(tài)與位置控制</p><p>　　所在學院 </p><p>　　專業(yè)班級 控

2、制工程 </p><p>　　學生姓名 學號 </p><p>　　指導教師 職稱 </p><p>　　完成日期 年 月 </p><p><b>　　獨創(chuàng)性聲明</b&

3、gt;</p><p>　　本人聲明，所呈交的學位論文是在導師的指導下完成的。論文中取得的研究成果除加以標注和致謝的地方外，不包含其他人己經(jīng)發(fā)表或撰寫過的研究成果，也不包括本人為獲得其他學位而使用過的材料。與我一同工作的同志對本研究所做的任何貢獻均己在論文中作了明確的說明并表示謝意。</p><p><b>　　學位論文作者簽名：</b></p><

4、;p><b>　　日 期：</b></p><p>　　學位論文版權使用授權書</p><p>　　本學位論文作者和指導教師完全了解東北大學有關保留、使用學位論文的規(guī)定：即學校有權保留并向國家有關部門或機構送交論文的復印件和磁盤，允許論文被查閱和借閱。本人同意東北大學可以將學位論文的全部或部分內(nèi)容編入有關數(shù)據(jù)庫進行檢索、交流。</p><

5、;p>　　作者和導師同意網(wǎng)上交流的時間為作者獲得學位后：</p><p>　　半年 □ 一年□ 一年半□ 兩年□</p><p>　　學位論文作者簽名： 導師簽名：</p><p>　　簽字日期： 簽字日期：</p><p>　　室內(nèi)小型四旋

6、翼無人機的姿態(tài)與位置控制</p><p><b>　　摘 要</b></p><p>　　地震、火災等自然災害具有突發(fā)性特點，災害應急救援的關鍵是災害發(fā)生后的快速反應。及時快捷的災情信息對于及時制定救援策略，提高救援效率和質(zhì)量起著至關重要的作用。由于四旋翼無人機良好的機動性和靈活性，四旋翼在災難救援活動中逐步顯示出巨大的優(yōu)勢。然而,災害現(xiàn)場室內(nèi)環(huán)境復雜多變而難以預知

7、，給無人機自主導航和探索帶來了很大的困難。本文針對未知室內(nèi)環(huán)境研究無人機姿態(tài)與位置控制方法，為無人機在未知室內(nèi)環(huán)境中自主探索提供現(xiàn)實的可能性。</p><p>　　本文首先對題目相關技術的國內(nèi)外研究現(xiàn)狀進行了深入的分析。在此基礎上，對小型四旋翼無人機進行了運動學和動力學研究，建立了動力學和運動學模型。同時闡釋了兩種典型的對室內(nèi)小型四旋翼無人機飛行影響較大的空氣動力學現(xiàn)象，說明了這兩種現(xiàn)象對無人機飛行的影響和它們在

8、無人機控制系統(tǒng)研究中的作用，為后續(xù)章節(jié)中的理論分析部分和濾波器設計等問題奠定了理論基礎。</p><p>　　其次，本文研究了小型四旋翼無人機的姿態(tài)檢測和控制方法。通過對多種姿態(tài)檢測傳感器的數(shù)據(jù)進行融合，提高了無人機姿態(tài)檢測的實時性和精度，并構建了基于Arduino的無人機姿態(tài)控制系統(tǒng)。實現(xiàn)了閉環(huán)的小型四旋翼無人機的姿態(tài)控制。在懸停狀態(tài)下，無人機姿態(tài)控制誤差約為，存在周期約為0.5s的震動。</p>

9、<p>　　再次，本文實分析了基于ROS的小型四旋翼無人機的室內(nèi)未知環(huán)境中的位置檢測算法，包括單目視覺方法，立體視覺方法和激光雷達測距方法，并展開了實驗研究。通過對幾種算法的比較，確定了室內(nèi)小型四旋翼無人機的定位算法，在定位的同時生成室內(nèi)地圖，為無人機在未知室內(nèi)環(huán)境中進行探索和救援提供了現(xiàn)實的可能。在此基礎上，本文完成了四旋翼無人機的位置控制系統(tǒng)，并進行了室外依靠GPS的位置控制實驗，控制誤差可以控制在2m以內(nèi)，證明了位置控

10、制器的有效性。</p><p>　　最后，本文將位置檢測、位置控制、姿態(tài)檢測和姿態(tài)控制四個部分整合到一起，設計了更加完善的四旋翼無人機機體，提出了一種未知環(huán)境中自動探索的路徑規(guī)劃算法，形成了室內(nèi)小型四旋翼無人機系統(tǒng)，并進行了相關的實驗。在沒有GPS輔助定位的室內(nèi)環(huán)境中，無人機位置控制的精度可以達到，在長達5分鐘的自主位置控制實驗中，沒有觀察到明顯的位置漂移。</p><p>　　實驗結(jié)果表

11、明，完全自主的四旋翼無人機可以用于位置室內(nèi)環(huán)境的建圖與探索。</p><p>　　關鍵詞：室內(nèi)環(huán)境；四旋翼無人機；姿態(tài)與位置控制；ROS操作系統(tǒng)</p><p>　　Position and Attitude Control for a Indoor Mini Quadrotor UAV</p><p><b>　　Abstract</b>&

12、lt;/p><p>　　Natural disasters such as Earthquakes and fires burst out all in sudden, response after the disaster rapidly is the key to rescue, and timely and efficient information is the key to response after t

13、he disaster. Quadrotor in disaster rescueing has a great advantage as the good mobility and flexibility. However, indoor environmental after disaster are complex and hard to predict, for UAV navigation and exploration, g

14、reat difficulties are encountered. Aiming the unknown indoor environments, study o</p><p>　　Firstly, the current studies on the related technology were introduced. On this basis, a Mini quadrotor kinematics

15、and dunamics were studied and models were builded. Two main aerodynamic phenomenon and its affect on control system of quadrotor were illustrated which lays a theoretical foundation for the subsequent chapters. </p>

16、;<p>　　Secondly, the paper studies the attitude detection and control methods of small quadrotor. Through fusion of variety of alttitude sensor, timeliness and accuracy were improved. Quadrotor attitude control sy

17、stem was constructed based on the Arduino. In the hovering, Quadrotor attitude control has an error about ± 2°, vibration period of about 0.5s.</p><p>　　Then, with the base of ROS, positioning and

18、mapping of indoor unknown environment algorithms were experimental studied, including monocular vision, stereo vision and laser radar methods. Through the comparison of several algorithms,the algorithm for mini quadrotor

19、 indoor positioning were resolved, that enabled the quadrotor to explore and rescue in unknown indoor environment. A quadrotor position control system were builded, and rely on the GPS positioning such control system wer

20、e proved by expe</p><p>　　Finally, the position detection, position control, attitude detection and attitude control were made up into a quadrotor control system, a better quadrotor airframe were designed, a

21、nd a path planning algorithm for automatic discovery of unknown environments were proposed. Related experiments in unknown environment without GPS shows that the quadrotor position control accuracy of about ± 1m, fo

22、r up to five minutes no significant position drift were detected.</p><p>　　Experimental results show that fully autonomous quadrotor UAV can be used in exploration and mapping the unknown indoor environment.

23、</p><p>　　Key words: Indoor; quadrotor; Attitude and position control; ROS</p><p><b>　　緒 論</b></p><p>　　課題研究背景、目的及意義</p><p>　　近年來，無人機（UAV）在軍用和民用各個方面發(fā)揮了舉足輕重的

24、作用，受到了社會各界的廣泛關注，國內(nèi)外許多研究小組針對無人機的理論和應用進行了大量的研究。由于無人機可以進入很多人類無法到達的危險環(huán)境，在一些復雜而危險的環(huán)境中，擁有高度自主能力的無人機可以完成包括探測、監(jiān)視、搜救等意義重大的任務。</p><p>　　自然災害具有突發(fā)性特點，災害應急救援的關鍵是災害發(fā)生后的快速反應。及時快捷的災情信息對于及時制定救援策略，提高救援效率和質(zhì)量起著至關重要的作用。近年來，四旋翼無人

25、飛行器以其諸多優(yōu)點和在無人偵查、交通管理、森林防火、城市巡邏等領域的廣闊應用前景，成為國際上的研究熱點。</p><p>　　四旋翼無人機（Quadrotor UAV）是一種能夠垂直起降的六自由度飛行器，與固定翼無人機相比，四旋翼無人機能夠完成懸停、低速飛行、垂直起降等固定翼無人機無法完成的任務。相比于傳統(tǒng)意義上的直升飛機，四旋翼無人機通過分別控制四個旋翼產(chǎn)生的升力實現(xiàn)穩(wěn)定精確的飛行控制，這種結(jié)構具有結(jié)構簡單、緊

26、湊的優(yōu)點。因此，四旋翼無人機具有制造精度要求較低、結(jié)構穩(wěn)定性更好、陀螺效應較弱等優(yōu)勢，可以更好的實現(xiàn)飛行器的控制[2]，更加適合城市復雜環(huán)境和室內(nèi)飛行環(huán)境。利用四旋翼無人機在未知室內(nèi)環(huán)境中進行自主飛行和探索在軍用和民用各個領域都可以發(fā)揮重要的作用。</p><p>　　無人機在環(huán)境中的自我定位能力，對無人機的自主性有決定性的作用。事實上，為了提高飛行性能和根據(jù)環(huán)境靈活的進行任務規(guī)劃，精確定位是非常重要的。在電子通

27、信和計算機領域的不斷的突破與創(chuàng)新，如傳感系統(tǒng)，全球衛(wèi)星定位系統(tǒng)（GPS），航位推算系統(tǒng)，無線電技術以及機載的視覺傳感器和圖像處理技術，為無人機在復雜環(huán)境中定位與導航，并完成相對復雜的任務提供了可能。由于室內(nèi)環(huán)境的特殊性，無人機無法通過GPS等方法方便的獲得定位信息。即時定位與構圖（Simultaneous Localization and Mapping，SLAM）是一種廣泛應用于地面機器人和無人機上的未知區(qū)域內(nèi)的建圖與定位技術，在無人

28、機導航方面有著很好的應用潛力。</p><p>　　本研究室內(nèi)小型四旋翼無人機的位置與姿態(tài)閉環(huán)控制方法。其中，室內(nèi)未知環(huán)境中的定位問題是在室內(nèi)完成小型無人機實驗的關鍵。因此，我們將著重研究室內(nèi)未知環(huán)境中的無人機定位方法。</p><p>　　本文研究課題來源于國家自然科學基金《基于多媒體傳感器網(wǎng)絡的建筑災難現(xiàn)場三維全景實時重建關鍵問題研究》（編號：61273078）和東北大學985工程三期

29、導向類研究項目《復雜環(huán)境下突發(fā)災難信息精確感知與救援智能決策系統(tǒng)》，目的是研究和解決實現(xiàn)無人機自主探索的關鍵性技術問題。</p><p><b>　　國內(nèi)外研究現(xiàn)狀</b></p><p>　　近年來室內(nèi)小型四旋翼無人機發(fā)展日趨迅速，其導航系統(tǒng)的微型化及其室內(nèi)應用需求給導航引導技術帶來新的挑戰(zhàn)。賓夕法尼亞大學、麻省理工大學、斯坦福大學及北京航空航天大學等國內(nèi)外著名高校

30、積極開展室內(nèi)微型無人機自主導航引導技術研究。一年一度的國際空中機器人競賽 (IARC) 第五、六階段任務（即室內(nèi)探索搜尋任務）進一步促進了室內(nèi)未知環(huán)境中無人機自主定位與導航技術的發(fā)展[3]。</p><p><b>　　國外研究現(xiàn)狀</b></p><p>　　目前國外四旋翼無人機的研究正在緊鑼密鼓的進行，已經(jīng)有許多四旋翼無人機項目取得了世界矚目的成果。</p&

31、gt;<p>　　STARMAC項目（Stanford Testbed of Autonomous Rotorcraft for Multi-AgentControl，如圖 1.1）是有斯坦福大學的Gabriel Hoffmann教授主持設計和研發(fā)的多智能體控制四旋翼無人機研究平臺。這個無人機系統(tǒng)包括兩個級聯(lián)的控制器，一套慣性測量單元，一個超聲波傳感器和一套高精度差分GPS設備，并配有一套地面站，通過藍牙或Wi-fi進行通

32、信。</p><p>　　圖 1.1斯坦福大學的 STARMAC</p><p>　　Fig. 1.1 STARMAC from Stanford</p><p>　　賓西法尼亞大學的Quad-Rotor 項目利用HMX-4作為研平臺，HMX4 [4]重約700g，長76cm，并在機體下方布置了五個用于圖像識別的標志。地面攝像機通過檢測盒跟蹤標記點的位置得到無人機的

33、位置與姿態(tài)信息，并與機載陀螺儀獲得的角速度數(shù)據(jù)進行融合，用于無人機的控制。</p><p>　　同時，HMX4開發(fā)團隊開發(fā)了一套地面和機載雙攝像頭的位置與姿態(tài)估計系統(tǒng)，可以進一步提高位置和姿態(tài)測量的精度。目前，賓夕法尼亞大學的Grasp實驗室利用Ascending Technology的蜂鳥無人機平臺和ViconMX系統(tǒng)（主要包含硬件控制模塊、攝像機、計算機主機和分析和處理軟件等，該系統(tǒng)能夠同多種外接設備相連）準

34、確獲取無人機位置，并實現(xiàn)了室內(nèi)狹小環(huán)境中的大機動動作飛行，抓取物品以及多機協(xié)同等相對復雜的任務（圖 1.2）。</p><p>　　圖 1.2 賓夕法尼亞大學Grasp 實驗室的無人機實現(xiàn)了室內(nèi)狹小環(huán)境中的大機動動作飛行，抓取物品以及多機協(xié)同等相對復雜的任務</p><p>　　Fig. 1.2 University of Pennsylvania Grasp UAV Laborator

35、y make many complex tasks possible</p><p>　　除各高校以及研究所致力于四旋翼無人機的研制，各國一些著名的公司也在緊鑼密鼓的進行四旋翼無人機的研制。其中較為著名的包括美國 Draganflyer Innovations 公司研制的Draganflyer，德國 Ascending Technologies GmBH 公司研制的 Pelican 無人機（如圖 1.3），GmB

36、H 公司研制的 Falcon8、X4-Four-Rotor，德國的 MikroKopter 等。這些系統(tǒng)的研究，為本文的無人機系統(tǒng)總體構架提供了有益的參考和借鑒。</p><p>　　圖 1.3 Asctec 公司公司研制的pelican無人機</p><p>　　Fig. 1.3 pelican from Asctec</p><p>　　在定位方面，SLAM（S

37、imultaneous Localization and Mapping）算法，即同步定位與地圖構建算法，正逐步向無人機應用拓展，對引導無人機自主探索未知環(huán)境具有極其重要的意義。SLAM算法首先提取場景特征作為地標，解算地標相對于載體的位置。當室內(nèi)無人機再次進入該區(qū)域后，通過對記錄的地標模板進行匹配，可以判斷無人機全局位置信息。SLAM算法的研究重點在于地標的表示、數(shù)據(jù)關聯(lián)以及位置誤差、位姿解算誤差和錯誤的數(shù)據(jù)關聯(lián)帶來的積累誤差。采用擴

38、展卡爾曼濾波等方式可以有效提高地圖建立的精度和魯棒性。相關研究在地面機器人領域早已開始并取得了豐碩的成果。然而室內(nèi)無人機自由度較多，載荷較小，給SLAM算法中地標的描述方法帶來了新的挑戰(zhàn)[3]。 </p><p><b>　　國內(nèi)研究現(xiàn)狀</b></p><p>　　我國的四旋翼無人機研究已經(jīng)取得了一定的進展，大疆無人機公司用于室外飛行的四旋翼無人機產(chǎn)品得到了國內(nèi)外用

39、戶的廣泛認可（圖 1.4）。在GPS導航的情況下，其姿態(tài)與位置控制相當精確和穩(wěn)定。</p><p>　　圖 1.4 大疆飛控公司的無人機</p><p>　　Fig. 1.4 UAV made by DIJ</p><p>　　在我國，無人機在未知環(huán)境中的位置檢測方法已經(jīng)探索了幾種可行的辦法，包括視覺方法和SLAM等。</p><p>　　基

40、于視覺信息的地標識別與位姿估計方法常用于無人機的輔助起飛與降落。由于地標的形狀位置和方向等空間幾何特征已知，用已知參數(shù)的攝像頭提取地標幾何特征，利用攝像機投影關系，可以獲得無人機的位姿信息。浙江大學的任沁源利用嵌入先驗知識的Boost算法識別地標，通過SUSAN角點檢測算法提取匹配特征點，結(jié)合levenberg-marquardt算法獲得無人直升機位姿參數(shù)的估計[9]?；谝曈X信息的地標識別與位姿估計方法適用于已知飛行環(huán)境的情況，無法應

41、用于未知環(huán)境，因此其應用范圍有限，但是該方法簡單可靠，相關研究也較為成熟。</p><p>　　天津大學研究了大尺度環(huán)境下移動機器人同步定位與地圖創(chuàng)建。其文獻提出了一種針對大尺度未知數(shù)據(jù)關聯(lián)問題中的FastSLAM新方法。該方法采用環(huán)境否定信息方法及單個粒子的極大似然數(shù)據(jù)關聯(lián)的方法相結(jié)合，這樣既保證了當前時刻運動噪聲不會對下一步的關聯(lián)數(shù)據(jù)的精度產(chǎn)生影響，也避免了所謂的“失蹤”問題；同時，還保證了能及時刪除添加到環(huán)

42、境地圖中的錯誤特征。改進的方法解決了大尺度未知環(huán)境下的數(shù)據(jù)關聯(lián)問題，可真正實現(xiàn)機器人在未知環(huán)境的自主導航。</p><p>　　立體視覺定位在機器人領域的應用較為成熟[10]。雙目立體視覺通過匹配左右視野的圖像特征，利用空間特征的投影關系，獲得載體的相對位置信息。北航張博翰等人在2010年提出基于無人機雙目立體視覺[11]，依靠區(qū)域灰度相關法進行立體匹配，實現(xiàn)走廊中橫向位置坐標定位并檢測未知環(huán)境中的障礙物，并生成

43、避障導航點的方法。</p><p>　　論文研究內(nèi)容與章節(jié)安排</p><p>　　本文將四旋翼無人機在室內(nèi)未知環(huán)境中遇到的問題分為了四個部分，姿態(tài)檢測、姿態(tài)控制、位置檢測和位置控制。檢測、姿態(tài)控制兩個部分組成了無人機的姿態(tài)控制系統(tǒng)。位置檢測、位置控制兩部分組成了無人機的位置控制系統(tǒng)。</p><p>　　這四個部分中，姿態(tài)檢測和姿態(tài)控制是無人機的基礎，其控制精度直

44、接決定了無人機的性能。</p><p>　　在未知室內(nèi)環(huán)境中的無人機的位置檢測問題很長時間以來是機器人領域的前沿問題。通過查閱文獻和尋求開源項目的幫助，我們比較了三種主流的位置檢測方法，并將其中一種最為可行的方法應用到了無人機中。</p><p>　　無人機位置的控制器設計是工作量最大的部分，其中涉及多種不同的協(xié)議、接口和軟件平臺。同樣的，由于位置檢測過程中難以克服的不穩(wěn)定的滯后和數(shù)據(jù)抖動

45、，需要控制器有相當好的魯棒性。</p><p><b>　　論文研究內(nèi)容</b></p><p>　　本文研究了小型四旋翼無人機在未知室內(nèi)環(huán)境中自主進行姿態(tài)與位置控制的方法，主要包括系統(tǒng)硬件搭建，軟件開發(fā)和實驗等三個方面。課題研究目的在于構建可以未知室內(nèi)環(huán)境中不依賴基礎設施進行自主定位、自主飛行的無人機系統(tǒng)，對室內(nèi)環(huán)境進行自主探索并將探索到的室內(nèi)環(huán)境實時的傳回地面。具

46、體內(nèi)容如下：</p><p>　　(1) 開發(fā)精度較高的無人機姿態(tài)控制系統(tǒng)，并為位置控制器預留輸入接口。</p><p>　　(2) 研究未知室內(nèi)環(huán)境中無人機的定位方法，并設計針對該算法的位置控制器。在ROS開源機器人操作系統(tǒng)中實現(xiàn)這種定位和控制算法，并與底層姿態(tài)控制器級聯(lián)，實現(xiàn)無人機的姿態(tài)與位置控制。</p><p>　　(3) 設計并構建四旋翼飛行器，對控制算法

47、進行實驗驗證。</p><p><b>　　論文章節(jié)安排</b></p><p>　　根據(jù)研究內(nèi)容，論文章節(jié)被安排如下：</p><p>　　第一章闡述論文的研究背景、目的及意義，給出了論文研究內(nèi)容和框架安排。</p><p>　　第二章闡述四旋翼無人機的原理，對四旋翼無人機進行了動力學和運動學分析，為四旋翼無人機的姿態(tài)

48、和位置控制奠定理論基礎。</p><p>　　第三章分析四旋翼無人機的姿態(tài)檢測和控制方法，設計多傳感器數(shù)據(jù)融合算法，將陀螺儀數(shù)據(jù)和加速度傳感器數(shù)據(jù)相結(jié)合，實現(xiàn)了四旋翼無人機姿態(tài)控制器的硬件和控制軟件，并對無人機的姿態(tài)控制器進行了實驗驗證。</p><p>　　第四章通過實驗比較不同的室內(nèi)未知環(huán)境位置檢測方法，開發(fā)無人機位置控制軟件，并在室外環(huán)境對無人機的位置控制軟件盡心了實驗</p&

49、gt;<p>　　第五章設計了小型四旋翼無人機的機械和動力系統(tǒng)，定義所有的接口，在仿真環(huán)境和真實室內(nèi)環(huán)境中進行了實驗，實現(xiàn)了小型四旋翼無人機在室內(nèi)環(huán)境中的姿態(tài)與位置控制。</p><p>　　第六章對本論文的工作進行了回顧與總結(jié)，并對課題的進一步研究做出了展望。</p><p>　　本文的架構圖如圖 1.5所示。</p><p>　　圖 1.5論文結(jié)構

50、圖</p><p>　　Fig. 1.5 Structure diagram of the thesis</p><p>　　四旋翼無人機的空氣動力學分析</p><p>　　本章分析了四旋翼無人機的基本原理，通過理論分析和數(shù)學推導，建立了四旋翼無人機在理想情況下的動力學和運動學模型。之后本文通過對四個旋翼進行空氣動力學分析，修正了理想情況下的無人機模型，為后續(xù)章節(jié)

51、四旋翼無人機姿態(tài)和位置控制奠定了理論基礎。</p><p>　　四旋翼無人機的基本原理</p><p>　　四旋翼無人機是最常見的多旋翼無人機。其機械結(jié)構十分簡單，四個獨立的螺旋槳固定安裝在十字型的機架上（如圖 2.1）。四旋翼無人機的控制通過分別控制四個電動機的輸出實現(xiàn)。</p><p>　　圖 2.1 Asctec 公司生產(chǎn)的一款研究用四旋翼無人機</p&

52、gt;<p>　　Fig. 2.1 A quadrotor made by Ascending Technologies</p><p>　　在理想情況下，四旋翼無人機四個旋翼安裝在同一水平面上，四個旋翼均勻的分布在飛機幾何中心的四周，相對的兩個旋翼旋轉(zhuǎn)方向相同，而相鄰的兩個旋翼旋轉(zhuǎn)方向相反。通?？梢远x其中一個逆時針旋轉(zhuǎn)的旋翼為飛行器的前進方向，本章為了方便分析，暫時使用這種定義。在后面的章節(jié)中，

53、為了方便無人機的室內(nèi)飛行，將確定的兩個旋翼旋轉(zhuǎn)中心與飛行器質(zhì)心連線的角平分線作為前進方向，這樣做可以提高飛行器對狹小區(qū)域的通過性，具體方法將在第三章中詳細描述。四旋翼無人機在空間中具有六個空間自由度，其中四個是可控的基本運動，因而機是一個欠驅(qū)動系統(tǒng)。這六個空間自由度包括三個方向上的位移（x，y，z）和航向角、俯仰角、橫滾角（roll，pitch，yaw）三個姿態(tài)角（Fig. 2.3）[29]。</p><p>　

54、　圖 2.2四旋翼無人機的基本原理</p><p>　　Fig. 2.2 The basic principles of quadrotor</p><p>　　四旋翼無人機的位置和姿態(tài)控制是通過分別調(diào)節(jié)四個旋翼的推力完成的。由圖 2.2可知，無人機的俯仰和總推力（roll，pitch，thrust）可以直接通過調(diào)整四個旋翼的推力控制。同時增加四個螺旋槳的推力，使旋翼產(chǎn)生的總推力大于機體本

55、身的重力，即可產(chǎn)生向上的加速度；增加旋翼3的推力，同時減小旋翼1的推力，則會產(chǎn)生繞y軸的正力矩，產(chǎn)生繞y軸的角加速度，從而改變無人機的俯仰角；增加旋翼2的升力，同時減小旋翼4的升力，會產(chǎn)生繞x軸的角加速度，從而改變無人機的橫滾角。以上三種方法同樣可以應用于推力減小的相反情況。</p><p>　　圖 2.3 利用roll、pitch、yaw表達無人機姿態(tài)</p><p>　　Fig. 2.

56、3 altitude expressed by roll，pitch and yaw</p><p>　　水平轉(zhuǎn)動（yaw）所需要的力矩是通過四個旋翼轉(zhuǎn)動產(chǎn)生的反力矩提供的，旋翼1、3可以提供逆時針的反力矩，旋翼2、4可以提供順時針的反力矩，同時增加旋翼1、3的推力，減小2、4的推力，則無人機繞y軸的角加速度增大，同時減小旋翼1、3的推力，增大2、4的推力，則無人機繞y軸的角加速度減小。不會影響無人機繞x、y軸的

57、角加速度，也不會影響無人機的總推力。</p><p>　　由于四旋翼無人機系統(tǒng)是欠驅(qū)動的，其多余的兩個自由度對應于大地坐標系中x-y方向的速度，這兩個多余的自由度必須通過系統(tǒng)動力學進行控制[31]。</p><p>　　理想情況下四旋翼無人機模型</p><p>　　理想情況下四旋翼無人機剛體運動學模型</p><p>　　如圖 2.4所示，

58、坐標系{A}為右手慣性坐標系系，，為坐標系{A}中的三個單位向量。利用向量r=(x，y，z)∈{A}表示無人機機體中心在坐標系{A}中的位置。坐標系{B}為無人機機體坐標系，為坐標系{B}中的三個單位向量。無人機的姿態(tài)由旋轉(zhuǎn)矩陣：</p><p><b>　　(2.1)</b></p><p>　　式中，，表示了矩陣b中三個規(guī)范正交基在{A}中的表達。</p&g

59、t;<p>　　本文用Z-X-Y的順序構建歐拉角，首先令坐標系{A}沿旋轉(zhuǎn)產(chǎn)生偏航角，得到新坐標系{E}，新的坐標系有規(guī)范正交基{}，其中與重合，之后，繞新坐標中的x軸旋轉(zhuǎn)產(chǎn)生滾轉(zhuǎn)角，最后y軸旋轉(zhuǎn)產(chǎn)生俯仰角。</p><p>　　圖 2.4四旋翼無人機的姿態(tài)和位置在全局坐標系中的表達</p><p>　　Fig. 2.4 The position and orientatio

60、n of the robot in the global frame are denoted by ξand R respectively</p><p>　　在確定了roll，pitch和yaw的定義后可以方便的得到坐標變換矩陣：</p><p><b>　　(2.2)</b></p><p>　　令表示無人機質(zhì)心的位置，表示{B}相對于{

61、A}運動的線速度，表示{B}相對于{A}運動的角速度，m表示無人機質(zhì)量，表示無人機的恒慣量矩陣（在機體坐標系{B}中表示），則理想無人機的剛體運動學方程可以表示為：</p><p><b>　　(2.3a)</b></p><p><b>　　(2.3b)</b></p><p><b>　　(2.3c)<

62、/b></p><p><b>　　(2.3d)</b></p><p>　　表示Ω的斜對稱矩陣，即。向量，∈{B}表示了由旋翼提供給無人機機體的主要的非保守力和力矩。</p><p>　　理想情況下四旋翼無人機動力學模型</p><p>　　在理想情況下，假設四旋翼無人機所受外力完全由旋翼產(chǎn)生，并且作用在旋翼的旋

63、轉(zhuǎn)軸上，而且與旋翼選裝方向重合。</p><p>　　在無人機平穩(wěn)懸停的情況下（旋翼沒有相對于空氣的水平運動和垂直揮舞）旋翼推力可以表示為:</p><p><b>　　(2.4)</b></p><p>　　其中對于旋翼i，表示旋翼旋轉(zhuǎn)所覆蓋的面積，表示旋翼半徑，表示旋翼的角速度，表示了旋翼的效率，它是由旋翼的截面形狀和輪廓決定的，通?？梢酝?/p>

64、過實驗得到，表示空氣密度。對于在相對穩(wěn)定環(huán)境中運動的確定旋翼來說，旋翼推力公式可以簡化為：</p><p><b>　　(2.5)</b></p><p>　　根據(jù)文獻[32]中的結(jié)論，旋翼的反力矩與推力成正比關系：</p><p><b>　　(2.6)</b></p><p>　　同樣的，公式中

65、（與、 和 有關）也是可以通過實驗得到的常數(shù)。</p><p>　　盡管在實際空氣中運動的旋翼情實際況比較復雜，本文暫時假設旋翼推力總是沿無人機軸線向上的（在后兩個小節(jié)中詳細討論并進行修正）。對于一個N旋翼無人機，我們把N個旋翼編號為，x軸正方向編號1，序號逆時針增加（如圖2.1所示）。</p><p><b>　　總推力公式：</b></p><

66、;p><b>　　(2.7a)</b></p><p><b>　　(2.7b)</b></p><p>　　用表示空氣作用在無人機機體上的扭矩，可得扭矩公式：</p><p><b>　　(2.8a)</b></p><p><b>　　(2.8b)</

67、b></p><p><b>　　(2.8c)</b></p><p>　　對于四旋翼飛行器，可以將以上方程組寫為矩陣形式：</p><p><b>　　(2.9)</b></p><p>　　給定推力和無人機的瞬態(tài)，則可以得到無人機旋翼的轉(zhuǎn)速，為了使無人機穩(wěn)定懸停，應該使，， 。</p

68、><p>　　四旋翼無人機中主要的空氣動力學效應</p><p>　　旋翼的空氣動力學原理在載人直升飛機中得到了充分的研究，在文獻[32][33]中的闡釋非常充分，其中的很多內(nèi)容對于設計完整尺寸的直升飛機有著重要的意義。但對于小型四旋翼無人機來說，其中的旋翼和電動機是從標準的航模設備中獲得的，常用的航模設備有固定的搭配，可以的選擇空間不大。因此關于旋翼設計的大多數(shù)復雜的空氣動力學問題可以忽略，

69、這里僅對那些對飛行影響較大的空氣動力學效應進行必要的分析。</p><p>　　在電動機轉(zhuǎn)子和螺旋槳的運動過程中，有許多與陀螺效應和空氣動力學相關的力作用在四旋翼無人機上。對于微型四旋翼無人機來說，其中大多數(shù)力非常微小，幾乎不會對無人機的飛行產(chǎn)生影響，可以忽略不計。但槳葉揮舞和誘導阻力對于研究無人機的穩(wěn)定性和傳感器的工作原理有很大幫助。</p><p>　　四旋翼無人機通常使用比較輕便的，

70、螺距不可調(diào)節(jié)的塑料或纖維材料制成的固定旋翼。這種旋翼通常比較柔軟，很容易彎曲。飛行過程中，高速旋轉(zhuǎn)的旋翼上的空氣動力的作用和慣性的作用對旋翼本身的影響十分顯著，這些力會導致旋翼明顯變形。從某種程度來說，這些變形時有意義的，如果旋翼的剛度太大，則剛才提到的空氣動力學作用和旋翼的慣性作用都會直接作用在電動機的轉(zhuǎn)動軸上，或者這些力將作用在無人機機體上，這將給電動機的轉(zhuǎn)動軸或者機體造成破壞。因此，小型四旋翼無人機的旋翼要明顯比全尺寸飛行器更加柔

71、軟。槳葉揮舞運動是由于槳葉的完全造成的，而誘導阻力則與槳葉的剛度有關，對于一個典型的小型四旋翼無人機的槳葉來說，這兩個作是同時發(fā)生的。由于二者的數(shù)學表達相近，可以用一個集總參數(shù)將正兩個參數(shù)的作用表達出來。</p><p><b>　　槳葉揮舞運動</b></p><p>　　槳葉揮舞運動指的是螺旋槳在轉(zhuǎn)動過程中的上下方運動，這種運動導致了旋翼升力的不平衡，使得旋翼推力

72、并不總是沿機體z軸向上的（如圖 2.5所示）。這種現(xiàn)象為加速度計測量無人機姿態(tài)提供了可能性。</p><p><b>　　圖 2.5槳葉揮舞</b></p><p>　　Fig. 2.5 blade flapping</p><p>　　如圖 2.6，在無人機向前運動的過程中，向前運動的葉片比向后運動的葉片有更高的相對空氣的速度，更高的相對速度

73、導致了升力的增加。旋翼向上揮舞，使旋翼的相對迎風角度變小，從而使升力的增加量減小，在旋翼向后的過程中，這個規(guī)律對于相反方向的槳葉揮舞同樣成立。</p><p>　　圖 2.6 旋翼位置與相對氣流方向的關系</p><p>　　Fig. 2.6 Relate between rotor pose and related wind</p><p>　　因此，如圖 2.7

74、，在迎風運動的槳葉上，槳葉最右邊的旋翼有最高的爬升速度，槳葉最左邊的旋翼有最大的下降速度，而如圖 2.7，最前端的槳葉升力最大，最后邊的槳葉升力最小。</p><p>　　圖 2.7槳葉揮舞過程中揮舞運動與攻角變化</p><p>　　Fig. 2.7 Change angle of attack in blade flapping</p><p>　　小型四旋翼無

75、人機在穩(wěn)定懸停的情況下，槳葉揮舞產(chǎn)生的影響比較顯著[32]。為方便研究槳葉揮舞對無人機的影響，假設無人機僅沿大地坐標系的X軸運動，即，則有：</p><p><b>　　(2.10)</b></p><p>　　這里與是兩個正常數(shù)，，表示穩(wěn)態(tài)情況下旋翼面徑向于入射氣流傾斜的角度，而表示旋翼面法相傾斜的角度。由于無人機鐘兩個相鄰的螺旋槳旋轉(zhuǎn)方向相反，總是成對的互相抵消。

76、根據(jù)文獻[6]，對于四旋翼無人機來說，四個旋翼的槳葉揮舞響應可以表達為：</p><p><b>　　(2.11)</b></p><p><b>　　誘導阻力</b></p><p>　　當機翼產(chǎn)生升力時，機翼下表面的壓力比上表面的大，而機翼翼展長度又是有限的，所以下翼面的高壓氣流會繞過兩端翼尖，向上翼面的低壓區(qū)流去（如

77、圖2.7）。當氣流繞過翼尖時，在翼尖部份形成旋渦，這種旋渦的不斷產(chǎn)生而又不斷地向后流去即形成了所謂翼尖渦流。</p><p>　　圖 2.8誘導阻力的形成</p><p>　　Fig. 2.8 Induced Drag</p><p>　　翼尖渦流使流過機翼的空氣產(chǎn)生下洗速度，而向下傾斜形成下洗流。氣流方向向下傾斜的角度，叫下洗角。由翼尖渦流產(chǎn)生的下洗速度，在兩翼尖

78、處最大，向中心逐漸減少，在中心處最小。這是因為空氣有粘性，翼尖旋渦會帶動它周圍的空氣一起旋轉(zhuǎn)，越靠內(nèi)圈，旋轉(zhuǎn)越快，越靠外圈，旋轉(zhuǎn)越慢。因此離翼尖越遠，氣流下洗速度越小。</p><p>　　將旋翼的彈性變形簡化的假設為一個簡單的扭轉(zhuǎn)彈簧，則誘導阻力的大小正比于扭轉(zhuǎn)彈簧的形變量，忽略旋翼撲角（如上一小節(jié)所述）對誘導阻力方向的影響和機體彈性變形的影響，可以得到誘導阻力為：</p><p>&l

79、t;b>　　(2.12)</b></p><p>　　這里表示誘導阻力系數(shù)。</p><p>　　將式(2.12)帶入式(2.7a)，可得修正的總推力公式：</p><p><b>　　(2.13)</b></p><p>　　這里表示了旋翼的公稱推力。</p><p>　　誘導

80、阻力和槳葉揮舞產(chǎn)生了一個對于四旋翼無人機十分重要的效果，就是在水平方向上自然的穩(wěn)定性，定義：</p><p><b>　　(2.14)</b></p><p>　　為到x-y平面的投影矩陣。則速度在水平面中的投影表示為：</p><p><b>　　(2.15)</b></p><p>　　根據(jù)公式

81、（2.3d），在速度的水平分量下可以得到如下結(jié)論：</p><p><b>　　(2.16)</b></p><p>　　當無人機處于平穩(wěn)飛行狀態(tài)，也就是說時，則有，可以將上式寫為如下形式：</p><p><b>　　(2.17)</b></p><p>　　這個公式表達了槳葉揮舞和誘導阻力的共同

82、作用，這兩個作用的大小用用集總參數(shù)D表達。</p><p><b>　　本章小結(jié)</b></p><p>　　本章通過對四旋翼無人機的基本原理進行分析和數(shù)學推導，建立了四旋翼無人機在理想情況下的動力學和運動學模型。之后本文通過研究誘導阻力和槳葉揮舞對機體的作用，修正了這些模型，為四旋翼無人機的控制系統(tǒng)奠定了理論基礎。</p><p>　　無人機

83、機載姿態(tài)控制系統(tǒng)</p><p>　　小型室內(nèi)四旋翼無人機的姿態(tài)檢測包括多種原理不同，特性各異的姿態(tài)傳感器。通過分析不同傳感器的原理與特性，本文提出了一種無人機姿態(tài)數(shù)據(jù)融合的方法。然后，本章在Arduino控制器的基礎上完成了小型四旋翼無人機的姿態(tài)控制系統(tǒng)并進行了實驗。</p><p>　　無人機姿態(tài)控制系統(tǒng)的和核心控制器</p><p>　　Arduino 是一款

84、便捷靈活、方便的開源電子平臺，包含硬件（各種型號的Arduino板）和軟件（Arduino IDE)。APM是一款以Arduino為基礎的開源飛控，APM在Arduino硬件平臺基礎上增加了陀螺儀、加速度計、氣壓高度計、地磁傳感器等一系列傳感器，并為開發(fā)者提供了豐富的底層開發(fā)庫和專門的集成開發(fā)環(huán)境。本文針對開源硬件編寫了無人機姿態(tài)控制軟件。</p><p>　　無人機姿態(tài)控制系統(tǒng)以Arduino Mega 256

85、0 作為核心控制器（如圖3.1）。Arduino Mega 2560是一款以ATmega 2560 單片機為基礎的開源微控制器。包括開源的硬件設計和一款開源的集成開發(fā)環(huán)境，并提供了大量的庫文件。</p><p>　　表 3.1 Arduino Mega 2560 主要參數(shù)</p><p>　　Table 3.1 Parameters about Arduino Mega 2560</

86、p><p>　　Arduino Mega 2560有54個數(shù)字I/O 引腳，其中的15個可以作為PWM輸出使用，16個模擬輸入引腳，4個UART串口，晶振為16MHz。ATmega2560有256KB程序寄存器，其中8KB為引導程序（Boot loader）準備，8KB SRAM和4KB EEPROM，Arduino IDE中提供了專門的讀寫庫用于讀寫EEPROM。Arduino Mega 2560上搭載的AT Me

87、ga 2560中預燒寫了引導程序boot loader，不需要在下載程序時使用外部編程器，只要使用USB鏈接電腦即可。Arduino Mega 2560 的USB端口上有一個自恢復保險絲，保護計算機的USB端口，防止短路和過流。如果USB端口的電流超過500mA，保險絲會自動斷開。</p><p>　　圖 3.1 Arduino Mega 2560是一款以ATmega 2560單片機為基礎的開源微控制器</

88、p><p>　　Fig. 3.1 Arduino Mega 2560 is an open sorce controller based on ATmega 2560</p><p>　　小型四旋翼無人機的飛行姿態(tài)檢測</p><p>　　常用的無人機慣性姿態(tài)單元包括三軸加速度計和三軸陀螺儀。三軸陀螺儀提供響應速度快、精度高的姿態(tài)信號，但是存在累積誤差，三軸加速度計提供

89、低頻的姿態(tài)估計，由于沒有漂移誤差，通常用于對陀螺儀得到的位置信息進行修正。本文使用了MPU-6000作為慣性測量器件。</p><p>　　為了實現(xiàn)芯片的小型化，MPU-6000/6050設備將三軸陀螺儀、三軸加速度計和一個片內(nèi)的數(shù)字運動處理器（DMP）集成在一塊芯片中。MPU-6000/6050中的DMP集成了這種9軸運動數(shù)據(jù)融合算法，并且預留了I2C從機接口方便連接外部地磁傳感器或者其它的傳感器。該設備使用了

90、4*4*0.9mm的QFN封裝。其接口與MPU-3000系列三軸陀螺儀相同。因此，MPU-6000/6050設備有很好的通用性，并且其體積更加適合空間十分有限的小型電路板。</p><p>　　InvenSense公司為MPU-6000/6050芯片提供了一個InvenSense MotionApps接口，這個接口抽象了運動檢測中的復雜問題，并且為操作系統(tǒng)解決了傳感器管理中遇到的問題，并且提供了一套用開發(fā)的API

91、接口。</p><p>　　圖 3.2 MPU-6000邏輯結(jié)構</p><p>　　Fig. 3.2 logic composition of MPU-6000</p><p>　　MPU-6000內(nèi)含InvenSense公司的姿態(tài)運動數(shù)據(jù)融合和在線校準固件。這種固件可以使開發(fā)者免于分立器件選型或系統(tǒng)級集成類似的繁瑣勞動。</p><p>

92、　　圖 3.3 MPU-6000的外部引腳與方向定義</p><p>　　Fig. 3.3 MPU-6000 pins and directions</p><p>　　為了方便實驗，本章使用了現(xiàn)有的開發(fā)板（如圖 3.4）進行開發(fā)。</p><p>　　圖 3.4 MPU-6000開發(fā)板</p><p>　　Fig. 3.4 Developm

93、ent board of MPU-6000</p><p>　　基于陀螺儀的飛行姿態(tài)檢測</p><p>　　陀螺儀是飛行器姿態(tài)檢測中最主要的器件，用于檢測機體相對于大地坐標系的角速度：</p><p><b>　　(3.1)</b></p><p>　　通過機體運動學公式（微分形式），將角速度積分可以得到機體姿態(tài)：&l

94、t;/p><p><b>　　(3.2)</b></p><p>　　這里η代表了觀測過程中的噪聲，表示觀測過程中的恒定誤差。代表了Ω的斜對稱矩陣，使?？傮w來說，MPU6000的誤差和噪聲都在可以接受范圍內(nèi)。</p><p>　　圖 3.5 MPU-6000連接Arduino原理圖</p><p>　　Fig. 3.5 Sc

95、hematics for MPU-6000 and Arduino connection</p><p>　　MPU-6000提供了±250, ±500, ±1000，和 ±2000°/sec四個可編程調(diào)節(jié)的量程，為了提高精度，使用最低的±250°/sec作為飛機的角速度傳感器。MPU-6000有自帶的16位模數(shù)轉(zhuǎn)換器，可以通過IIC總線讀取角

96、速度測量值。MPU-6000與Arduino Mega 2560控制器連接如圖 3.5所示。</p><p>　　利用角速度傳感器檢測飛機的姿態(tài)問題在于角速度傳感器得到的位置誤差是發(fā)散的，需要利用其他傳感器進行修正。具體方法將會在本章中討論。</p><p>　　基于加速度傳感器的飛行姿態(tài)檢測</p><p>　　加速度計和地磁計可以提供飛行器的絕對姿態(tài)數(shù)據(jù)，與陀螺

97、儀得到的姿態(tài)增量數(shù)據(jù)互補，得到更好的姿態(tài)測量值。</p><p>　　加速度計用于檢測機體在外力作用下的加速度：</p><p><b>　　(3.3)</b></p><p>　　這里η代表了觀測過程中的噪聲，表示觀測過程中的恒定誤差。和屬于大地坐標系。</p><p>　　APM可以通過一系列指令并且把飛機固定到特定

98、的角度，讀取加速度計和陀螺儀估算和。</p><p>　　利用加速度計得到飛行姿態(tài)數(shù)據(jù)的原理十分微妙，如果假設升力總是沿飛行器軸線向上，則有：</p><p><b>　　(3.4)</b></p><p>　　這個公式說明了在簡單、理想的四旋翼無人機模型中，加速度計度數(shù)總是指向機體上方的，并不包含飛行器的姿態(tài)信息。但如上一章中所說，槳葉揮舞效

99、應對于機體姿態(tài)估計是有作用的，故上式可修正為：</p><p><b>　　(3.5)</b></p><p>　　加速度計信號只有低頻部分被用于姿態(tài)估計，假設，可以估計：</p><p><b>　　(3.6)</b></p><p><b>　　用代替可得到：</b><

100、;/p><p><b>　　(3.7)</b></p><p>　　基于地磁計的飛行姿態(tài)檢測</p><p>　　利用地磁計進行姿態(tài)檢測具有原理簡單，數(shù)據(jù)穩(wěn)定，誤差不會累積的優(yōu)點。如(3.8)所示，基于地磁計的姿態(tài)檢測方法可以簡單地通過求解姿態(tài)變換矩陣得到。但是地磁計增加了硬件設計的負擔，同時地磁計受到周圍磁場影響較大，對環(huán)境的要求較高。</

101、p><p><b>　　(3.8)</b></p><p>　　地磁姿態(tài)檢測模塊原理圖如圖 3.6所示。</p><p>　　其中HMC5883L為數(shù)字接口的三軸磁場強度傳感器，內(nèi)置ASIC放大器，12位A/D轉(zhuǎn)換器，IIC總線輸出。采用4*4*1.3的LCC封裝，具有體積小、精度高、成本低廉、使用方便的優(yōu)點。</p><p&g

102、t;　　PCA9306是一款支持I2C總線和SMBus的雙向電平轉(zhuǎn)換器，支持從1.0V～3.6V到1.8V～5.5V的電平轉(zhuǎn)換，并且?guī)в惺鼓茌斎搿?lt;/p><p>　　PCA9306支持1.0V到5.0V的雙向電平轉(zhuǎn)換并且不需要方向引腳的控制。開關導通時具有很小的導通電阻，可以以最小的傳輸延遲進行連接。當使能端為高電平，轉(zhuǎn)換開關打開，SCL1和SDA1分別連接到SCL2和SDA2，允許數(shù)據(jù)在兩端傳輸。當使能端為低

103、電平，開關關閉，端口為高阻態(tài)。</p><p>　　圖 3.6磁場強度傳感器連接Arduino原理圖</p><p>　　Fig. 3.6 Schematics for magnetoresistive sensor</p><p>　　四旋翼無人機的飛行姿態(tài)數(shù)據(jù)融合與濾波</p><p>　　本文中使用的MPU-6000慣性傳感器和HMC5

104、883L地磁傳感器精度有限，穩(wěn)定性不高，單純依靠陀螺儀產(chǎn)生的姿態(tài)數(shù)據(jù)在較長時間內(nèi)會產(chǎn)生很大的累積誤差。同時，加速度傳感器受機體震動影響較大，而磁場強度傳感器容易受到外部磁場的干擾，所以，為了穩(wěn)定飛行控制算法，提高飛行器的位置控制精度，必須在使用中對三種傳感器得到的數(shù)據(jù)進行數(shù)據(jù)融合與濾波。</p><p><b>　　互補濾波器</b></p><p>　　陀螺儀積分得

105、到的姿態(tài)數(shù)據(jù)響應速度快，在短時間有較高的精度，但較長時間使用會產(chǎn)生累積誤差，因此它的誤差是發(fā)散的，從加速度計得到的姿態(tài)數(shù)據(jù)沒有累積誤差，但動態(tài)響應較差，而地磁傳感器得到的姿態(tài)數(shù)據(jù)精度比較低[4]。因此，將角速度、磁場強度、加速度三個傳感器數(shù)據(jù)通過互補濾波器進行濾波（如圖圖 3.7），可以大幅提高姿態(tài)解算精度。</p><p>　　圖 3.7角速度、磁場強度、加速度傳感器通過互補濾波器進行數(shù)據(jù)融合</p>

106、;<p>　　Fig. 3.7 data fusion by a complementary filter of angular velocity ,magneto resistive and acceleration sensor</p><p>　　用R表示第二章中介紹的無人機姿態(tài)變換矩陣，表示經(jīng)過互補濾波器得到的姿態(tài)變換矩陣，表示加速度計和磁場強度傳感器得到的姿態(tài)變換矩陣，表示觀測過程中的高

107、頻噪聲，則有，表示了陀螺儀數(shù)據(jù)積分得到的無人機姿態(tài)變換矩陣，表示積分過程中的累積誤差，有。取低通濾波器，高通濾波器，則有：</p><p><b>　　(3.9)</b></p><p>　　因此，互補濾波器可以有效地去除高頻和低頻干擾，實現(xiàn)姿角速度傳感器、磁場強度傳感器和角速度傳感器的數(shù)據(jù)同和。將角速度傳感器帶入上式，可得：</p><p>

108、<b>　　(3.10)</b></p><p>　　由上式可得互補濾波器模型（圖 3.8）。</p><p>　　圖 3.8互補濾波器模型</p><p>　　Fig. 3.8 Model of complementary filter</p><p>　　用固定常數(shù)代替，則可以得到高通濾波器的截止頻率。將上式變換到時

109、域可得表達式如下：</p><p><b>　　(3.11)</b></p><p>　　為方便程序設計，根據(jù)文獻[3]中所得結(jié)果，可以將上式中的互補濾波器具體化：</p><p><b>　　(3.12)</b></p><p>　　這里，，和表示非負的觀測量修正系數(shù)，表示了歐幾里得矩陣在斜對稱矩

110、陣上的投影，如果相對應的觀測量不可靠或者無法得到，可以將修正系數(shù)取零。因為本文中不存在輔助的狀態(tài)估計傳感器值，所以取。</p><p><b>　　卡爾曼濾波器</b></p><p>　　卡爾曼濾波是一個最優(yōu)化自回歸數(shù)據(jù)處理算法。在四旋翼無人機中，需要對陀螺儀、加速度傳感器和地磁計所得到的數(shù)據(jù)進行數(shù)據(jù)融合。簡單的均值濾波很難滿足精度與實時性的要求。故通過卡爾曼濾波將