基于MPU9150的四軸飛行器姿態控制系統設計

朱 陽，王偉成，王民慧

（貴州大學 電氣工程學院，貴陽 550025）

0 引言

我國地域廣大，自然條件復雜，是世界上自然災害最為嚴重的國家之一。地震等自然災害不僅給救災帶來不利，也進一步增加了施救的難度，加大了施救者受傷的危險程度。雖然自然災害的發生無法阻止，但是減少災害帶來的二次傷害是可以實現的[1]，例如可以開發微型無人駕駛飛行器，對上述不可達的危險地域進行探測和搜尋即可減少傷害的發生，尤其是在地勢險要或者人員無法到達的地區，能夠準確地把偵查到的信息傳送回來。而四軸飛行器能以靈活性、多功能性以及高效的處理能力來勝任復雜環境中的信息獲取的角色。同時，與傳統的搜救直升機相比，四軸飛行器以其簡單的機械結構、更小的螺旋槳、優良的飛行安全性能和先進控制算法的應用等優勢具有更廣泛的前景。

本文基于STM32F103C8T6微處理器，采用MPU9150慣性測量模塊，設計了四軸飛行器姿態控制系統。STM32系列處理器是ST公司推出的一款基于高性能、低成本、低功耗、外設豐富的嵌入式處理器，應用專門設計的低功耗高速內核豐富的片上資源可滿足各類傳感器通訊需求，與傳統的飛行控制器相比可大大降低系統的開發成本、節約資源。MPU9150系統集成度高，包含了三軸加速度計、三軸陀螺儀和三軸磁力計，電路簡單可靠，測量范圍廣，準確度高，動態響應快，體積小、功耗低，能夠快速的融合陀螺儀和加速度計數據得出姿態四元數，適合用于四軸飛行器的姿態控制系統。

1 四軸飛行器結構與原理

四軸飛行器的基本結構如圖1所示，飛行器在空中運動中，為了使整個機體轉矩平衡，采用正反槳設計，即對角線的兩組槳相同，相鄰的兩個槳相反，分為順時針旋轉（1、3）和逆時針旋轉（2、4），這樣正常飛行時兩個槳正轉兩個槳反轉，轉矩抵消，避免飛行器打轉[2]，當然，旋轉時需加大兩個正漿或兩個反漿來改變總的轉矩，從而改變偏航角，控制對角線上的一組槳的轉速不同，使機體傾斜一個角度產生水平分力推動飛行器平移，飛行速度可以由俯仰角的大小與電機的轉速來控制，最終實現飛行器的偏航運動、上下飛行與前后飛行。

圖1 四軸飛行器的基本結構圖

2 姿態控制系統功能

四軸飛行器有6個自由度，而只有4個控制輸入，因此，四軸飛行器的控制問題屬于欠驅動控制問題，具有不穩定和強耦合等特點。除了受自身機械結構和旋翼空氣動力學影響外，還很容易受到外界干擾。其中姿態控制是四軸飛行器研究的重點，姿態傳感器數據的采集并對數據進行融合處理都是姿態控制的關鍵，要求飛行器具有快速響應能力，及時進行自適應調整，以確保飛行器姿態穩定。

本文主要研究對象是四軸飛行器的姿態，根據姿態控制子系統的數學模型[3]，有兩個基本坐標系：“地理”坐標系和“載體”坐標系。地理坐標系即當地水平坐標系，也稱NED坐標系，而“載體”坐標系指的是四軸自己的坐標系。姿態的數據來源有五個：重力、地磁、陀螺儀、加速度計和電子羅盤。其中前兩個來自“地理”坐標系，后三個來自“載體”坐標系。在“地理”坐標系中，重力的值始終是（0,0,1g），地磁的值始終是（0,1,x），這值就是由放置在四軸上的傳感器測量出來的。姿態控制系統需要檢測的狀態有：飛行器在機體坐標系下3個軸向的角速度、角度和相對地面的高度。機體坐標系如圖2所示，該坐標系固定在機體上，原點在飛行器重心，軸OX與前后螺旋槳連線平衡，前方指向X軸正方向；軸OY與左右螺旋槳連線平衡，右方指向Y軸正方向；軸OZ與軸OY、OX所在平面垂直，并與軸OY、軸OX組成右手坐標系。

圖2 機體坐標系

飛行器運動過程可以分解成機體繞三個軸運動來描述：當飛行器升降運動時，飛行器即做俯仰運動，飛行器在X-Z平面進行繞Y軸運動，產生的角度即俯仰角(pitch)；當飛行器發生轉向運動時，飛行器將產生偏航，飛行器在X-Y平面進行繞Z軸運動，產生的角度即航向角(yaw)；當讓飛行器側身移動時，飛行器將要做橫滾運動，飛行器在Y-Z平面進行繞X軸轉動，產生的角度即航滾角(roll)。

整個姿態控制系統擔負著傳感器信息采集、數據融合及姿態解算等各種任務，其主要工作過程是主控制器能快速獲得各傳感器的數據，實時檢測無人機的狀態，包括姿態、位置、速度等信息，并對數據進行處理；在計算出自身姿態之后，飛行器需要控制電機來執行相應的姿態調整，采用PID控制器來獲得電機控制量，以PWM的方式輸出驅動電機，以實現對其姿態的控制。

3 硬件電路設計

3.1 MPU9150性能分析

隨著微機械加工技術和微電子技術的不斷發展與相互融合，各種基于MEMS(微機電系統)技術的元器件應運而生，它們具有功耗低、可靠性、工作效率高及成本低等特點，易于實現智能化、數字化及批量化。MPU9150芯片就是一種MEMS傳感器芯片，其具有MEMS的抗沖擊能力強、系統集成度高、體積小、成本低、功耗低，性能優良的特點。MPU9150是全球第一款九軸慣性傳感芯片，包含了三軸加速度計、三軸陀螺儀和三軸磁力計，其中加速度和陀螺儀的精度為16bit，磁力計的精度為13bit，保證了測量的精確度，其加速度計的量程有±2g，±4g，±8g，±16g可選，陀螺儀的量程為±250，±500，±1000，和±2000O/sec可選，磁力計的滿量程為±1200uT，并且內置DMP用于姿態融合[4]。使MPU9150在無人機飛行控制系統中使用非常方便。MPU9150引腳功能描述如表1所示，典型接線圖如圖3所示。

表1 MPU9150引腳描述

圖3 MPU9150典型接線圖

3.2 硬件電路結構

為了實現飛行器姿態控制，需要得到飛行器的俯仰角和滾轉角作為姿態反饋，形成閉環控制，為提高其飛行穩定性，需加入角速率反饋以增加阻尼，飛行器的飛行姿態通過慣性測量單元來獲取，系統采用整合了3軸陀螺儀、3軸加速計、3軸磁力計的9軸運動處理組件MPU-9150，消除了組合陀螺儀與加速計時存在的軸差問題；并引入磁力傳感器互補數據，可采用四元數來描述姿態，可以避免歐拉角的奇異問題；另外陀螺儀具備增強偏置和溫度穩定的功能，減少了用戶校正操作，且具備改進的低頻噪聲性能；加速計則具備可編程中斷和自由降落中斷的功能。MPU-9150姿態檢測電路圖如圖4所示。

圖4 MPU-9150姿態檢測電路

4 姿態解算與數據融合分析

4.1 姿態解算

姿態解算部分是飛行器控制系統的核心，主要負責讀取姿態傳感器數據，從中解算出飛行器姿態，并將其作為姿態反饋，形成閉環控制，將期待的姿態與當前姿態的數據差值提供給PID控制器，從而計算得到電機的控制量。

姿態解算主要用到陀螺儀、加速度計和磁力計三個傳感器的數據。由于陀螺儀輸出的角速度瞬時數據積分后存在較大累積誤差，且積分角度偏差隨著時間增大而增大；而加速度計對繞重力加速度軸的轉動無法感知，因此長時間的自旋運動將無法估計，這就需要引入磁力計來彌補這一不足。因此，用加速度計對陀螺儀進行不斷的校正，引入磁力計互補數據，融合三者的數據才能更準確的反應出當前飛行器的姿態[5]。

目前常用的姿態解算方法主要有梯度下降法，互補濾波法，卡爾曼融合DMP輸出和磁場數據。考慮到MPU9150傳感器有一個內置的DMP，能夠快速融合陀螺儀和加速度計的輸出，而且輸出的姿態數據很穩定，動態性能很好，雖然DMP中算法沒有融合磁場數據，導致航向角在使用一段時間后會不可逆轉的偏離正確位置，但可以采用卡爾曼濾波的方法，對兩個航向角進行濾波，同時將磁場數據融合進去，以糾正航向角的偏差，從而既保證了其動態性能，又保證長時間運行航向角不會出現偏差。因此這里采用卡爾曼融合DMP輸出和磁場數據的飛行姿態解算方法。算法流程圖如圖5所示。

圖5 卡爾曼融合DMP和磁場數據流程圖

4.2 數據融合分析

4.2.1 靜態效果分析

實驗方法為先將MPU9150模塊靜止放置20s，然后再對其連續采樣12s，采樣頻率為50Hz。數據處理方法是，對每個軸輸出的歐拉角數據求平均值。

然后再求方差：

通過靜止時方差的大小來分析其靜態性能。卡爾曼融合DMP和磁場數據算法的核心在于確定卡爾曼濾波中的系統自協方差Q和測量值的自協方差R兩個系數。不同Q值和R值的靜態航向角方差結果如表2所示。

表2 卡爾曼融合DMP輸出和磁場數據在靜態時航向角方差。

圖6 卡爾曼融合DMP輸出和磁場數據在靜態時航向角方差

從上圖中可以明顯看出航向角的靜態方差隨著R的增大而變小，隨著Q的變小而變小，且Q越大方差隨R變化效果越明顯。最后列出Q=0.00001，R=0.9靜態效果如圖7所示。

圖7 卡爾曼融合DMP輸出和磁場數據靜態效果

從靜態效果圖可以看出，卡爾曼融合DMP和磁場數據的方法中航向角的誤差最小可達到0.00113。表明在保證糾正效果--即姿態角不漂移的前提下卡爾曼融合DMP輸出和磁場數據的算法靜態效果理想。

4.2.2 動態效果分析

在確保靜態性能良好的條件下，采用一個將航向角快速旋轉180°的方法來驗證卡爾曼融合DMP輸出和磁場數據算法的動態性能。結果如圖8所示。

由圖8可以看出卡爾曼融合DMP和磁場數據的算法能很好的反應真實的旋轉過程。

綜合靜態和動態性能分析，卡爾曼融合DMP和磁場數據的算法不僅在靜態性能上表現很好，而且在動態性能方面也表現良好。可以得到穩定的姿態數據，實現良好的姿態控制，從而使飛行器達到穩定飛行的效果。

5 結束語

圖8 卡爾曼融合DMP和磁場數據(Q=0.00001，R=0.1)

四軸飛行器是一種特殊結構的飛行器，其在民用及軍事方面都有著廣闊的應用前景[6]。本文根據四軸飛行器姿態控制系統的數學模型，對姿態控制系統的功能要求進行了分析，以STM32為主控制器，采用MUP-9150芯片進行姿態控制，并采用卡爾曼融合DMP和磁場數據的算法進行姿態解算，實現了飛行器良好的姿態控制。整個姿態控制系統集成度高，精度高，在測量精度、動態性能以及硬件電路方面均能滿足要求，為飛行器姿態控制系統的設計提供了一種高效穩定的實現方法[6]。實驗結果表明，系統滿足四軸飛行器飛行姿態控制的要求。

[1] 張鵬.一種新型四軸搜救飛行器設計[J].科技廣場.2010(9):145-146.

[2] 郭曉鴻.一種基于STM32的四旋翼飛行器[J].應用科技.2011.38(7).36-37.

[3] Pau Segui-Gasco. A Novel Actuation Concept for a Multi Rotor UAV[A].2013 International Conference on Unmanned Aircraft Systems (ICUAS)[C].2013, Atlanta, GA.373-376.

[4] InvenSense Inc.MPU9150 Product Speci fi cation Revision 4.0[S].InvenSense Inc,2012.

[5] 秦永元.慣性導航[M].科學出版社,2006:287-327.

[6] 黃成功.基于MPX4115的小型無人機氣壓高度測量系統設計[J].宇航計測技術.2009(8).31-32.