改進的Sage?Husa自適應濾波算法在MEMS航姿參考系統中的應用

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

（1.南京航空航天大學自動化學院，南京210016；2.南京航空航天大學無人機研究院，南京210016）

0 引言

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

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

1 定姿方案

1.1 整體定姿方案

如圖1所示，本文利用三軸陀螺儀估計的姿態角作為過程信息，將磁力計與加速度計估算的角度作為量測信息，將各個傳感器的優劣勢進行互補，建立了改進的Sage-Husa自適應Kalman濾波算法，對融合多傳感器信息的航姿參考系統進行無人機姿態的最優估計。

圖1 定姿總體結構圖Fig.1 Overall structure diagram of attitude determination

1.2 三軸陀螺儀定姿算法

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

基于工程需求，為了消除Euler角解算無人飛行系統姿態而產生的奇點，由四元素法得到的矩陣坐標變換矩陣如下式所示

此時，得到四元素微分向量方程為

采用四階Runge-Kutta法求解四元素微分方程，令T為更新步長，計算得到量測更新K陣與Q陣的更新表達式為

四階Runge-Kutta法吸取了Taylor展開法中精度高的理念，同時摒棄了高階求導的難點，令數值結果緊湊而對稱。四階Runge-Kutta法是解算微分方程的有利工具，具有很好的工程意義。將式（5）代入Euler角解算式，可得出無人機姿態角

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

1.3 磁力計和加速度計姿態更新算法

當運動載體處于靜止或勻速運動狀態時，磁力計和加速度計所采集到的信息濾波經處理后可得到姿態角信息為磁力計與加速度計的估計誤差間的差值，故有

運動載體為任意姿態時，假設測量運動加速度在三軸坐標系上的表達式為

令g0為當地重力加速度，則有

其中，Cbn為捷聯式慣導的方向余弦矩陣，從而可解算出俯仰角與橫滾角

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

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

運動載體的姿態是不斷變化的，故量測系統存在俯仰角與滾轉角姿態的修正

2 改進Sage-Husa自適應濾波算法

2.1 簡化Sage-Husa自適應擴展Kalman濾波

擴展Kalman濾波算法在其被使用之前就假定系統的噪聲統計特性為已知，而在實際的應用中，無人飛行系統的系統噪聲的方差值和由觀測得到的噪聲誤差會隨時間而變動，擴展Kalman濾波算法會使姿態解算值產生發散。Sage-Husa自適應擴展濾波算法的核心是實時運用量測更新的數據對系統和量測噪聲的特性進行動態的統計估算，并最終解算出所求的狀態參數的預測值，減少由模型誤差參數帶來的影響［16］。

Sage-Husa自適應擴展濾波算法無法在系統和量測噪聲都未知的條件下進行估算，本文將在系統噪聲矩陣已知的情況下估測量測噪聲方差矩陣。基于三軸陀螺儀計算所得的狀態方程與磁力計、加速度計描述的量測方程，得到多傳感器融合的簡化Sage-Husa自適應擴展濾波算法［17］。該算法本身已消除了傳感源自有的錯誤，其算法更新的基本步驟如式（13）與式（14）所示。

具有時變性能的噪聲估計器如下

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

2.2 Sage-Husa自適應濾波算法的改進

理論而言，Sage-Husa自適應濾波算法可以同時在線估計出系統的噪聲＾Qk及量測噪聲＾Rk。但是，ek的異常改變會影響＾Qk和＾Rk的計算準確性，從而得到不精確的濾波效果。在實際工程應用中，Sage-Husa自適應擴展Kalman濾波實現性強、精確度高，但其計算量較大，并且魯棒性較差。因此，對Sage-Husa自適應濾波算法進行改進，顯得尤為重要［19］。

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

在Sage-Husa自適應濾波算法中，遺忘因子b選擇的恰當與否直接關系到濾波的精度及濾波是否會出現發散現象。很多學者利用試湊法和最優估計算法，將每次迭代得到的遺忘因子解算出來，從而得到最優的濾波算法。為了對濾波狀況進行判斷，本文采用了預測殘差作為監測的方法。若濾波出現發散，則有

當儲備系數ξ的取值為1時，可得到最佳的濾波估計效果。將式（15）代入式（14），可求取遺忘因子的最優值

綜上所述， 式（14）與式（16）的組合構成了由最佳遺忘因子改進的Sage-Husa自適應濾波算法，提高了濾波的魯棒性。

（2）抑制Sage-Husa自適應濾波的發散

Sage-Husa自適應濾波是一種具有次優性質的濾波，次優濾波算法在運用一步預測值和濾波值來替代全平滑值時將可能得到精度較差的噪聲統計特性。為了使得Qk與Rk滿足無偏估計，可在式（14）中采用兩式相減的算法，這將會致使Qk和Rk失去正定性與對稱性。以上兩種不足將會引發濾波結果發散，因而本文將采用下面兩種方法抑制濾波發散。

①使用有偏估計，進而保證Qk和Rk的對稱性與正定性

對＾Qk和＾Rk進行實時監控，如果＾Rk為正定矩陣，＾Qk為半正定或正定矩陣，則利用式（14）進行求解，否則利用式（17）進行求解。

②通過調整和修正Pk/k-1，進而調控增益矩陣Kk， 來防止出現濾波發散現象

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

其中，Sk為濾波加權系數，其取值為1。

3 實驗驗證

3.1 Matlab仿真驗證

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

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

圖3 滾轉角誤差曲線Fig.3 Curves of roll angle error

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

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

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

3.2 基于航向角的航姿系統算法驗證

本實驗搭建了ARM9+慣導+OEM板的航姿系統，慣導模塊為GY953的低成本航姿模塊（包含加速度計、陀螺儀及磁場感應器，通過I2C接口與總線連接進行導航數據的收發），原始數據為航姿參考系統被置于三軸轉臺上時GY953模塊內置的經典四元數算法直接解算的導航姿態角數值。

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

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

圖6 滾轉角曲線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可以看出，自適應Sage-Husa擴展Kalman濾波算法誤差在-8°～7°之間。由于動態性的影響，其算法誤差比單純的Matlab軟件仿真實驗要大；而改進的自適應Sage-Husa算法的俯仰角誤差絕對值保持在0.2°以內，滾轉角誤差絕對值均小于0.18°，航向角誤差最大絕對值也小于0.4°。其相比自適應Sage-Husa算法的精度提高了一個數量級，可有效地對微小型無人機航姿進行跟蹤。

4 結論

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