基于MPU9150的四軸飛行器姿態(tài)控制系統(tǒng)設(shè)計

朱 陽，王偉成，王民慧

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

0 引言

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

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

1 四軸飛行器結(jié)構(gòu)與原理

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

圖1 四軸飛行器的基本結(jié)構(gòu)圖

2 姿態(tài)控制系統(tǒng)功能

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

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

圖2 機體坐標系

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

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

3 硬件電路設(shè)計

3.1 MPU9150性能分析

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

表1 MPU9150引腳描述

圖3 MPU9150典型接線圖

3.2 硬件電路結(jié)構(gòu)

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

圖4 MPU-9150姿態(tài)檢測電路

4 姿態(tài)解算與數(shù)據(jù)融合分析

4.1 姿態(tài)解算

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

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

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

圖5 卡爾曼融合DMP和磁場數(shù)據(jù)流程圖

4.2 數(shù)據(jù)融合分析

4.2.1 靜態(tài)效果分析

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

然后再求方差：

通過靜止時方差的大小來分析其靜態(tài)性能?？柭诤螪MP和磁場數(shù)據(jù)算法的核心在于確定卡爾曼濾波中的系統(tǒng)自協(xié)方差Q和測量值的自協(xié)方差R兩個系數(shù)。不同Q值和R值的靜態(tài)航向角方差結(jié)果如表2所示。

表2 卡爾曼融合DMP輸出和磁場數(shù)據(jù)在靜態(tài)時航向角方差。

圖6 卡爾曼融合DMP輸出和磁場數(shù)據(jù)在靜態(tài)時航向角方差

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

圖7 卡爾曼融合DMP輸出和磁場數(shù)據(jù)靜態(tài)效果

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

4.2.2 動態(tài)效果分析

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

由圖8可以看出卡爾曼融合DMP和磁場數(shù)據(jù)的算法能很好的反應(yīng)真實的旋轉(zhuǎn)過程。

綜合靜態(tài)和動態(tài)性能分析，卡爾曼融合DMP和磁場數(shù)據(jù)的算法不僅在靜態(tài)性能上表現(xiàn)很好，而且在動態(tài)性能方面也表現(xiàn)良好?？梢缘玫椒€(wěn)定的姿態(tài)數(shù)據(jù)，實現(xiàn)良好的姿態(tài)控制，從而使飛行器達到穩(wěn)定飛行的效果。

5 結(jié)束語

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

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

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

[2] 郭曉鴻.一種基于STM32的四旋翼飛行器[J].應(yīng)用科技.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的小型無人機氣壓高度測量系統(tǒng)設(shè)計[J].宇航計測技術(shù).2009(8).31-32.