改進調節因子的GPS/SINS組合導航自適應濾波算法*

孫淑光 陳建達

中國民航大學電子信息與自動化學院，天津 300300

0 引言

隨著微機電系統(MEMS)技術的發展，集陀螺儀和加速度計于一體的微慣性測量單元(MIMU)在導航領域得到廣泛應用。受價格因素制約，低成本傳感器導航方式成為無人機市場中主要的應用需求。

捷聯式慣性導航系統(SINS)短期導航時間內精度較高，但存在定位誤差隨時間積累的固有缺點[1]。全球定位系統(GPS)可以全天候、連續地提供精確的位置、速度信息，但動態響應能力較差，信號受遮擋效應和多路徑效應干擾明顯[2]，在高動態復雜環境中單獨依賴GPS定位往往會出現導航短暫不可用的情況。無人機通常在山區林地運行，環境復雜，飛行過程機動性大，且要求設備成本低廉。因此，現代無人機多采用GPS/SINS組合導航的形式，通過優勢互補，在提高導航定位精度的同時,確保系統的魯棒性和穩定性。

傳統的組合導航方式通常采用卡爾曼濾波作為數據融合的方法，綜合系統狀態估計信息和量測信息，通過濾波算法獲取最優估計。傳統卡爾曼濾波將傳感器誤差特性描述為靜態白噪聲，但實際上，隨著運行時間及工作環境的變化，傳感器噪聲的數學統計特性(均值、方差)是時變的，基于靜態特征的噪聲表征方式不能很好地反映目標傳感器的噪聲特性[3]。在高動態運行情況下，會導致無人機濾波不穩定，甚至產生發散現象。針對這一問題，一些自適應濾波算法應運而生。

文獻[4]通過調整不同定位精度下GNSS測量的噪聲協方差，并基于機器學習和模糊邏輯[5]方法獲得相比于傳統卡爾曼濾波更好的定位精度。文獻[6]采用三段式方法評估自適應比例因子，與其它現有算法相比，其在靜態和動態條件下均能有效減少漂移和隨機噪聲帶來的影響。文獻[7]為提高MEMS陀螺儀的估計性能，提出一種加權魯棒Sage-Husa自適應濾波算法。該算法利用一種基于殘差估計的方法，減少了異常值對角速度估計的影響。文獻[8]提出了一種基于滑動衰減因子的Sage-Husa自適應組合導航算法。該算法根據量測噪聲統計特性的變化，選擇合適的衰減因子，以適應實際噪聲變化帶來的影響。

上述文獻通過不同的改進方法提高了組合導航系統的整體性能，為了進一步提高無人機在運行中的自適應性，本文提出一種基于速度信息改進調節因子γ的Sage-Husa自適應濾波方法。該算法可以克服傳統卡爾曼濾波在高動態條件下濾波不穩定的缺陷，增強組合導航算法的自適應性，提高定位精度。

1 GPS/SINS組合導航系統建模

1.1 組合導航整體架構

無人機組合導航系統采用包含三軸陀螺儀和三軸加速度計MIMU模塊和GPS接收模塊的集成傳感元件，通過組合導航算法進行數據融合，整體分為慣性導航模塊、GPS模塊以及數據融合模塊3部分。具體方案如圖1所示。

圖1 無人機組合導航方案

由圖1可知，首先利用經過初始誤差補償的慣性測量元件數據進行慣性導航解算，利用GPS模塊解算無人機的位置與速度信息，同時將GPS速度信息作為濾波算法中調節因子計算的要素，將GPS、SINS輸出的導航信息利用改進的Sage-Husa自適應濾波算法進行數據融合，以獲取高精度定位信息。

1.2 慣性傳感器誤差方程

陀螺儀與加速度計分別為載體的角運動與線運動測量元件，兩者本身存在誤差且誤差存在方式相似，主要包括常值誤差以及白噪聲誤差。陀螺組件模型表示為：

(1)

εb=εb+wg

(2)

式中，εb為常值漂移，wg為白噪聲誤差。

加速度計組件模型表示為：

(3)

(4)

1.3 GPS/SINS組合導航方程

1.3.1 狀態方程

組合導航系統的狀態方程參量選擇SINS的位置誤差、速度誤差、姿態角誤差、陀螺儀漂移誤差及加速度計零偏誤差。誤差狀態向量為：

X=[X1X2X3X4X5]T

(5)

系統狀態方程表示為：

(6)

位置誤差微分方程表示為：

(7)

(8)

(9)

速度誤差微分方程表示為：

(10)

式中，fn為導航坐標系中的比力信息。

姿態誤差角微分方程表示為：

(11)

(12)

(13)

F為系統狀態轉移矩陣，表達式為：

(14)

式中，Fω為誤差狀態轉移矩陣，由系統狀態變量的微分方程推導得到

(15)

G為噪聲分配矩陣，表達式為：

(16)

W為系統噪聲，表達式為：

W=[ωgxωgyωgzωaxωayωaz]T

(17)

式中，ωgx,ωgy,ωgz為陀螺儀白噪聲；ωax,ωay,ωaz為加速度計白噪聲。

1.3.2 量測方程

SINS解算出的位置為(LI,λI,hI)，速度為(veI,vnI,vuI)，GPS計算出的位置為(LG,λG,hG)，速度為(veG,vnG,vuG)。量測方程表示為：

(18)

式中：

(19)

量測噪聲矩陣V可表示為：

(20)

1.4 改進的Sage-Husa自適應濾波

傳統卡爾曼濾波主要適用于線性系統，常用離散化模型來進行系統實現，需對狀態方程和量測方程進行離散化處理，處理后可得：

(21)

式中，Φk,k-1表示離散系統狀態一步轉移矩陣；Γk-1表示離散系統噪聲分配矩陣；Wk-1表示離散系統噪聲矩陣；Hk表示離散系統量測轉移矩陣；Vk表示量測噪聲矩陣。

假定系統噪聲Wk與量測噪聲Vk均值分別為qk和rk，即：

E[Wk]=qk,Cov(Wj,Wk)=Qkδjk

(22)

E[Vk]=rk,Cov(Vj,Vk)=Rkδjk

(23)

從理論上分析，利用卡爾曼濾波進行組合導航解算時，其系統噪聲與量測噪聲被視為白噪聲處理，得到的估計并不是最優估計。Sage-Husa自適應濾波算法擺脫了傳統卡爾曼濾波對噪聲統計特性視為白噪聲的束縛，其實時估算噪聲統計特性，以此獲得狀態參數的最優估計。

Sage-Husa自適應濾波算法為：

(24)

(25)

(26)

(27)

(28)

Pk=(I-KkHk)Pk,k-1

(29)

(30)

(31)

dk=(1-b)/(1-bk+1)

(32)

式中，b為遺忘因子，一般取值在(0.95,0.99)。

在Sage-Husa自適應濾波算法中，雖能解決傳統卡爾曼濾波由于將系統噪聲與量測噪聲視為白噪聲而造成的濾波不穩定問題，但依舊存在2點缺陷：1)實時估算時，每次運算都需要對噪聲的統計特性進行重復估計。迭代次數越多，濾波計算量越大，處理器負擔越重，信息輸出的實時性越差；2)不能同時動態估算量測噪聲協方差矩陣Rk與系統噪聲協方差矩陣Qk。

針對上述問題，引入濾波異常判定條件來減少非必要的估計次數。另外，上述基于新息序列vk的自適應濾波算法，主要解決外部量測噪聲強度發生不確定變化的問題[9]，所以本文將在系統噪聲協方差矩陣Qk已知的情況下估測量測噪聲方差矩陣Rk。

1.4.1 調節因子γ的改進

引入濾波異常判定條件[10]：

(33)

(34)

式中，γ為調節因子，該值反映了判定濾波異常的嚴格程度。vk為新息序列，tr表示矩陣的跡。一般情況下，γ=1時濾波異常判別嚴格程度達到最大，此時可獲取最優遺忘因子取值。

如果在濾波過程中，各參量迭代滿足式(33)，則認定為濾波異常，說明先前的模型已不適用于當前的濾波。通過更新校正量測噪聲協方差陣Rk，使其適應模型變化對濾波產生的影響，以提高系統的自適應性，增強濾波效果。

速度信息反映無人機在不同階段內的飛行動態，因此，可以利用GPS輸出的速度信息改進調節因子γ。在無人機飛行狀態下，速度越大，飛行阻力越大[11]，與低速飛行相比，高速飛行無人機參數時變性更高[12]，更容易受工作環境和模式的影響[13]，也就更需要可靠的導航信息滿足其飛行需求。因此，可以根據無人機飛行速度的大小來決定調節因子的取值。速度越大，調節因子取值越小，異常判據條件越嚴格。

改進的調節因子表示為：

γ=1+exp(-(a1vh+b1))

(35)

式中，vh為飛行速度，a1,b1為輔助調節因子，a1>0，兩者取值可根據實際濾波異常判別嚴格程度設定。需要注意的是，調節因子的取值不能過大，否則濾波異常判別嚴格程度過小，引入濾波異常判定條件(式33)就無意義。上述改進過程選取指數函數形式，使得調節因子變化比較平緩，避免了由于速度急劇變化導致的調節因子大范圍波動造成的濾波異常判別嚴格程度不足或者過嚴的問題。

1.4.2 最佳遺忘因子b的選取

為獲取最佳遺忘因子b的取值，選取最嚴格濾波異常判定條件(γ=1)。根據式(33)和(34)可得以下關系：

(36)

滿足式(36)則證明濾波異常，將新息序列vk、一步預測協方差陣Pk,k-1、量測協方差陣Rk代入式(33),可獲得最佳遺忘因子取值[14]。

(37)

改進的Sage-Husa算法流程如圖2所示。

圖2 改進調節因子的濾波算法流程

2 算法仿真及結果分析

為分析上述方法的可行性與正確性，利用模擬飛行數據進行了仿真實驗。主要包括無人機三維航跡仿真模塊、改進調節因子仿真以及導航傳感器數據融合仿真模塊。其中，三維航跡仿真模塊模擬無人機實際飛行的理想位置和速度；改進調節因子仿真部分根據無人機動態數據改進調節因子，實時判定濾波是否異常；數據融合模塊輸出濾波校正后的位置信息。三維航跡仿真主要包括：短時間滑跑、加速爬升、定高飛行、轉彎、減速飛行和俯沖等階段。無人機飛行航跡如圖3所示。

圖3 無人機飛行航跡圖

2.1 仿真條件設置

仿真以東北天坐標系作為導航坐標系， 起始位置設置為(39.10167° 117.34531° 100m)，仿真時間1000s，周期1s,傳感器參數設置見表1。

表1 仿真參數設置

2.2 仿真結果分析

通過仿真對比不同調節因子取值結果，確定改進方法的效果。同時采用傳統卡爾曼濾波與改進的Sage-Husa自適應濾波分別進行信息融合，進一步驗證改進算法的可行性。改進后的調節因子γ如圖4所示。

圖4 調節因子變化曲線

由圖4可知，改進后的調節因子的取值范圍是1.3<γ<3.5，分別選擇γ=1.3，γ=3.5作為固定調節因子參考值并進行對比實驗。可見速度越大，調節因子取值越小，濾波異常判定程度越嚴格。

仿真模塊對基于速度信息改進調節因子方法的濾波結果與固定調節因子(γ=3.5和γ=1.3)的濾波結果進行了對比，分別如圖5～6中所示。

圖5 γ=3.5及改進調節因子算法位置誤差曲線

與未引入濾波異常判定條件相比，改進后的濾波算法仿真迭代次數減少了164次，有效減少了計算量。從圖5中看出，調節因子γ=3.5時，固定調節因子算法濾波效果相較于靈活改進調節因子的濾波效果整體偏差。從圖6看出，調節因子γ=1.3時，兩者濾波效果整體上相當，但隨著無人機速度的變化，固定調節因子算法出現了濾波不穩定現象，誤差值甚至超過了相同時間段γ=3.5時的誤差值。該現象主要集中在1s～40s，440s～480s和620s～640s的飛行時間內，此時飛行狀態包括無人機兩次加速爬升和一次定高減速飛行。以上結果可以看出，隨無人機的飛行動態改變調節因子的取值可以有效確保濾波精度，并能在滿足導航性能要求的前提下降低濾波計算量。圖7與表2表示均方根誤差情況。

圖6 γ=1.3及改進調節因子算法位置誤差曲線

表2 各方向均方根誤差值

圖7 東方向均方根誤差曲線

由圖7可知，改進調節因子的整體效果優于取定值時的仿真結果。調節因子γ=3.5時，其均方根誤差相較于改進后調節因子，整體偏大。調節因子γ=1.3時，濾波嚴格程度提高，但無人機出現加速或減速等飛行速度變化時，均方根誤差增大，在以上時間段內的濾波效果并不理想，而通過動態選取調節因子，達到的濾波效果較好。

為進一步證明改進后的Sage-Husa自適應濾波算法的正確性，分別采用2種數據融合算法進行仿真實驗。

從圖8可以看出，傳統卡爾曼濾波具有一定的穩定性，但改進調節因子的濾波算法誤差則趨近在一個更小的范圍內。以東方向為例，分別采用2種數據融合算法時，均方根誤差分別為2.85和1.32。仿真結果表明，雖然傳統卡爾曼濾波能有效克服SINS誤差累積的問題，但改進后調節因子的濾波算法濾波自適應能力更強，定位效果更好。

圖8 位置誤差對比曲線

3 結論

針對無人機高動態復雜情況下的導航需求，提出了改進調節因子的Sage-Husa自適應濾波算法。利用GPS速度信息約束改進算法調節因子，實時估計修正量測噪聲協方差陣，以此調節狀態增益，使之更好地適應當前濾波。仿真結果表明，相較于傳統數據融合算法，改進的濾波算法可獲得更穩定的定位結果，解決了傳統卡爾曼濾波不能準確測量噪聲統計特性導致濾波不穩定的問題，降低了自適應濾波算法的計算負荷，為高動態環境下的無人機組合導航提供了較好的解決方案。