李 翔，王 濤

(桂林電子科技大學(xué)電子工程與自動化學(xué)院，廣西 桂林 541004)

微機電系統(tǒng)(Micro-Electro-Mechanical System，MEMS)陀螺儀和加速度計具有體積小、成本低等顯著優(yōu)點，被廣泛用于無人機等領(lǐng)域?qū)崿F(xiàn)姿態(tài)測量[1－2]。為實現(xiàn)這兩種傳感器間的數(shù)據(jù)融合，一種普遍采用的解決方案是互補濾波(Complementary Filter，CF)算法。CF 的主要優(yōu)勢在于計算量小，適合于在計算能力有限的單片機上使用，且能達(dá)到與卡爾曼濾波(Kalman Filter，KF)相近的姿態(tài)估計精度[3－5]。然而，姿態(tài)測量領(lǐng)域目前所用的各種CF 算法存在如下兩個不可忽視的問題。

第一個問題是各種CF 算法普遍基于連續(xù)時間架構(gòu)，然而CF 算法在單片機或其他處理單元上實際是以離散時間的方式運行。盡管這種從連續(xù)時間到離散時間的轉(zhuǎn)換可通過雙線性變換等方法實現(xiàn)，但直接在離散時間條件下設(shè)計并實現(xiàn)CF 顯然更為簡便可靠。

第二個問題是CF 的參數(shù)整定。在姿態(tài)測量領(lǐng)域，針對加速度計測量值易受動態(tài)干擾的問題，出現(xiàn)了各種對CF 參數(shù)作自適應(yīng)調(diào)節(jié)的算法[6－10]，以及對參數(shù)變化不敏感的CF 算法[11]等。然而此類自適應(yīng)算法往往未能充分考慮傳感器噪聲及誤差的統(tǒng)計特性，使得其參數(shù)調(diào)節(jié)缺乏嚴(yán)謹(jǐn)客觀的依據(jù)。僅有極少數(shù)文獻(xiàn)對傳感器噪聲頻譜進(jìn)行了實測，并據(jù)此進(jìn)行CF 的參數(shù)整定[12]。

針對上述問題，本文提出了用于姿態(tài)估計的離散時間互補濾波(Discrete-Time Complementary Filter，DTCF)算法，推導(dǎo)了根據(jù)傳感器噪聲統(tǒng)計特性實現(xiàn)DTCF 參數(shù)整定的方法，通過仿真和實驗驗證了DTCF 用于姿態(tài)估計的有效性。

1 姿態(tài)測量及互補濾波原理

1.1 姿態(tài)測量

三軸加速度計可測量載體坐標(biāo)系下的重力加速度矢量g＝(g1g2g3)T，由此即可利用反三角函數(shù)計算俯仰角θ與橫滾角φ，分別如式(1)及式(2)所示[11，13]:

三維姿態(tài)的完整描述需要橫滾、俯仰和航向這三個歐拉角，航向角的測量可借助三軸磁強計測量載體坐標(biāo)系下的地磁場矢量h＝(h1h2h3)T來實現(xiàn)。航向角ψ的計算如式(3)所示[14－15]:

三軸陀螺儀可測量載體的角速度ω＝(ω1ω2ω3)T，進(jìn)而可根據(jù)角速度測量值來推算姿態(tài)變化率[10]。將重力矢量和地磁矢量分別與角速度叉乘，即得它們對時間的導(dǎo)數(shù)，如式(4)所示:

1.2 互補濾波

傳統(tǒng)CF 在頻域?qū)崿F(xiàn)數(shù)據(jù)融合。以姿態(tài)估計為例:由加速度計測量值解算的姿態(tài)xA采用具有低通特性的濾波器G(s)，以濾除噪聲及動態(tài)干擾；對陀螺儀測量值積分得到的姿態(tài)xB則采用具有高通特性的濾波器[1－G(s)]，以抑制陀螺漂移引起的累積誤差；由此可得CF 的s域傳遞函數(shù)，如式(5)所示。

Mahony 等[16]針對姿態(tài)估計問題，將傳統(tǒng)CF 改造成具有閉環(huán)反饋結(jié)構(gòu)的廣義互補濾波器(Generalized Complementary Filter，GCF)，如圖1 所示。GCF 的傳遞函數(shù)如式(6)所示，其中的G(s)若僅含比例環(huán)節(jié)，即G(s)＝KP，則為一階互補濾波；若G(s)為比例與積分環(huán)節(jié)的結(jié)合，即G(s)＝KP＋KI×s－1，則構(gòu)成二階互補濾波。目前姿態(tài)估計領(lǐng)域所用CF算法大多以圖1 所示GCF 架構(gòu)為基礎(chǔ)[17－20]。

圖1 Mahony 型互補濾波框圖

2 離散時間姿態(tài)互補濾波器設(shè)計

2.1 DTCF 算法原理

k時刻載體坐標(biāo)系下的重力矢量既可直接由加速度計進(jìn)行測量，也可由陀螺儀測得的角速度結(jié)合式(4)進(jìn)行計算。將上述兩途徑進(jìn)行加權(quán)融合，即構(gòu)成本文的DTCF 算法，其具體步驟如下:

DTCF 第2 步:設(shè)k時刻加速度計測得的重力矢量為，按式(8)進(jìn)行加權(quán)融合得到k時刻的重力矢量估計值，其中0≤G≤1 為加權(quán)系數(shù)，亦是DTCF 的唯一參數(shù)。

式(7)和式(8)所構(gòu)成的DTCF 似乎只是在時域?qū)铀俣扔嫼屯勇輧x的測量值進(jìn)行了簡單的加權(quán)，但將它轉(zhuǎn)換為z域形式即可看出其互補特性。在上述DTCF 中，待估計物理量x為重力矢量，加速度計給出的是x的直接測量值序列xA，k，而陀螺儀測量值代入式(7)所得到的則是x的增量序列ΔxB，k。又注意到z域中的差分算子可表示為(1－z－1)，故可得式(9)所示的z域表示式:

由式(9)可以看出，式(7)和式(8)構(gòu)成的DTCF 算法在頻域確實具有互補性，因而將其稱為離散時間條件下的互補濾波器是恰當(dāng)?shù)摹?/p>

2.2 DTCF 參數(shù)整定

DTCF 的參數(shù)G可根據(jù)方差最小化原則決定。在式(8)中，記協(xié)方差矩陣為Pk－1，Δgk協(xié)方差矩陣為Q，協(xié)方差矩陣為R，則的協(xié)方差矩陣Pk可由式(10)表示:

一般而言，傳感器各軸的噪聲大小近似相同且相互獨立，則式(10)中各協(xié)方差矩陣可近似為標(biāo)量矩陣(即各對角元大小相等且非對角元為零)，故可將式(10)改寫為標(biāo)量形式，如式(11)所示:

為使DTCF 保持穩(wěn)定，應(yīng)當(dāng)有Pk－1＝Pk＝P為常數(shù)，故可得式(12):

為獲得最佳濾波效果，應(yīng)使P為極小，即令?P/?G＝0，且注意G為正數(shù)，由此解得

式中:Q和R可由實測數(shù)據(jù)統(tǒng)計得到。Q是由陀螺儀噪聲及矢量更新算法所引起的誤差，可由式(14)計算，其中:N為采樣點個數(shù)，gr，k為k時刻重力矢量真值，Δgr，k為從(k－1)時刻到k時刻重力矢量真值的改變量，其余記號與式(7)和式(8)中一致。

另一方面，R反映加速度計的測量誤差，可由式(15)計算:

值得注意的是，若記λ＝R/Q，則式(13)可改寫為:

亦即DTCF 的參數(shù)整定結(jié)果實際是由Q和R的比值決定。

3 算法驗證

3.1 半實物仿真

為驗證上文所述DTCF 及其參數(shù)整定方法，采用荷蘭Xsens 的MTi300 微型航姿模塊采集兩組姿態(tài)數(shù)據(jù)，分別記為數(shù)據(jù)集A 和數(shù)據(jù)集B，如圖2所示。

圖2 仿真所用姿態(tài)數(shù)據(jù)

仿真中，首先由圖2 所示姿態(tài)數(shù)據(jù)生成加速度計和陀螺儀采樣數(shù)據(jù)，兩個數(shù)據(jù)集A 和B 各自對應(yīng)的時間長度均為40 s，采樣頻率為20 Hz，故每一數(shù)據(jù)集中的采樣點個數(shù)均為N＝800。由三維姿態(tài)得到載體坐標(biāo)系下的重力矢量后進(jìn)行歸一化處理(即令重力矢量模長為1)，并加入標(biāo)準(zhǔn)差為0.1 的白噪聲后得到加速度計測量值。另一方面，三維角速度加入標(biāo)準(zhǔn)差為0.005 rad/s 的白噪聲以及隨機游走誤差后得到陀螺儀測量值。

生成加速度計及陀螺儀采樣數(shù)據(jù)后，分別按式(14)和式(15)進(jìn)行誤差統(tǒng)計，得到方差Q和R，再按式(13)計算DTCF 的參數(shù)G，其結(jié)果列于表1。由表1 可見，兩個數(shù)據(jù)集對應(yīng)的DTCF 參數(shù)整定結(jié)果相互吻合得很好。

表1 誤差統(tǒng)計及DTCF 參數(shù)整定結(jié)果

為進(jìn)一步驗證表1 所列DTCF 參數(shù)整定結(jié)果的可靠性，改變DTCF 的參數(shù)G取值，并觀察其濾波效果隨G取值的變化。圖3 所示為兩個數(shù)據(jù)集分別采用DTCF 進(jìn)行姿態(tài)估計時，俯仰角與橫滾角的均方根誤差(Root-Mean-Square Error，RMSE)隨參數(shù)G取值而變化的情況。由圖3 可見，使俯仰角與橫滾角的RMSE 達(dá)到最小的參數(shù)G取值確實與表1 所列參數(shù)整定結(jié)果非常接近。因此，本文所述DTCF 及其參數(shù)整定方法是可行的。

圖3 DTCF 參數(shù)取值對姿態(tài)誤差的影響

3.2 無人機飛行實驗

采用基于開源飛控PIXHAWK2.4.8 的四旋翼無人機對本文提出的DTCF 算法作進(jìn)一步測試，并與常用的擴展卡爾曼濾波(Extended Kalman Filter，EKF)[4]及GCF[3]算法進(jìn)行對比。實驗中，由PIXHAWK 的飛行日志文件中讀取無人機姿態(tài)作為參考值，并分別采用EKF、GCF 和DTCF 三種濾波算法各自根據(jù)傳感器測量數(shù)據(jù)估計姿態(tài)角。圖4、圖5和圖6 依次為各算法的航向角、俯仰角、橫滾角估計值與參考值對比情況。

圖4 不同濾波算法估計的航向角對比

圖5 不同濾波算法估計的俯仰角對比

由圖4～圖6 可見，無人機飛行過程中姿態(tài)變化較劇烈，但在此種高動態(tài)條件下，EKF、GCF 以及DTCF 三種算法估計的姿態(tài)相差不大。對三種算法各自的均方根誤差進(jìn)行統(tǒng)計，結(jié)果如表2 所示，可見本文的DTCF 算法具有與EKF 和GCF 相當(dāng)?shù)淖藨B(tài)估計精度。

表2 三種濾波算法姿態(tài)誤差對比

3.3 運行時間對比

采用基于增強型8051 內(nèi)核的STC15W4K56S4單片機對上述三種算法(EKF、GCF 及本文的DTCF)的運行時間進(jìn)行對比。STC15W4K56S4 單片機內(nèi)部具有5 個16 位定時器，可分別記錄EKF、GCF 和DTCF 三種算法各自的運行時間(以單片機的系統(tǒng)時鐘周期為單位)。

圖7 所示為傳感器的每個采樣周期中，以上三種算法在單片機上各自需要的時鐘周期數(shù)的對比。此處所用的傳感器原始數(shù)據(jù)來自上文所述無人機飛行實驗，PIXHAWK 飛行日志文件記錄時長為3 min，采樣頻率為10 Hz，故采樣點個數(shù)N ＝1 800，即每種算法均進(jìn)行了1800 次遞推計算。對圖7 所示各算法所需時鐘周期數(shù)分別取平均值，再乘以單片機系統(tǒng)時鐘頻率的倒數(shù)，即得各算法在單片機上的平均耗時。在11.059 2 MHz 時鐘頻率下，上述三種算法的平均耗時如表3 所示。

表3 三種濾波算法耗時對比

由表3 可見，GCF 和DTCF 的運行時間均遠(yuǎn)低于EKF，這是由于EKF 涉及大量的矩陣運算。另一方面，DTCF 的運行時間又比GCF 減少了約一半，其原因在于DTCF 直接對重力和地磁矢量進(jìn)行估計，而無需在這兩個矢量和姿態(tài)四元數(shù)之間相互轉(zhuǎn)換，從而比GCF 進(jìn)一步降低了計算量。

3.4 討論

GCF 的估計對象為姿態(tài)四元數(shù)，而四元數(shù)與重力矢量(或地磁矢量)間的關(guān)系是非線性的，這就使得其參數(shù)與傳感器噪聲特性間的關(guān)系變得復(fù)雜，故而在實踐中往往直接采用試湊來進(jìn)行參數(shù)調(diào)節(jié)。

DTCF 直接以重力矢量為估計對象，如2.2 節(jié)所述，DTCF 的參數(shù)可由方差Q和R的比值來確定，而這兩個方差分別反映了獲取姿態(tài)的兩種途徑(通過角速度積分推算姿態(tài)以及直接由重力矢量定姿)各自的誤差大小。在實際應(yīng)用中，方差Q和R可分別由式(14)和(15)統(tǒng)計得到，從而使DTCF 的參數(shù)選取充分反映傳感器誤差特性。3.1 節(jié)的仿真結(jié)果證明了2.2 節(jié)的理論推導(dǎo)與實際情況相吻合，從而為DTCF 參數(shù)整定提供了可靠依據(jù)。

對于需要輸出完整三維姿態(tài)信息(即同時提供航向角、俯仰角、橫滾角)的場合，由于姿態(tài)解算需同時用到重力矢量和地磁矢量，因而濾波器參數(shù)整定必須兼顧加速度計與磁強計各自的特性。對于GCF 而言，這將進(jìn)一步增加其參數(shù)整定的難度。而DTCF 可以利用兩個子濾波器分別估計重力矢量和地磁矢量，且這兩個子濾波器的參數(shù)可相互獨立地進(jìn)行調(diào)節(jié)，從而能更好地分別適應(yīng)加速度計及磁強計的噪聲特性。

4 結(jié)論

本文提出的DTCF 可根據(jù)傳感器噪聲的統(tǒng)計特性直接確定其參數(shù)，為姿態(tài)估計所用的互補濾波器提供了新的設(shè)計思路。本文提出的DTCF 及其參數(shù)整定方法具有明確的理論基礎(chǔ)和簡便的實施步驟，達(dá)到了理論與實踐的統(tǒng)一。實驗表明，DTCF 的姿態(tài)估計精度與常用的EKF 及GCF 相近，但DTCF 具有更低的計算量及耗時，非常適合在計算能力有限的單片機上使用，有助于提高三維姿態(tài)估計的實時性。