微型四旋翼控制系統(tǒng)的設計—畢業(yè)論文設計

1、畢業(yè)設計（論文）0 前言無人飛行器（UAV）自主飛行技術多年來一直是航空領域研究的熱點，并且在實際應用中存在大量的需求，例如：偵察與營救任務，科學數(shù)據(jù)收集，地質(zhì)、林業(yè)勘探，農(nóng)業(yè)病蟲害防治，以及視頻監(jiān)控，影視制作等。通過無人飛行器來完成上述任務可以大大降低成本和提高人員安全保障。無人飛行器的主要優(yōu)點包括：系統(tǒng)制造成本低，在執(zhí)行任務時人員傷害小，具有優(yōu)良的操控性和靈活性等。而旋翼式飛行器與固定翼飛行器相比，其優(yōu)勢還包括：飛行器起飛和降落所需空間少，在障礙物密集環(huán)境下的可控性強，以及飛行器姿態(tài)保持能力高。由國際無人運輸系統(tǒng)協(xié)會(International Association for Unmann

2、ed Vehicle Systems）組織的一年一度的國際空中機器人競賽(International Aerial Robotics Competition),為自主旋翼式飛行器的應用潛力研究提供了一個很好的展示平臺。該競賽吸引了來自全世界不同國家研究團隊的參與，來完成預先設定的自主飛行任務。在無人飛行器自主飛行的眾多技術當中，飛行器自主飛行控制算法的設計一直是控制領域眾多研究者最關心的問題之一。經(jīng)典的控制策略在飛行器系統(tǒng)的某個特定作用點上往往首先將系統(tǒng)模型線性化，然后在此基礎上運用經(jīng)典控制理論對系統(tǒng)進行分析和控制，控制精度和控制能力偏弱。相比之下，運用現(xiàn)代非線性控制理論設計的控制算法，其性能

3、明顯優(yōu)于經(jīng)典控制算法。小型四旋翼飛行器與其它飛行器相比，其優(yōu)勢在于其機械結(jié)構較為簡單，并且只需通過改變四個馬達的轉(zhuǎn)速即可實現(xiàn)控制，且飛行機動能力更加靈活。另一方面，小型四旋翼飛行器具有較高的操控性能，并具有在小區(qū)域范圍內(nèi)起飛，盤旋，飛行，著陸的能力。飛行器可以飛至離目標更近的區(qū)域，而不像傳統(tǒng)直升機由于其巨大的單旋翼而不能近距離靠近目標。同時，小型四旋翼飛行器研究也為自動控制，先進傳感技術以及計算機科學等諸多領域的融合研究提供了一個平臺。在機器人的智能控制，三維路徑規(guī)劃，多飛行器的空中交通管理和碰撞規(guī)避等方面，小型四旋翼飛行器自主飛行技術都具有極高的研究價值。1 四旋翼飛行器1.1 早期四旋翼飛

4、行器設計早在 20 世紀初期，就有人開始研制載人四旋翼飛行器。Breguet-Richet 四旋翼飛行器建造于 1907 年，在其十字支架的四端固定了四個長為 8.1 米的旋翼。其中兩個旋翼順時針方向旋轉(zhuǎn)，另外兩個旋翼逆時針方向旋轉(zhuǎn)。駕駛員坐在十字支架得中心位置，油門為唯一的控制設備，而導致對飛行器的穩(wěn)定性控制并不理想。飛行器起飛時，在其四端需要有工作人員幫助來實現(xiàn)穩(wěn)定起飛。雖然自主飛行并未實現(xiàn)，但同時使用順時針旋轉(zhuǎn)旋翼和逆時針旋轉(zhuǎn)旋翼的思想是 Breguet-Richet 四旋翼飛行器的顯著特點。1921 年 1 月，美國空軍軍團（US Army Air Corps）與 George de

5、Bothezat和 Ivan Jerome 簽訂合約共同建造垂直飛行器。1678kg 的“X”形結(jié)構支架用來支撐位移支架四端的直徑為 8.1 米的六翼片旋翼。一個小型的提升旋翼被置于支架交點的 180hp 的 Le Rhone 放射裝引擎上方，但隨后不久就被認為多余而被拆卸。每個旋翼可單獨控制其轉(zhuǎn)速以產(chǎn)生不同的升力，使飛行器傾斜而產(chǎn)生前后移動。該飛行器重 1700kg，于1922 年十月進行其第一次試飛。飛行器的引擎很快被換為 220hp 旋轉(zhuǎn)式 Bentley BR-2。1922年12月18日于 Wright Field 進行的一次試飛中，該飛行器飛行高度達 1.8 米，空中飛行達 1 分

6、42 秒。1923 年 1月 19 日的另一次試飛，飛行器將兩人抬至 1.2 米的高度。截止 1923 年底，該飛行器于俄亥俄州代頓市共試飛約 100 次。盡管合約中要求，飛行高度達到 100米，該飛行器實際飛行高度只有 5 米。在該研究花費了約 20 萬美元后，Bothezat展示了他設計的飛行器可以實現(xiàn)穩(wěn)定飛行，實際應用的直升機理論上是可以制造出來的。然而，該飛行器動力不足，響應性能不高，機械結(jié)構過于復雜，并存在潛在的穩(wěn)定性隱患。在嘗試橫向飛行時，需要大量的人力和物力的支持。因此，美國軍方最終對其失去了興趣。Convertawings于1950年在紐約的Amityville建造了一架四旋翼

7、飛行器。此飛行器的每個旋翼直徑達19英尺，并通過用兩個引擎來改變每個旋翼的升力來控制飛行器。經(jīng)試飛驗證，Convertawings飛行器在空中飛行性能良好，但由于當時人們對此種飛行器缺乏興趣而停止生產(chǎn)。1.2 四旋翼飛行器研究現(xiàn)狀近年來，關于如何建造和控制四旋翼飛行器的文章層出不窮。其中一些項目主要關注飛行器的建模以及通過仿真來驗證對其控制的策略是否有效。而另一些項目則著眼于飛行器在真實環(huán)境下是否能飛行成功。以下介紹近年來研制成功的一些有代表性的四旋翼飛行器。Draganflyer 是RC玩具公司開發(fā)的商業(yè)產(chǎn)品，它由 R/C 信號發(fā)射設備和板載控制芯片共同實現(xiàn)對其的控制。板載芯片包括一個對領航

8、者輸入的接收裝置，三個傳感器測量飛行器三個方向的角速度，一個微控制器來執(zhí)行控制算法計算。最新出品的Draganflyer 還包括四個紅外熱傳感裝置來幫助飛行器在室外飛行時的平衡。EADS Quattrocopter原本是用來當作研制微型飛行器控制單元的測試平臺，而如今因其良好的性能被工業(yè)界大量生產(chǎn)。飛行器板載芯片由MEMS 慣性測量單元（IMU ），氣流變化傳感器以及一個 GPS 接受裝置，R/C 信號接受器，16位模數(shù)轉(zhuǎn)換器和驅(qū)動馬達的功率發(fā)達器組成。Quattrocopter 使用的鋰電池在一次性充滿電后可維持其20分鐘的飛行。此飛行器長約 65cm,重約0.5 千克，機身可拆卸。電動馬達

9、使飛行器飛行噪音很小。飛行器有50% 的功率余量來裝載負重。X-4 Flyers在澳大利亞研制，研制人員對兩個慣性測量單元（IMU ）進行檢測，一個IMU 為Crossbow公司生產(chǎn)的商用IMU ，重約475 克, 另一個名為EiMU的IMU 有澳大利亞的一個機器人小組研制而成，重約100 克。X-4 Flyers研制人員稱商用IMU 較大的重量會影響飛行器的性能，因此最終采用 EiMU。一個雙核的板載計算機用來紀錄來自R/C 接收器的輸入命令，IMU 上的串行接口是數(shù)據(jù)以120Hz 的頻率被紀錄下來，而另一個串行接口用來實現(xiàn)IMU 和地面計算系統(tǒng)之間的通信。X-4 Flyers的研制提出了該

10、飛行器的非線性模型。此模型在飛行器處于盤旋狀態(tài)時被線性化，并在此線性模型的基礎上得出領航員控制輸入指令到歐拉角輸出的傳遞函數(shù)（transfer functions）。來 自 IMU 的傳感信號被高通濾波后用來測量內(nèi)環(huán)控制所需的角速度。而外環(huán)控制最終被放棄，因為從 IMIU 獲得飛行器高度值存在一定難度。 賓夕法尼亞大學的一個研究小組使用一個商業(yè)模型HMX-4來研制自己的四旋翼飛行器。此四旋翼飛行器的控制由板載計算機和地面計算系統(tǒng)處理傳感器和攝像機傳送的數(shù)據(jù)來實現(xiàn)。三個板載傳感器用來提供飛行器內(nèi)環(huán)穩(wěn)定控制的數(shù)據(jù)。置于地面的攝像機作為主傳感器來使用。置于飛行器底座的五個2.5cm 的彩色標記用為攝

11、像機提供位置信息，從而用標記定位算法便可計算出飛行器上標記的相對位置變化，進而計算出四旋翼的俯仰角，橫滾角，橫擺角和平動位置坐標。由于 HMX-4模型的重量限制，飛行器不能另外裝配GPS 系統(tǒng)合加速度測量儀。地面計算系統(tǒng)用于接受和處理由地面攝像機傳送的圖像信息，為四旋翼飛行器設定目標指令值，以及用串口傳送計算出的馬達驅(qū)動輸入。板載計算機通過傳感器信號來穩(wěn)定飛行器，并通過板上R/C 接收設備來獲取地面計算系統(tǒng)發(fā)送的控制信號。 該研究小組提出了一個四旋翼飛行器的動力學模型以及兩種控制策略，分別為反饋線性化方法（feedback linearization ）和后推法（backstepping）。仿

12、真結(jié)果顯示后推控制器的性能較優(yōu)，并隨后設計了此種控制器進行實驗。最近，四旋翼飛行器新添加了一個板載攝像機，聯(lián)合地面的攝像機來估計飛行器的位置。在原先有地面攝像機觀測的五個標記的基礎上，另一個標記被至于地面攝像機上，供板載攝像機觀測。這種雙攝像機的方法使計算出的角位移誤差和線位移誤差更小。 康奈爾大學啟動了兩個四旋翼飛行器的項目。第一個項目的目標是用三個板載傳感器和地面視頻系統(tǒng)來計算飛行器的飛行高度。四個LED 被置于飛行器十字支架的四端，用于給地面三個攝像機組成的視頻系統(tǒng)提供數(shù)據(jù)。板載計算機將傳感數(shù)據(jù)傳送給地面計算系統(tǒng)，并根據(jù)地面計算系統(tǒng)傳來的指令數(shù)據(jù)調(diào)整馬達轉(zhuǎn)速。地面計算系統(tǒng)根據(jù)傳感數(shù)據(jù)和視

13、頻信號計算出馬達輸入，并將其發(fā)送至板載計算機。該項目對四旋翼飛行器高度進行實時估測時，使用一個卡爾曼濾波器。此濾波器主要用于保留高頻的傳感器數(shù)據(jù)（300Hz ）和低頻的視頻信號（10Hz），并過濾掉其他頻率成分的干擾信號。實驗結(jié)果顯示，該濾波器成功的消除了傳感器偏移的不利影響。第二個項目主要關注飛行器的四個升力產(chǎn)生裝置以及飛行器的整體結(jié)構。研制小組使用MATLAB和有限元軟件來設計四旋翼飛行器的支架，以確定其結(jié)構單元的大小和受力強度。盡管無刷電動機的驅(qū)動電路很復雜，但還是得以采用，以實現(xiàn)較高的功率質(zhì)量比（Power to weight ratios）。使用大直徑的旋翼以保證盤旋狀態(tài)的穩(wěn)定系能。

14、此四旋翼飛行器使用板載電源和傳感器，這兩部分占據(jù)飛行器一半的質(zhì)量,慣性測量單元由Systron-Donner 生產(chǎn)。此IMU 存在一定程度的漂移，但足以保證飛行器盤旋狀態(tài)下的穩(wěn)定。 2 微型四旋翼飛行器結(jié)構和控制原理簡介目前飛行器控制方式主要有：遙控飛行、自主飛行以及半自主飛行三種方式。遙控飛行是指沒有安裝飛行控制系統(tǒng)，可看成是航模，只能在視距內(nèi)飛行，應用價值不大；自主飛行是指在飛行過程中完全脫離人的干預實現(xiàn)飛行，通常采用磁羅盤測量姿態(tài)，與角速率陀螺組成穩(wěn)定內(nèi)回路，并采用導航系統(tǒng)進行導航1，而半自主飛行是介于這兩者之間的飛行方式，飛行任務主要由人干預完成，飛行器裝有由角度傳感器和角速率陀螺組成

15、的姿態(tài)角穩(wěn)定內(nèi)回路，飛行穩(wěn)定性和可操作性大大提高。由于任務量和技術方面的原因，暫時實現(xiàn)半自主飛行。傳統(tǒng)直升機的旋翼系統(tǒng)由一個主旋翼和一個尾旋翼構成，通過變化旋翼翼片旋轉(zhuǎn)時的切角來改變飛行器的升力大小。與此不同的是，小型四旋翼飛行器旋翼翼片的旋轉(zhuǎn)切角是固定的，它是通過改變每個旋翼旋轉(zhuǎn)的角速度來控制整個飛行器的飛行。螺旋槳旋轉(zhuǎn)時，把空氣對螺旋槳的壓力在軸向和側(cè)向兩個方向分解，得到兩種力學效應：推力和轉(zhuǎn)矩。當四軸飛行器懸停時，合外力為 0，螺旋槳的推力用于抵消重力，轉(zhuǎn)矩則由成對的正槳反槳抵消。當飛行器運動時，因為推力只能沿軸向，所以只能通過傾斜姿態(tài)來提供水平的動力，控制運動由控制姿態(tài)來間接實現(xiàn)。 圖

16、2-1 四旋翼飛行器旋翼旋轉(zhuǎn)方向示意圖Fig.2-1 Four rotor aircraft rotor rotation direction假設四軸為剛體，根據(jù)質(zhì)點系動量矩定理，角速度和角加速度由外力矩決定2，通過控制四個螺旋槳，可以產(chǎn)生需要的力矩。首先對螺旋槳編號：第一象限的為 0 號，然后逆時針依次遞增，如圖(2-1) 。同步增加0號和1號、減小2號和3號槳的功率，可以在不改變推力的情況下，提供x軸的力矩；同步增加 1號和2號、減小0號和3號槳的功率，可以在不改變推力的情況下，提供y軸的力矩；同步增加 1號和3號、減小0號和2號槳的功率，可以在不改變推力的情況下，提供z軸的力矩。以上“增

17、加”和“減小”只是表明變化的方向，可以增加負數(shù)和減小負數(shù)，提供的力矩就沿對應軸的負方向了。把三個軸的力矩疊加起來，就得到各螺旋槳功率變化與提供的力矩的對應關系，可以用一個矩陣等式表示，見(2-1)式。T是螺旋槳的功率變化量，為41矩陣，每行分別對應0到3號螺旋槳；是力矩，為31 矩陣。mx、my和mz是各軸的力矩系數(shù)，用于把力矩轉(zhuǎn)換成功率變化量，具體數(shù)值與電機力矩特性、電機安裝位置等有關。 （2-1）各個電機實際輸出的功率記為，推力油門對應的功率量為，則有： （2-2）3 系統(tǒng)硬件設計3.1 系統(tǒng)總體介紹本系統(tǒng)采用STM32F103處理器作為主控芯片，此處理器是基于arm內(nèi)核的cortex-m

18、3系列處理器，具有運算速度快，內(nèi)置高速緩存，3個通用16位定時器和一個PWM定時器以及豐富的外設和I/O等特點。采用MPU6050作為姿態(tài)測量芯片，此芯片為系統(tǒng)的核心芯片，它集成了3軸MEMS陀螺儀，3軸MEMS加速度計，對陀螺儀和加速度計分別用了三個16位的ADC，將其測量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。采用霍尼韋爾的HM5883L芯片系統(tǒng)的羅盤芯片，此芯片是帶有數(shù)字接口的弱磁傳感器芯片，應用于低成本羅盤和磁場檢測領域。它包括最先進的高分辨率HMC118X 系列磁阻傳感器，并附帶霍尼韋爾專利的集成電路包括放大器、自動消磁驅(qū)動器、偏差校準、能使羅盤精度控制在12的12位模數(shù)轉(zhuǎn)換器和簡易的I2C

19、系列總線接口。采用NRF24L01+芯片作為系統(tǒng)的數(shù)據(jù)傳輸芯片，此芯片是一款工作在2.4GHz世界通用ISM頻段的單片無線收發(fā)器模塊。內(nèi)置頻率發(fā)生器、增強型SchockBurst模式控制器、功率放大器、晶體振蕩器、調(diào)制器、解調(diào)器。輸出功率、頻道選擇和協(xié)議都可以由用戶進行設置。為了減小體積和減輕重量，本系統(tǒng)把無線通信模塊固化在了PCB上。采用場效應管驅(qū)動四個電機旋轉(zhuǎn)，并增加了二極管續(xù)流，防止電機反電動勢擊穿場效應管。采用SP6205為處理器和其他芯片輸出電源，此芯片是一款低噪聲、低壓差、低靜態(tài)電流的線性穩(wěn)壓器，性能穩(wěn)定，出色的線性/負載調(diào)節(jié)特性，并具有過流保護及過熱關斷模塊。另外，本系統(tǒng)還集成了

20、TP4056充電芯片，此芯片是一款完整的單節(jié)鋰離子電池恒流、恒壓的線性充電器。內(nèi)部采用了放倒沖電路，當充電電流達到最終浮充電壓之后降至設定值的1/10時，將自動終止充電循環(huán)。本系統(tǒng)分為兩大部分，第一部分為四旋翼飛行器（圖3-1）。第二部分為無線遙控（圖3-2）。圖3-1 四旋翼飛行器原理框圖Fig.3-1 Four rotor aircraft principle block diagram圖3-2 無線遙控器原理框圖Fig.3-2 Wireless controller principle diagram3.2 四旋翼飛行器3.2.1 微控制器模塊本設計采用STM32F103增強型系列使用高

21、性能的ARM/Cortex-M3/32位的RISC內(nèi)核，工作頻率為72MHz，內(nèi)置高速存儲器(高達128K字節(jié)的閃存和20K字節(jié)的SRAM)，豐富的增強I/O端口和聯(lián)接到兩條APB總線的外設。所有型號的器件都包含2個12位的ADC、3個通用16位定時器和一個PWM定時器，還包含標準和先進的通信接口：多達2個I2C和SPI、3個USART、一個USB和一個CAN。工作于-40至+105的溫度范圍，供電電壓2.0V至3.6V，一系列的省電模式保證低功耗應用的要求。本設計采用ST公司生產(chǎn)的STM32F103T8U6作為系統(tǒng)的主處理器。主要負責采集傳感器檢測到的姿態(tài)角速率（俯仰角速率、橫滾角速率和偏航

22、角速率）、三軸的線加速度并實時解算；根據(jù)檢測到的飛行信息，結(jié)合指定的控制方案，計算輸出控制量；通過無線通信模塊與地面進行數(shù)據(jù)的傳輸，實現(xiàn)接收控制命令改變飛行狀態(tài)和上傳飛行狀態(tài)的數(shù)據(jù)。STM32F103T8U6只需要少量的電容和晶振即可工作（圖3-3），外接姿態(tài)傳感器模塊、地磁傳感器模塊、無線模塊、充電模塊等。圖3-3 stm32最小系統(tǒng)原理圖Fig.3-3 Stm32 minimum system schematic diagram3.2.2 加速度計和陀螺儀模塊MPU-6050是全球首例9軸運動處理傳感器。它集成了3軸MEMS陀螺儀，3軸MEMS加速度計，以及一個可擴展的數(shù)字運動處理器DMP

23、（DigitalMotionProcessor），可用I2C接口連接一個第三方的數(shù)字傳感器，比如磁力計。擴展之后就可以通過其I2C或SPI接口輸出一個9軸的信號（SPI接口僅在MPU-6000可用）。MPU-6050也可以通過其I2C接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。MPU-6050對陀螺儀和加速度計分別用了三個16位的ADC，將其測量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運動，傳感器的測量范圍都是用戶可控的，陀螺儀可測范圍為250，500，1000，2000/秒（dps），加速度計可測范圍為2，4，8，16g。一個片上1024字節(jié)的FIFO，有助于降低系統(tǒng)功耗。和所

24、有設備寄存器之間的通信采用400kHz的I2C接口或1MHz的SPI接口（SPI僅MPU-6000可用）。對于需要高速傳輸?shù)膽?，對寄存器的讀取和中斷可用20MHz的SPI。另外，片上還內(nèi)嵌了一個溫度傳感器和在工作環(huán)境下僅有1%變動的振蕩器。微型四旋翼飛行器在某個時刻的狀態(tài)由6個物理量來描述，包括三維坐標整的3個位置量和沿3個軸的姿態(tài)兩（即稱為六自由度）。MPU6050集成了3軸陀螺儀和3軸加速度計，陀螺儀起到測量四旋翼飛行器的角速率的作用，在慣性導航系統(tǒng)中非常重要。加速度計感應3個軸向的線加速度。圖3-4 加速度計和陀螺儀模塊原理圖Fig.3-4 The principle diagram

25、of the accelerometer and gyroscope module下面以MPU6050為例來說明I2C的工作方式：（一）開始（S）和結(jié)束（P）標志MPU6050通過I2C通訊協(xié)議與處理器進行通訊,I2C接口包括串行數(shù)據(jù)線（SDA）和串行時鐘線（SCL）。連接到I2C接口的設備可做主設備或從設備。主設備將Slave地址傳到總線上，從設備用與其匹配的地址來識別主設備。當MPU6050連接到處理器時，MPU6050作為從設備。MPU6050的Slave地址為b110110X，7位字長，最低有效位X由A0管教上的邏輯電平?jīng)Q定，本設計A0接地，所以設備地址為b1101100。當SCL線為

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

27、狀態(tài)。當MPU6050空閑后，并且釋放時鐘線，原來的數(shù)據(jù)傳輸才會繼續(xù)。圖3-6 I2C總線的應答方式Fig.3-6 The response of the bus（三）通信開始標志（S）發(fā)出后，處理器會傳送一個7位的Slave地址，并且后面跟著一個第8位，稱為Read/Write位。R/W位表示處理器是在接受MPU6050的數(shù)據(jù)還是在向其寫數(shù)據(jù)。然后，處理器釋放SDA線，等待MPU6050的應答信號（ACK）。每個字節(jié)的傳輸都要跟隨有一個應答位。應答產(chǎn)生時，MPU6050將SDA線拉低并且在SCL為高電平時保持為低。數(shù)據(jù)傳輸總是以停止標志（P）結(jié)束，然后釋放通信線路。然而，處理器也可以產(chǎn)生重復

28、的開始信號去操作另外的I2C設備，而不發(fā)出結(jié)束標志。綜上可知，所有的SDA信號變化都要在SCL時鐘為低電平時進行，除了開始和結(jié)束標志。圖3-7 完整的I2C數(shù)據(jù)傳輸Fig.3-7 Complete I2C data transfer如果要寫MPU6050寄存器，處理器除了發(fā)出開始標志（S）和地址位，還要加一個R/W位，0為寫，1為讀。在第九個時鐘周期（高電平）時，MPU6050產(chǎn)生應答信號。然后處理器開始傳送寄存器地址，接到應答后，開始傳送寄存器數(shù)據(jù)，然后仍然要有應答信號，以此類推。如果要讀取MPU6050寄存器的值，首先由處理器產(chǎn)生開始信號（S），然后發(fā)送MPU6050的地址位和一個寫數(shù)據(jù)位

29、，然后發(fā)送寄存器地址，才能開始讀寄存器。緊接著，收到應答信號后，處理器再發(fā)一個開始信號，然后發(fā)送MPU6050地址位和一個讀數(shù)據(jù)位。然后，作為從設備的MPU6050產(chǎn)生應答信號并開始發(fā)送寄存器數(shù)據(jù)。通信以處理器產(chǎn)生的拒絕應答信號（NACK）和結(jié)束標志（P）結(jié)束。拒絕應答信號（NACK）產(chǎn)生定義為SDA數(shù)據(jù)在第9個時鐘周期一直為高。3.2.3 數(shù)字羅盤模塊霍尼韋爾 HMC5883L是一種表面貼裝的高集成模塊，并帶有數(shù)字接口的弱磁傳感器芯片，應用于低成本羅盤和磁場檢測領域。HMC5883L 包括最先進的高分辨率HMC118X 系列磁阻傳感器，并附帶霍尼韋爾專利的集成電路包括放大器、自動消磁驅(qū)動器、

30、偏差校準、能使羅盤精度控制在12的12位模數(shù)轉(zhuǎn)換器和簡易的I2C 系列總線接口。 HMC5883L 廣泛應用于手機、筆記本電腦、消費類電子、汽車導航系統(tǒng)和個人導航系統(tǒng)。原理圖如圖3-8。在慣性導航算法中，導航參數(shù)會隨著傳感器的測量誤差積累和發(fā)散，因而不能滿足長時間自主飛行的需要，故選用霍尼韋爾公司的三周是數(shù)字羅盤HMC5883L對慣性導航系統(tǒng)進行姿態(tài)校準。它的工作電壓時5V，采用USART協(xié)議通信，通過轉(zhuǎn)換電路之后可以和STM32的SCI口通信，并且自帶模數(shù)轉(zhuǎn)換，以便于微處理器直接通信。在實際測試中發(fā)現(xiàn)，電機旋轉(zhuǎn)會對數(shù)字羅盤的數(shù)據(jù)產(chǎn)生干擾，故暫時不加入數(shù)字羅盤的數(shù)據(jù)。3-8 數(shù)字羅盤模塊Fig

31、.3-8 Digital compass module3.2.4 無線數(shù)據(jù)傳輸模塊無線數(shù)據(jù)傳輸模塊是四旋翼無人飛行器和控制中心之間通信的橋梁。本設計選用NORDIC公司生產(chǎn)的NRF24L01芯片。NRF24L01 是一款工作在 2.42.5GHz 世界通用 ISM 頻段的單片無線收發(fā)器芯片。無線收發(fā)器包括: 頻率發(fā)生器、增強型SchockBurst模式控制器、功率放大器、晶體振蕩器、調(diào)制器、解調(diào)器，輸出功率頻道選擇和協(xié)議的設置可以通過SPI 接口進行設置。圖3-9 無線數(shù)據(jù)傳輸模塊Fig.3-9 Wireless data transmission module極低的電流消耗當工作在發(fā)射模式下

32、發(fā)射功率為-6dBm 時電流消耗為9.0mA 接收模式時為12.3mA。掉電模式和待機模式下電流消耗更低。本系統(tǒng)通過無線模塊發(fā)送數(shù)據(jù)，無線遙控器接收數(shù)據(jù)并傳送至電腦上位機。通過上位機，可以實時地顯示四旋翼的姿態(tài)，并可通過上位機控制飛行器的飛行。原理圖如圖3-9。NRF24L01模塊與處理器通過SPI接口進行通訊，SPI為四線同步串行接口，兩個控制線，兩個數(shù)據(jù)線。其中，串行時鐘輸出（SCLK）、數(shù)據(jù)輸出（SDO）和數(shù)據(jù)輸入（SDI）三條線為所有設備共享。主設備為每個從設備分配一個獨立的片選（CS）（本系統(tǒng)僅NRF24L01為SPI設備）；傳輸開始后CS為低，傳輸結(jié)束CS又變?yōu)楦?。當NRF24L0

33、1沒有被片選信號啟動時，它的數(shù)據(jù)輸出線（SDO）保持為高阻態(tài)。SPI傳輸特點：1） 數(shù)據(jù)的最高位（MSB）先傳，最低位（LSB）后傳2） 數(shù)據(jù)在時鐘（SCLK）的上升沿捕獲3） 數(shù)據(jù)在時鐘（SCLK）的下降沿轉(zhuǎn)換4） 最大時鐘頻率為1MHz5） SPI的每次讀/寫操作需要16個時鐘周期或更多。傳輸?shù)牡谝粋€字節(jié)為SPI地址，第二個字節(jié)為SPI數(shù)據(jù)。首字節(jié)的第一位為讀/寫位，0為寫，1為讀，后面7個位為寄存器地址。如果要發(fā)送多個字節(jié)，連續(xù)發(fā)送數(shù)據(jù)位即可。SPI讀寫操作時序圖如下：圖3-10 SPI讀操作Fig.3-10 SPI read operation圖3-11 SPI寫操作Fig.3-11

34、SPI write operation3.2.5 電機驅(qū)動模塊本設計采用PWM控制直流電機的轉(zhuǎn)速。微型四旋翼飛行器的動力系統(tǒng)由電機和旋翼組成。電機選用直接為7mm的空心杯電機。直流電機是功率器件，需要很大的驅(qū)動電流，控制器的驅(qū)動能力有限，因此必須選用專門的驅(qū)動芯片。本設計選用N溝道場效應管SI2302來控制和驅(qū)動直流電機，它具有較大的漏極電流，最大能通過2.8A的電流。由于飛行器的電機在轉(zhuǎn)動過程中會產(chǎn)生比較大的沖擊電流，所以加二極管D1、D3給電機的反電動勢提供放電回路（圖3-12）。下拉電阻R4防止上電時電機轉(zhuǎn)動。圖3-12 電機驅(qū)動模塊Fig.3-12 driver module of e

35、lectric motor3.2.6 電源模塊穩(wěn)壓芯片采用低壓差線性穩(wěn)壓器SP6205，此芯片噪聲低，紋波小，電路簡單。本系統(tǒng)采用4.2V動力電池提供電源，兩路3.3V電壓輸出，一路供主控芯片，一路供陀螺儀輸出，兩路電源相互隔離，防止陀螺儀的數(shù)據(jù)被干擾。原理圖如圖3-13。圖3-13 電源模塊Fig.3-13 power module3.2.7 充電模塊充電電路采用TP4056芯片，此芯片是一款完整的單節(jié)鋰離子電池線性充電器。其底部帶有散熱片的DOP8/MSOP8封裝與較少的外部引腳數(shù)目使得TP4056成為便攜式應用的理想選擇。TP4056可以適合USB電源和適配器電源工作。由于采用了內(nèi)部PM

36、OSFET架構，加上防倒充電路，所以不需要外部隔離二極管。熱反饋可對充電電流進行自動調(diào)節(jié)，以便在大功率操作或高環(huán)境溫度條件下對芯片溫度加以限制。充電電壓固定于4.2V，而充電電流可通過一個電阻器進行外部設置。當充電電流在達到最終浮充電壓之后降至設定值1/10時，TP4056將自動終止充電循環(huán)。當輸入電壓（交流適配器或USB電源）被拿掉時，TP4056自動進入一個低電流狀態(tài)，將電池漏電流降至2uA以下。TP4056在有電源時也可置于停機模式，從而將供電電流降至55uA。TP4056的其他特點包括電池溫度檢測、欠壓閉鎖、自動再充電和兩個用于指示充電、結(jié)束的LED狀態(tài)引腳。原理圖如圖3-14。圖3-

37、14 充電模塊Fig.3-14 charger module3.2.8 電池本設計的電池選用的是飛行航模專用的動力電池，可以提供足夠的電流來保證飛行器的飛行，參數(shù)如下：電量：400mAh電池電壓：3.7V放電倍率：25C充電終止電壓：4.2V放電終止電壓：3.0V尺寸：39mm 0.5mm重量：11g放電倍率指的是放電電流，以電池容量的倍數(shù)計算，此電池的最大放電電流可根據(jù)下面的公式計算：400mAh 25C = 10A，足以滿足飛行器飛行需要。3.3 無線遙控器圖3-15 遙控器核心板原理圖Fig.3-15 Schematic diagram of remote control遙控器采用意法半

38、導體公司的32位處理器STM32F103C8T6作為控制器，它是專門為為控制系統(tǒng)、工業(yè)控制系統(tǒng)和無線網(wǎng)絡等對功耗和成本敏感的嵌入式應用領域而設計的。STM32F103C8T6是一款基于ARMv7-M體系結(jié)構的32位標準處理器，具有1.25DMIPS/MHz的處理能力；擁有USB、USART、SPI、IC等共9個外設接口，由圖3-10可知，STM32F103C8T6處理器的外圍電路僅需要外部晶振和少量電容即可，內(nèi)部自帶的USB接口便于調(diào)試和程序下載。主控制器通過1個SPI與無線通信模塊連接，1個USART與USB轉(zhuǎn)串口模塊連接，USB轉(zhuǎn)串口模塊連接至電腦。本設計通過上位機顯示并控制飛行器的飛行姿

39、態(tài)，遙控器接收來自飛行器的姿態(tài)信號，并把信號傳送至上位機，上位機能夠顯示實時地顯示飛行器姿態(tài)，同時上位機能夠控制飛行器飛行。遙控器采用STM32處理器，通過NRF24L01無線模塊傳輸數(shù)據(jù)，并用USB轉(zhuǎn)串口線連接至電腦，圖3-15為STM32最小系統(tǒng)原理圖，圖3-16為處理器與無線模塊的接口電路。圖3-16 NRF24L01接口原理圖Fig.3-16 The schematic of Nrf24l01 interface4 系統(tǒng)軟件設計4.1 總體設計四旋翼飛行器控制系統(tǒng)軟件設計的總體目標是啟動飛行器控制系統(tǒng)的各個功能模塊并使之正常工作，按照既定規(guī)劃實現(xiàn)穩(wěn)定飛行。由于四旋翼飛行器為六自由度的系

40、統(tǒng)，而其控制量只有4個，這就意味著被控量之間存在耦合關系，所設計的控制算法應該能夠?qū)@種欠驅(qū)動系統(tǒng)足夠有效，用4個控制量對3個角位移和3個線位移進行穩(wěn)態(tài)控制。本設計首先通過姿態(tài)傳感器的數(shù)據(jù)解算出飛行器的姿態(tài)，然后適當?shù)倪x取控制量，運用控制理論中經(jīng)典的PID控制算法對飛行器系統(tǒng)進行控制。飛行控制系統(tǒng)的中央控制模塊主要完成系統(tǒng)初始化、系統(tǒng)自檢、解算傳感器數(shù)據(jù)、執(zhí)行控制算法、計算并輸出控制量等功能。系統(tǒng)程序流程如圖4-1所示。圖4-1 系統(tǒng)程序流程Fig.4-1 System process4.2 無線通信模塊無線通信模塊實現(xiàn)上位機對四旋翼飛行器的飛行控制和跟蹤定位，實時與上位機控制系統(tǒng)交換信息，接

41、收上位機控制系統(tǒng)傳輸?shù)娘w行控制指令信號、向上位機控制系統(tǒng)發(fā)送當前飛行器實時飛行和姿態(tài)數(shù)據(jù)等相應信息，以完成指定的飛行控制任務。STM32通過SPI接口與NRF24L01通信，將ROM中的數(shù)據(jù)發(fā)送出去。STM32控制NRF24L01發(fā)送數(shù)據(jù)，主要分為兩個步驟：一是STM32先向NRF24L01中寫入數(shù)據(jù)，二是控制NRF24L01發(fā)送數(shù)據(jù)。在執(zhí)行過程中，本設計先寫入發(fā)送數(shù)據(jù)的目標地址再寫入數(shù)據(jù)，然后再控制NRF24L01發(fā)送地址和數(shù)據(jù)。NRF24L01發(fā)送模式會自動產(chǎn)生字頭和CRC校驗碼，當發(fā)送過程完成后，數(shù)據(jù)準備好引腳通知STM32數(shù)據(jù)發(fā)送完畢。NRF24L01的發(fā)送和接收流程如圖4-2、圖4-

42、3。圖4-2 發(fā)送數(shù)據(jù)流程圖 圖4-3 接收數(shù)據(jù)流程圖Fig.4-2 The flow diagram of send data Fig.4-3 The flow diagram of receive data4.3 姿態(tài)的表示方法4.3.1 坐標系統(tǒng)飛行器涉及兩個空間直角坐標系統(tǒng)：地理坐標系和機體坐標系。地理坐標系是固連在地面的坐標系，機體坐標系是固連在飛行器上的坐標系。四軸飛行器運動范圍小，可以不考慮地面曲率，且假設地面為慣性系。地理坐標系采用“東北天坐標系”，X軸指向東，為方便羅盤的使用，Y軸指向地磁北，Z軸指向天頂。機體坐標系原點在飛行器中心，xy平面為電機所在平面，電機分布在|x|=

43、|y|,z=0的直線上，第一象限的電機帶正槳，z軸指向飛行器上方。如圖4-4所示。圖4-4 四軸的模型圖Fig.4-4 The model of four axis4.3.2 姿態(tài)的運算飛行器的姿態(tài)，是指飛行器的指向，一般用三個姿態(tài)角表示，包括偏航角(yaw)、俯仰角(pitch)和滾轉(zhuǎn)角(roll)。更深一層，姿態(tài)其實是一個旋轉(zhuǎn)變換，表示機體坐標系與地理坐標系的旋轉(zhuǎn)關系，這里定義姿態(tài)為機體坐標系向地理坐標系的轉(zhuǎn)換。旋轉(zhuǎn)變換有多種表示方式，包括變換矩陣、姿態(tài)角、轉(zhuǎn)軸轉(zhuǎn)角、四元數(shù)等。在本文中，矩陣用加粗大寫字母表示，如，左上標和左下標表示從機體坐標系(Aircraft)變換到地理坐標系(Eart

44、h)；四元數(shù)用加粗小寫字母表示，如，上下標意義與變換矩陣一樣；向量用帶箭頭加粗小寫字母表示，如，左上標A表示向量的值是在機體坐標系的坐標值。因為姿態(tài)實質(zhì)是一個旋轉(zhuǎn)變換，根據(jù)剛體有限轉(zhuǎn)動的歐拉定理，旋轉(zhuǎn)變換是可以串聯(lián)的，所以一個姿態(tài)可以經(jīng)過一個旋轉(zhuǎn)變換，變成另一個姿態(tài)。類比點和向量的概念，姿態(tài)相當于點，旋轉(zhuǎn)相當于向量，點可以通過加向量，變成另一個點。如果用矩陣表示旋轉(zhuǎn)，旋轉(zhuǎn)的串聯(lián)由矩陣乘法來實現(xiàn)。如果用四元數(shù)表示旋轉(zhuǎn)，則由四元數(shù)的乘法來實現(xiàn)旋轉(zhuǎn)串聯(lián)。用四元數(shù)來表示旋轉(zhuǎn)，組合旋轉(zhuǎn)時比用其他方法運算量更少，所以無論在計算機圖形學、飛行器控制等涉及剛體旋轉(zhuǎn)的領域，四元數(shù)都有舉足輕重的地位3。飛行器的姿

45、態(tài)計算是圍繞姿態(tài)四元數(shù)進行的，下面簡要介紹一下四元數(shù)的運算。1個四元數(shù)由4個實數(shù)組成： （4-1）規(guī)范化的四元數(shù)可以表示旋轉(zhuǎn)，見下式（4-2），為旋轉(zhuǎn)的轉(zhuǎn)角，單位向量為旋轉(zhuǎn)的轉(zhuǎn)軸。 （4-2）記四元數(shù)乘法的符號為。四元數(shù)乘法跟矩陣一樣，有結(jié)合律，但沒有交換律。運算過程見（4-3）式。 （4-3）四元數(shù)轉(zhuǎn)換成矩陣的函數(shù)記為R( )，具體過程見（4-4）式。 （4-4）4.4 姿態(tài)測量獲取當前姿態(tài)是控制飛行器平穩(wěn)飛行的基礎，姿態(tài)的測量要求低噪聲、高輸出頻率，當采用陀螺儀等需要積分的傳感器時，還需要考慮積分誤差等問題。近年來MEMS傳感器越來越成熟、應用越來越廣泛，稱為低成本姿態(tài)測量的首選器件4，因

46、此本設計使用的傳感器全部都是MEMS傳感器。在使用傳感器的值進行測量姿態(tài)之前，有必要校正傳感器。由于實驗條件限制，傳感器的校正有兩項，分別對兩種類型的傳感器：陀螺儀-靜止時0輸出的傳感器、加速度計-測量某向量場強度的傳感器。4.4.1 傳感器的校正（一） 陀螺儀對于陀螺儀等靜止時0輸出的傳感器，可以很方便地校正零偏。把傳感器固定好，這時對輸出值Xf求平均值，得到的A即為零偏，如（4-5）式。實際使用時，把測量得到的值減去零偏，得到的值就是校正值。實際應用的公式如（4-6），A為零偏值，31矩陣，單位：LSB；Yi為校正好的值，31矩陣，單位：rad/s；Xi為測量原始值，單位：LSB；gain

47、為轉(zhuǎn)換系數(shù)，單位：(rad/s)/LSB，由傳感器的數(shù)據(jù)手冊給出。 （4-5） （4-6）（二） 加速度計加速度計是測量所在點的重力場的值的傳感器，靜態(tài)時加速度計測的是等效重力加速度場。下面介紹加速度計的校正方法。 加速度計測量的對象是比力，也就是等效重力加速度和運動加速度的和，當靜止時，運動加速度為0，加速度計的測量值為等效重力加速度，可以利用這一點校正加速度計。加速度計的校正的思路為：對測量值平移和縮放，把測量值擬合到重力加速度。因此校正的任務為：尋找最佳的平移和縮放參數(shù)，使總體測量的數(shù)據(jù)值更接近重力加速度。記測量值為，校正后的值為，平移參數(shù)為，縮放參數(shù)為，他們之間的關系為（4-7）式。

48、（4-7）定義誤差u為測量值長度與重力加速度常數(shù)G的平方差 （4-8）把公式（4-7）代入（4-8），得： （4-9）記 （4-10） （4-11） （4-12）則u可以表示成 （4-13）設目標函數(shù)U，用來衡量整體誤差，這里用單個誤差的平方和。 （4-14）校正的具體任務為：尋找參數(shù)a,b,c,d,e,f,g，使U最小。因為U是a,b,c,d,e,f,g的多項式函數(shù)，使U最小的點必為極值點，一階偏導為0，得（4-15）式。 （4-15）記 （4-16）由式（4-10）、（4-11）和（4-15）： （4-17）為了求出P，需要解式（4-17）的齊次線性方程組。由于數(shù)據(jù)噪聲、運算精度限制等原因

49、，方程（4-17）極少會出現(xiàn)非零解，所以轉(zhuǎn)向求近似解，解法為經(jīng)典的高斯消元法。為了減小誤差，行的處理順序有講究，先處理待消變量系數(shù)最大的行。當處理到最后一個變量時，系數(shù)已經(jīng)變得很小了，強制令為0，以釋放一個自由度，這樣就有非零解了。解出的解帶一個任意常數(shù)，設為C，Pb為某基礎解系，見（4-18）式。 （4-18）由（4-11）、（4-12）和（4-18）得到6個校正參數(shù)： （4-19）所以校正加速度計的整體流程為：測量一批靜態(tài)數(shù)據(jù)，然后用這批數(shù)據(jù)根據(jù)（4-16）式生成方陣B，然后求方程（4-17）的近似解P，再代入式（4-19），得到校正參數(shù)。4.4.2 數(shù)據(jù)融合有了傳感器的數(shù)據(jù)，就可以用來計

50、算姿態(tài)了。加速度計測量對象為比力，受運動加速度的影響很大，特別是受飛行器機架振動的影響。而陀螺儀受外部影響弱，穩(wěn)定性好，但輸出量為角速度，需要積分才能得到姿態(tài)，無法避免誤差的累積。為了得到穩(wěn)定的、近實時的姿態(tài)，對各傳感器的數(shù)據(jù)取長補短，需要研究各種數(shù)據(jù)融合算法。（一） 姿態(tài)插值法首先介紹“姿態(tài)插值法”，過程分兩部分：一、如果已知初始姿態(tài)，可以利用陀螺儀積分，不斷推算出下一個姿態(tài)，這部分動態(tài)性能好，但誤差會累積；二、利用加速度計，可以直接計算出一個姿態(tài)，這個姿態(tài)的期望是正確的，但正如上文所述，這個姿態(tài)噪聲大，不穩(wěn)定。為了把兩個系統(tǒng)的優(yōu)點結(jié)合起來，對兩個系統(tǒng)的結(jié)果進行插值，得到的值作為當前的姿態(tài)。

51、下面介紹“姿態(tài)插值法”的計算過程。首先是第一個部分-陀螺儀積分算姿態(tài)。陀螺的輸出為時間離散的角速度，要對時間積分才得到角度。記陀螺輸出的角速度為，單位為rad/s，采樣間隔為。假定采樣間隔足夠短，在一個采樣時間間隔里，角速度不變，且轉(zhuǎn)角足夠小，忽略三角函數(shù)高階項，各軸間相互的影響忽略不計，根據(jù)式(4-2)，該間隔里的旋轉(zhuǎn)可以用四元數(shù)表示成 （4-20）更新前的姿態(tài)為，為在的基礎上的旋轉(zhuǎn)，則把他們相乘，即得到新的當前的姿態(tài)。 （4-21）然后是第二個部分。姿態(tài)的定義為兩個坐標系統(tǒng)的旋轉(zhuǎn)變換，因此只要知道兩對在兩個坐標系對應的向量，就可以把姿態(tài)求出來。兩對變量為加速度和磁場強度，測量出的加速度和磁

52、場強度是在機體坐標系的，而地理坐標系的加速度和磁場是常量，存在一個旋轉(zhuǎn)，可以把機體坐標系的加速度和磁場強度轉(zhuǎn)換到與地理坐標系的對應的常量重合，這個旋轉(zhuǎn)就是所求的姿態(tài)。由于傳感器系統(tǒng)誤差、噪聲等影響，測量出的加速度和磁場的夾角不是恒定值，不可能精確旋轉(zhuǎn)到與常量一樣，只能求最接近的旋轉(zhuǎn)。最接近的原則可定為：一、旋轉(zhuǎn)后四個向量共面；二、加速度和磁場成一定角度，旋轉(zhuǎn)后的角平分線與常量的角平分線重合。為了達到最接近原則，把處理的量由加速度和磁場強度，轉(zhuǎn)成它們的平面法線和角平分線。記加速度計輸出為，羅盤輸出為，規(guī)范化的等效重力加速度為，規(guī)范化的地磁場強度為 測量的加速度和磁場強度的平面法線向量和對角線向量

53、為： （4-22） （4-23）等效重力加速度敞亮和地磁場強度常量的平面法線向量和對角線為： （4-24） （4-25）定義一個函數(shù)，用于把旋轉(zhuǎn)軸和轉(zhuǎn)角轉(zhuǎn)換成四元數(shù)： （4-26）再定義一個函數(shù)，用于獲取把一個向量旋轉(zhuǎn)到另一個向量的旋轉(zhuǎn)四元數(shù)： （4-27）第一次旋轉(zhuǎn)，把平面法線旋轉(zhuǎn)到重合： （4-28）經(jīng)過第一次旋轉(zhuǎn)，資格向量已經(jīng)共面，測量的加速度和磁場強度的對角線向量變成： （4-29）再進行第二次旋轉(zhuǎn)，把對角線重疊在一起： （4-30）把兩次旋轉(zhuǎn)組合起來，就是第二部分所求的姿態(tài)了。 （4-31）把兩個部分算出的姿態(tài)進行插值，就可以得到當前的姿態(tài)。因為兩個部分算出的姿態(tài)相差小，用普通線性插

54、值即可。為插值系數(shù)，范圍在0,1，越接近0，第一部分的姿態(tài)占的權重就越大，一般取接近0的值。 （4-32）即為輸出的姿態(tài)。姿態(tài)插值法的流程如圖4-5。圖4-5 姿態(tài)插值法數(shù)據(jù)流程圖Fig.4-5 Data flowchart of interpolation（二） 梯度下降法從實際的使用效果來說，姿態(tài)插值法已經(jīng)滿足要求，但用了三角函數(shù)，即使經(jīng)過優(yōu)化，運算量還是很大，不方便應用到低性能MCU或提高運算頻率。Sebastian O.H. Madgwick提出了更加有效的方法用梯度下降法計算姿態(tài)5。其思路為：陀螺儀不斷積分姿態(tài)；同時把加速度和磁場強度的誤差表示成姿態(tài)的函數(shù)，令誤差最小的點為極值點，然后用梯度下降法逼近誤差最小的點，即糾正了陀螺的積分誤差。這個方法，陀螺計算的姿態(tài)占主要，加速度計和羅盤只是輔助糾正。 用陀螺積分姿態(tài)的方法跟“姿態(tài)插值法”的第一部分一樣，關鍵是如何用加速度和磁場強度糾正誤差。首先要定義誤差，最直觀就是定義為測量值與常量值之差，這樣加速度和磁場強度就分別有一個誤差了。因為只考慮方向，相減前要規(guī)范化。為了減少運算量、利用常量值里的 0和1分量，相減之前統(tǒng)一到機體坐標系，而不是地理坐標系。旋轉(zhuǎn)矩陣的逆等于其轉(zhuǎn)置矩陣。 （4-33）（4-34）加速度和磁場的誤差都是向量，需要把他們合并，而