基于STM32的四旋翼飛行器姿態(tài)解算的研究

李文鵬，唐海洋

李文鵬，唐海洋

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

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

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

引言

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

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

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

1.1歐拉角法

歐拉角是用來(lái)唯一確定定點(diǎn)轉(zhuǎn)動(dòng)剛體位置的3個(gè)一組獨(dú)立角參量[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ū)別，都采用歐拉角表示。方向余弦法避免了歐拉角法的“奇異點(diǎn)”現(xiàn)象，沒有方程退化問(wèn)題，可以全姿態(tài)工作，但需要求解9個(gè)微分方程[6]，由于計(jì)算量大、實(shí)時(shí)性差，無(wú)法滿足飛行控制要求。

1.3四元數(shù)法

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

i2=j2=k2=ijk=-1

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

利用互補(bǔ)濾波算法實(shí)現(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;

//把加速度計(jì)的三維向量轉(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é)合起來(lái)，為姿態(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ù)，一般進(jìn)一步轉(zhuǎn)換為較為直觀的歐拉角。

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

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

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

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

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

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

結(jié)語(yǔ)

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

參考文獻(xiàn)

[1] 張浩,任芊.四旋翼飛行器航姿測(cè)量系統(tǒng)的數(shù)據(jù)融合方法[J].兵工自動(dòng)化,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)姿態(tài)控制中的應(yīng)用[D].西安:西北工業(yè)大學(xué),2001.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

李文鵬(碩士研究生)，主要研究方向?yàn)榍度胧较到y(tǒng)應(yīng)用；唐海洋(碩士研究生)，主要研究方向?yàn)殡娏﹄娮优c電力傳動(dò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

中圖分類號(hào):TP271

文獻(xiàn)標(biāo)識(shí)碼:A

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