離散時(shí)間姿態(tài)互補(bǔ)濾波器及其參數(shù)整定研究?

李 翔，王 濤

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

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

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

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

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

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

1.1 姿態(tài)測(cè)量

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

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

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

1.2 互補(bǔ)濾波

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

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

圖1 Mahony 型互補(bǔ)濾波框圖

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

2.1 DTCF 算法原理

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

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

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

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

2.2 DTCF 參數(shù)整定

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

一般而言，傳感器各軸的噪聲大小近似相同且相互獨(dú)立，則式(10)中各協(xié)方差矩陣可近似為標(biāo)量矩陣(即各對(duì)角元大小相等且非對(duì)角元為零)，故可將式(10)改寫(xiě)為標(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í)測(cè)數(shù)據(jù)統(tǒng)計(jì)得到。Q是由陀螺儀噪聲及矢量更新算法所引起的誤差，可由式(14)計(jì)算，其中:N為采樣點(diǎn)個(gè)數(shù)，gr，k為k時(shí)刻重力矢量真值，Δgr，k為從(k－1)時(shí)刻到k時(shí)刻重力矢量真值的改變量，其余記號(hào)與式(7)和式(8)中一致。

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

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

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

3 算法驗(yàn)證

3.1 半實(shí)物仿真

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

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

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

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

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

3.2 無(wú)人機(jī)飛行實(shí)驗(yàn)

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

圖4 不同濾波算法估計(jì)的航向角對(duì)比

圖5 不同濾波算法估計(jì)的俯仰角對(duì)比

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

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

3.3 運(yùn)行時(shí)間對(duì)比

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

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

表3 三種濾波算法耗時(shí)對(duì)比

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

3.4 討論

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

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

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

4 結(jié)論

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