朱 陽(yáng)，王偉成，王民慧

（貴州大學(xué) 電氣工程學(xué)院，貴陽(yáng) 550025）

0 引言

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

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

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

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

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

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

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

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

圖2 機(jī)體坐標(biāo)系

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

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

3 硬件電路設(shè)計(jì)

3.1 MPU9150性能分析

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

表1 MPU9150引腳描述

圖3 MPU9150典型接線圖

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

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

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

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

4.1 姿態(tài)解算

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

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

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

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

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

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

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

然后再求方差：

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

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

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

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

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

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

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

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

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

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

5 結(jié)束語(yǔ)

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

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

[1] 張鵬.一種新型四軸搜救飛行器設(shè)計(jì)[J].科技廣場(chǎng).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] 秦永元.慣性導(dǎo)航[M].科學(xué)出版社,2006:287-327.

[6] 黃成功.基于MPX4115的小型無(wú)人機(jī)氣壓高度測(cè)量系統(tǒng)設(shè)計(jì)[J].宇航計(jì)測(cè)技術(shù).2009(8).31-32.