王守華, 鄧桂輝，紀(jì)元法，孫希延

(1.桂林電子科技大學(xué) 信息與通信學(xué)院，廣西 桂林 541004; 2.認(rèn)知無線電與信息處理教育部重點(diǎn)實(shí)驗(yàn)室，廣西 桂林 541004)

基于組合濾波的低成本飛行器姿態(tài)解算算法

王守華1,2, 鄧桂輝1*，紀(jì)元法1,2，孫希延1,2

(1.桂林電子科技大學(xué) 信息與通信學(xué)院，廣西 桂林 541004; 2.認(rèn)知無線電與信息處理教育部重點(diǎn)實(shí)驗(yàn)室，廣西 桂林 541004)

(*通信作者電子郵箱674149099@qq.com)

在低成本的飛行器姿態(tài)檢測系統(tǒng)中，互補(bǔ)濾波器由于原理簡單、計算量小，而被廣泛應(yīng)用。針對低成本飛行器在非勻速運(yùn)動時因?yàn)榧铀俣扔嫴荒軈^(qū)分重力加速度與運(yùn)動加速度引起基于互補(bǔ)濾波的姿態(tài)解算誤差較大的問題，提出了一種互補(bǔ)與自適應(yīng)限幅組合濾波的姿態(tài)解算算法，并給出了自適應(yīng)限幅濾波門閥的設(shè)計方法。通過融合陀螺儀輸出的角速度與加速度計輸出的加速度獲取限幅濾波的限幅閥值; 然后將歸一化的加速度計輸出增量通過限幅濾波的結(jié)果代替原互補(bǔ)濾波的加速度計輸入，提高非勻速運(yùn)動下姿態(tài)解算精度。經(jīng)實(shí)際系統(tǒng)實(shí)時性能測試表明，所提算法估計精度高、計算量小，易于在低成本飛行器控制系統(tǒng)中實(shí)現(xiàn)。

姿態(tài)解算；互補(bǔ)濾波器；限幅濾波；四元數(shù)法；自適應(yīng)

0 引言

近年來，低成本飛行器在物流業(yè)、環(huán)境監(jiān)測、地質(zhì)勘測及軍事偵察等領(lǐng)域的應(yīng)用越來越廣泛，作為控制低成本飛行器的關(guān)鍵技術(shù)的姿態(tài)解算算法成為當(dāng)前熱點(diǎn)研究之一[1]。陀螺儀、加速度計和磁場計等低成本慣性測量單元輸出的測量數(shù)據(jù)是飛行器載體姿態(tài)解算的原始數(shù)據(jù)，但是受到器件自身測量誤差、溫度和各種噪聲等諸多因素的影響，姿態(tài)解算往往誤差較大[2-3]。通過信息融合算法將多種傳感器的數(shù)據(jù)進(jìn)行融合往往能取長補(bǔ)短，解算出更精確的姿態(tài)角[4]。

在多傳感器姿態(tài)解算的信息融合方案上，國內(nèi)外研究者們提出了多種算法和改進(jìn)策略。賈瑞才[5]、吳濤等[6]提出利用擴(kuò)展卡爾曼濾波(Extended Kalman Filter, EKF)實(shí)現(xiàn)對陀螺儀隨機(jī)漂移誤差的補(bǔ)償，提高了姿態(tài)解算精度，但忽略了加速度計的高頻干擾問題，及EKF算法固有的線性化、截斷誤差和發(fā)散問題。為克服EKF固有的缺陷，Pan等[7]實(shí)現(xiàn)基于無跡卡爾曼濾波(Unscented Kalman Filter, UKF)的姿態(tài)估算算法，取得了比EKF更高的穩(wěn)定性、解算精度及收斂速度; 而Tang等[8]表示應(yīng)用在航天飛行器姿態(tài)解算上的容積卡爾曼濾波(Cubature Kalman Filter, CKF)算法估算性能要比UKF好, 但在低成本飛行器中，基于卡爾曼濾波的姿態(tài)算法存在計算量大、硬件要求高、難實(shí)現(xiàn)等缺點(diǎn)。Euston等[9]、Wu等[10]在低成本無人機(jī)上利用互補(bǔ)濾波器(Complementary Filter, CF)實(shí)現(xiàn)載體姿態(tài)估算，充分體現(xiàn)了CF原理簡單、計算量小、容易實(shí)現(xiàn)、收斂穩(wěn)定等優(yōu)點(diǎn)，但是未考慮因加速度計不能區(qū)分運(yùn)動加速度與重力加速度而產(chǎn)生的CF算法解算誤差。Wu等[11]、閻世梁等[12]在應(yīng)用CF解算姿態(tài)時通過計算三軸加速度計的輸出的合加速度的大小來判斷載體的運(yùn)動狀態(tài)，然后根據(jù)載體運(yùn)動狀態(tài)調(diào)節(jié)CF的轉(zhuǎn)換頻率，提高了基于CF的姿態(tài)解算精度，但是存在PI參數(shù)離散和高動態(tài)下互補(bǔ)濾波作用大大降低等問題。孫金秋等[13]設(shè)計了PI參數(shù)能自適應(yīng)的CF姿態(tài)解算算法，通過合加速度大小來連續(xù)調(diào)節(jié)PI參數(shù)，實(shí)現(xiàn)連續(xù)使姿態(tài)估算誤差小于1.47°，但存在對姿態(tài)三角無區(qū)分地使用同一個PI參數(shù)調(diào)節(jié)的問題，依然有高動態(tài)下互補(bǔ)濾波作用大大降低的問題。

本文提出自適應(yīng)限幅濾波與互補(bǔ)濾波組合濾波的策略，其中自適應(yīng)限幅濾波能較準(zhǔn)確區(qū)分三軸重力加速度分量和三軸運(yùn)動加速度分量，同時還保留了互補(bǔ)濾波器的計算量小、收斂穩(wěn)定及易于在低成本飛行器中實(shí)現(xiàn)的優(yōu)點(diǎn)，使基于重力加速度輸入的互補(bǔ)濾波算法也能在載體帶有運(yùn)動加速度時解算出更精確的姿態(tài)角。

1 四元數(shù)法姿態(tài)解算

載體坐標(biāo)系與導(dǎo)航坐標(biāo)系的轉(zhuǎn)動關(guān)系可以用四元數(shù)標(biāo)記為:

Q=q0+q1i+q2j+q3k

(1)

四元數(shù)確定出b系到n系的坐標(biāo)變換矩陣為:

(2)

n系與b系基本旋轉(zhuǎn)對應(yīng)的坐標(biāo)變換矩陣記為:

(3)

對比式(2)、(3)可得:

(4)

可見載體的姿態(tài)更新實(shí)質(zhì)上是如何計算四元數(shù)，記b系中xyz三軸旋轉(zhuǎn)的角速率向量[ωxωyωz]T為ω，四元數(shù)向量[q0q1q2q3]T為q，則更新四元數(shù)的微分方程為:

(5)

其中，式(5)中的ω由式(6)求取:

(6)

2 基于互補(bǔ)濾波器的四元數(shù)法姿態(tài)解算

短時間內(nèi)，陀螺儀提供高精度的姿態(tài)結(jié)算數(shù)據(jù)，而加速度計和磁場計精度較差；長時間內(nèi)，陀螺儀因漂移的影響，產(chǎn)生積分誤差，姿態(tài)精度隨時間增長而下降，但是加速度計和磁場計的測量誤差不會隨時間增加。因此，根據(jù)兩者在頻域上的特性的互補(bǔ)，可以采用互補(bǔ)濾波器融合這三種傳感器的數(shù)據(jù)，提高姿態(tài)解算的精度和系統(tǒng)的動態(tài)特性[13]。

記姿態(tài)角為η=[φθψ]，由三軸加速度計輸出的加速度向量a和磁場計輸出的地磁場強(qiáng)度向量m可以解算出一個帶有高頻噪聲的姿態(tài)角η1=η+ΔηH，同時陀螺儀輸出ω通過積分可以輸出一個含低頻噪聲的姿態(tài)角η2=η+ΔηL。將η1通過低通濾波器HL(s)=C(s)/(s+C(s))濾除加速度計和磁場計的測量誤差，并將η2通過高通濾波器HH(s)=s/(s+C(s))濾除陀螺儀因漂移的影響產(chǎn)生的積分誤差。那么，最后輸出接近真實(shí)值的姿態(tài)角:

(7)

取C(s)=kP+kI/s，kP為低通濾波器與高通濾波器的轉(zhuǎn)接頻率，引入積分環(huán)節(jié)kI/s是為了補(bǔ)償陀螺儀的漂移誤差[15]。在四元數(shù)法姿態(tài)解算算法流程中應(yīng)用互補(bǔ)濾波器時，算法流程為圖1。

圖1 基于互補(bǔ)濾波器的四元數(shù)法姿態(tài)解算過程Fig. 1 Quaternion attitude calculation process based on complementary filter

由基于互補(bǔ)濾波器的姿態(tài)解算過程可見，跟卡爾曼濾波算法相比，互補(bǔ)濾波法有計算量少、容易實(shí)現(xiàn)的優(yōu)點(diǎn)。

但是，圖中的要求加速度計輸出的是重力加速度g在b系的三軸分量時，才能使更新四元數(shù)得到高精度解算。實(shí)際上，在飛行器動態(tài)飛行過程中，往往受到的是重力加速度和運(yùn)動加速度的合力，而加速計無法區(qū)分這二者。這個原因當(dāng)飛行器作非勻速飛行時，即運(yùn)動加速度非零時，基于互補(bǔ)濾波器的姿態(tài)解算誤差隨運(yùn)動加速度的誤差增大而增大。

3 自適應(yīng)限幅濾波與互補(bǔ)濾波組合

給限幅濾波器設(shè)置一個閥值，通過限制輸入的幅值可消除無用或者有害幅值。于是可以設(shè)計一個自適應(yīng)限幅濾波器，使其限幅閥值始終跟隨到略大于重力加速度的大小的位置，在飛行器非勻速飛行時最大化地濾除運(yùn)動加速度。設(shè)計的自適應(yīng)限幅濾波器結(jié)構(gòu)如圖2。

圖2 自適應(yīng)限幅濾波器的設(shè)計方案Fig. 2 Design scheme of adaptive limited filter

(8)

記ρ為設(shè)計的限幅濾波器的自適應(yīng)限幅閥值，ρ的生成規(guī)則為:

(9)

若η0表示姿態(tài)角任一時刻的角度值，由于低成本飛行器的輸出頻率高，旋轉(zhuǎn)角速度較小，所以ω·Δt也很小，那么有:

(10)

(11)

4 實(shí)驗(yàn)測試及分析

4.1 實(shí)驗(yàn)平臺

算法設(shè)計驗(yàn)證及分析平臺為Matlab R2011b。用于實(shí)驗(yàn)測試的軟件及裝置如圖3所示。

航姿參考系統(tǒng)和數(shù)據(jù)采集選用微型航姿參考系統(tǒng)MAHRS 3DM-E10A，該系統(tǒng)通過RS-232串行接口方便連接，提供100 Hz的實(shí)時三軸慣性原始數(shù)據(jù)和參考姿態(tài)角。3DM-E10A提供的橫滾角、俯仰角靜態(tài)誤差±0.1°，動態(tài)誤差±1.0°；航向角靜態(tài)誤差±0.5°，動態(tài)誤差±2.0°。

如圖3(a)所示，雙軸電動轉(zhuǎn)臺使用TT-3DM-2E-10 Version 1.2, 具備串行輸出實(shí)時角度位置數(shù)據(jù)、串行輸入控制指令及旋轉(zhuǎn)角位置動態(tài)跟蹤測量與控制等優(yōu)點(diǎn)，主軸與俯仰軸轉(zhuǎn)角范圍連續(xù)無限、角度位置綜合測量精度為±0.08°、控制到位分辨率為±0.01°及速率范圍為0.1°/s～300°/s，滿足實(shí)驗(yàn)測試算法需求。

圖3(b)為TT-3DM-2E-10配套的轉(zhuǎn)臺控制Labview軟件。圖3(c)為作者開發(fā)的用于通過串口記錄理論姿態(tài)角及3DM-E10A系統(tǒng)輸出的9個原始數(shù)據(jù)的Labview軟件，記錄頻率與3DM-E10A輸出頻率同樣。

圖3 用于實(shí)驗(yàn)測試的軟件及裝置Fig. 3 Software and equipment for test

4.2 靜態(tài)及低動態(tài)測試

靜態(tài)及低動態(tài)測試數(shù)據(jù)采集方法為：通過圖3(b)所示控制轉(zhuǎn)臺的Labview開發(fā)的軟件通過串口控制如圖3(a)所示雙軸轉(zhuǎn)臺轉(zhuǎn)動，如圖3(c)所示的Labview開發(fā)的軟件通過串口記錄理論姿態(tài)角及慣性元件輸出9個原始數(shù)據(jù)。

利用自適應(yīng)限幅濾波與互補(bǔ)濾波器組合濾波算法解算的靜態(tài)及低動態(tài)下載體的姿態(tài)角解算及與航姿參考系統(tǒng)的誤差如圖4。

圖4 靜態(tài)及低動態(tài)測試Fig. 4 Static and low dynamic test

由圖4可見，在靜態(tài)狀態(tài)下，基于組合濾波算法解算的姿態(tài)角誤差在±0.2°內(nèi)；在低動態(tài)狀態(tài)下，誤差在±0.7°內(nèi)。基于典型互補(bǔ)濾波算法的解算結(jié)果，在靜態(tài)下的誤差與基于組合濾波算法解算的姿態(tài)角誤差基本一致，但是低動態(tài)下，特別是角速率比較大時，解算最大誤差可達(dá)±2°。

靜態(tài)及低動態(tài)測試表明，在載體靜態(tài)下，該算法保留了典型互補(bǔ)濾波算法的優(yōu)點(diǎn)；在低動態(tài)下，在載體角速率較大時，自適應(yīng)限幅濾波也發(fā)揮作用，有效減弱了運(yùn)動加速度帶來的影響，提高了姿態(tài)解算的精度。

4.3 快速滑動測試

為了驗(yàn)證算法消除運(yùn)動加速度對互補(bǔ)濾波法姿態(tài)解算影響的效果，將3DM-E10A置于一個接近水平面的平面上滑動，并通過串口采集原始數(shù)據(jù)。其中該實(shí)驗(yàn)用的平面為固定鋼化玻璃且?guī)缀鯖]有彈性，經(jīng)反復(fù)測量橫滾角約-0.3°，俯仰角約-0.5°，由于測試不同算法時使用的是同一個滑動過程采集的數(shù)據(jù)，所以不會對對比實(shí)驗(yàn)產(chǎn)生影響，可視為理論上的0°。

圖5為滑動時，三軸加速度計輸出的重力加速度和運(yùn)動加速度的合加速度。圖6為基于組合濾波、典型互補(bǔ)濾波及經(jīng)典卡爾曼濾波這三個姿態(tài)解算算法的比較。

圖5 平面快速滑動過程載體的三軸加速度Fig. 5 Three axis accelerations of carrier during fast moving on level surface

圖6 姿態(tài)解算算法比較Fig. 6 Comparison of attitude calculation algorithms

結(jié)合圖5和圖6可見: 理論上均為0°的橫滾角和俯仰角由于較大的運(yùn)動加速度的對互補(bǔ)濾波算法解算的影響產(chǎn)生了約6°的誤差，對經(jīng)典卡爾曼濾波的解算產(chǎn)生了7°的影響，而基于組合濾波基本不受影響?；诨パa(bǔ)濾波解算的航向角在停止滑動時也未能達(dá)到收斂的穩(wěn)定值，還需要約200 ms后才能穩(wěn)定，經(jīng)典卡爾曼濾波收斂速度較快；基于組合濾波收斂最快。

快速滑動測試表明，在運(yùn)動加速度突變或較大時，基于自適應(yīng)限幅濾波與互補(bǔ)濾波器組合的姿態(tài)解算算法極大地減少了運(yùn)動加速度對姿態(tài)解算影響，性能優(yōu)于基于典型的互補(bǔ)濾波算法及經(jīng)典卡爾曼濾波的姿態(tài)解算。

5 結(jié)語

本文分析了基于互補(bǔ)濾波器的姿態(tài)解算原理和實(shí)現(xiàn)方法，指出了運(yùn)動加速度對姿態(tài)解算的影響，并提出了根據(jù)陀螺儀輸出和運(yùn)動狀態(tài)的判斷來設(shè)計一個具有自適應(yīng)限幅門閥的限幅濾波器的方法，將三軸加速度計的輸出通過這個限幅濾波器后再用互補(bǔ)濾波算法解算出載體的姿態(tài)角。測試表明該算法在勻速或者較高動態(tài)狀態(tài)下，都能達(dá)到較高的解算精度，提高了姿態(tài)解算的精度。

References)

[1] 傅忠云, 劉文波, 孫金秋,等. 自適應(yīng)混合濾波算法在微型飛行器姿態(tài)估計中的應(yīng)用[J]. 傳感技術(shù)學(xué)報, 2014,27(5):698-703.(FU Z Y, LIU W B, SUN J Q, et al. Application of adaptive hybrid filter algorithm in the estimation of the micro air vehicle attitude[J]. Chinese Journal of Sensors and Actuators, 2014,27(5):698-703.)

[2] 金光明, 張國良, 陳林鵬,等. MEMS陀螺儀靜態(tài)漂移模型與濾波方法研究[J]. 傳感器與微系統(tǒng), 2007, 26(11):48-50.(JIN G M, ZHANG G L, CHEN L P, et al. Research on filter method and model of MEMS gyro static drift[J]. Transducer and Microsystem Technologies, 2007, 26(11):48-50.)

[3] DI L, FROMM T, CHEN Y Q. A data fusion system for attitude estimation of low-cost miniature UAVs[J]. Journal of Intelligent & Robotic Systems, 2012, 65(1):621-635.

[4] 彭孝東, 張鐵民, 李繼宇,等. 基于傳感器校正與融合的農(nóng)用小型無人機(jī)姿態(tài)估計算法[J].自動化學(xué)報, 2015, 41(4):854-860.(PENG X D, ZHANG T M, LI J Y, et al. Attitude estimation algorithm of agricultural small-UAV based on sensors fusion and calibration[J]. Acta Automatica Sinica, 2015, 41(4):854-860.)

[5] 賈瑞才. 基于四元數(shù)EKF的低成本MEMS姿態(tài)估計算法[J]. 傳感技術(shù)學(xué)報, 2014, 27(1):90-95.(JIA R C. Attitude estimation algorithm for low cost MEMS based on quaternion EKF[J]. Chinese Journal of Sensors and Actuators, 2014,27(1):90-95.)

[6] 吳濤, 白茹, 朱禮堯,等. 基于卡爾曼濾波的航姿參考系統(tǒng)設(shè)計[J]. 傳感技術(shù)學(xué)報, 2016, 29(4):531-535.(WU T, BAI R, ZHU L Y, et al. Design of AHRS based on Kalman filter[J]. Chinese Journal of Sensors and Actuators, 2016, 29(4):531-535.)

[7] PAN Y, SONG P, LI K, et al. Attitude estimation of miniature unmanned helicopter using unscented Kalman filter[C]// Proceedings of the 2011 International Conference on Transportation, Mechanical, and Electrical Engineering. Piscataway, NJ: IEEE, 2011:1548-1551.

[8] TANG X, WEI J, CHEN K. Square-root adaptive cubature Kalman filter with application to spacecraft attitude estimation[C]// Proceedings of the 2012 15th International Conference on International Conference on Information Fusion. Piscataway, NJ: IEEE, 2012:1406-1412.

[9] EUSTON M, COOTE P, MAHONY R, et al. A complementary filter for attitude estimation of a fixed-wing UAV[C]// Proceedings of the 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ: IEEE, 2008:340-345.

[10] WU J, ZHOU Z, CHEN J, et al. Fast complementary filter for attitude estimation using low-cost MARG sensors[J]. IEEE Sensors Journal, 2016, 16(18): 6997-7007.

[11] WU Z Y, ZENG Y J, SHAO H X, et al. Betterment of attitude estimation based on complementary filter applied in drumstick[C]// Proceedings of the 2012 World Congress on Information and Communication Technologies. Piscataway, NJ: IEEE, 2012:1162-1165.

[12] 閻世梁, 王銀玲, 張華. 基于改進(jìn)互補(bǔ)濾波器的低成本微小飛行器姿態(tài)估計方法[J]. 計算機(jī)應(yīng)用, 2013, 33(7):2078-2082.(YAN S L, WANG Y L, ZHANG H. Improved complementary filter for attitude estimation of micro air vehicles using low-cost inertial measurement units[J]. Journal of Computer Applications, 2013, 33(7):2078-2082.)

[13] 孫金秋, 游有鵬, 傅忠云. 基于自適應(yīng)顯式互補(bǔ)濾波的姿態(tài)解算方法[J]. 測控技術(shù), 2015, 34(4):24-27.(SUN J Q, YOU Y P, FU Z Y. Attitude estimation based on adaptive explicit complementary filter[J]. Measurement & Control Technology, 2015, 34(4):24-27.)

[14] 秦永元.慣性導(dǎo)航[M].2版.北京:科學(xué)出版社, 2014:244-260.(QIN Y Y. Inertial Navigation[M]. 2nd ed. Beijing: Science Press, 2014:244-260.)

[15] 梁延德, 程敏, 何福本,等. 基于互補(bǔ)濾波器的四旋翼飛行器姿態(tài)解算[J]. 傳感器與微系統(tǒng), 2011, 30(11):56-58.(LIANG Y D, CHENG M, HE F B, et al. Attitude estimation of a quad-rotor aircraft based on complementary filter[J]. Transducer and Microsystem Technologies, 2011, 30(11):56-58.)

This work is supported by the National Natural Science Foundation of China (61362005), the Key Laboratory of Cognitive Radio and Information Processing, Ministry of Education (CRKL150113), the Innovation Project of Graduate Education in Guilin University of Electronic Technology (2016YJCX90).

WANG Shouhua, born in 1975, M. S., associate professor. His research interests include information processing, navigation and positioning.

DENG Guihui, born in 1989, M. S. candidate. His reserch interests include integrated navigation, indoor naigation.

JI Yuanfa, born in 1975, Ph. D, professor. His research interests include satelite communication, satellite navigation and digital signal processing.

SUN Xiyan, born in 1973, Ph. D, professor. His research interests include satellite navigation, electronic countermeasure.

Attitude calculation algorithm of low-cost aircraft based on combined filter

WANG Shouhua1,2, DENG Guihui1*, JI Yuanfa1,2, SUN Xiyan1,2

(1.CollegeofInformationandCommunication,GuilinUniversityofElectronicTechnology,GuilinGuangxi541004,China;2.KeyLaboratoryofCognitiveRadioandInformationProcessingofMinistryofEducation,GuilinGuangxi541004,China)

In the low-cost aircraft attitude detection system, the complementary filter was widely used because of simple principle and low computational complexity. Aiming at the problem that the accelerometer can not distinguish between the acceleration of gravity and motion may cause attitude calculation error by complementary filtering, an attitude calculation algorithm of complementary and adaptive limited filter was proposed for low cost aircraft, and the design method of adaptive limited filter gate valve was given. The angular velocity output by a gyroscope and the acceleration output by an accelerometer were fused to obtain the gate valve of the limited filter, and then the normalized output increment of the accelerometer by limiting filter replaced the accelerometer input of the original complementary filter. The accuracy of attitude determination under uniform motion was improved. The actual system test shows that the proposed algorithm has high accuracy and low cost, and is easy to be realized in low cost aircraft control system.

attitude calculation; complementary filter; limiting filtering; quaternion method; adaptive

2016-10-28;

2016-12-13。 基金項(xiàng)目:國家自然科學(xué)基金資助項(xiàng)目(61362005);“認(rèn)知無線電與信息處理”教育部重點(diǎn)實(shí)驗(yàn)室2015年主任基金資助項(xiàng)目(CRKL150113); 桂林電子科技大學(xué)研究生教育創(chuàng)新計劃項(xiàng)目(2016YJCX90)。

王守華(1975—),男,山東濱州人,副教授,碩士,主要研究方向:信息處理、導(dǎo)航定位; 鄧桂輝(1989—),男,廣東茂名人,碩士研究生,主要研究方向:組合導(dǎo)航、室內(nèi)導(dǎo)航; 紀(jì)元法(1975—)，男，山東莘縣人，教授，博士，主要研究方向:衛(wèi)星通信、衛(wèi)星導(dǎo)航、數(shù)字信號處理; 孫希延(1973—)，女，山東濰坊人，研究員，博士，主要研究方向:衛(wèi)星導(dǎo)航、電子對抗。

1001-9081(2017)05-1507-05

10.11772/j.issn.1001-9081.2017.05.1507

TP391.9

A