四旋翼無人機(jī)畢業(yè)設(shè)計

1、精選優(yōu)質(zhì)文檔-傾情為你奉上渤海大學(xué)本科畢業(yè)論文（設(shè)計）四旋翼無人機(jī)設(shè)計與制作The Manufacture and Design of Quad Rotor Unmanned Aerial Vehicle學(xué) 院（系）： 專 業(yè)： 學(xué) 號： 學(xué) 生 姓 名： 入 學(xué) 年 度： 指 導(dǎo) 教 師： 完 成 日 期： 專心-專注-專業(yè)摘 要四旋翼無人機(jī)飛行器因為它的結(jié)構(gòu)簡單，而且控制起來也很方便，因此它成為了近幾年來發(fā)展起來的熱門產(chǎn)業(yè)。在這里本文詳細(xì)的介紹了四旋翼飛行器的設(shè)計和制作的過程，其中包括了四旋翼無人機(jī)飛行器的飛行原理，硬件的介紹和選型，姿態(tài)參考算法的推導(dǎo)和實現(xiàn)，系統(tǒng)軟件的具體實現(xiàn)。該四旋翼飛

2、行器控制系統(tǒng)以STM32f103zet單片機(jī)為核心，根據(jù)各個傳感器的特點，采用不同的校正方法對各個傳感器數(shù)據(jù)進(jìn)行校正以及低通數(shù)字濾波處理，之后設(shè)計了互補濾波器對姿態(tài)進(jìn)行最優(yōu)估計，實現(xiàn)精確的姿態(tài)測量。最后結(jié)合GPS控制與姿態(tài)控制疊加進(jìn)行PID控制四旋翼飛行器的四個電機(jī)，來達(dá)到實現(xiàn)各種飛行動作的目的。在制作四旋翼飛行器的過程中，進(jìn)行了大量的調(diào)試并且與現(xiàn)有優(yōu)秀算法做對比驗證，最終設(shè)計出能夠穩(wěn)定飛行的四旋翼無人機(jī)飛行器。關(guān)鍵詞：姿態(tài)傳感器；四元數(shù)姿態(tài)解算； STM32微型處理器；數(shù)據(jù)融合；PIDThe Manufacture and Design of Quad Rotor Unmanned Aeri

3、al VehicleAbstractQuad-rotor unmanned aerial vehicle aircraft have a simple structure, and it is very easy to control, so it has become popular in recent years. Here article describes in detail the design and the process of making the four-rotor aircraft, including Quad-rotor UAV aircraft flight pri

4、nciple, hardware introduction and selection, implementation and realization of derivation attitude reference algorithm, the system software . The Quad-rotor aircraft control system STM32f103zet microcontroller core, and the advantages and disadvantages based on the accelerometer sensor, a gyro senso

5、r and electronic compass sensors using different correction methods for correcting various sensor data and low-pass digital filter processing, after design complementary filter to estimate the optimal posture, precise attitude measurement. Finally, GPS control and attitude control PID control is sup

6、erimposed four-rotor aircraft four motors to achieve a variety of flight maneuvers to achieve the purpose. Four-rotor aircraft in the production process, a lot of debugging and do comparison with the existing excellent algorithm validation, the final design to stabilize the Quad-rotor UAV flying air

7、craft.Key Words：MEMS Sensor; Quaternion; STM32 Processor; Data Fusion; PID目 錄1 緒論1.1 研究背景及意義 隨著MEMS傳感器、無刷電機(jī)、單片機(jī)以及鋰電池技術(shù)的發(fā)展，四旋翼飛行器現(xiàn)在已經(jīng)成為航模界的后起之秀。與固定翼飛行器相比之下四旋翼飛行器具有結(jié)構(gòu)簡單，控制起來非常方便，能夠垂直起降，成本非常的低、穩(wěn)定性也高，機(jī)動性非常強(qiáng)等特點。在民用可以代替有人機(jī)完成一些任務(wù)，在軍事上有很強(qiáng)的戰(zhàn)場生存能力。因此在這些領(lǐng)域應(yīng)用廣泛，如軍事偵查、農(nóng)林業(yè)調(diào)查、災(zāi)害檢測、輸電線巡查、玩具航模、航拍、氣象探測等。四旋翼飛行器的飛行原理雖然

8、簡單，但是涉及到的知識面非常的廣 杜浩.基于GPS/INS的多旋翼MAVS自主飛行系統(tǒng)研制D.南京信息工程大學(xué),2012，從機(jī)體結(jié)構(gòu)的設(shè)計、傳感器濾波算法、控制系統(tǒng)的設(shè)計和軟件的設(shè)計都需要理論的支持。本次設(shè)計針對四旋翼飛行器姿態(tài)控制系統(tǒng)進(jìn)行更深入的研究，它的研究將推動中國四旋翼飛行器的研究發(fā)展，為四旋翼飛行器在環(huán)境保護(hù)、氣象、火災(zāi)、偵查追蹤等民用和軍用領(lǐng)域?qū)崿F(xiàn)產(chǎn)業(yè)化作出突出貢獻(xiàn)。廉價并且高性能的飛行器的研究將會擁有巨大的經(jīng)濟(jì)效益，能夠?qū)ξ覈目蒲惺聵I(yè)起到巨大的推動作用。1.2 國內(nèi)外四旋翼飛行器的研究現(xiàn)狀1.2.1國外四旋翼飛行器的研究現(xiàn)狀目前國外四旋翼飛行器的研究也是主要集中在飛行器姿態(tài)控制

9、系統(tǒng)的新的理論的研究,比如：神經(jīng)元網(wǎng)絡(luò)控制算法、模糊自適應(yīng)控制算法等。國外還在四旋翼飛行器的自主飛行以及多機(jī)協(xié)同運作等方面有很多研究。下面對一些四旋翼飛行器進(jìn)行簡單的介紹：首先非常具有代表性的是美國Draganflyer公司研發(fā)出來的Draganflyer系列四旋翼飛行器 郎哲彥.基于模態(tài)切換的無人直升機(jī)雙回路魯棒控制器的研究D.天津大學(xué),2012，如圖1.1所示。這種四旋翼飛行器主要使用碳纖材料制作，因其載重能力強(qiáng)能攜帶高清攝像機(jī)，因此主要用途為航拍。另外還有Parrot公司研發(fā)的AR.Drone飛行器也是非常具有代表性，如圖1.2所示。AR.Drone可以用手機(jī)遠(yuǎn)程控制，使用MEMS高精度

10、姿態(tài)傳感器，并且配備多種傳感器和攝像頭，使AR.Drone可以非常輕松地進(jìn)行飛行任務(wù) 馬遠(yuǎn)超.四旋翼飛行器導(dǎo)航及控制技術(shù)研究D.哈爾濱工程大學(xué),2013。德國在四旋翼飛行器研究方面也具有較高的水平，德國的MicroDrones公司推出的一款四旋翼飛行器MD4-200 姜洋.四旋翼垂直起降機(jī)的魯棒控制問題研究D.哈爾濱工業(yè)大學(xué),2009，如圖1.3所示。該型號飛行器采用全碳纖工藝制作，負(fù)載能力強(qiáng)，而且非常省電。該型號飛行器配備有GPS衛(wèi)星導(dǎo)航系統(tǒng)和攝像設(shè)備，能夠很輕松的在室內(nèi)和室外執(zhí)行航拍任務(wù)。圖1.1 DraganflyerX4四旋翼飛行器 圖 1.2 AR.Drone飛行器現(xiàn)在許多科研院所已

11、開始開展四旋翼飛行器相關(guān)科研項目，主要是針對四旋翼飛行器系統(tǒng)建模的研究和四旋翼飛行器飛行功能的實現(xiàn)。美國賓夕法尼亞大學(xué)GRASP實驗室設(shè)計出了一種能夠編隊飛行的四旋翼無人機(jī)飛行器，在這些飛行器上都安裝有光源，通過安裝在室內(nèi)墻壁上的攝像頭設(shè)備進(jìn)行拍攝，從而確定飛行器的空間位置并且對其進(jìn)行編隊飛行控制操作，如圖1.4所示。麻省理工學(xué)院設(shè)計的一款可以在室內(nèi)進(jìn)行地圖測繪，定位和壁障的四旋翼無人飛行器系統(tǒng)，該系統(tǒng)通過激光雷達(dá)對周圍環(huán)境進(jìn)行測量，而且能夠自動生成三維地圖數(shù)據(jù)，并且根據(jù)周圍的環(huán)境進(jìn)行自主壁障和飛行路徑規(guī)劃，可以用于為危險環(huán)境的探測和搜救，如圖1.5所示。圖 1.3 德國MD4-200四旋翼飛

12、行器 圖 1.4 賓夕法尼亞大學(xué)四旋翼編隊飛行1.2.2國內(nèi)四旋翼飛行器的研究現(xiàn)狀現(xiàn)今四旋翼飛行器的研究在國內(nèi)逐漸發(fā)展壯大并且已經(jīng)形成產(chǎn)業(yè)。目前國內(nèi)己經(jīng)有許多公司(如Dj大疆公司)將四旋翼飛行器應(yīng)用于商業(yè)化，如圖1.6所示。圖 1.5 麻省理工學(xué)院四旋翼飛行器 圖 1.6 大疆四旋翼飛行器目前對四旋翼飛行器的研究主要集中在以下幾個方面：(1)四旋翼飛行器的姿態(tài)控制。四旋翼飛行器研究的最主要技術(shù)難點在于對飛行姿態(tài)的控制。因其旋翼多，因此四旋翼飛行器比傳統(tǒng)的直升機(jī)控制起來復(fù)雜。目前該領(lǐng)域的研究方向主要集中在飛行器的數(shù)學(xué)建模、控制算法和濾波算法。目前主要的研究算法有剛體旋轉(zhuǎn)理論、非線性濾波法、四元數(shù)

13、、捷聯(lián)慣導(dǎo)算法、PID控制算法、模糊自適應(yīng)控制等。(2)適合于四旋翼飛行器的新的傳感器技術(shù)的發(fā)展，國內(nèi)外逐漸出現(xiàn)了通用的整合于一體的傳感器模塊，例如MPU6050傳感器就是把加速度計和陀螺儀集成在一起。(3)電機(jī)和電池領(lǐng)域的發(fā)展。近些年來，無刷電機(jī)和空心杯電機(jī)的進(jìn)一步普及和應(yīng)用于四旋翼飛行器上，四旋翼飛行器的動力得到了很大程度的提高。鋰電池和燃料電池的出現(xiàn)和應(yīng)用大大增加了飛行器的續(xù)航能力。(4)GPS的發(fā)展。隨著衛(wèi)星定位技術(shù)的發(fā)展壯大，GPS也逐漸應(yīng)用于旋翼飛行器，人們可以不用害怕飛行器故障之后會不會找不到，因為我們可以用GPS進(jìn)行衛(wèi)星定位，而且還可以設(shè)置航點，實現(xiàn)飛行器的自主飛行。(5)無線

14、傳輸模塊的發(fā)展?，F(xiàn)如今無線傳輸可以應(yīng)用的范圍越來越廣泛，藍(lán)牙、WIFI等無線傳輸方式越來越被普遍應(yīng)用到飛行器上，從而實現(xiàn)手機(jī)的遙控控制。1.3 本文研究內(nèi)容和方法本文研究基于MEMS傳感器的姿態(tài)參考系統(tǒng)，通過對姿態(tài)測量傳感器數(shù)據(jù)的分析，設(shè)計出了有效去噪的濾波方法；通過大量的查找資料對姿態(tài)解算算法和數(shù)據(jù)融合算法有了更深的理解，最后應(yīng)用于設(shè)計的飛行控制器上實現(xiàn)了姿態(tài)角的測量。最后通過大量的實驗驗證了它們的準(zhǔn)確性，實驗數(shù)據(jù)和曲線驗證了該姿態(tài)參考系統(tǒng)能夠穩(wěn)定的工作，具有很好地工作性能。本文一共分為五章，主要內(nèi)容安排如下：第一章緒論部分主要介紹了該項目的研究背景及意義、四旋翼飛行器在國內(nèi)外的研究現(xiàn)狀和發(fā)

15、展趨勢。第二章主要介紹了四旋翼飛行器的飛行原理和系統(tǒng)結(jié)構(gòu)框架。第三章詳細(xì)介紹了四旋翼無人機(jī)控制系統(tǒng)的硬件設(shè)計的工作。介紹了MEMS傳感器的原理、特性和型號的選擇和硬件電路圖。飛行器控制芯片選擇STM32，外圍電路包括有姿態(tài)測量系統(tǒng)、電源模塊、無線通訊、串口通訊、電機(jī)驅(qū)動、遙控器控制電路、GPS模塊。 第四章說明了姿態(tài)參考系統(tǒng)的核心算法-捷聯(lián)慣性導(dǎo)航算法的研究和實現(xiàn)過程。第五章針對軟件實現(xiàn)部分進(jìn)行了介紹，給出了編程的軟件流程圖和串級PID控制和定高控制方法。最后對本次設(shè)計進(jìn)行了總結(jié)，提出了不足之處并對今后的研究工作進(jìn)行了展望。2 四旋翼飛行器工作原理2.1 四旋翼飛行器的飛行原理四旋翼飛行器有兩

16、種模式，也就是X字模式（如圖2.1所示）和十字模式（如圖2.2所示）。其實這兩種模式差別不大，到X模式使用廣泛，因此我們采用X字模式。四旋翼飛行器的四個電機(jī)對稱分布在各個軸上，并且同一條軸線上電機(jī)的旋轉(zhuǎn)方向要保證相同，相鄰的電機(jī)旋轉(zhuǎn)方向相反 李堯,四旋翼飛行器控制系統(tǒng)設(shè)計D.大連理工大學(xué),2013。如果電機(jī)1、 3按照逆時針方向旋轉(zhuǎn)的話，電機(jī)2 、4就要按照順時針方向旋轉(zhuǎn)，這樣做為了克服反扭矩的影響。我們要通過控制4個電機(jī)的轉(zhuǎn)速來完成飛行器俯仰、橫滾、偏航等動作。 圖 2.1 X型四旋翼飛行器模型 圖 2.2 十字型四旋翼飛行器模型2.2 四旋翼飛行器系統(tǒng)結(jié)構(gòu)四旋翼無人機(jī)采用模塊化設(shè)計，如圖2

17、.3所示。分別由控制模塊、姿態(tài)測量系統(tǒng)、電源供電系統(tǒng)、無線通信模塊、GPS衛(wèi)星定位系統(tǒng)、遙控器控制模塊、電機(jī)驅(qū)動模塊、串口通信模塊、地面站系統(tǒng)。四旋翼飛行器控制器的核心任務(wù)是姿態(tài)的測量，它的作用是為飛行器控制系統(tǒng)提供實時、精確的飛行狀態(tài)測量數(shù)據(jù)。常見的四旋翼飛行器人們大多是采用基于MEMS傳感器來測量飛行器姿態(tài)數(shù)據(jù) 宋英麟,鮮斌,茹濱超,曹美會.無人機(jī)微型姿態(tài)航向系統(tǒng)數(shù)據(jù)處理J.中南大學(xué)學(xué)報.2013。但是這些初始的傳感器數(shù)據(jù)并不能直接應(yīng)用于姿態(tài)解算，需要對傳感器數(shù)據(jù)進(jìn)行濾波處理，并且需要對陀螺儀漂移問題進(jìn)行實時的數(shù)據(jù)補償，這樣做能夠有效提高飛行器姿態(tài)測量精度，確?？刂葡到y(tǒng)的姿態(tài)角的準(zhǔn)確性和穩(wěn)

18、定性。圖 2.3 四旋翼飛行器系統(tǒng)結(jié)構(gòu)框架四旋翼飛行器的主控板選擇的是意法半導(dǎo)體公司生產(chǎn)的STM32f103zet芯片，STM32系列的單片機(jī)是基于Cortex-M3內(nèi)核的處理器，功耗低，處理速度非?？?，最高工作頻率可達(dá)72MHz，7通道DMA控制器，支持定時器、ADC、SPI、IIC、USART等外設(shè)，多達(dá)112個I/O口，8個Timer定時器， 5個串行USART接口，3個SPI接口，2個IIC接口 滕守明,魯奕,李響.基于STM32芯片及CAN總線在汽車上的應(yīng)用J.無線互聯(lián)科技.2013。電源模塊采用11.1V鋰電池外部供電，連接電子調(diào)速器為控制器提供5V電壓。控制器上還有3.3V穩(wěn)壓芯

19、片，為控制芯片供電。遙控器控制模塊，控制器對遙控器數(shù)據(jù)進(jìn)行捕獲處理該部分我們通過對STM32定時器進(jìn)行輸入捕獲配置，捕獲接收機(jī)發(fā)出的PWM信號，把該信號轉(zhuǎn)化成控制量在經(jīng)過PID控制把輸出量給四個電機(jī)，進(jìn)而控制飛行器的動作。GPS衛(wèi)星定位導(dǎo)航系統(tǒng)，配合上位機(jī)在上位機(jī)上輸入一些GPS坐標(biāo)點，控制系統(tǒng)就會自動生成航線，并且能夠從GPS系統(tǒng)中讀取定位數(shù)據(jù) 黃鵬宇,曾路榮,楊川,彭遠(yuǎn)行,余成波.一種新型災(zāi)難救援四軸航拍飛行器設(shè)計J.四川兵工學(xué)報.2014，并且與存儲的定位坐標(biāo)做實時的對比，然后修正航線，將定位坐標(biāo)顯示在上位機(jī)上，處理并顯示當(dāng)前位置。3 四旋翼飛行器硬件系統(tǒng)設(shè)計3.1 微慣性組合系統(tǒng)傳感器

20、組成3.1.1 MEMS陀螺儀傳感器陀螺儀是一種能用來維持方向與角速度（獲取角速度）的裝置，設(shè)計原理是角動量守恒。簡單的說就是一個高速旋轉(zhuǎn)的物體的旋轉(zhuǎn)軸所指的方向在不受外力影響時不會改變。這種用來保持方向而制造出來的裝置就叫陀螺儀 王曙霞,梁洪潔,王小營,劉偉.基于虛擬儀器的機(jī)載陀螺儀測試系統(tǒng)研究J.電子設(shè)計工程.2010，如圖3.1所示。陀螺儀多用于導(dǎo)航定位系統(tǒng)中，姿態(tài)控制系統(tǒng)中多采用三軸陀螺儀，如圖3.2所示。 圖3.1 陀螺儀 圖 3.2 MEMS三軸陀螺儀3.1.2 MEMS加速度計傳感器能將物體加速度的信息轉(zhuǎn)換為電信號的傳感器稱之為加速度傳感器。在姿態(tài)控制系統(tǒng)中，加速度傳感器用來測量

21、與重力方向的夾角。當(dāng)應(yīng)用到實際中時我們就可以理解加速度傳感器輸出的信號是當(dāng)?shù)刈鴺?biāo)系下加速度在導(dǎo)航坐標(biāo)系下投影。加速度計能夠在沒有加速度存在的條件下可以感應(yīng)重力產(chǎn)生的加速度，然而在有加速度存在時，根本無法測量出姿態(tài)角，需要陀螺儀傳感器的數(shù)據(jù)相結(jié)合，才能夠?qū)崿F(xiàn)動態(tài)條件下的姿態(tài)測量 鄭健.基于9軸傳感器的姿態(tài)參考系統(tǒng)研究與實現(xiàn)D.電子科技大學(xué),2013。3.1.3 三軸數(shù)字羅盤傳感器數(shù)字電子羅盤也叫指南針，顧名思義指南針是用來指示方向的。傳統(tǒng)羅盤通過磁針來感應(yīng)地磁場方向，電子羅盤通過磁阻傳感器測量地磁方向信息，再將所測信息轉(zhuǎn)換為信號輸出。數(shù)字電子羅盤的優(yōu)勢在于它克服了只能夠在水平面使用的缺點，這種數(shù)

22、字電子羅盤內(nèi)部有傾斜補償裝置，這個裝置一般是由加速度傳感器來完成，如果在完全動態(tài)的情況下，也需要陀螺儀檢測姿態(tài)角，通過這個角度和磁場方向信息可以補償?shù)玫綔?zhǔn)確的角度信息，而姿態(tài)測量系統(tǒng)中的電子羅盤實際上就是三軸數(shù)字電子羅盤。3.2 姿態(tài)測量系統(tǒng)傳感器選型目前市場上出現(xiàn)的一款I(lǐng)nvenSense公司的MPU6050芯片內(nèi)部集成了三軸加速度計和三軸陀螺儀，這樣做不僅消除了焊接電路時易造成加速度計和陀螺儀之間的對準(zhǔn)誤差的問題，而且因為芯片內(nèi)部結(jié)構(gòu)上有數(shù)字可編程低通濾波器。所以在飛行器經(jīng)受較大震動的時候，可以用軟件設(shè)置適當(dāng)頻率的低通濾波器，濾掉高頻震動，這種方法很有效的減少了四旋翼機(jī)身震動對姿態(tài)測量的影

23、響。因此MPU6050被廣泛應(yīng)用于姿態(tài)控制系統(tǒng)之中，其特征如下：（1）三軸角速度傳感器具有±250、±500、±1000 與±2000（°/s）測量范圍 劉杰.四軸飛行器研究與設(shè)計D.南京郵電大學(xué).2013；三軸加速度量程控制范圍有±2g、±4g、±8g、和±16g。（2）具備較低功耗：芯片供電電壓VDD 為2.5V±5%、3.0V±5%、3.3V±5% 吳冬冬.基于MEMS陀螺儀姿態(tài)檢測系統(tǒng)研制D.浙江理工大學(xué).2014；陀螺儀工作電流5mA，待機(jī)電流為5uA；加速度計工作

24、電流為500uA，在10Hz低功耗模式下僅需40uA的電流 馮旭光.四旋翼無人機(jī)自主控制系統(tǒng)設(shè)計D.內(nèi)蒙古科技大學(xué).2014。（3）陀螺儀和加速度計都具備16 位ADC 同步采樣功能。（4）IIC接口傳輸頻率可高達(dá)400KHz，內(nèi)建頻率發(fā)生器在所有溫度范圍只有1%頻率變化。綜合MPU6050特性，我們采用如圖3.3所示的電路讀取三軸加速度和三軸陀螺儀數(shù)據(jù)。圖 3.3 MPU6050電路圖HMC5883傳感器是三軸數(shù)字羅盤，它可以用來測量四軸飛行器所處位置的三軸磁場信息，該傳感器內(nèi)置了三軸磁阻模塊和放大采樣電路，直接輸出數(shù)字信號，用來測量航向角并進(jìn)行姿態(tài)解算，HMC5883電路圖如圖3.4所示。

25、HMC5883的特點如下：1. IIC 數(shù)字量輸出總線接口，設(shè)計使用簡單，尺寸非常小。 2.有較高的測量精度，內(nèi)置12位 A/D轉(zhuǎn)換。 圖 3.4 HMC5883電路圖4. 擁有自動校準(zhǔn)功能，簡化了應(yīng)用的步驟。 5. 內(nèi)置有自測試電路，量產(chǎn)測試非常方便，不需要增加額外的高昂測試設(shè)備 高勇.瓦型鐵氧體磁場檢測裝置及先進(jìn)制備工藝研發(fā)D.遼寧工業(yè)大學(xué).2015。 6. 功耗較低，供電電壓只需要1.8V，睡眠模式功耗-2.5uA，測量模式功耗-0.6mA。3.3 電源系統(tǒng)設(shè)計為了滿足飛行控制系統(tǒng)的需要，電源系統(tǒng)為飛行控制器和功能模塊提供了12V、5V和3.3V電壓，電路設(shè)計如圖3.5所示。選擇12V的

26、鋰電池作為供電電源，通過穩(wěn)壓芯片將12V電壓降為5V，為GPS模塊、超聲波傳感器、接收機(jī)等提供供電電源；然后通過AMS1117_3.3穩(wěn)壓芯片再把5V電壓降為3.3V，為飛行控制主板、姿態(tài)測量傳感器和無線通信模塊提供電能。圖 3.5 電源系統(tǒng)電路圖3.4 其它硬件模塊3.4.1 無線通信模塊該模塊為上位機(jī)和控制器建立了聯(lián)系。通過兩塊NRF24L01進(jìn)行數(shù)據(jù)的傳輸，控制器將姿態(tài)和高度等信息傳輸至地面站，地面站將位置信息和導(dǎo)航信息告訴控制器，從而達(dá)到可控的要求。NRF24L01的工作頻段為2.4 2.5GHz，而且具備自動重發(fā)功能，有6個數(shù)據(jù)傳輸通道，最大傳輸速率高達(dá)2Mbits 臺述鵬,趙文斐.

27、以ARM為核心的嵌入式體感遙控器設(shè)計J.單片機(jī)與嵌入式系統(tǒng)應(yīng)用.2012。STM32主控板可以通過SPI接口對NRF24L01的寄存器進(jìn)行配置，無線通信模塊的電路設(shè)計圖，如圖3.6所示。圖 3.6 NRF24L01電路圖3.4.2 電機(jī)和電機(jī)驅(qū)動模塊我們常用的直流電機(jī)按結(jié)構(gòu)及工作原理可以劃分為無刷電機(jī)和有刷電機(jī)。無刷電機(jī)是航模電機(jī)的主流選擇，因為它力量大而且耐用。小型的四旋翼飛行器一般選用空心杯電機(jī)，屬于有刷電機(jī)一類。由于我們設(shè)計的是大型四旋翼飛行器，故采用的是無刷電機(jī)。無刷電機(jī)根據(jù)廠商的不同，種類也是非常的多，市面上的主要有朗宇、新西達(dá)、銀燕等品牌。在這些無刷電機(jī)中口碑最好的就是朗宇電機(jī)，用

28、的人非常的多，因此采用朗宇無刷電機(jī)。四旋翼飛行器所用的無刷電機(jī)主要有電機(jī)尺寸和電機(jī)KV值兩個參數(shù)。電機(jī)尺寸指的是電機(jī)轉(zhuǎn)子的直徑和高度，電機(jī)KV值是指外加1V電壓時對應(yīng)的每分鐘空載轉(zhuǎn)速。根據(jù)參數(shù)設(shè)置最后選擇采用朗宇A(yù)2212、KV1400無刷電機(jī)。電機(jī)實物圖，如圖3.9所示。無刷電機(jī)的驅(qū)動就是俗稱的電子調(diào)速器也稱電調(diào)，如圖3.10所示。黑色和紅色的是11.1V電源線，紅色接電源正極，黑色接電源負(fù)極。右邊三根線和電機(jī)的三根線相連，如果轉(zhuǎn)向反了，只需將其中任意兩根線互換就可以。白紅黑三根線是和電調(diào)相連的，白色為信號線，紅色為控制板提供5V供電電源，黑色的線接控制器的GND。 圖 3.9 朗宇無刷電機(jī)

29、 圖 3.10 電子調(diào)速器實物圖3.4.3 機(jī)架和螺旋槳的選型對于機(jī)架的選擇，差的機(jī)架會使姿態(tài)傳感器讀取到的數(shù)據(jù)噪聲較大，加大四旋翼飛行器在飛行過程中的不穩(wěn)定性，因此選用結(jié)構(gòu)穩(wěn)定，質(zhì)量較輕的碳纖機(jī)架，這樣很大程度上也提高了飛行器的載重。機(jī)架實物圖如圖3.11所示。螺旋槳是由電機(jī)轉(zhuǎn)動帶動為四旋翼飛行器提供升力的。螺旋槳分為正反槳，順時針方向轉(zhuǎn)的電機(jī)需要配正槳，逆時針轉(zhuǎn)的電機(jī)需要配反槳。槳的選型需要結(jié)合所選的電機(jī)來配備，從槳的型號我們可以讀出槳的直徑和角度，例如1047中的前兩位10表示槳的直徑單位是英寸，后兩位代表槳的角度。的螺旋槳，同樣轉(zhuǎn)速情況下產(chǎn)生的升力就會越大，也就需要更大的電機(jī)驅(qū)動，這就

30、需要根據(jù)電機(jī)的大小去選擇槳的型號。在設(shè)計時選用1047的槳，槳的實物圖如圖3.12所示。 圖 3.11 四旋翼飛行器機(jī)架 圖 3.12 1047正反槳3.4.4 遙控控制模塊該遙控器控制模塊是由一個7通道的遙控器和配套的接收機(jī)組成的，接收機(jī)上的油門、副翼、升降舵、方向舵通道分別與控制器上的接口相連。控制器能通過定時器的捕獲功能，捕獲到遙控器通過接收機(jī)傳出的控制信號，遙控器就是這樣控制四旋翼飛行器的飛行動作的。遙控器和接收機(jī)的實物圖如圖3.13所示。圖 3.13遙控器和接收機(jī)實物圖4 四旋翼飛行器姿態(tài)參考系統(tǒng)設(shè)計4.1 姿態(tài)參考系統(tǒng)原理姿態(tài)參考系統(tǒng)是利用慣性導(dǎo)航器件來測量載體姿態(tài)角的一種慣性導(dǎo)航

31、系統(tǒng)。因為MEMS傳感器的數(shù)據(jù)帶有噪聲，因此姿態(tài)參考系統(tǒng)需要對MEMS傳感器的測量信息進(jìn)行濾波處理。然后對濾波信號進(jìn)行姿態(tài)解算。由于姿態(tài)傳感器各自特點的不同，所以需要對每個傳感器信號進(jìn)行濾波然后進(jìn)行數(shù)據(jù)融合，以此來提高測量的精度和系統(tǒng)的動態(tài)特性。姿態(tài)參考系統(tǒng)的原理圖，如圖4.1所示。 圖 4.1 姿態(tài)參考系統(tǒng)原理圖從MEMS傳感器采集到的數(shù)據(jù)傳輸?shù)娇刂破魈幚淼倪^程中會受到很多因素的干擾，造成傳輸誤差，為了減小誤差，提高檢測的精度就需要用濾波器進(jìn)行數(shù)據(jù)處理。濾波器可以通過硬件實現(xiàn)，也可以通過軟件實現(xiàn)，具有較大的靈活性 基于子帶能量和特征檢測技術(shù)的語音信號端點檢測方法.中國科學(xué)院自動化研究所.20

32、03。從加速度傳感器提取到的信號經(jīng)過姿態(tài)解算后的姿態(tài)角信號在和陀螺儀數(shù)據(jù)進(jìn)行融合的過程中以低頻信號為主的。根據(jù)這一特性，加速度傳感器的輸出信號可以通過低通濾波器或者均值濾波器提高信噪比。陀螺儀信號具有高動態(tài)的特點，它的信號噪聲主要為高斯白噪音，我們可以通過卡爾曼濾波器來濾除這種高斯白噪聲。因為需要融合加速度傳感器獲得的姿態(tài)角信號，所以選擇均值濾波來去除噪聲。 電子羅盤傳感器測量的是地磁強(qiáng)度，我們都知道物體在運動時地磁強(qiáng)度變化慢，它在姿態(tài)測量系統(tǒng)中屬于低頻信號，因此采用均值濾波來濾除噪聲信號。歐拉角和四元數(shù)方法都是針對陀螺儀姿態(tài)測量進(jìn)行處理的數(shù)學(xué)方法，可以有效地解算處姿態(tài)角。把解算出來的姿態(tài)角進(jìn)

33、行數(shù)據(jù)融合。考慮到高動態(tài)下陀螺儀測得的數(shù)據(jù)解算出的姿態(tài)角比較準(zhǔn)確因此在高動態(tài)條件下選用陀螺儀信號，低動態(tài)下加速度信號解算出的姿態(tài)角更穩(wěn)定因此在低動態(tài)條件下選擇加速度信號。4.2 傳感器信號處理4.2.1 加速度傳感器信號處理加速度傳感器的輸出信號主要表示直線加速度大小和與重力方向的夾角 董景新.微機(jī)械加速度計M.北京:清華大學(xué)出版社, 2003。 常用的數(shù)字濾波器有很多種，例如：限值濾波、遞推平均濾波法（又稱滑動平均濾波法）、算術(shù)平均濾波法、中位值濾波法和中位值平均濾波法等 于東.前束動態(tài)檢測系統(tǒng)研究與開發(fā)D.長安大學(xué).2012。在實際的姿態(tài)解算中一般采用低通濾波器或者均值濾波器去除高頻噪聲干

34、擾信號。均值濾波因其具備低通濾波的特性并且計算簡單，因此選用均值濾波來處理加速度信號。4.2.2 陀螺儀信號處理陀螺儀信號濾波要選擇卡爾曼濾波算法。對于卡爾曼濾波器，首先要清楚系統(tǒng)狀態(tài)的描述方法，系統(tǒng)狀態(tài)的更新方法和系統(tǒng)測量值的更新方法，從卡爾曼濾波算法的數(shù)學(xué)公式，我們可以歸納出它的核心思想：首先根據(jù)系統(tǒng)上一次得來的最優(yōu)值計算出當(dāng)前的估計值和協(xié)方差，再根據(jù)協(xié)方差大小計算出卡爾曼增益的大小，最后根據(jù)當(dāng)前估計值和測量值計算出當(dāng)前最優(yōu)值和協(xié)方差。下面分步建立卡爾曼濾波方程式：預(yù)估計最優(yōu)值方程： X(k|k-1) = A(k,k-1) * X(k-1|k-1) + B(k) * U(k) (4-1)上

35、式中，X(k|k-1)表示的是根據(jù)k-1時刻的最優(yōu)值計算出的k時刻估計值；X(k|k-1)表示k-1時刻的最優(yōu)值；A(k,k-1)是狀態(tài)轉(zhuǎn)移矩陣；B(k)是輸入控制加權(quán)矩陣；U(k)表示k時刻的輸入控制信號，這些參數(shù)需要根據(jù)具體的應(yīng)用場合進(jìn)行設(shè)計。預(yù)估計最優(yōu)值協(xié)方差： P(k|k-1) = A(k,k-1) * P(k-1|k-1) * A(k,k-1) + Q(k) (4-2)其中，P(k|k-1)表示預(yù)估計最優(yōu)值X(k|k-1)對應(yīng)的協(xié)方差；P(k-1|k-1)表示X(k-1|k-1)對應(yīng)的協(xié)方差，表示了對預(yù)測值的信任度；Q(k)表示k時刻系統(tǒng)過程的協(xié)方差，即對上一次測量估計值的信任程度，

36、Q矩陣值越大表示信任度越低，需要根據(jù)具體應(yīng)用進(jìn)行設(shè)計。P和Q矩陣的區(qū)別在于Q矩陣是根據(jù)模型直接得到的，P矩陣是算法過程中通過計算獲得的。計算卡爾曼增益矩陣：K(k)=P(k|k-1) * H(k) / (H(k) * P(k|k-1) * H(k) + R(k) (4-3) 其中，K(k)表示卡爾曼增益；R(k)表示k時刻觀測過程的covariance，即對測量的信任程度；H(k)表示觀測矩陣。更新估計： X(k|k)=X(k|k-1)+ K(k) * (Z(k) H(k) * X(k|k-1) (4-4) P(k|k)=(1- K(k)* H(k) * P(k|k-1) (4-5)其中，Z(

37、k)表示k時刻的觀測值，I為單位矩陣。4.2.3 電子羅盤信號處理 電子羅盤信號非常容易受到高頻干擾，在系統(tǒng)應(yīng)用中，應(yīng)當(dāng)過濾系統(tǒng)的高頻噪聲，最好采用去極值濾波算法 黃曉英,傅士冀,陳俊,曹作群.去極值平均濾波技術(shù)在智能微歐電阻校準(zhǔn)儀中的應(yīng)用J.電測與儀表,2011, 48(549):47-48。去極值濾波就是將連續(xù)測量的n個采樣值，按照數(shù)據(jù)的大小順序進(jìn)行排序，去掉最大值和最小值后對剩下的n-2個數(shù)值計算求取平均值，這種方法是最常用的濾波算法 于敬芬.低溫推進(jìn)劑液位檢測系統(tǒng)軟件設(shè)計與實現(xiàn)D.武漢理工大學(xué).2007。4.3 坐標(biāo)系坐標(biāo)系是描述物體在空間的相對位置和運動規(guī)律的，而導(dǎo)航技術(shù)就是為了確定

38、載體的空間位置。只有選定參考坐標(biāo)系，才能對系統(tǒng)的運動進(jìn)行描述。不同的坐標(biāo)系下載體的描述規(guī)律和運動形式也是不同的，選擇合適的坐標(biāo)系是非常重要的 胡恩偉.基于MEMS多傳感器數(shù)據(jù)融合的慣性組合導(dǎo)航系統(tǒng)算法設(shè)計與實現(xiàn)D.重慶大學(xué).2013。目前比較常用的坐標(biāo)系有：(1)地理坐標(biāo)系(g系)坐標(biāo)系和地球固連，其原點位于地球球心，通常選取東北天坐標(biāo)系，即坐標(biāo)Xg軸指向水平東方，Yg軸指向水平北方，Zg軸垂直于當(dāng)?shù)厮矫妫禺?dāng)?shù)卮咕€向上 王建文.無人直升機(jī)狀態(tài)估計算法研究D.國防科學(xué)技術(shù)大學(xué).2008。(2)導(dǎo)航坐標(biāo)系(n系)一般選取當(dāng)?shù)氐乩碜鴺?biāo)系作為導(dǎo)航坐標(biāo)系，坐標(biāo)Xn軸指向地理東方，Yn軸指向地理北方，

39、Zn軸垂直于當(dāng)?shù)厮矫?，沿?dāng)?shù)卮咕€向上 劉剛.基于視覺導(dǎo)航小型無人機(jī)自主著陸控制策略研究與應(yīng)用D.南京航空航天大學(xué).2014,如圖4.2所示。(3)載體坐標(biāo)系(b系)載體坐標(biāo)系，原點位于機(jī)體的質(zhì)心位置，通常選取右前上坐標(biāo)系，其Xb軸沿機(jī)體橫軸向右，Yb軸沿機(jī)體縱軸向前，Zb軸沿機(jī)體豎軸向上，如圖4.2所示。圖 4.2 載體坐標(biāo)系和導(dǎo)航坐標(biāo)系 4.4 姿態(tài)角定義姿態(tài)角也是我們常說的歐拉角，是根據(jù)導(dǎo)航坐標(biāo)系和載體坐標(biāo)系之間的關(guān)系來定義的。歐拉角是飛行器的三個姿態(tài)角即俯仰角、橫滾角和偏航角，根據(jù)歐拉旋轉(zhuǎn)定律可以用三次旋轉(zhuǎn)使得飛行器本身的坐標(biāo)系與地理參考系重合，每一次的旋轉(zhuǎn)以機(jī)體坐標(biāo)系的x、y、z軸中

40、的一個坐標(biāo)軸來轉(zhuǎn)動，轉(zhuǎn)過的角度即為歐拉角，三次坐標(biāo)的變換矩陣相乘的積就是歐拉姿態(tài)矩陣 唐懋.基于Arduino兼容的Stm32單片機(jī)的四旋翼飛行器設(shè)計D.廈門大學(xué),2014，形如4-1： (4-6)最終的姿態(tài)矩陣與這三次轉(zhuǎn)動的先后順序是有關(guān)系的，通常我們都按照Z-X-Y軸的順序。定義機(jī)體繞本體系x軸轉(zhuǎn)動的角度為俯仰角；機(jī)體繞本體系y軸轉(zhuǎn)動的角度為橫滾角；機(jī)體繞本體系z軸轉(zhuǎn)動的角度為航向角；三個角當(dāng)轉(zhuǎn)動方向與旋轉(zhuǎn)軸符合右手定則為正方向 姜成平.一種四旋翼無人機(jī)控制系統(tǒng)的設(shè)計與實現(xiàn)研究D.哈爾濱工業(yè)大學(xué).2014。俯仰角、橫滾角和航向角合稱歐拉角。得到如下姿態(tài)矩陣4-2： (4-7) 4.5 四元

41、數(shù)姿態(tài)解算算法四元數(shù)其實是 1個單位實數(shù)和3個虛數(shù)單位、 和 的線性組合，一般可表示為d + a + b + c， a、b、c、d代表實數(shù) 郭曉鴻.微型四旋翼無人機(jī)控制系統(tǒng)設(shè)計與實現(xiàn)D.南京航空航天大學(xué).2012。四元數(shù)乘法運算關(guān)系如下：式中，表示的是四元數(shù)的乘法。四元數(shù)與姿態(tài)矩陣之間的關(guān)系：設(shè)有參考坐標(biāo)系R，坐標(biāo)軸X0、 Y0、 Z0，坐標(biāo)軸方向的單位向量為、 、 韓嘯.基于陀螺儀的機(jī)器人自主運動定位研究D.哈爾濱工業(yè)大學(xué).2013。剛體相對于坐標(biāo)系R作定點轉(zhuǎn)動，定點為O，把坐標(biāo)系b與剛體固聯(lián)，b系的坐標(biāo)軸為x、y、z，坐標(biāo)軸方向的單位向量為、 、 胡志娟.理論力學(xué)教學(xué)現(xiàn)代化探索D.四川師范

42、大學(xué).2003。假設(shè)初始時刻b系與R系重合。在剛體上任取一點A，從 O點向該點引向量,如圖4.3所示。則該位置向量描述了剛體的空間角位置 彭勁松.機(jī)載激光捷聯(lián)慣導(dǎo)優(yōu)化算法研究D西北工業(yè)大學(xué).2006。圖4.3 剛體的等效轉(zhuǎn)換設(shè)剛體以= 相對R系旋轉(zhuǎn)，初始時刻位置處于=，經(jīng)過時間t后位置向量處于。根據(jù)歐拉定理，剛體從位置轉(zhuǎn)到位置的轉(zhuǎn)動可等效成繞瞬軸轉(zhuǎn)過角一次完成 秦永元.慣性導(dǎo)航M.北京:科學(xué)出版社,2006。這樣，位置向量做圓錐運動，和位于同一圓上，和位于同一圓錐面上。在圓上取一點B，是，由圖4.3可得。所以由三重矢量計算公式：即所以將上式向R系內(nèi)投影：記，根據(jù)叉乘關(guān)系表達(dá)式：記 (4-8)則

43、所以 (4-9)令 (4-10)則式(4-4)可以寫成： (4-11)記剛體固聯(lián)坐標(biāo)系為b系。所以而在轉(zhuǎn)動過程中，位置向量和b系都同剛體固聯(lián)，所以位置和b系的相對位角位置不變，即有因此可得： 該式說明即為b系至R系的坐標(biāo)變換矩陣，根據(jù)式(4-3)和(4-5)即 (4-12)令 (4-13)并以、構(gòu)造四元數(shù)： (4-14)可得如下結(jié)論：(1)四元數(shù) 描述了剛體的定點運動 司宏源.超視距空空導(dǎo)彈導(dǎo)航精度分析及誤差分配技術(shù)研究D上海交通大學(xué).2012， 包含了等效旋轉(zhuǎn)的全部信息，為旋轉(zhuǎn)瞬軸的旋轉(zhuǎn)方向，為轉(zhuǎn)過的角度。(2)四元數(shù)可以確定出b系至R系的坐標(biāo)變換矩陣。將(4-8)代入(4-7)得： (4-

44、15)由于，所以可進(jìn)一步得出如下結(jié)論： (4-16)如果將向量和看作零標(biāo)量的四元數(shù)，則和間的變換關(guān)系可采用四元數(shù)乘法表示：該式稱為坐標(biāo)變換的四元數(shù)乘表示方法，其中為R系至b系的旋轉(zhuǎn)四元數(shù)。證明如下： (4-17)對比式(4-11)知上式矩陣中右下角的33方塊即為，所以式(4-12)可寫成：即該式稱為坐標(biāo)變換的矩陣表示法。所以四元數(shù)乘法表示法和矩陣表示法是等價的。如果參考坐標(biāo)系R是導(dǎo)航坐標(biāo)系n,剛體的固聯(lián)坐標(biāo)系b為機(jī)體坐標(biāo)系，則坐標(biāo)變換矩陣就是姿態(tài)矩陣，而由姿態(tài)矩陣可計算出姿態(tài)角 賈宏光,陳濤,張躍,張榮輝.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算J.光學(xué)精密工程,2008,16(10):19

45、63-1970。記，由于在坐標(biāo)系旋轉(zhuǎn)的過程中坐標(biāo)系始終保持直角坐標(biāo)系，所以為正交矩陣對比上式可得姿態(tài)角 (4-18)因此，四元數(shù)包含了所有的姿態(tài)信息，姿態(tài)解算實際上是如何計算四元數(shù)。下面來介紹如何計算四元數(shù)：(1)初始化四元數(shù)假設(shè)當(dāng)前的坐標(biāo)系為地理坐標(biāo)系，則四元數(shù)列向量(2)從傳感器獲取載體加速度和角速度從MPU6050讀取三軸加速度計的測量值即加速度、，陀螺儀的測量值即角速度，。(3)將加速度計得出來三個軸的加速度值、轉(zhuǎn)化為三維的單位向量得到： (4-19)(4)將地理坐標(biāo)系的重力向量轉(zhuǎn)換到機(jī)體坐標(biāo)系可得三軸的重力量，： (4-20)(5)將地理坐標(biāo)系轉(zhuǎn)換到載體坐標(biāo)系下的重力向量和載體坐標(biāo)系

46、測量的加速度向量外積，得到兩坐標(biāo)系的誤差 趙建軍,陳濱,楊利斌.基于四元數(shù)的大地坐標(biāo)轉(zhuǎn)換算法及實現(xiàn)J.計算機(jī)工程與應(yīng)用,2013,49(4):202-205： (4-21)(6)陀螺儀誤差是導(dǎo)致機(jī)體坐標(biāo)系誤差的根本原因，因此用兩坐標(biāo)系誤差的PI來補償陀螺儀使得機(jī)體坐標(biāo)系更加準(zhǔn)確。 (4-22) 其中：和是調(diào)整參數(shù)，在實際調(diào)試中確定。其中可以等于0，可以以0為初始值，0.01步進(jìn)調(diào)節(jié)。(7)四元數(shù)姿態(tài)更新方程 孫冬梅,田增山,韓令軍.捷聯(lián)慣導(dǎo)系統(tǒng)中四元素法求解姿態(tài)角仿真模擬J.彈箭與制導(dǎo)學(xué)報, 2009,29(1): 51-60。四元數(shù)微分方程為： (4-23)其中，將上式寫成矩陣形式 (4-2

47、4)對四元數(shù)一階微分方程進(jìn)行一階畢卡算法可得： (4-25)(8)對四元數(shù)進(jìn)行規(guī)范化處理，于是得下式4-21： (4-26)(9)以上得到的新的四元數(shù)代表完成了一次四元數(shù)的運算，將此四元數(shù)回到開頭，將舊的四元數(shù)更新為新四元數(shù)，作為下一次四元數(shù)運算的初始數(shù)，再從(1)開始下一次的四元數(shù)運算 宮琛.四軸飛行器的研究與設(shè)計D安徽理工大學(xué).2015。與此同時將新的四元數(shù)更新規(guī)范化后轉(zhuǎn)化成三個歐拉角得下式(4-22)，完成了姿態(tài)的初步運算 趙翔,杜普選,李虎,等.基于MEMS加速度計和陀螺儀的姿態(tài)檢測系統(tǒng)J.鐵路計算機(jī)應(yīng)用,2012,21(3):15-18.： (4-27)4.6 校準(zhǔn)載體航向角通過三軸

48、數(shù)字電子羅盤可以校準(zhǔn)陀螺儀積分獲得的航偏角以消除累計誤差。如果電子設(shè)備干擾強(qiáng)烈的情況下需要暫停數(shù)字羅盤的數(shù)據(jù)融合 嚴(yán)懷成,黃心漢,王敏.多傳感器數(shù)據(jù)融合技術(shù)及其應(yīng)用J.傳感器技術(shù),2005,24(10):1-4，因為這樣測到的數(shù)據(jù)誤差非常的大。對地磁傳感器常用的校準(zhǔn)方法之一是平面校準(zhǔn)法 胡琦逸.四旋翼飛行器的姿態(tài)估計與優(yōu)化控制研究D杭州電子科技大學(xué).2014：使用數(shù)字羅盤前，對其進(jìn)行初始化，然后進(jìn)行磁場校準(zhǔn)，將傳感器水平放置并旋轉(zhuǎn)一周得到新的圓周圓心位置 楊云濤,石志勇,關(guān)貞珍,等,一種基于磁偶極子磁場分布理論的磁場干擾補償方法J.兵工學(xué)報, 2008,29(12):1485-1491.。這是

49、對水平的平面的校準(zhǔn)，校準(zhǔn)前后對比如圖4.4所示。圖 4.4 xy平面校準(zhǔn)前后對比同理需要對豎直平面進(jìn)行校準(zhǔn)，校準(zhǔn)前后的對比圖，如圖4.5所示。圖 4.5 xz平面校準(zhǔn)前后對比當(dāng)載體靜止時，設(shè)地球磁場強(qiáng)度在載體坐標(biāo)系b系各個軸的分量為，在導(dǎo)航坐標(biāo)系R系各軸的分量，根據(jù)可得： (4-28)將4-22式求得的、代入上式，即可求出和。航向角可由式4-24得出： (4-29)5 四旋翼飛行器系統(tǒng)軟件設(shè)計5.1 系統(tǒng)程序設(shè)計5.1.1 姿態(tài)參考系統(tǒng)軟件設(shè)計姿態(tài)參考系統(tǒng)的軟件部分主要是對MEMS傳感器數(shù)據(jù)和三軸數(shù)字電子羅盤傳感器的數(shù)據(jù)進(jìn)行處理和數(shù)據(jù)融合，圖5.1給出了姿態(tài)參考系統(tǒng)的軟件流程圖。圖 5.1 姿

50、態(tài)參考系統(tǒng)程序流程圖通過軟件流程圖可以清楚地看到，系統(tǒng)首先完成MCU內(nèi)部初始化的工作，即配置STM32使用到的外設(shè)，包括時鐘系統(tǒng)、NVIC中斷控制器、USART串口通信、I2C接口、定時器；然后進(jìn)行硬件初始化以及系統(tǒng)的姿態(tài)解算，采用模塊化的設(shè)計，具體包括以下4個部分：(1)初始化硬件；(2) MEMS慣性測量單元的數(shù)據(jù)采集模塊；(3)多傳感器數(shù)據(jù)融合的姿態(tài)解算模塊；(4)姿態(tài)信息輸出模塊。首先開啟定時器中斷，在2.5ms的計時中斷實現(xiàn)通過I2C接口讀取MEMS傳感器和三軸數(shù)字電子羅盤的測量數(shù)據(jù)，根據(jù)傳感器的特點進(jìn)行濾波后，再利用初始校正所得到的校正數(shù)據(jù)對讀取的數(shù)據(jù)作校正，校正完后轉(zhuǎn)換成實際的物理量，再對其做加權(quán)平均減小誤差，然后進(jìn)行四元數(shù)姿態(tài)解算和地磁數(shù)據(jù)校準(zhǔn)，最后進(jìn)行數(shù)據(jù)融合輸出姿態(tài)角信息。5.1.2 PID控制算法設(shè)計根據(jù)四