李文鵬，唐海洋

李文鵬，唐海洋

(西安工程大學(xué) 電子信息學(xué)院，西安 710048)

摘要:分析了四旋翼飛行器的姿態(tài)解算原理，提出了一種基于STM32的姿態(tài)測量系統(tǒng)。系統(tǒng)由STM32F407微控制器和捷聯(lián)慣性測量組件(IMU)組成。利用四元數(shù)描述姿態(tài)進行坐標(biāo)換算，采用多傳感器數(shù)據(jù)融合方案，通過互補濾波算法進行數(shù)據(jù)融合，獲取精確的姿態(tài)角，并完成姿態(tài)解算。實驗結(jié)果表明，采用互補濾波算法有效融合了捷聯(lián)慣性測量組件的傳感器數(shù)據(jù)，實現(xiàn)了四旋翼飛行器的高精度姿態(tài)解算。

關(guān)鍵詞:四旋翼飛行器；飛行控制系統(tǒng)；姿態(tài)解算；互補濾波器；STM32F407

引言

四旋翼飛行器具有機械結(jié)構(gòu)簡單、操縱靈活方便、維修成本低等優(yōu)點，近年來得益于微機電系統(tǒng)(MEMS)的飛速發(fā)展，四旋翼飛行器的姿態(tài)測量系統(tǒng)開始向著低成本、小體積、高性能的方向快速發(fā)展。四旋翼飛行器飛行過程中，是否可以實時準(zhǔn)確獲得飛行器的姿態(tài)信息決定了飛行器的控制精度和穩(wěn)定性[1]，所以姿態(tài)解算是飛行控制系統(tǒng)的核心。本文設(shè)計一種以STM32F407為主控制器的姿態(tài)測量系統(tǒng)，系統(tǒng)整合有3軸陀螺儀、3軸加速度計的MPU6050傳感器和AK8975(3軸電子羅盤)傳感器作為捷聯(lián)慣性測量組件(Inertial Measurement Unit，IMU)，利用四元數(shù)對運動坐標(biāo)進行描述，通過互補濾波解算出歐拉角。

1四旋翼飛行器的姿態(tài)描述

姿態(tài)解算屬于飛行控制系統(tǒng)的核心部分，通過姿態(tài)矩陣可以得到飛行器的姿態(tài)和導(dǎo)航參數(shù)計算需要的數(shù)據(jù)，飛行器的姿態(tài)和航向體現(xiàn)了載體坐標(biāo)系與地理坐標(biāo)系之間的方位關(guān)系[2]。目前描述動坐標(biāo)相對參考坐標(biāo)系方位關(guān)系的方法有歐拉角法、方向余弦矩陣法、四元數(shù)法等[3]。

1.1歐拉角法

歐拉角是用來唯一確定定點轉(zhuǎn)動剛體位置的3個一組獨立角參量[4]，定義ψ、θ、φ分別為繞Z軸、Y軸、X軸的旋轉(zhuǎn)角度,如圖1所示。用Tait-bryan angle分別表示為yaw(偏航角)、pitch(俯仰角)、roll(橫滾角),如圖2所示。

圖1　旋轉(zhuǎn)角度示意圖

圖2　Tait-bryan angle形式表示的歐拉角

1.2方向余弦矩陣法

載體坐標(biāo)軸x、y、z分別與參考坐標(biāo)軸X、Y、Z的方向余弦，可以組成方向余弦矩陣。方向余弦法和歐拉角法沒有本質(zhì)上的區(qū)別，都采用歐拉角表示。方向余弦法避免了歐拉角法的“奇異點”現(xiàn)象，沒有方程退化問題，可以全姿態(tài)工作，但需要求解9個微分方程[6]，由于計算量大、實時性差，無法滿足飛行控制要求。

1.3四元數(shù)法

四元數(shù)是一種超復(fù)數(shù)。如果把四元數(shù)的集合考慮成多維實數(shù)空間，四元數(shù)就代表著一個四維空間，相對于復(fù)數(shù)為二維空間[7]。復(fù)數(shù)是由實數(shù)加虛數(shù)單位i組成的，其中i2=-1，類似地，四元數(shù)是由實數(shù)加上三個元素i、j、k組成[8]，而且它們有如下關(guān)系：

i2=j2=k2=ijk=-1

通過旋轉(zhuǎn)軸及繞其旋轉(zhuǎn)的角速度可以構(gòu)造一個四元數(shù)：

其中α是繞旋轉(zhuǎn)軸旋轉(zhuǎn)的角度，cos (βx)、cos (βy)、cos (βz)是旋轉(zhuǎn)軸在x、y、z方向上的分量。四元數(shù)法只需求解4個一階微分方程組(比方向余弦矩陣法的計算量小得多)，即可滿足飛控系統(tǒng)對實時性的要求。

在姿態(tài)解算中，積分和修正采用運算量較少、無奇點誤差的四元數(shù)法，并用角度比較直觀的歐拉角輸出控制PID。用歐拉角直接表示四元數(shù)為：

2四旋翼飛行器的姿態(tài)解算

四旋翼飛行器處于飛行狀態(tài)時，電子羅盤易受外部磁場干擾[9]，加速度計受機身振動影響會產(chǎn)生測量白噪聲，陀螺儀因存在溫度漂移誤差[10]會產(chǎn)生零點誤差和累積誤差，因此這幾種傳感器直接測得的偏航角、俯仰角、橫滾角都存在一定的誤差[11]，必須采用一定的姿態(tài)解算算法通過融合各種傳感器的數(shù)據(jù)信息進行誤差補償，以實現(xiàn)精確的姿態(tài)解算[12]。捷聯(lián)慣導(dǎo)系統(tǒng)中常用的算法有卡爾曼濾波算法和互補濾波算法。

2.1卡爾曼濾波算法的姿態(tài)解算原理

卡爾曼濾波為最優(yōu)化自回歸數(shù)據(jù)處理算法[13]，是一種能夠從包含噪聲的有限信號中預(yù)測出目標(biāo)真實值的濾波算法，對于解決大部分?jǐn)?shù)據(jù)融合方面的問題，它是非常有用的[14]。但卡爾曼濾波算法計算量大，對飛控MCU的運算速度和精度要求都比較高。

2.2互補濾波算法的姿態(tài)解算原理

陀螺儀動態(tài)響應(yīng)特性比較好，瞬時測得的角速度精度高，因此常對陀螺儀測得的角速度進行積分來獲取三軸姿態(tài)角，但由于積分是離散的，長時間會產(chǎn)生累積誤差。采用加速度計和電子羅盤測量姿態(tài)時，雖然沒有累積誤差，但噪聲比較大，而且在飛行過程中，受機體振動的影響比陀螺儀明顯，短時間內(nèi)的可靠性不高。

該算法相較于傳統(tǒng)的卡爾曼濾波算法，具有運算量小、對采樣頻率要求低等優(yōu)點[15]，能夠滿足四旋翼飛行器對姿態(tài)測量的要求。

陀螺儀積分所得數(shù)據(jù)與加速度計、電子羅盤的測量數(shù)據(jù)進行對比，使用其偏差來修正陀螺儀的輸出。濾掉加速度計的高頻信號和陀螺儀的低頻信號，然后相加得到整個頻帶的信號。它們在頻域上可以實現(xiàn)特性互補，因此采用互補濾波算法融合兩者的數(shù)據(jù)，可以提高測量精度和系統(tǒng)的動態(tài)性能?；パa濾波算法應(yīng)用示意圖如圖3所示。

圖3　互補濾波算法應(yīng)用示意圖

由圖3可得一階低通濾波器GL(s)和一階高通濾波器GH(s)，傳遞函數(shù)為：

其中角度補償參數(shù)K的確定在互補濾波算法中起著至關(guān)重要的作用，它決定了該濾波器是否能夠較好地結(jié)合加速度計的低頻精度和陀螺儀的動態(tài)性能[16]。K值較大，截止頻率較高，數(shù)據(jù)融合結(jié)果主要依靠陀螺儀輸出得到，陀螺儀累計誤差小，但加速度計噪聲大；K值較小， 截止頻率較低，數(shù)據(jù)融合結(jié)果主要靠加速度計輸出得到，但陀螺儀累計誤差大，容易受陀螺儀漂移影響。所以其太大或者太小在四旋翼飛控系統(tǒng)姿態(tài)解算中都是不允許的。

則互補濾波算法估計的姿態(tài)矩陣為：

其中，C0(s)為加速度計和電子羅盤獲取到的姿態(tài)矩陣；Cω(s)為陀螺儀數(shù)據(jù)計算得到的姿態(tài)矩陣；C(s)為四旋翼飛行器運動過程中的真實姿態(tài)矩陣；μH為C0(s)的高頻觀測噪聲；μL為Cω(s)中的低頻誤差累積；K為角度補償參數(shù)。

所以，互補濾波算法能夠同時濾除低頻干擾和高頻干擾，能夠有效實現(xiàn)多路傳感器數(shù)據(jù)的融合。

2.3互補濾波算法在STM32中的實現(xiàn)

利用互補濾波算法實現(xiàn)姿態(tài)解算的程序段如下：

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az){

float norm;

float vx, vy, vz;float ex, ey, ez;

//把加速度計的三維向量轉(zhuǎn)換成單位向量

norm =invSqrt(ax * ax + ay * ay + az * az);

ax*= norm;

ay*= norm;

az*= norm;

//陀螺儀積分后的姿態(tài)推算出重力分量

vx = 2 * (q1 * q3 - q0 * q2);

vy = 2 * (q0 * q1 + q2 * q3);

vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

//得到誤差并消除

ex = (ay * vz - az * vy);

ey = (az * vx - ax * vz);

ez = (ax * vy - ay * vx);

exInt+= ex * Ki;

eyInt+= ey * Ki;

ezInt+= ez * Ki;

gx+= Kp * ex + exInt;

gy+= Kp * ey + eyInt;

gz+= Kp * ez + ezInt;

//四元數(shù)微分方程，將陀螺儀與四元數(shù)結(jié)合起來，為姿態(tài)解算的核心算子

q0+= (-q1 * gx - q2 * gy - q3 * gz) * halfT;

q1+= (q0 * gx + q2 * gz - q3 * gy) * halfT;

q2+= (q0 * gy - q1 * gz + q3 * gx) * halfT;

q3+= (q0 * gz + q1 * gy - q2 * gx )* halfT;

//四元數(shù)規(guī)一化

norm =invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

q0*= norm;

q1*= norm;

q2*= norm;

q3*= norm;

}

完成姿態(tài)解算后，就得到了表示姿態(tài)的四元數(shù)，一般進一步轉(zhuǎn)換為較為直觀的歐拉角。

3姿態(tài)解算的試驗驗證與分析

搭建的四旋翼飛行器的姿態(tài)測量平臺如圖4所示。其中機架采用DJI的F450機架，動力部分采用DJI的2312 960 kV無刷電機、420S電調(diào)和9450自緊螺旋槳，基于STM32的姿態(tài)測量系統(tǒng)固定于機架的中心位置。MPU6050的角速度測量范圍為±250 °/s、±500 °/s、±1000 °/s和±2000 °/s，可程序配置的加速度計測量范圍為±2g、±4g、±8g和±16g。

圖4　四旋翼飛行器的姿態(tài)測量平臺

操控四旋翼飛行器盡可能保持定點懸停狀態(tài)，通過串口讀取出傳感器采集到的原始數(shù)據(jù)以及姿態(tài)解算后的數(shù)據(jù)，在上位機軟件中使用模擬示波器觀測姿態(tài)角的變化，并進行數(shù)據(jù)分析。

飛行器做俯仰運動時，因存在溫度漂移，陀螺儀會產(chǎn)生零點誤差和累積誤差，即采用陀螺儀測量得到的角速度進行積分得到姿態(tài)角會產(chǎn)生積分誤差，并且隨著時間的推移會產(chǎn)生累積誤差。同時由于無刷電機引起的機架振動、機體重力分布不均等因素也會導(dǎo)致零點誤差增大。四旋翼飛行器在定點懸停狀態(tài)下經(jīng)姿態(tài)解算后的俯仰角和橫滾角圖略——編者注，可以看出隨著時間的推移，零點誤差逐漸趨于平穩(wěn)。

在實驗過程中使飛行器盡可能保持航向，機頭朝向地理正北方時，偏航角為0°，順時針偏轉(zhuǎn)航向，偏航角為正且角度絕對值增大，逆時針偏轉(zhuǎn)航向，偏航角為負且角度的絕對值增大，持續(xù)順時針偏航，偏航角將增大至180°并跳變?yōu)?180°，繼續(xù)偏航，偏航角將由-180°增大至0°，之后以此類推，逆時針偏航同理。飛行器做偏航運動時，電子羅盤經(jīng)姿態(tài)解算后測量得到偏航角圖略——編者注。本系統(tǒng)中采用陀螺儀積分值對電子羅盤進行校正，對于偏航角，以電子羅盤采集的數(shù)據(jù)為主。電子羅盤受外界因素干擾嚴(yán)重，零點誤差較大，經(jīng)過互補濾波后，雖然消除了部分干擾，電子羅盤數(shù)據(jù)波動減弱，誤差穩(wěn)定保持在±0.8°以內(nèi)，但由于電子羅盤自身具有不可忽略的零位、靈敏度及非正交誤差，因此通過軟件算法無法徹底消除零點誤差[17]。

結(jié)語

本文分析了四旋翼飛行器的姿態(tài)解算算法理論、互補濾波數(shù)據(jù)融合的方法，以及姿態(tài)解算在STM32上的實現(xiàn)。在以STM32F407微控制器為控制核心， MPU6050和AK8975姿態(tài)傳感器模塊作為捷聯(lián)慣性測量組件的四旋翼飛行實驗平臺上對軟件算法進行了驗證。結(jié)果表明，互補濾波算法較好地融合了姿態(tài)測量系統(tǒng)陀螺儀、加速度計和電子羅盤的數(shù)據(jù)，為四旋翼飛行器穩(wěn)定飛行提供了可靠的數(shù)據(jù)，滿足了四旋翼飛行器姿態(tài)解算的要求。

參考文獻

[1] 張浩,任芊.四旋翼飛行器航姿測量系統(tǒng)的數(shù)據(jù)融合方法[J].兵工自動化,2013(1):28-31.

[2] 張榮輝,賈宏光,陳濤,等.基于四元數(shù)法的捷聯(lián)式慣性導(dǎo)航系統(tǒng)的姿態(tài)解算[J].光學(xué)精密工程,2008(10):1963-1970.

[3] 李敏行.非線性控制理論在三軸轉(zhuǎn)臺姿態(tài)控制中的應(yīng)用[D].西安:西北工業(yè)大學(xué),2001.

[4] 邵濤,李明,姜良銀,等.基于LabVIEW的miniAHRS型傳感器信號采集研究[J].現(xiàn)代機械,2010(3):33-35.

[5] 賀毅岳,耿國華,周明全,等.基于VTK的三維模型切割研究[J].計算機應(yīng)用研究,2010(11):4322-4323,4327.

[6] 賀海鵬,閻妍,馬良,等.基于IEKF的四旋翼無人機姿態(tài)測量方法研究[J].計算機仿真,2015(4):56-60.

[7] 邢燕.四元數(shù)及其在圖形圖像處理中的應(yīng)用研究[D].合肥:合肥工業(yè)大學(xué),2009.

[8] 李曉琴,田壟,孫波.基于復(fù)數(shù)矩不變性的遙感邊緣信息提取[J].遙感信息,2011(4):7-10.

[9] 于春曉.人形機器人的電子羅盤航向估計[D].重慶:重慶大學(xué),2010.

[10] 徐麗娜,鄧正隆,張廣瑩,等.陀螺儀溫度試驗與建模研究[J].宇航學(xué)報,1999(2):100-104.

[11] 沈曉衛(wèi),姚敏立,趙鵬.基于互補濾波的動中通天線姿態(tài)估計[J].中國慣性技術(shù)學(xué)報,2011(2):194-197.

[12] 萬曉鳳,康利平,余運俊,等.基于多傳感器數(shù)據(jù)融合的四旋翼飛行器的姿態(tài)解算[J].科技導(dǎo)報,2014(19):31-35.

[13] 王一夫,陳松喬.非加性噪聲情形下卡爾曼濾波器的推廣[J].計算機工程與應(yīng)用,2008(4):24-26.

[14] 錢鋆,歐陽紅林,范祝霞,等.基于EKF的雙Y移30°六相PMSM無速度傳感器控制系統(tǒng)研究[J].科技導(dǎo)報,2009(17):65-68.

[15] 施聞明,徐彬,陳利敏.捷聯(lián)式航姿系統(tǒng)中四元素算法Kalman濾波器的實現(xiàn)研究[J].自動化技術(shù)與應(yīng)用,2005(11):9-11.

[16] 余彥霖,祖家奎,曾國貴.微小型無人直升機姿態(tài)信號的互補濾波融合算法[J].直升機技術(shù), 2014(3).

[17] 何川,李智,王勇軍.基于STM32的四旋翼飛行器的姿態(tài)最優(yōu)估計研究[J].電子技術(shù)應(yīng)用,2015(12):61-64.

李文鵬(碩士研究生)，主要研究方向為嵌入式系統(tǒng)應(yīng)用；唐海洋(碩士研究生)，主要研究方向為電力電子與電力傳動。

Li Wenpeng,Tang Haiyang

(College of Electronic and Information,Xi’an Polytechnic University,Xi’an 710048,China)

Abstract：The quad-rotor aircraft principle of attitude solution is analyzed,and the attitude determination system is proposed which consists of STM32F407 and inertial measurement unit(IMU).The quaternion is used to do coordinate conversion.The complementary filter data fusion is used to obtain the accurate attitude angle,and then completes the attitude calculation.The experiment results show that the filtering algorithm effectively combines the complementary sensor data of the inertial measurement unit to achieve high precision attitude calculation.

Key words:quad-rotor aircraft;flight control system;attitude solution;complementary filter;STM32F407

中圖分類號:TP271

文獻標(biāo)識碼:A

收稿日期：(責(zé)任編輯：薛士然2016-01-14)