馬 敏，許中沖，常辰飛

(中國民航大學(xué) 航空自動化學(xué)院，天津 300300)

?

低成本無人機(jī)姿態(tài)解算研究

馬 敏，許中沖，常辰飛

(中國民航大學(xué) 航空自動化學(xué)院，天津 300300)

針對小型四旋翼無人機(jī)姿態(tài)解算數(shù)據(jù)精度低、缺少余度控制、易發(fā)散等問題，提出一種基于GPS、三軸陀螺儀加速度計(jì)、三軸磁力計(jì)的隨機(jī)加權(quán)自適應(yīng)濾波算法估計(jì)無人機(jī)姿態(tài)；建立四旋翼無人機(jī)姿態(tài)旋轉(zhuǎn)矩陣，搭建加速度計(jì)和磁力計(jì)獲取無人機(jī)姿態(tài)信息的模型，以及采用四元數(shù)解算法的陀螺儀定姿解算模型;采用隨機(jī)加權(quán)自適應(yīng)估計(jì)法，依據(jù)多元函數(shù)求極值定理，在保證總體均方差最小的情況下導(dǎo)出最優(yōu)隨機(jī)加權(quán)因子，進(jìn)而解算出姿態(tài)角信息，提高四旋翼無人機(jī)姿態(tài)解算濾波精度與穩(wěn)定性;仿真與試驗(yàn)結(jié)果表明：隨機(jī)加權(quán)自適應(yīng)濾波與平均值濾波算法相比解算精度更高，輸出結(jié)果更平穩(wěn)，且無人機(jī)各項(xiàng)預(yù)期功能均能正常實(shí)現(xiàn)，能夠滿足四旋翼無人機(jī)自主飛行的需要。

姿態(tài)解算;多傳感器數(shù)據(jù)融合;隨機(jī)加權(quán);無人機(jī)

0 引言

四旋翼無人機(jī)(unmanned aerial vehicle,UAV)可以實(shí)現(xiàn)倒飛、急轉(zhuǎn)彎等機(jī)動飛行特點(diǎn)具有很廣闊的應(yīng)用前景，是當(dāng)前各大科研單位的研究熱點(diǎn)[1-2]。姿態(tài)信息的精度與可靠性是實(shí)現(xiàn)無人機(jī)自主飛行的前提條件，隨著科學(xué)技術(shù)的快速發(fā)展，飛行器定姿理論和技術(shù)取得了顯著進(jìn)步[3]。

目前，常用的定姿傳感器有陀螺儀、磁力計(jì)、GPS等。但陀螺儀易產(chǎn)生溫漂、磁力計(jì)易受外部磁場干擾等，因此單個(gè)傳感器很難得到相對準(zhǔn)確的姿態(tài)角信息[4]。為了解決這類問題，進(jìn)行多傳感器信息融合，能夠提高信號估計(jì)的精度。

多傳感器信息融合通過整合多元信息的優(yōu)點(diǎn)提高信息精度。文獻(xiàn)[5]采用卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合解算出姿態(tài)角信息，有效的消除了測量白噪聲，抑制了陀螺儀的漂移，但主要適用于噪聲為高斯分布的線性系統(tǒng)，且量測方程為線性的，而實(shí)際導(dǎo)航系統(tǒng)則是非高斯噪聲、非線性系統(tǒng)。文獻(xiàn)[6]采用直接加權(quán)的方法將傳感器信息進(jìn)行加權(quán)平均得到融合值，適用于動態(tài)系統(tǒng)但精度較低。

針對三軸加速度計(jì)陀螺儀、三軸磁力計(jì)及GPS建立的姿態(tài)測量系統(tǒng)，建立其特征模型，利用隨機(jī)加權(quán)自適應(yīng)濾波的方法對采集的數(shù)據(jù)進(jìn)行融合。該方法既能提高被處理漂移信號的精度又能充分利用各個(gè)權(quán)因子所占的比重，提高動態(tài)系統(tǒng)的定位解算精度，實(shí)驗(yàn)證明了所提算法的有效性和正確性。

1 系統(tǒng)構(gòu)成

該四旋翼無人機(jī)采用三軸陀螺儀加速度計(jì)、三軸磁力計(jì)、GPS構(gòu)成多傳感器姿態(tài)測量系統(tǒng)，實(shí)現(xiàn)無人機(jī)的姿態(tài)測量功能，構(gòu)成圖如1所示。

圖1 融合系統(tǒng)總體框圖

通過對三軸加速度計(jì)、三軸磁力計(jì)的信息進(jìn)行相關(guān)反三角函數(shù)處理可以得到無人機(jī)靜止或勻速運(yùn)動狀態(tài)下的俯仰角、滾轉(zhuǎn)角和偏航角；三軸陀螺儀的輸出即為四旋翼的滾轉(zhuǎn)角速率，俯仰角速率和偏航角速率，通過相應(yīng)的解算方法可以得到無人機(jī)的姿態(tài)角信息。系統(tǒng)將前兩者的數(shù)據(jù)與后者的數(shù)據(jù)進(jìn)行隨機(jī)加權(quán)自適應(yīng)濾波，獲得最終的姿態(tài)角信息。

針對加速度計(jì)只能測量靜態(tài)或勻速運(yùn)動物體重力加速度的問題，采用GPS數(shù)據(jù)與加速度計(jì)信息進(jìn)行融合補(bǔ)償運(yùn)動加速度信息，測得機(jī)動狀態(tài)下無人機(jī)的機(jī)體加速度。

2 姿態(tài)源值獲取

2.1 坐標(biāo)轉(zhuǎn)換和姿態(tài)角原理

(1)

(2)

機(jī)體坐標(biāo)系下的物體相對于導(dǎo)航坐標(biāo)系運(yùn)動的姿態(tài)角可解算得：

(3)

其中俯仰角的真值域?yàn)?-90°,90°)，偏航角ψ和滾轉(zhuǎn)角θ的真值可通過真值表判斷[7]。

2.2 加速度計(jì)/磁力計(jì)姿態(tài)解算

(4)

經(jīng)過磁標(biāo)定后的三軸磁力可獲得無人機(jī)的偏航角。當(dāng)機(jī)體處于任意姿態(tài)時(shí)，保持偏航角不變，將磁強(qiáng)計(jì)的輸出投影到水平面上(令姿態(tài)旋轉(zhuǎn)矩陣中ψ=0即可)，則有機(jī)體坐標(biāo)系磁力計(jì)輸出mb到水平面的投影ms為:

(5)

不考慮磁偏角的影響下偏航角ψ為:

(6)

2.3 陀螺儀定姿

除采用加速度計(jì)/磁力計(jì)的方法定姿外，可以用陀螺儀完成無人機(jī)的姿態(tài)解算。針對坐標(biāo)系旋轉(zhuǎn)矩陣?yán)猛勇輧x進(jìn)行姿態(tài)解算的方法有多種。考慮到參數(shù)個(gè)數(shù)以及計(jì)算量方面的優(yōu)勢設(shè)計(jì)中采用四元數(shù)法完成低空低動態(tài)實(shí)時(shí)姿態(tài)解算[8]。

(7)

常用的四元數(shù)微分方程求解方法有旋轉(zhuǎn)矢量法、和龍格庫塔法以及泰勒展開式法[9-10]，實(shí)際應(yīng)用中根據(jù)控制器精度與更新時(shí)間的考慮可以采用單子樣、多子樣旋轉(zhuǎn)矢量法，一階、二階、四階龍格庫塔法。當(dāng)陀螺儀的輸出為角度增量時(shí)多采用旋轉(zhuǎn)矢量法，而MEMS三軸陀螺儀輸出為角速率形式，鑒于設(shè)計(jì)中使用的STM32微控制器采用二階龍格庫塔法求解四元數(shù)。

(8)

式中，T為采樣周期。

(9)

3 隨機(jī)加權(quán)自適應(yīng)濾波法姿態(tài)融合定姿

3.1 融合算法理論導(dǎo)出

對于圖1中的無人機(jī)定姿系統(tǒng)而言，姿態(tài)角的獲得方法有前面分析的兩種：一種是陀螺儀導(dǎo)出姿態(tài)信息, 二是由加速度計(jì)、磁力計(jì)解算姿態(tài)角。陀螺儀不受無人機(jī)運(yùn)動狀態(tài)、飛行環(huán)境的磁場影響動態(tài)性能良好，但陀螺儀存在漂移誤差，且隨著工作時(shí)間的增加將出現(xiàn)嚴(yán)重失真。而磁力計(jì)與加速度計(jì)組合定姿則在動態(tài)與磁場干擾下出現(xiàn)姿態(tài)角解算的嚴(yán)重偏差，故采用姿態(tài)融合定姿是一個(gè)十分必要的解決方案。數(shù)據(jù)融合能提高系統(tǒng)的容錯(cuò)率、抗干擾能力與動態(tài)性能，常用的信息融合方法有卡爾曼濾波[11-12]、梯度下降法[13-14]，設(shè)計(jì)中采用自適應(yīng)濾波中的隨機(jī)加權(quán)法進(jìn)行無人機(jī)姿態(tài)融合解算。

(10)

對于均方差σ2，因?yàn)閄i(i=1,2,...,n)相互獨(dú)立，且為X的無偏估計(jì)，所以總方差可以表示為：

(11)

想求的總體均方差σ2最小時(shí)的權(quán)系數(shù)vi，由多元函數(shù)求極值定理(拉格朗日定理)，可知式(10)有解時(shí)，最優(yōu)隨機(jī)加權(quán)因子為：

(12)

3.2 導(dǎo)航融合算法流程

各傳感器的數(shù)據(jù)經(jīng)過初始標(biāo)定矯正、濾波、補(bǔ)償?shù)忍幚砗?，由三軸陀螺儀采用四元數(shù)法導(dǎo)出的姿態(tài)角Xi=(ψiγiθi)T與加速度計(jì)和磁力計(jì)解算姿態(tài)角Xj=(ψiγiθi)T。利用以上隨機(jī)加權(quán)自適應(yīng)濾波估計(jì)的運(yùn)算步驟為：

1)采用式(14)、(15)推算出K采樣時(shí)刻的Yii和Yij；

4 實(shí)驗(yàn)結(jié)果分析

圖2給出了Matlab中模擬兩組姿態(tài)信息，采用隨機(jī)加權(quán)在線自適應(yīng)濾波得到的曲線圖，仿真數(shù)據(jù)選定兩組不相干的零均值白噪聲進(jìn)行模擬，其方差分別取為0.005、0.03，待估量為X=0.55，取400個(gè)點(diǎn)的采樣數(shù)據(jù)進(jìn)行模擬。表1給出了兩種方法的輸出平均值和方差值。

圖2 估計(jì)值輸出曲線及其局部放大曲線

隨機(jī)加權(quán)融合法平均加權(quán)法平均值0.54990.5484方差0.00110.0025

通過對比圖2以及表1可知，兩組數(shù)據(jù)經(jīng)過隨機(jī)加權(quán)自適應(yīng)濾波融合后的結(jié)果比采用平均值法輸出結(jié)果變化幅度小，估計(jì)值的方差小，即前者能提供更加有效的數(shù)據(jù)融合方法。

實(shí)驗(yàn)中將無人機(jī)固定在高精度慣導(dǎo)測試平臺上，對其進(jìn)行驗(yàn)證飛行實(shí)驗(yàn)。微控制器采用STMF32F405，采集三軸陀螺儀加速度計(jì)、三軸磁力計(jì)(采用I2C總線接口讀取傳感器數(shù)據(jù))及GPS(采用串口通信采集數(shù)據(jù))的信息進(jìn)行姿態(tài)解算，姿態(tài)解算周期為30ms，得到的姿態(tài)角拖過串口與上位機(jī)進(jìn)行通訊，通過上位機(jī)軟件可以獲得姿態(tài)角的實(shí)時(shí)解算值。測試時(shí)先進(jìn)行轉(zhuǎn)臺歸零使姿態(tài)角回到相對基準(zhǔn)，然后進(jìn)行靜態(tài)測試(讓轉(zhuǎn)臺沿一個(gè)方向做勻速運(yùn)動到一定的角度)和動態(tài)測試。動態(tài)測試時(shí)設(shè)定測試平臺內(nèi)框(俯仰角)、中框(滾轉(zhuǎn)角)分別以的幅度和0.5Hz的頻率做正弦擺動，設(shè)定測試平臺的外框(偏航角)以的幅度和0.2Hz的頻率做正弦擺動。表2為隨機(jī)采樣下姿態(tài)數(shù)據(jù)的靜、動態(tài)誤差性能指標(biāo)，圖3為動態(tài)測試下采集到的俯仰角、滾轉(zhuǎn)角、偏航角繪制的曲線圖。

表2 轉(zhuǎn)臺測試姿態(tài)角性能指標(biāo)

圖3 姿態(tài)角動態(tài)輸出曲線

體的實(shí)時(shí)位置，滿足無人機(jī)的飛行要求。與實(shí)際飛行時(shí)飛機(jī)面臨的環(huán)境有所偏差，實(shí)際的誤差可能會有所增加。

5 結(jié)論

文中介紹了小型四旋翼無人機(jī)利用低成本傳感器進(jìn)行解算航姿解算的兩種方法，由于單一的姿態(tài)解算方法的航姿可信度不高的問題，采用隨機(jī)加權(quán)融合這種自適應(yīng)濾波方法，對兩種不同的解算輸出結(jié)果進(jìn)行融合濾波。實(shí)驗(yàn)結(jié)果統(tǒng)計(jì)說明：采用隨機(jī)加權(quán)濾波融合后的姿態(tài)信息提高了四旋翼無人機(jī)的測量精度，計(jì)算量小，能夠適應(yīng)低成本的航姿控制系統(tǒng)。

[1] 聶博文，馬宏緒，王 劍，等.微小型四旋翼飛行器的研究現(xiàn)狀與關(guān)鍵技術(shù)[J].電光與控制，2007，14(6):113-117.

[2] 趙 勃，鮮 斌，張 鑫，等.四旋翼飛行器硬件在環(huán)仿真平臺研究[A].第三十一屆中國控制會議論文集[C].合肥，2012，c卷:5008-5013.

[3] 吳 勃，徐 歡，喬相偉.狀態(tài)切換UKF的飛行器姿態(tài)確定算法[J].電機(jī)與控制學(xué)報(bào)，2012，16(6):98-104.

[4] 馮智勇，曾 瀚，張 力，等.基于陀螺儀及加速度計(jì)信號融合的姿態(tài)角度測量[J].西南師范大學(xué)學(xué)報(bào):自然科學(xué)版，2011，36(4): 137-141.

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

[6] 李媛媛,張立峰,多傳感器自適應(yīng)加權(quán)融合算法及其應(yīng)用研究[J].自動化與儀器儀表，2008(2):10-13.

[7]吳 杰,閆建國.基于修正的卡爾曼濾波的姿態(tài)估計(jì)算法研究[J].計(jì)算機(jī)真,2012, 29(2):54-57.

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

[9]葛泉波,李文斌,孫若愚,等.基于EKF的集中式融合估計(jì)研究[J].自動化學(xué)報(bào), 2012, 39(6):816-825.

[10]郭曉鴻,楊 忠,陳 喆,等.EKF和互補(bǔ)濾波器在飛行姿態(tài)確定中的應(yīng)用[J].傳感器與微系統(tǒng),2011,30(11):149-152.

[11] 辛 琪,史忠科.基于多源信息的飛行姿態(tài)估計(jì)方法[J].飛行力學(xué),2012,30(6): 527-531.

[12] 梁延德,程 敏,何福本,等.基于多源信息的飛行姿態(tài)估計(jì)方法[J].飛行力學(xué), 2011,30(11):56-58.

[13] Madgwick S O H,Harrison A J L,Vaidyanathan R H.Estimation of IMU and MARG orientation using a gradient descent algorithm[A].Proceedings of the 2011 IEEE International Conference on Rehabilitation Robotics (ICORR)[C].Switzerland:IEEE,2011：1-7.

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

[15] 振 江,康健一,張 青,等.數(shù)據(jù)融合技術(shù)在溫室溫度檢測中的應(yīng)用[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),2006, 37(10):101-103.

[16] 李 偉,何鵬舉,高社生.多傳感器加權(quán)信息融合算法研究[J].西北工業(yè)大學(xué)學(xué)報(bào),2010, 28(5):674-678.

Low Cost UAV Attitude Algorithm Research

Ma Min, Xu Zhongchong, Chang Chenfei

(Aviation Institute of Automation, Civil Aviation University of China, Tianjin 300300, China)

In order to improve the drawbacks of low accuracy, lack of redundancy and divergence that occurred in the attitude calculation of micro quadrotor unmanned aerial vehicle (UAV), a random weighted adaptive filter algorithm is proposed based on GPS, 3-axis gyroscope-accelerometer and 3-axis magnetometer, to estimate the UAV attitude. A Four rotor UAV attitude rotation matrix is established，A model for obtaining the attitude information of UAV with accelerometer and magnetometer，and Attitude calculation model of gyroscope sensor using four element method.By randomly weighted adaptive estimation method, based on the multiple function extremum theorem, in ensuring the overall variance under the condition of minimum to derive the optimal random weighting factor, and then calculate the attitude information, improve the quadrotor UAV attitude estimation filtering accuracy and stability.The simulation and experimental results show that the random weighting adaptive filtering and the average filtering algorithm are more accurate and more stable, and the expected function of the UAV can be realized, which can meet the needs of the autonomous flight of the quadrotor UAV.

attitude algorithm; multi-sensor data fusion; random weighted;UAV

2015-11-30；

2016-01-19。

天津市應(yīng)用基礎(chǔ)與前沿技術(shù)研究計(jì)劃(13JCYBJC39000)。

馬 敏(1971-)，女，安徽霍邱人，教授，主要從事航空發(fā)動機(jī)狀態(tài)監(jiān)測及故障診斷、電學(xué)成像技術(shù)、航空無損檢測技術(shù)及新型傳感器等方向的研究。

1671-4598(2016)06-0161-03

10.16526/j.cnki.11-4762/tp.2016.06.044

TP274

A