改進(jìn)的Sage?Husa自適應(yīng)濾波算法在MEMS航姿參考系統(tǒng)中的應(yīng)用

路小燕，楊柳慶，郭 錦，楊 碩，王博豪

（1.南京航空航天大學(xué)自動(dòng)化學(xué)院，南京210016；2.南京航空航天大學(xué)無(wú)人機(jī)研究院，南京210016）

0 引言

航姿參考系統(tǒng)（AHRS）是一種在空間三維坐標(biāo)系中用于精確測(cè)量載體姿態(tài)（俯仰角、滾轉(zhuǎn)角、航向角）的慣性裝置系統(tǒng)，包括了MEMS中的陀螺儀、磁力計(jì)和加速度計(jì)。三軸陀螺儀的測(cè)量結(jié)果在短時(shí)間內(nèi)精度較高，但誤差會(huì)隨時(shí)間的推移而累積；磁力計(jì)和加速度計(jì)為姿態(tài)運(yùn)動(dòng)載體提供了更加精確的靜態(tài)角度，但是動(dòng)態(tài)性能差［1］。常規(guī)的Kalman濾波器的增益和預(yù)測(cè)調(diào)整能力存在缺陷，在計(jì)算時(shí)只依據(jù)先驗(yàn)信息，沒有引入實(shí)際觀測(cè)值。隨著數(shù)據(jù)的增加，Kalman濾波會(huì)出現(xiàn)飽和現(xiàn)象，導(dǎo)致濾波器失去原有用途。無(wú)人飛行系統(tǒng)大部分都是非線性的系統(tǒng)，擴(kuò)展Kalman濾波利用Taylor級(jí)數(shù)展開法將無(wú)人非線性系統(tǒng)線性化，然后采用Kalman濾波的相關(guān)步驟更新系統(tǒng)狀態(tài)［2-3］。 文獻(xiàn)［4］設(shè)計(jì)了基于四元素的九態(tài)擴(kuò)展Kalman濾波算法。此算法適合弱的非線性系統(tǒng)，實(shí)時(shí)性差且濾波容易發(fā)散。文獻(xiàn)［5］和文獻(xiàn)［6］提出了一種利用Sage-Husa自適應(yīng)濾波對(duì)系統(tǒng)和量測(cè)噪聲進(jìn)行實(shí)時(shí)估計(jì)的算法，此算法在各領(lǐng)域中得到了廣泛運(yùn)用，但該算法在與工程應(yīng)用結(jié)合時(shí)會(huì)出現(xiàn)許多問題。文獻(xiàn)［7］和文獻(xiàn)［8］提出了 Sage-Husa自適應(yīng)濾波的簡(jiǎn)化改進(jìn)算法，在濾波過(guò)程中直接忽略了單個(gè)采樣周期內(nèi)的在線估計(jì)，在濾波的某個(gè)時(shí)間段內(nèi)因觀測(cè)值誤差增大而致使新息中被引入誤差量，從而導(dǎo)致狀態(tài)預(yù)測(cè)出現(xiàn)了發(fā)散，無(wú)法適應(yīng)情況突變時(shí)的導(dǎo)航精度的需求，魯棒性差。

自適應(yīng)濾波在利用觀測(cè)信息進(jìn)行濾波時(shí)，不斷對(duì)不確切或者未知的模型參數(shù)、噪聲統(tǒng)計(jì)特性進(jìn)行實(shí)時(shí)估計(jì)與修正，減小了相關(guān)模型的誤差與算法的計(jì)算量［9-11］。自適應(yīng)濾波包含許多種類，如新息自適應(yīng)估計(jì)Kalman濾波算法、協(xié)方差匹配法、相關(guān)法、Bayes估計(jì)法、極大似然法、模糊自適應(yīng)濾波法、Sage-Husa自適應(yīng)濾波算法等［12-15］。Sage-Husa自適應(yīng)濾波算法通過(guò)極大后驗(yàn)（MAP）信息對(duì)量測(cè)噪聲和系統(tǒng)噪聲進(jìn)行估計(jì)，具有易造成濾波發(fā)散、計(jì)算量大與實(shí)時(shí)性較差的缺點(diǎn)。本文針對(duì)Sage-Husa自適應(yīng)濾波算法進(jìn)行了改進(jìn)，將新的算法應(yīng)用于基于多傳感器融合的航姿參考系統(tǒng)中，實(shí)時(shí)估計(jì)并解算無(wú)人機(jī)的姿態(tài)。

1 定姿方案

1.1 整體定姿方案

如圖1所示，本文利用三軸陀螺儀估計(jì)的姿態(tài)角作為過(guò)程信息，將磁力計(jì)與加速度計(jì)估算的角度作為量測(cè)信息，將各個(gè)傳感器的優(yōu)劣勢(shì)進(jìn)行互補(bǔ)，建立了改進(jìn)的Sage-Husa自適應(yīng)Kalman濾波算法，對(duì)融合多傳感器信息的航姿參考系統(tǒng)進(jìn)行無(wú)人機(jī)姿態(tài)的最優(yōu)估計(jì)。

圖1 定姿總體結(jié)構(gòu)圖Fig.1 Overall structure diagram of attitude determination

1.2 三軸陀螺儀定姿算法

陀螺儀本身存在姿態(tài)累計(jì)誤差問題，其直接獲取的數(shù)據(jù)也摻夾著外界的噪聲。因此，在對(duì)傳感器原始數(shù)據(jù)進(jìn)行零漂補(bǔ)償和誤差補(bǔ)償后，假設(shè)運(yùn)動(dòng)載體處在非加速度狀態(tài)，在k時(shí)刻姿態(tài)角的真實(shí)值為三軸陀螺儀的角度估計(jì)誤差值為由于采用Euler角法計(jì)算的姿態(tài)角會(huì)產(chǎn)生奇異值，進(jìn)而可采用四元素法求取三軸陀螺儀的航姿角度故有

基于工程需求，為了消除Euler角解算無(wú)人飛行系統(tǒng)姿態(tài)而產(chǎn)生的奇點(diǎn)，由四元素法得到的矩陣坐標(biāo)變換矩陣如下式所示

此時(shí)，得到四元素微分向量方程為

采用四階Runge-Kutta法求解四元素微分方程，令T為更新步長(zhǎng)，計(jì)算得到量測(cè)更新K陣與Q陣的更新表達(dá)式為

四階Runge-Kutta法吸取了Taylor展開法中精度高的理念，同時(shí)摒棄了高階求導(dǎo)的難點(diǎn)，令數(shù)值結(jié)果緊湊而對(duì)稱。四階Runge-Kutta法是解算微分方程的有利工具，具有很好的工程意義。將式（5）代入Euler角解算式，可得出無(wú)人機(jī)姿態(tài)角

狀態(tài)向量x（k）＝［θγψbxbybz］取自Euler角與陀螺的漂移。利用Euler角的微分方程式及MEMS陀螺儀的誤差模型表達(dá)式，可以得到如下的系統(tǒng)狀態(tài)方程

1.3 磁力計(jì)和加速度計(jì)姿態(tài)更新算法

當(dāng)運(yùn)動(dòng)載體處于靜止或勻速運(yùn)動(dòng)狀態(tài)時(shí)，磁力計(jì)和加速度計(jì)所采集到的信息濾波經(jīng)處理后可得到姿態(tài)角信息為磁力計(jì)與加速度計(jì)的估計(jì)誤差間的差值，故有

運(yùn)動(dòng)載體為任意姿態(tài)時(shí)，假設(shè)測(cè)量運(yùn)動(dòng)加速度在三軸坐標(biāo)系上的表達(dá)式為

令g0為當(dāng)?shù)刂亓铀俣?，則有

其中，Cbn為捷聯(lián)式慣導(dǎo)的方向余弦矩陣，從而可解算出俯仰角與橫滾角

磁力計(jì)可應(yīng)用于水平方位航向角的計(jì)算。其中，mx和my分別為地磁場(chǎng)在機(jī)體軸X軸、Y軸上的分量，Δψ為磁偏角，ψm為機(jī)體縱軸相對(duì)磁場(chǎng)指北方位的航向角。假設(shè)ψ為機(jī)體軸縱軸相對(duì)真實(shí)指北方位的航向角，則有

可建立系統(tǒng)觀測(cè)方程y＝h（xk）+v。 其中，v＝［v1v2v3v4］為系統(tǒng)的量測(cè)噪聲，h（xk）＝［-cosθsinγsinθsinψcosθcosψcosθ］T。

運(yùn)動(dòng)載體的姿態(tài)是不斷變化的，故量測(cè)系統(tǒng)存在俯仰角與滾轉(zhuǎn)角姿態(tài)的修正

2 改進(jìn)Sage-Husa自適應(yīng)濾波算法

2.1 簡(jiǎn)化Sage-Husa自適應(yīng)擴(kuò)展Kalman濾波

擴(kuò)展Kalman濾波算法在其被使用之前就假定系統(tǒng)的噪聲統(tǒng)計(jì)特性為已知，而在實(shí)際的應(yīng)用中，無(wú)人飛行系統(tǒng)的系統(tǒng)噪聲的方差值和由觀測(cè)得到的噪聲誤差會(huì)隨時(shí)間而變動(dòng)，擴(kuò)展Kalman濾波算法會(huì)使姿態(tài)解算值產(chǎn)生發(fā)散。Sage-Husa自適應(yīng)擴(kuò)展濾波算法的核心是實(shí)時(shí)運(yùn)用量測(cè)更新的數(shù)據(jù)對(duì)系統(tǒng)和量測(cè)噪聲的特性進(jìn)行動(dòng)態(tài)的統(tǒng)計(jì)估算，并最終解算出所求的狀態(tài)參數(shù)的預(yù)測(cè)值，減少由模型誤差參數(shù)帶來(lái)的影響［16］。

Sage-Husa自適應(yīng)擴(kuò)展濾波算法無(wú)法在系統(tǒng)和量測(cè)噪聲都未知的條件下進(jìn)行估算，本文將在系統(tǒng)噪聲矩陣已知的情況下估測(cè)量測(cè)噪聲方差矩陣?；谌S陀螺儀計(jì)算所得的狀態(tài)方程與磁力計(jì)、加速度計(jì)描述的量測(cè)方程，得到多傳感器融合的簡(jiǎn)化Sage-Husa自適應(yīng)擴(kuò)展濾波算法［17］。該算法本身已消除了傳感源自有的錯(cuò)誤，其算法更新的基本步驟如式（13）與式（14）所示。

具有時(shí)變性能的噪聲估計(jì)器如下

定義式（14）中dk＝（1-b）/（1-bk+1）中b的取值范圍為0＜b＜1， 并將其作為遺忘因子，遺忘因子的取值區(qū)間一般為［0.95， 0.99］。 將解算得到的當(dāng)前時(shí)刻的狀態(tài)值、誤差方差、增益值等作為下一時(shí)刻的初始值，不斷進(jìn)行迭代，則可得到Sage-Husa 自適應(yīng)濾波算法［18］。

2.2 Sage-Husa自適應(yīng)濾波算法的改進(jìn)

理論而言，Sage-Husa自適應(yīng)濾波算法可以同時(shí)在線估計(jì)出系統(tǒng)的噪聲＾Qk及量測(cè)噪聲＾Rk。但是，ek的異常改變會(huì)影響＾Qk和＾Rk的計(jì)算準(zhǔn)確性，從而得到不精確的濾波效果。在實(shí)際工程應(yīng)用中，Sage-Husa自適應(yīng)擴(kuò)展Kalman濾波實(shí)現(xiàn)性強(qiáng)、精確度高，但其計(jì)算量較大，并且魯棒性較差。因此，對(duì)Sage-Husa自適應(yīng)濾波算法進(jìn)行改進(jìn)，顯得尤為重要［19］。

（1）最佳遺忘因子的選擇

在Sage-Husa自適應(yīng)濾波算法中，遺忘因子b選擇的恰當(dāng)與否直接關(guān)系到濾波的精度及濾波是否會(huì)出現(xiàn)發(fā)散現(xiàn)象。很多學(xué)者利用試湊法和最優(yōu)估計(jì)算法，將每次迭代得到的遺忘因子解算出來(lái)，從而得到最優(yōu)的濾波算法。為了對(duì)濾波狀況進(jìn)行判斷，本文采用了預(yù)測(cè)殘差作為監(jiān)測(cè)的方法。若濾波出現(xiàn)發(fā)散，則有

當(dāng)儲(chǔ)備系數(shù)ξ的取值為1時(shí)，可得到最佳的濾波估計(jì)效果。將式（15）代入式（14），可求取遺忘因子的最優(yōu)值

綜上所述， 式（14）與式（16）的組合構(gòu)成了由最佳遺忘因子改進(jìn)的Sage-Husa自適應(yīng)濾波算法，提高了濾波的魯棒性。

（2）抑制Sage-Husa自適應(yīng)濾波的發(fā)散

Sage-Husa自適應(yīng)濾波是一種具有次優(yōu)性質(zhì)的濾波，次優(yōu)濾波算法在運(yùn)用一步預(yù)測(cè)值和濾波值來(lái)替代全平滑值時(shí)將可能得到精度較差的噪聲統(tǒng)計(jì)特性。為了使得Qk與Rk滿足無(wú)偏估計(jì)，可在式（14）中采用兩式相減的算法，這將會(huì)致使Qk和Rk失去正定性與對(duì)稱性。以上兩種不足將會(huì)引發(fā)濾波結(jié)果發(fā)散，因而本文將采用下面兩種方法抑制濾波發(fā)散。

①使用有偏估計(jì)，進(jìn)而保證Qk和Rk的對(duì)稱性與正定性

對(duì)＾Qk和＾Rk進(jìn)行實(shí)時(shí)監(jiān)控，如果＾Rk為正定矩陣，＾Qk為半正定或正定矩陣，則利用式（14）進(jìn)行求解，否則利用式（17）進(jìn)行求解。

②通過(guò)調(diào)整和修正Pk/k-1，進(jìn)而調(diào)控增益矩陣Kk， 來(lái)防止出現(xiàn)濾波發(fā)散現(xiàn)象

其中，ρ≥1為可變系數(shù)，它的取值越小，濾波發(fā)散的抑制性能越強(qiáng)。如果式（18）不成立，則用代替式表達(dá)如下

其中，Sk為濾波加權(quán)系數(shù)，其取值為1。

3 實(shí)驗(yàn)驗(yàn)證

3.1 Matlab仿真驗(yàn)證

選取遺忘因子數(shù)值為0.95，濾波初始值為X0＝［0 0 0 0 0 0］T， 協(xié)方差初始值為 P0＝0.00052·I6×6， 量測(cè)噪聲方差矩陣初值為 R0＝［0.0005 0.0005 0.0005］， 系統(tǒng)噪聲矩陣Q0＝0.0182·I6×6。 在 Matlab 中， 采樣時(shí)間為 0.1s， 仿真總時(shí)間為600s，可得出多傳感器融合算法下的Kalman濾波、自適應(yīng)Sage-Husa擴(kuò)展Kalman濾波與改進(jìn)的自適應(yīng)Sage-Husa擴(kuò)展Kalman濾波這3種不同算法下的航姿誤差對(duì)比圖，如圖2～圖4及表1所示。

圖2 俯仰角誤差曲線Fig.2 Curves of pitch angle error

圖3 滾轉(zhuǎn)角誤差曲線Fig.3 Curves of roll angle error

圖4 航向角誤差曲線Fig.4 Curves of yaw angle error

表1 各濾波算法下的姿態(tài)誤差Table 1 Attitude errors under various filter algorithms

從圖2～圖4及表1中可以看出，基于多傳感器融合下的經(jīng)典Kalman濾波算法的姿態(tài)誤差大，有明顯的波動(dòng)，并且俯仰角誤差和滾轉(zhuǎn)角誤差的波動(dòng)隨時(shí)間推移而呈現(xiàn)出了發(fā)散趨勢(shì)，即使在當(dāng)航向角誤差在300s后穩(wěn)定下來(lái)時(shí)，誤差依舊超過(guò)了4°，無(wú)法滿足高精度導(dǎo)航的需求；相比經(jīng)典Kalman濾波算法，自適應(yīng)Sage-Husa擴(kuò)展Kalman濾波算法的融合精度有明顯的提高，從理論上而言其可被用于對(duì)導(dǎo)航精度需求較低的無(wú)人飛行系統(tǒng)，但是其誤差值的絕對(duì)值在一定時(shí)間范圍內(nèi)依然偏大，也無(wú)法滿足高精度導(dǎo)航的需求；改進(jìn)自適應(yīng)Sage-Husa算法不僅可抑制濾波發(fā)散，而且可令航姿誤差絕對(duì)值明顯降低，提高了航姿系統(tǒng)的魯棒性。Matlab仿真實(shí)驗(yàn)為后續(xù)的實(shí)驗(yàn)室實(shí)物搭建時(shí)的姿態(tài)算法估計(jì)試驗(yàn)奠定了基礎(chǔ)。

3.2 基于航向角的航姿系統(tǒng)算法驗(yàn)證

本實(shí)驗(yàn)搭建了ARM9+慣導(dǎo)+OEM板的航姿系統(tǒng)，慣導(dǎo)模塊為GY953的低成本航姿模塊（包含加速度計(jì)、陀螺儀及磁場(chǎng)感應(yīng)器，通過(guò)I2C接口與總線連接進(jìn)行導(dǎo)航數(shù)據(jù)的收發(fā)），原始數(shù)據(jù)為航姿參考系統(tǒng)被置于三軸轉(zhuǎn)臺(tái)上時(shí)GY953模塊內(nèi)置的經(jīng)典四元數(shù)算法直接解算的導(dǎo)航姿態(tài)角數(shù)值。

在實(shí)驗(yàn)室條件下轉(zhuǎn)動(dòng)三軸轉(zhuǎn)臺(tái)，不間斷地改變航姿系統(tǒng)的航向。本次實(shí)驗(yàn)選用基于擴(kuò)展Kalman濾波的自適應(yīng)Sage-Husa濾波和基于擴(kuò)展Kalman濾波的改進(jìn)自適應(yīng)Sage-Husa濾波2種算法對(duì)無(wú)人航姿系統(tǒng)進(jìn)行姿態(tài)解算。與GY953模塊的原始解算數(shù)據(jù)進(jìn)行比較，通過(guò)數(shù)據(jù)傳輸模塊及人機(jī)交互界面將實(shí)驗(yàn)室模擬飛行數(shù)據(jù)傳輸并保存在EXL表格中，然后將實(shí)際模擬飛行的EXL表格數(shù)據(jù)導(dǎo)入到Matlab中，進(jìn)行姿態(tài)仿真驗(yàn)證。本實(shí)驗(yàn)對(duì)無(wú)人飛行系統(tǒng)的姿態(tài)角（俯仰角、滾轉(zhuǎn)角及航向角）進(jìn)行算法數(shù)據(jù)仿真，并以航向角為例對(duì)其姿態(tài)誤差進(jìn)行了對(duì)比分析，具體分析結(jié)果如圖5～圖8所示。

圖5 俯仰角曲線Fig.5 Curves of pitch angle

圖6 滾轉(zhuǎn)角曲線Fig.6 Curves of roll angle

圖7 航向角曲線Fig.7 Curves of yaw angle

圖8 兩種方法的航向角誤差曲線Fig.8 Yaw angle error curves of the two methods

由圖5～圖8可以看出，自適應(yīng)Sage-Husa擴(kuò)展Kalman濾波算法誤差在-8°～7°之間。由于動(dòng)態(tài)性的影響，其算法誤差比單純的Matlab軟件仿真實(shí)驗(yàn)要大；而改進(jìn)的自適應(yīng)Sage-Husa算法的俯仰角誤差絕對(duì)值保持在0.2°以內(nèi)，滾轉(zhuǎn)角誤差絕對(duì)值均小于0.18°，航向角誤差最大絕對(duì)值也小于0.4°。其相比自適應(yīng)Sage-Husa算法的精度提高了一個(gè)數(shù)量級(jí)，可有效地對(duì)微小型無(wú)人機(jī)航姿進(jìn)行跟蹤。

4 結(jié)論

本文應(yīng)用的多傳感器信息融合（將陀螺儀輸出數(shù)據(jù)作為預(yù)測(cè)信息，將磁力計(jì)與加速度計(jì)輸出數(shù)據(jù)作為觀測(cè)信息）算法滿足了微小型無(wú)人機(jī)對(duì)低復(fù)雜度和高精度的需求。仿真實(shí)驗(yàn)和模擬飛行試驗(yàn)有效證明了基于EKF改進(jìn)的自適應(yīng)Sage-Husa算法在抑制濾波發(fā)散的同時(shí)提高了算法的穩(wěn)定性與實(shí)時(shí)性。此算法精度高、實(shí)時(shí)性強(qiáng)、計(jì)算量少且具有良好的工程應(yīng)用價(jià)值，可應(yīng)用于低成本、高精度的航姿參考平臺(tái)系統(tǒng)中。