改進(jìn)調(diào)節(jié)因子的GPS/SINS組合導(dǎo)航自適應(yīng)濾波算法*

孫淑光 陳建達(dá)

中國民航大學(xué)電子信息與自動(dòng)化學(xué)院，天津 300300

0 引言

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

捷聯(lián)式慣性導(dǎo)航系統(tǒng)(SINS)短期導(dǎo)航時(shí)間內(nèi)精度較高，但存在定位誤差隨時(shí)間積累的固有缺點(diǎn)[1]。全球定位系統(tǒng)(GPS)可以全天候、連續(xù)地提供精確的位置、速度信息，但動(dòng)態(tài)響應(yīng)能力較差，信號(hào)受遮擋效應(yīng)和多路徑效應(yīng)干擾明顯[2]，在高動(dòng)態(tài)復(fù)雜環(huán)境中單獨(dú)依賴GPS定位往往會(huì)出現(xiàn)導(dǎo)航短暫不可用的情況。無人機(jī)通常在山區(qū)林地運(yùn)行，環(huán)境復(fù)雜，飛行過程機(jī)動(dòng)性大，且要求設(shè)備成本低廉。因此，現(xiàn)代無人機(jī)多采用GPS/SINS組合導(dǎo)航的形式，通過優(yōu)勢互補(bǔ)，在提高導(dǎo)航定位精度的同時(shí),確保系統(tǒng)的魯棒性和穩(wěn)定性。

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

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

上述文獻(xiàn)通過不同的改進(jìn)方法提高了組合導(dǎo)航系統(tǒng)的整體性能，為了進(jìn)一步提高無人機(jī)在運(yùn)行中的自適應(yīng)性，本文提出一種基于速度信息改進(jìn)調(diào)節(jié)因子γ的Sage-Husa自適應(yīng)濾波方法。該算法可以克服傳統(tǒng)卡爾曼濾波在高動(dòng)態(tài)條件下濾波不穩(wěn)定的缺陷，增強(qiáng)組合導(dǎo)航算法的自適應(yīng)性，提高定位精度。

1 GPS/SINS組合導(dǎo)航系統(tǒng)建模

1.1 組合導(dǎo)航整體架構(gòu)

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

圖1 無人機(jī)組合導(dǎo)航方案

由圖1可知，首先利用經(jīng)過初始誤差補(bǔ)償?shù)膽T性測量元件數(shù)據(jù)進(jìn)行慣性導(dǎo)航解算，利用GPS模塊解算無人機(jī)的位置與速度信息，同時(shí)將GPS速度信息作為濾波算法中調(diào)節(jié)因子計(jì)算的要素，將GPS、SINS輸出的導(dǎo)航信息利用改進(jìn)的Sage-Husa自適應(yīng)濾波算法進(jìn)行數(shù)據(jù)融合，以獲取高精度定位信息。

1.2 慣性傳感器誤差方程

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

(1)

εb=εb+wg

(2)

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

加速度計(jì)組件模型表示為：

(3)

(4)

1.3 GPS/SINS組合導(dǎo)航方程

1.3.1 狀態(tài)方程

組合導(dǎo)航系統(tǒng)的狀態(tài)方程參量選擇SINS的位置誤差、速度誤差、姿態(tài)角誤差、陀螺儀漂移誤差及加速度計(jì)零偏誤差。誤差狀態(tài)向量為：

X=[X1X2X3X4X5]T

(5)

系統(tǒng)狀態(tài)方程表示為：

(6)

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

(7)

(8)

(9)

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

(10)

式中，fn為導(dǎo)航坐標(biāo)系中的比力信息。

姿態(tài)誤差角微分方程表示為：

(11)

(12)

(13)

F為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，表達(dá)式為：

(14)

式中，F(xiàn)ω為誤差狀態(tài)轉(zhuǎn)移矩陣，由系統(tǒng)狀態(tài)變量的微分方程推導(dǎo)得到

(15)

G為噪聲分配矩陣，表達(dá)式為：

(16)

W為系統(tǒng)噪聲，表達(dá)式為：

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

(17)

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

1.3.2 量測方程

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

(18)

式中：

(19)

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

(20)

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

傳統(tǒng)卡爾曼濾波主要適用于線性系統(tǒng)，常用離散化模型來進(jìn)行系統(tǒng)實(shí)現(xiàn)，需對狀態(tài)方程和量測方程進(jìn)行離散化處理，處理后可得：

(21)

式中，Φk,k-1表示離散系統(tǒng)狀態(tài)一步轉(zhuǎn)移矩陣；Γk-1表示離散系統(tǒng)噪聲分配矩陣；Wk-1表示離散系統(tǒng)噪聲矩陣；Hk表示離散系統(tǒng)量測轉(zhuǎn)移矩陣；Vk表示量測噪聲矩陣。

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

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

(22)

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

(23)

從理論上分析，利用卡爾曼濾波進(jìn)行組合導(dǎo)航解算時(shí)，其系統(tǒng)噪聲與量測噪聲被視為白噪聲處理，得到的估計(jì)并不是最優(yōu)估計(jì)。Sage-Husa自適應(yīng)濾波算法擺脫了傳統(tǒng)卡爾曼濾波對噪聲統(tǒng)計(jì)特性視為白噪聲的束縛，其實(shí)時(shí)估算噪聲統(tǒng)計(jì)特性，以此獲得狀態(tài)參數(shù)的最優(yōu)估計(jì)。

Sage-Husa自適應(yīng)濾波算法為：

(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自適應(yīng)濾波算法中，雖能解決傳統(tǒng)卡爾曼濾波由于將系統(tǒng)噪聲與量測噪聲視為白噪聲而造成的濾波不穩(wěn)定問題，但依舊存在2點(diǎn)缺陷：1)實(shí)時(shí)估算時(shí)，每次運(yùn)算都需要對噪聲的統(tǒng)計(jì)特性進(jìn)行重復(fù)估計(jì)。迭代次數(shù)越多，濾波計(jì)算量越大，處理器負(fù)擔(dān)越重，信息輸出的實(shí)時(shí)性越差；2)不能同時(shí)動(dòng)態(tài)估算量測噪聲協(xié)方差矩陣Rk與系統(tǒng)噪聲協(xié)方差矩陣Qk。

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

1.4.1 調(diào)節(jié)因子γ的改進(jìn)

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

(33)

(34)

式中，γ為調(diào)節(jié)因子，該值反映了判定濾波異常的嚴(yán)格程度。vk為新息序列，tr表示矩陣的跡。一般情況下，γ=1時(shí)濾波異常判別嚴(yán)格程度達(dá)到最大，此時(shí)可獲取最優(yōu)遺忘因子取值。

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

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

改進(jìn)的調(diào)節(jié)因子表示為：

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

(35)

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

1.4.2 最佳遺忘因子b的選取

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

(36)

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

(37)

改進(jìn)的Sage-Husa算法流程如圖2所示。

圖2 改進(jìn)調(diào)節(jié)因子的濾波算法流程

2 算法仿真及結(jié)果分析

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

圖3 無人機(jī)飛行航跡圖

2.1 仿真條件設(shè)置

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

表1 仿真參數(shù)設(shè)置

2.2 仿真結(jié)果分析

通過仿真對比不同調(diào)節(jié)因子取值結(jié)果，確定改進(jìn)方法的效果。同時(shí)采用傳統(tǒng)卡爾曼濾波與改進(jìn)的Sage-Husa自適應(yīng)濾波分別進(jìn)行信息融合，進(jìn)一步驗(yàn)證改進(jìn)算法的可行性。改進(jìn)后的調(diào)節(jié)因子γ如圖4所示。

圖4 調(diào)節(jié)因子變化曲線

由圖4可知，改進(jìn)后的調(diào)節(jié)因子的取值范圍是1.3<γ<3.5，分別選擇γ=1.3，γ=3.5作為固定調(diào)節(jié)因子參考值并進(jìn)行對比實(shí)驗(yàn)。可見速度越大，調(diào)節(jié)因子取值越小，濾波異常判定程度越嚴(yán)格。

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

圖5 γ=3.5及改進(jìn)調(diào)節(jié)因子算法位置誤差曲線

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

圖6 γ=1.3及改進(jìn)調(diào)節(jié)因子算法位置誤差曲線

表2 各方向均方根誤差值

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

由圖7可知，改進(jìn)調(diào)節(jié)因子的整體效果優(yōu)于取定值時(shí)的仿真結(jié)果。調(diào)節(jié)因子γ=3.5時(shí)，其均方根誤差相較于改進(jìn)后調(diào)節(jié)因子，整體偏大。調(diào)節(jié)因子γ=1.3時(shí)，濾波嚴(yán)格程度提高，但無人機(jī)出現(xiàn)加速或減速等飛行速度變化時(shí)，均方根誤差增大，在以上時(shí)間段內(nèi)的濾波效果并不理想，而通過動(dòng)態(tài)選取調(diào)節(jié)因子，達(dá)到的濾波效果較好。

為進(jìn)一步證明改進(jìn)后的Sage-Husa自適應(yīng)濾波算法的正確性，分別采用2種數(shù)據(jù)融合算法進(jìn)行仿真實(shí)驗(yàn)。

從圖8可以看出，傳統(tǒng)卡爾曼濾波具有一定的穩(wěn)定性，但改進(jìn)調(diào)節(jié)因子的濾波算法誤差則趨近在一個(gè)更小的范圍內(nèi)。以東方向?yàn)槔?，分別采用2種數(shù)據(jù)融合算法時(shí)，均方根誤差分別為2.85和1.32。仿真結(jié)果表明，雖然傳統(tǒng)卡爾曼濾波能有效克服SINS誤差累積的問題，但改進(jìn)后調(diào)節(jié)因子的濾波算法濾波自適應(yīng)能力更強(qiáng)，定位效果更好。

圖8 位置誤差對比曲線

3 結(jié)論

針對無人機(jī)高動(dòng)態(tài)復(fù)雜情況下的導(dǎo)航需求，提出了改進(jìn)調(diào)節(jié)因子的Sage-Husa自適應(yīng)濾波算法。利用GPS速度信息約束改進(jìn)算法調(diào)節(jié)因子，實(shí)時(shí)估計(jì)修正量測噪聲協(xié)方差陣，以此調(diào)節(jié)狀態(tài)增益，使之更好地適應(yīng)當(dāng)前濾波。仿真結(jié)果表明，相較于傳統(tǒng)數(shù)據(jù)融合算法，改進(jìn)的濾波算法可獲得更穩(wěn)定的定位結(jié)果，解決了傳統(tǒng)卡爾曼濾波不能準(zhǔn)確測量噪聲統(tǒng)計(jì)特性導(dǎo)致濾波不穩(wěn)定的問題，降低了自適應(yīng)濾波算法的計(jì)算負(fù)荷，為高動(dòng)態(tài)環(huán)境下的無人機(jī)組合導(dǎo)航提供了較好的解決方案。