STM32飛控算法理論

來源 https://zhuanlan.zhihu.com/p/107044187

參考 https://www.zhihu.com/column/c_1208718933341220864

 

前言

前面我介紹了很多有關微型四軸飛行器嵌入式底層開發的知識，雖然對於很多初學者來說，其中有一些概念比較晦澀難懂，但是它們確實能夠讓大家對於嵌入式底層開發有更深入的理解與掌握。好了言歸正傳，在本篇文章中我會向大家介紹有關微型四軸飛行器飛控算法理論方面的相關內容，而至於什么是飛控，我想很多自己買過成品飛控板、組裝過航模或四軸飛行器的同學都不會陌生，顧名思義，飛控指的是飛行控制算法，即通過獲取IMU（慣性傳感單元）數據，並采用濾波、PID等算法對飛行器當前的姿態進行實時解算，得到各電機的轉速和轉向，從而完成對飛行器的控制。由於飛控算法本身涉及到很多自動控制原理和線性代數方面的理論知識，所以推薦大家先把自控和線代好好復習一遍再去研究飛控算法，這樣理解起來可能會容易一些。

研究現狀

• 國內研究現狀

DJI

目前國內做四軸飛行器的公司有很多，其中做飛控系統最好的當屬大疆創新（DJI）。大疆創新（DJI）目前擁有NAZE、WooKong以及A系列的多種商業飛控系統，其中作為NAZA飛控系列的最新一代產品，N3多旋翼飛控系統采用DJI最新的控制導航算法，新增內置的雙IMU冗余設計，可實現數據實時互為備份，並結合全新內減震結構設計，賦予了飛行器更高的可靠性，為無人機愛好者及行業應用探索者提供穩定而全面的系統級解決方案。

Crazepony

Crazepony微型四軸飛行器由深圳創客火（Maker Fire）公司設計、制造並發行，其內置的飛控算法可以實現飛行器的定高懸停、有頭（X模式）和無頭模式飛行等功能。相比於商業飛控來說，Crazepony的飛控開源（包括源代碼、原理圖、工作原理、系統框架、設計思路等），且核心算法部分有詳細的教程文檔，既方便各大高校的學生和愛好者學習相關知識，也利於進行二次開發，以實現自己的創意。

MiniFly

MiniFly是廣州市星翼電子科技有限公司（ALIENTEK）最新推出的一款開源微型四軸飛行器，它所使用的飛控算法可以支持：定高和手動飛行、4D翻飛、拋飛、有頭（X模式）和無頭模式飛行、一鍵起飛和降落等功能。由於MiniFly微型四軸的硬件電路設計主要是基於國外的Crazeflie項目修改而來，所以其硬件性能在國內同等規格的四軸飛行器里絕對算是一流的，當然強大的硬件性能也為MiniFly實現更為復雜的飛控算法提供了一個絕佳的平台。

• 國外研究現狀

國外高校和科研機構對飛行器控制系統的研究已經有很多年了，技術相對來說比較成熟，而且基本都是以開源飛控為主。以下簡要介紹幾個目前比較流行的開源飛控系統：

APM

APM(ArduPilot Mega)是在2007年由DIY無人機社區（DIY Drones）推出的飛控產品，是當今最為成熟的開源硬件項目。APM基於Arduino開源平台，對多處硬件做出了改進，包括加速度計、陀螺儀和磁力計組合慣性測量單元（IMU）。由於APM良好的可定制性，APM在全球航模愛好者范圍內迅速傳播開來。通過開源軟件Mission Planner，開發者可以對APM進行相應的配置，接受並顯示傳感器的數據，使用Google Map完成自動駕駛等功能。除此之外，APM連接外置GPS傳感器以后能夠增強飛行的穩定性，並能夠完成自主起降、自主航線飛行、回家、定高、定點等豐富的飛行模式。APM可以使用外置的超聲波傳感器和光流傳感器，在室內實現定高和定點飛行。

PX4

PX4是一個軟硬件開源項目（遵守BSD協議），目的在於為學術、愛好和工業團體提供一款低成本、高性能的高端自駕儀。這個項目源於蘇黎世聯邦理工大學的計算機視覺與幾何實驗室、自主系統實驗室和自動控制實驗室的PIXHawk項目。於2004年推出的PIXHawk飛控是PX4飛控的升級版本，它擁有PX4和APM兩套固件和相應的地面站軟件。該飛控是目前全世界飛控產品中硬件規格最高的產品，也是當前愛好者手中最炙手可熱的產品。PIXHawk擁有168MHz的運算頻率，並突破性地采用了整合硬件浮點運算核心的Cortex-M4的單片機作為主控芯片，內置兩套陀螺和加速度計MEMS傳感器，互為補充矯正，內置三軸磁場傳感器並可以外接一個三軸磁場傳感器，同時可外接一主一備兩個GPS傳感器，在故障時自動切換。

CC3D

CC3D是Openpilot旗下最流行的飛控系統，此飛控板只采用一顆72MHz的32位STM32單片機和一顆MPU6000就能夠完成四旋翼、固定翼、直升機的姿態控制飛行。與所有開源飛控不同，它不需要GPS融合或者磁場傳感器參與修正，就能保持長時間的姿態控制，而且通過設置就可以更改飛機種類、飛行模式、支持雲台增穩等功能。此外，CC3D飛控編譯完的固件容量只有大約100KB，代碼效率令人驚嘆，而且其地面站軟件集成了完整的電子地圖，可以通過電台實時監測飛機狀態。

MWC

MultiWiiCopter(MWC)飛控是一款典型的Arduino衍生產品，是專為多旋翼開發的低成本飛控，它完整地保留了Arduino IDE開發和Arduino設備升級和使用的方法。由於成本低、架構簡單、固件比較成熟，因此該飛控在國內外擁有大量愛好者。除了支持常見的四、六、八旋翼以外，該飛控的最大特點是支持很多奇特的飛行器類型，比如三旋翼、Y4型多旋翼（其中兩軸為上下對置）等。

PPZ

Paparazzi(PPZ)是一個軟硬件全開源的項目，它始於2003年，開發目標是建立一個配置靈活且性能強大的開源飛控項目。PPZ的一大特點是，該開源飛控方案中除了常見的飛控硬件、飛控軟件和地面站軟件之外，還包含地面站硬件，包括各種調制解調器、天線等設備。從功能上講，PPZ已經接近一個小型的無人機系統了。該開源項目的另一個特點是采用Ubuntu操作系統，它將全部地面站軟件和開發環境集成於該系統下，官方稱之為Live CD。一張CD加飛控硬件就可完成從開發到使用的全部工作。PPZ目前最流行的硬件版本擁有大量的擴展接口，方便開發者進行DIY。

Crazeflie

Crazeflie是國外最著名的微型四軸飛行器開源項目，它由三個來自瑞典的嵌入式工程師所創建，目的是使用盡可能少的零件來構造一款可以在室內使用的小型飛行機器人。得益於其出色、嚴謹的硬件電路設計、穩定且可靠的飛控系統以及模塊化的擴展能力，Crazeflie四軸飛行器可以完成很多同類型四軸飛行器無法做到的高難度實驗任務。除此之外，Bitcraze團隊還為Crazeflie微型四軸飛行器項目編寫了大量的使用和發教程，方便全世界的四軸愛好者們進行學習和開發（國內Crazepony和MiniFly兩大開源微型四軸項目就在很大程度上參考了Crazeflie中的一些設計理念和內容）。

基本概念

• 電機布局

四軸飛行器的四個電機呈十字形排列，驅動四片槳旋轉產生向上的推力。由於四個電機的軸距幾何中心的距離相等，所以當對角兩個軸產生的升力相同時能夠保證力矩的平衡，四軸不會向任何一個方向傾轉。而當四個電機一對正轉一對反轉時，可使得繞豎直軸方向旋轉的反扭矩平衡，保證了四軸航向的穩定。

與傳統的直升機相比，四軸飛行器有着下列的優勢：各個旋翼對機身所施加的反扭矩與旋翼的旋轉方向相反，因此當處於同一對角線的兩個電機向相同方向旋轉時（不同對角線上的電機轉向相反），就可以平衡旋翼對機身的反扭矩。

電機布局模式

如上圖所示，根據用戶自定義的機頭的位置不同，四軸飛行器可以分為×模式和+模式。×模式的機頭方向位於兩個電機之間，而+模式的機頭方向位於某一個電機上。×和+就是表示正對機頭方向時飛行器的形狀。相對而言，×模式穩定一些，但動作更靈活。但如果要完成翻跟頭等特技動作，可能需要用+模式。

• 動力學原理

四軸飛行器在空間共有6個自由度（分別沿3個坐標軸作平移和旋轉動作），這6個自由度的控制都可以通過調節不同電機的轉速來實現。基本運動狀態分別為：垂直運動、俯仰運動、滾轉運動、偏航運動、前后運動和側向運動。

垂直與俯仰運動

 

垂直運動

垂直運動相對來說比較容易。在上圖(a)中，因有兩對電機轉向相反，可以平衡其對機身的反扭矩，當同時增加四個電機的輸出功率，旋翼轉速增加使得總的拉力增大，當總拉力足以克服整機的重量時，四軸飛行器便離地垂直上升。反之，同時減小四個電機的輸出功率，四軸飛行器則垂直下降，直至平衡落地，實現了沿Z軸的垂直運動。當外界擾動量為零時，在旋翼產生的升力等於飛行器的自重時，飛行器便保持懸停狀態。保證四個旋翼轉速同步增加或減小是垂直運動的關鍵。

俯仰運動

在上圖(b)中，電機1的轉速上升，電機3的轉速下降，電機2、電機4的轉速保持不變。為了不因為旋翼轉速的改變引起四軸飛行器整體扭矩及總拉力改變，旋翼1與旋翼3轉速該變量的大小應相等。由於旋翼1的升力上升，旋翼3的升力下降，產生的不平衡力矩使機身繞Y軸旋轉（方向如圖所示），同理，當電機1的轉速下降，電機3的轉速上升，機身便繞y軸向另一個方向旋轉，實現飛行器的俯仰運動。

滾轉與偏航運動

滾轉運動

與上圖(b)原理相同，在圖(c)中，改變電機2和電機4的轉速，保持電機1和電機3的轉速不變，則可使機身繞X軸旋轉（正向和反向），實現四軸飛行器的滾轉運動。

偏航運動

四軸飛行器偏航運動可以借助旋翼產生的反扭矩來實現。旋翼轉動過程中由於空氣阻力作用會形成與轉動方向相反的反扭矩，為了克服反扭矩影響，可使四個旋翼中的兩個正轉兩個反轉，且對角線上的各個旋翼轉動方向相同。反扭矩的大小與旋翼轉速有關，當四個電機轉速相同時，四個旋翼產生的反扭矩相互平衡，四軸飛行器不發生轉動；當四個電機轉速不完全相同時，不平衡的反扭矩會引起四軸飛行器轉動。在圖(d)中，當電機1和電機3的轉速上升，電機2和電機4的轉速下降時，旋翼1和旋翼3對機身的反扭矩大於旋翼2和旋翼4對機身的反扭矩，機身便在富余反扭矩的作用下繞Z軸轉動，實現飛行器的偏航運動，轉向與電機1、電機3的轉向相反。

前后與側向運動

 

前后運動

要想實現飛行器在水平面內前后、左右的運動，必須在水平面內對飛行器施加一定的力。在圖(e)中，增加電機3轉速，使拉力增大，相應減小電機1轉速，使拉力減小，同時保持其他兩個電機轉速不變，反扭矩仍然要保持平衡。按圖(b)的理論，飛行器首先發生一定程度的傾斜，從而使旋翼拉力產生水平分量，因此可以實現飛行器的前飛運動。向后飛行與向前飛行正好相反。當然在圖(b)和圖(c)中，飛行器在產生俯仰、翻滾運動的同時也會產生沿X、Y軸的水平運動。

側向運動

在圖(f)中，由於結構對稱，所以側向飛行的工作原理與上面講到的前后運動完全一樣。

• 飛行模式

飛行模式分為有頭模式和無頭模式兩種。其中飛行器在飛行的過程中，其運動的前后左右以地理坐標系為參考坐標系，則為無頭模式飛行；而有頭模式則是飛行器運動的前后左右以自身的坐標系為參考坐標系。

這里對有頭和無頭模式進行展開講解。任何飛行器都一定有個自身的坐標系，也就是飛行器的頭和尾，這也就是前面說的飛行器的自身坐標系。如果推動遙控器向前運動，飛行器總是向它頭的方向飛行，那么這個飛行器就是運行在有頭模式。如果推動遙控器的向前飛行，飛行器還是向它起飛時頭指示的方向飛行，即使這個時候飛行器在飛行的過程中改變了機頭方向（操縱了遙控的航向角），那么這個飛行器依舊運行在無頭模式下。

因為飛行器的無頭模式是以地理坐標系為參考，而有頭模式則是以飛行器自身的坐標系為參考，所以對於不同的飛行模式，姿態解算算法是不太一樣的。對於無頭模式，姿態解算部分可以使用磁力計來測量飛行器相對於地球磁場的角度，從而算出機頭在磁場中的方向。另外，也可以直接對航向角進行積分，算出飛行器相對於起飛時機頭旋轉的角度，目前，Breeze微型四軸飛行器使用的就是這種實現方式。

軟件框架

嵌入式系統軟件實現部分沒有使用實時操作系統（RTOS），而是直接通過編寫裸機代碼，依靠中斷嵌套來完成整體功能的。在代碼的初始化階段，程序對STM32的定時器4進行了初始化，它可以中斷while死循環，用於更新遠程遙控數據、機身姿態融合、PID控制計算輸出和電機PWM輸出等操作。

總之，定時器4中斷是核心中斷，所有的飛控算法都是在這里實現的，機身的穩定也是依靠這個中斷來實現的。接下來，本文章將結合程序執行流程圖來詳細地講解定時器4中斷內部的組織結構和實現的功能：

Breeze微型四軸程序執行流程圖

如上圖所示，程序會在主循環中不斷輪詢各個定時器的標志位，從而產生三個大小不同的時間段。在這些時間段內，系統便可以完成像接收遙控器指令、更新傳感器數據以及更新電機控制等任務。主程序通過划分不同頻率的定時器循環，使得所有任務可以分時並發地執行，這有效地降低了函數串行執行時的延時問題，並且提高了主控對資源的利用率。

• 系統初始化

主函數在開始執行階段會先調用Hardware_Init()函數進行硬件相關設備和功能的初始化。初始化完成之后程序就會進入主循環while(1)中，主循環也是整個軟件功能實現的關鍵，程序一旦進入便會在里面不斷地運行，只有發生中斷時程序才會暫時離開主循環，而去執行優先級高的中斷服務程序。

• 主循環-100Hz循環

主循環體內會首先判斷timer_loop_flag_100hz的值是否為真，而timer_loop_flag_100hz標志位是在TIM4定時器中斷中每10ms置位一次，即該循環會每10ms執行一次其中的工作。

在這個循環之中，STM32會讀取MPU6050和MS5611數據並進行融合。因為本論文采用的是軟件解算MPU6050的姿態，所以讀取的數據應為陀螺儀和加速度計的AD值，之后再將數據進行標定、濾波、校正后通過四元數融合得到三軸歐拉角，具體如下圖所示：

歐拉角計算流程

加速度傳感器采集數據容易失真，造成姿態解算出來的歐拉角錯誤。只用角度單環的情況下，系統很難穩定運行，因此可以加入角速度作為內環。角速度由陀螺儀采集數據輸出，采集值一般不存在受外界影響情況，抗干擾能力強，並且角速度變化靈敏，當受外界干擾時回復迅速增強了系統的魯棒性。

姿態雙環PID控制

如上圖所示，把角度作為外環，角速度作為內環，即通過將角度環的輸出值作為角速度環的輸入來進行姿態雙環PID控制。

• 主循環-50Hz循環

這個循環每20ms執行一次，其任務主要是解析通過無線通信發送過來的控制指令，並結合當前四軸飛行器的姿態，更新飛控核心程序，使得其可以按照指令進行移動。

• 主循環-10Hz循環

這個循環會以每100ms的時間間隔執行一次，它的任務主要是當四軸飛行器出現異常情況時，可以控制飛行器平穩地降落。異常情況包括電池電量過低、飛行高度超出范圍以及無線通信失敗等。

算法講解

• 姿態解算簡介

姿態解算也叫做姿態分析，姿態估計或姿態融合，它是指根據采集到的IMU數據（陀螺儀、加速度計、磁力計等）來求解得到四軸飛行器的空中姿態，姿態解算分為快速解算和深度解算兩種。

如果根據陀螺儀的三軸角速度對時間的積分得到了四軸飛行器的俯仰、翻滾和航向角，這是快速解算。快速解算得到的姿態是存在誤差的，而且誤差會伴隨積分而累加，如果再結合三軸地磁和三軸加速度數據進行漂移補償和校正，得到准確的姿態，這就是深度解算。不過由於通過快速解算得到的姿態誤差往往較大，因此目前姿態解算通常指的就是深度解算。

• 姿態數學表示

數學模型

姿態是用來描述一個剛體的固連坐標系和參考坐標系之間的角位置關系。四軸飛行器使用的參考坐標系是當地水平坐標系，即地理坐標系，而其自身的固連坐標系叫做載體坐標系。地理坐標系有很多種，如下圖所示，這里使用的是比較常用的NED（即“北東地”）坐標系。

NED坐標系

四軸飛行器在空中飛行時，我們使用下圖中的姿態角來描述其在空間里的角度關系。其中姿態角包含有翻滾角Roll（記作  ）、俯仰角Pitch（記作  ）和航向角Yaw（記作  ）。

通常我們一般選擇把  軸作為四軸飛行器的正前方，那么俯仰角  則為載體繞  軸旋轉的角度，指向水平面以下為正，指向水平面以上為負，角度范圍從−90°至90°；翻滾角  為載體繞  軸旋轉的角度，坐標  指向水平面以上為正，指向水平面以下為負，角度范圍從−180°至180°；而航向角  為機體繞  軸旋轉的角度，俯視圖逆時針為正，順時針為負。

機體坐標系

如地理坐標系圖所示，定義導航坐標系為  ，坐標原點  為載體的轉動中心，地理坐標系在載體運動時作為基准坐標系，所以求解載體航行姿態時，需要先將載體坐標系內測量到的數據轉換到地理坐標系中，再進行姿態解算。

如機體坐標系圖所示，定義載體坐標系為  ，其坐標系原點  在載體的質心或者中心，  軸沿載體水平方向向前，  軸指向朝着載體正前方看的左側方向，  軸則垂直於  平面沿載體豎軸向上，且載體坐標系  滿足右手法則。

表示方式

• 歐拉角

歐拉角由萊昂哈德·歐拉創立，用來描述剛體在三維歐幾里得空間的取向。對於在三維空間里的一個參考系，任何坐標系的取向，都可以用三個歐拉角來表現。參考系又稱為實驗室參考系，是靜止不動的，而坐標系則固定於剛體，隨着剛體的旋轉而旋轉。

歐拉角表示

地理坐標系 與載體坐標系之間的坐標變換矩陣即方向余弦矩陣可以通過三次基本旋轉得到。對應的旋轉變換矩陣為：

使用下面的歐拉角微分方程可以方便地對四軸飛行器的姿態進行解算：

 

上面公式中左側是更新后的歐拉角，右側是上個周期測算出來的角度，而三個角速度則由安裝在四軸飛行器上的三軸陀螺儀測得。因此，求解這個微分方程就能解算出當前的歐拉角。

不過，由於歐拉微分方程中包含了大量的三角函數運算，這給實時解算帶來了一定的難度，而且當俯仰角為正負90°時，方程式會出現神奇的GimbalLock（萬向鎖）問題，因此歐拉角解算只適用於水平姿態變化不大的情況，而不適用於全姿態飛行器的姿態確定。

• 四元數

四元數是由愛爾蘭數學家威廉·盧雲·哈密頓在1843年發現的數學概念。明確地講，四元數是復數的不可交換延伸，如把四元數的集合考慮成多維實數空間的話，四元數就代表着一個四維空間，相對於復數為二維空間。

四元數是簡單的超復數。復數是由實數加上虛數單位i組成，其中  。相似地，四元數都是由實數加上三個虛數單位  組成，而且它們有如下的關系：  和  。每個四元數都是  的線性組合，即四元數可用下面的公式進行表示，其中  和  是實數，  是轉動的角度，  為旋轉軸。

采用四元數表示姿態變換時，由四元數的運算法則，將其中的四元數按照元素展開並按照運算符法則進行計算可以得到四元數表示的方向余弦矩陣：

最后綜合以上公式可得歐拉角和四元數之間的轉換關系：

相對於另幾種旋轉表示法（矩陣，歐拉角，軸角），四元數具有某些方面的優勢，如速度更快、提供平滑插值、有效避免萬向鎖問題、存儲空間較小等等。

• 姿態數據測量

陀螺儀

陀螺儀是測量角速度的傳感器，具有高動態特性，它是一個間接測量角度的器件，它測量的是角度的導數，即角速度，要將角速度對時間積分才能得到角度。陀螺儀內部有一個陀螺，它的軸由於陀螺效應始終與初始方向平行，這樣就可以通過與初始方向的偏差計算出旋轉方向和角度。

理論上講只用陀螺儀是可以完成姿態解算任務的，只需要對3個軸的陀螺儀角速度進行積分，得到3個方向上的旋轉角度，也就是姿態數據。不過由於誤差以及噪聲的存在，對陀螺儀的積分並不能夠得到完全准確的姿態，尤其是運轉一段時間以后，積分誤差的累加會讓得到的姿態和實際的相差甚遠。以下幾種原因會導致陀螺儀得到的姿態結果不准確：

• 零點漂移
  零點漂移指的是陀螺儀在零點處有誤差，從而導致隨着積分時間的增長，測量值會偏離真實值越來越遠。因此，陀螺儀只有在短時間內才有較大的參考價值。
• 白噪聲
  電信號的測量中，一定會帶有白噪聲，陀螺儀數據的測量也不例外。所以獲得的陀螺儀數據中也會帶有白噪聲，而且這種白噪聲會隨着積分而累加。
• 溫度/加速度
  陀螺儀是一個溫度和加速度敏感的元器件。例如對於加速度，多軸飛行器中的馬達一般會帶來較強烈的振動，一旦減震控制不好，就會在飛行過程中產生很大的加速度，必會帶來陀螺輸出的變化，引入誤差。
• 積分誤差
  對陀螺儀角速度的積分是離散的，長時間的積分會出現漂移的情況。所以要考慮積分誤差的問題。

由於陀螺儀測量姿態存在這么多的誤差，所以我們必須要使用其它傳感器輔助校正，其中最重要的就是下面要講的加速度計。

加速度計

加速度計的低頻特性好，可以測量低速的靜態加速度。對於四軸飛行器來說，3軸加速度計輸出重力加速度g（也就是前面說的靜態加速度）在加速度計所在機體坐標系3個軸上的分量大小，由於重力加速度的方向和大小是固定的，所以通過這種關系，就可以得到加速度計所在平面與地面的角度關系。當然，加速度計若是繞着重力加速度的軸轉動，則測量值不會改變，也就是說加速度計無法感知這種水平旋轉。

通過使用加速度計並結合陀螺儀的數據來做融合，可以矯正姿態的偏差，從而獲取系統的最優解。但是由於加速度計無法測量出航向角，所以只用陀螺儀和加速度計的飛行器會存在航向漂移的情況。解決這個問題的方法是再使用一個磁力計傳感器來測出當前飛行器的地磁角來修正漂移，不過由於飛行器自身裝備有電機等對磁場影響較大的設備，因此由磁力計測出的地磁角往往是不准確的。目前，Breeze微型四軸飛行器未涉及與磁力計相關的內容。

• 姿態解算算法

解算流程

姿態解算的核心在於旋轉，旋轉共有4種表示方式：矩陣、歐拉角、軸角和四元數。其中矩陣適合變換向量，歐拉角最直觀，軸角則適合幾何推導，而在組合旋轉方面，四元數表示最佳。因為姿態解算需要頻繁組合旋轉並用旋轉變換向量，所以應采用四元數來保存飛行器的姿態（地理坐標系中的俯仰/翻滾/航向角）。下圖是姿態解算的整個流程：

姿態解算流程

如上圖所示，STM32會通過IIC總線采集MPU6050中陀螺儀和加速度計的AD值，之后再通過姿態解算算法得到飛行器當前的姿態（姿態使用四元數表示），然后將四元數轉化為歐拉角，用於后面的姿態PID控制。

解算算法

姿態解算算法會通過巧妙的方法來使用加速度計數據去修正由陀螺儀數據經過快速解算所產生的姿態誤差，並最終得到准確的飛行器姿態。目前常用的軟件姿態解算算法為：非線性互補濾波算法、Kalman濾波算法、Mahony互補濾波算法等。下面對其中的Mahony互補濾波算法進行介紹（Breeze微型四軸飛行器使用的就是這種）。

Mahony互補濾波算法的原理是根據加速度計和磁力計的數據，與轉換到載體坐標系的參考重力向量和地磁向量進行求誤差，用這個誤差來矯正陀螺儀的輸出，然后用它來更新四元數，最后再將四元數轉換為歐拉角。以下是算法執行的具體步驟：

1. 對加速度數據進行歸一化，得到單位加速度。
2. 將飛行器上次計算得到的姿態（四元數）換算成方向余弦矩陣中第三列的三個元素。根據余弦矩陣和歐拉角的定義，地理坐標系的重力向量轉換到載體坐標系下，正好是方向余弦矩陣第三列的三個元素。
3. 計算叉乘誤差。在載體坐標系上，加速度計測出來的重力向量和根據上次姿態解算的姿態所推算出的重力向量之間的誤差向量，就是上次姿態解算（可以認為是陀螺儀積分）后的姿態和加速度計測出來的姿態之間的誤差。向量間的誤差，可以用向量積來表示。因為叉積和陀螺儀積分誤差都處於載體坐標系下而且成正比，所以可以使用叉積向量來修正陀螺儀積分誤差。由於陀螺儀是對載體直接進行積分的，所以對陀螺儀的糾正量會直接體現在對載體坐標系的糾正。
4. 對叉乘誤差對時間進行積分。
5. 使用叉乘誤差來做PI修正陀螺儀零點漂移，通過調節參數，可以控制加速度計修正陀螺儀積分姿態的速度。
6. 使用四元數微分方程，得到修正后的陀螺儀數據，再對其進行時間積分，得到使用四元數表示的飛行器當前姿態，最后對四元數進行單位化處理就完成整個算法執行流程。

• 姿態控制算法

控制原理

四軸飛行器的旋翼與空氣發生相對運動，產生了向上的升力，當升力大於四軸的重力時四軸飛行器就可以飛起來了。理想情況下，只要四個電機的轉速是完全相同，就可以使四軸飛行器在飛行過程中保持水平，但由於電機和旋翼本身制造上的差異，想要控制四個電機達到相同的轉速是不可能的。因此，為了防止四軸飛行器發生側翻的情況，需要使用PID自動反饋控制系統來完成對四軸飛行器的自穩定。

PID控制理論

PID控制是最常見，應用最為廣泛的自動反饋系統。PID控制器由偏差的比例（P: Proportional）、積分（I: Integral）和微分（D: Derivative）來對被控對象進行控制。這里的積分或微分都是偏差對時間的積分或微分。

PID基本原理

對於一個自動反饋控制系統來說，共有以下幾個基本指標：

• 穩定性（P和I降低系統穩定性，D提高系統穩定性）：在平衡狀態下，系統受到某個干擾后，經過一段時間其被控量可以達到某一穩定狀態。
• 准確性（P和I提高穩態精度，D無作用）：系統處於穩態時，其穩態誤差。
• 快速性（P和D提高響應速度，I降低響應速度）：系統對動態響應的要求。一般由過渡時間的長短來衡量。
• 比例控制
  比例控制是一種最簡單的控制方式，其控制器的輸出與輸入誤差信號成比例關系。當僅有比例控制時系統輸出存在穩態誤差。比例項輸出：

• 積分控制
  在積分控制中，控制器的輸出與輸入誤差信號的積分成正比關系。對於只有比例控制的系統存在穩態誤差，為了消除穩態誤差，在控制器中必須引入積分項。積分項是誤差對時間的積分，隨着時間的增加，積分項會增大，這樣，即便誤差很小，積分項也會隨着時間的增加而加大，它推動控制器的輸出增大使穩態誤差進一步減小，直到等於零。因此，比例積分（PI）控制器可以使系統在進入穩態后無穩態誤差。積分項輸出：

• 微分控制
  在微分控制中，控制器的輸出與輸入誤差信號的微分成正比關系。微分調節的是偏差值的變化率，使用其能夠實現系統的超前控制。如果輸入偏差值線性變化，則需要在調節器輸出側疊加一個恆定的調節量。大部分控制系統不需要調節微分時間，因為只有時間滯后的系統才需要附加這個參數。微分項輸出：

綜上所述，PID控制的數學公式為：

單環PID控制

單環PID控制

如上圖所示，輸入的期望角度就是遠程遙控端控制飛行器的角度值，反饋當前角度就是由傳感器測得的飛行器角度，這里的角度值分別指的是翻滾角、俯仰角和航向角，在做PID控制計算的時候，它們是相互獨立的。這里以翻滾角為例，介紹一下PID計算的全過程：

1. 計算角度誤差
   角度誤差=期望角度-當前角度
2. 計算比例項
   比例項=比例系數角×度誤差
3. 計算積分項
   微分項=微分項系數×角度誤差積分
4. 計算微分項
   微分項=微分系數×角速度的微分
5. 整合結果輸出
   結果輸出=比例項+積分項+微分項

以上步驟適用於翻滾角和俯仰角的PID計算，但航向角比較特殊，因為航向角法線方向剛好和地球重力平行，因此這個方向的角度無法由加速度計直接測得。由於Breeze微型四軸飛行器並沒有使用磁力計，所以使用PID計算航向角時候，就不存在比例項，只能使用微分項來調節。

串級PID控制

角度單環PID控制算法僅僅考慮了四軸飛行器的角度信息，如果要增加四軸飛行器的穩定性並提高控制質量，可以使用下面的角度/角速度串級PID控制算法：

串級PID控制

如上圖所示，串級PID控制算法其實就是將角度環PID和角速度環PID控制算法串聯了起來，因為兩個控制器比單個能控制更多的變量，因此它增強了系統的抗干擾性，使得四軸飛行器的適應能力更強。它的計算過程如下：

1. 計算角度誤差
   角度誤差=期望角度-當前角度
2. 計算外環比例項
   外環比例項=外環比例系數×角度誤差
3. 計算外環積分項
   外環積分項=外環積分系數×角度誤差積分
4. 整合外環輸出
   外環結果輸出=外環比例項+外環積分項
5. 計算角速度誤差
   角速度誤差=外環結果輸出-當前角速度
6. 計算內環比例項
   內環比例項=內環比例系數×角速度誤差
7. 計算內環積分項
   內環積分項=內環積分系數×角速度誤差積分
8. 計算內環微分項
   內環微分項=內環微分系數×當前角速度微分
9. 整合內環結果輸出
   內環結果輸出=內環比例項+內環積分項+內環微分項

電機輸出

電機輸出是姿態控制算法的最后一步，它在油門基准值的基礎之上整合上面通過PID控制算法得到的翻滾角、俯仰角和航向角進行電機控制。

• 自主懸停

自主懸停指的是四軸飛行器能夠懸停在某個位置上，在發生了偏移之后依然能夠自動校正並回到原來懸停的位置。由於四軸飛行器在空中的位置是用一個三維坐標來表示的，對於自主懸停，其中涉及了兩個維度。第一是水平面上的自主懸停，飛行器不能夠發生左右或前后的位置漂移。第二個是在垂直方向上，飛行器不能夠發生太大的高度變化。針對這兩個不同的維度，有不同的解決辦法。在水平面上，為了確定飛行器的位置，可以使用GPS或光流傳感器進行定位。而在垂直方向上，一般使用氣壓計或超聲波模塊進行定高。

目前，Breeze微型四軸飛行器擁有一個板載的高精度氣壓計MS5611，所以可以實現高度的自主懸停。在水平面上，雖然該飛行器還裝備有一個小型的攝像頭圖傳模塊，但由於視覺算法部分還存在着一些問題，所以目前僅靠3軸加速度計和3軸陀螺儀數據來做的雙環PID控制是無法使其在水平面上做到真正意義上的自主懸停的。以下簡單地講解一下目前Breeze微型四軸飛行器所使用的高度雙環PID控制算法：

高度雙環PID控制

如上圖所示，由於MS5611氣壓計的精度為10cm，所以需要融合加速度計互補濾波得到較為准確的高度值。用高度作為外環，速度作為內環形成高度雙環PID控制，最后調節輸出油門以實現Z軸的自主懸停。

總結

到此為止，Breeze微型四軸飛行器飛控算法方面的內容就全部介紹完了，我想有很多同學看完之后還是會覺得雲里霧里。的確，飛控這一部分在整個四軸飛行器的開發過程中算是最具挑戰性，同時也是最難以理解的，所以不要指望僅僅只看幾篇網上的博客教程就能入門飛控。如果你的研究方向是基於四軸飛行器平台來做上層研究的話（比如視覺避障、視覺SLAM、自主懸停和運動規划等），那么目前你所了解到的飛控知識應該已經足夠，但是如果你想研究四軸飛行器的底層飛控算法實現，甚至是想自己實現一整套飛控算法的話，那么你還需要閱讀更多飛控方面的專業書籍、論文，並要對國內外優秀的開源飛控代碼進行深入的研究。

 

=============== End