孫淑光 陳建達

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

0 引言

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

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

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

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

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

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

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

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

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

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

1.2 慣性傳感器誤差方程

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

(1)

εb=εb+wg

(2)

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

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

(3)

(4)

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

1.3.1 狀態(tài)方程

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

X=[X1X2X3X4X5]T

(5)

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

(6)

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

(7)

(8)

(9)

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

(10)

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

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

(11)

(12)

(13)

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

(14)

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

(15)

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

(16)

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

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自適應(yīng)濾波

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

(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)

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

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點缺陷：1)實時估算時，每次運算都需要對噪聲的統(tǒng)計特性進行重復(fù)估計。迭代次數(shù)越多，濾波計算量越大，處理器負擔(dān)越重，信息輸出的實時性越差；2)不能同時動態(tài)估算量測噪聲協(xié)方差矩陣Rk與系統(tǒng)噪聲協(xié)方差矩陣Qk。

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

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

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

(33)

(34)

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

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

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

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

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

(35)

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

1.4.2 最佳遺忘因子b的選取

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

(36)

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

(37)

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

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

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

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

圖3 無人機飛行航跡圖

2.1 仿真條件設(shè)置

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

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

2.2 仿真結(jié)果分析

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

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

由圖4可知，改進后的調(diào)節(jié)因子的取值范圍是1.3<γ<3.5，分別選擇γ=1.3，γ=3.5作為固定調(diào)節(jié)因子參考值并進行對比實驗?？梢娝俣仍酱?，調(diào)節(jié)因子取值越小，濾波異常判定程度越嚴格。

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

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

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

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

表2 各方向均方根誤差值

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

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

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

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

圖8 位置誤差對比曲線

3 結(jié)論

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