引入系統(tǒng)不確定度的模糊IAE-UKF組合導(dǎo)航

邢東峰，張琳婧*，楊菊花，陳光武，劉 昊

（1.蘭州交通大學(xué)自動(dòng)控制研究所，甘肅蘭州730070；2.蘭州交通大學(xué)甘肅省高原交通信息工程及控制重點(diǎn)實(shí)驗(yàn)室，甘肅蘭州730070；3.蘭州交通大學(xué)交通運(yùn)輸學(xué)院，甘肅蘭州730070）

1 引 言

在交通應(yīng)用中有兩種較廣泛使用的位置服務(wù)方法，分別為全球定位系統(tǒng)（Global Positioning System，GPS）和慣性導(dǎo)航系統(tǒng)（Inertial Naviga?tion System，INS），GPS/INS組合系統(tǒng)具有互補(bǔ)性，從而比任何一個(gè)獨(dú)立系統(tǒng)都能夠提供更加準(zhǔn)確和強(qiáng)大的導(dǎo)航方案［1］。隨之而來的問題即為如何選擇組合結(jié)構(gòu)與組合方法，針對低成本組合系統(tǒng)，松耦合的組合結(jié)構(gòu)一般滿足需求，即可將問題整合轉(zhuǎn)化為非線性濾波［2］。由于任何系統(tǒng)都存在不同程度的非線性，用線性濾波處理則代表默認(rèn)忽視了實(shí)際情況中的非線性，尤其是對低精度傳感器構(gòu)成的組合導(dǎo)航系統(tǒng)。將線性處理非線性系統(tǒng)導(dǎo)致的一系列問題歸結(jié)為系統(tǒng)模型建立不準(zhǔn)確的問題，運(yùn)載體的類型、所處環(huán)境、機(jī)動(dòng)性大小、精度需求的高低等對其都有影響，需要分別對待具體問題，所以目前并沒有一個(gè)統(tǒng)一的處理方式，但總是有跡可循的。組合導(dǎo)航主要解決的是隨時(shí)間逼近和跟蹤的目標(biāo)點(diǎn)連續(xù)定位問題［3］，在傳感器數(shù)據(jù)融合層的過程中，每一次的遞推過程都被系統(tǒng)的不確定性影響。無論是何種濾波算法，它們正常運(yùn)行的假設(shè)為系統(tǒng)模型是準(zhǔn)確的。

擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）將非線性進(jìn)行近似線性處理［4］，雅可比矩陣推導(dǎo)過程較繁瑣。無跡卡爾曼濾波（Unscent?ed Kalman Filter，UKF）不需要推導(dǎo)雅可比矩陣且精度較高［5］，UKF求解預(yù)測值和量測值時(shí)需要應(yīng)用系統(tǒng)模型的采樣點(diǎn)信息，但系統(tǒng)模型采樣點(diǎn)初始信息的不確定性容易導(dǎo)致濾波器發(fā)散，從而增大狀態(tài)估計(jì)誤差，所以如何量化并限制系統(tǒng)的不確定性是重點(diǎn)。使用較廣泛的克服系統(tǒng)不確定性的方法有強(qiáng)跟蹤、漸消遺忘因子、模糊控制、集成控制等［6-9］方法。在不確定性因素的影響下，一致的狀態(tài)估計(jì)是濾波估計(jì)器有效工作的重要保證，并且要考慮是否存在過度收斂。交互多模型算法的噪聲匹配［10］與新息估計(jì)［11］都可以使得自適應(yīng)程度達(dá)到最佳，前者的計(jì)算量隨著預(yù)設(shè)模型增多而增大。對此，本文基于濾波器的量測輸出中隱含關(guān)于系統(tǒng)模型信息的事實(shí)［12］，引入了新息自適應(yīng)估計(jì)（Innovation Adaptive Estimation，IAE）方法［13］控制濾波穩(wěn)定。

針對低成本組合導(dǎo)航系統(tǒng)，在載體機(jī)動(dòng)性變化時(shí)，系統(tǒng)該時(shí)刻的不確定性不同于系統(tǒng)最初建模時(shí)的不確定性，當(dāng)改變后的不確定性作用于系統(tǒng)，會導(dǎo)致濾波精度下降，同時(shí)濾波器工作穩(wěn)定性降低。不同于將組合導(dǎo)航建模處理為預(yù)設(shè)、評估或者假設(shè)、檢驗(yàn)的問題，本文將建模問題設(shè)定為判斷、明確、控制的問題，不需要精確的數(shù)學(xué)模型且便于實(shí)時(shí)處理。

2 系統(tǒng)不確定度

由組合導(dǎo)航濾波模型［14］，系統(tǒng)的觀測值為為一步預(yù)測，得到預(yù)測殘差如式（1）：

其中，rk的本質(zhì)為濾波觀測量與狀態(tài)一步預(yù)測之差，包含量測與系統(tǒng)模型信息，通常稱其為新息。當(dāng)系統(tǒng)是一致可觀與可控時(shí)，濾波被認(rèn)為處于穩(wěn)定狀態(tài)，此時(shí)的新息序列為零均值、方差如式（2）的平穩(wěn)白噪聲［12］，并且服從δrk～N(0，σ)的正態(tài)分布，將式（2）歸一化得式（3）：

從新息的角度給出濾波器不穩(wěn)定工作時(shí)的解釋，歸結(jié)為兩種情況：

（1）由系統(tǒng)模型不準(zhǔn)確所引起的狀態(tài)估計(jì)偏差，具體表現(xiàn)在對系統(tǒng)噪聲方差陣Q的估計(jì)，此時(shí)的預(yù)測誤差方差陣Pk/k?1失去了反應(yīng)狀態(tài)估計(jì)值的真實(shí)性（標(biāo)準(zhǔn)卡爾曼濾波將量測噪聲方差的統(tǒng)計(jì)特性看作固定值）；

（2）由外部觀測噪聲不準(zhǔn)確所引起的觀測精度下降（包含量測野值與量測噪聲時(shí)變兩種情況），具體表現(xiàn)為對量測噪聲方差陣R的估計(jì)，當(dāng)新息序列突變，此時(shí)的濾波增益Kk會使遞推值偏離狀態(tài)估計(jì)真實(shí)值，導(dǎo)致出現(xiàn)系統(tǒng)發(fā)散。

針對情況（1），本文首先明確系統(tǒng)模型的準(zhǔn)確度，然后根據(jù)新息含義對系統(tǒng)模型進(jìn)行不確定度的估計(jì)，最后依據(jù)不確定度設(shè)計(jì)模糊隸屬度函數(shù)進(jìn)行系統(tǒng)噪聲方差的在線調(diào)整。針對情況（2），本文為使得量測在濾波過程中一直保持穩(wěn)定且有界，引入IAE方法對量測噪聲方差R進(jìn)行自適應(yīng)估計(jì)，克服量測突變導(dǎo)致的濾波發(fā)散。最后探討同時(shí)調(diào)整Q與R的成立條件，嵌入U(xiǎn)KF算法框架中，實(shí)現(xiàn)組合導(dǎo)航。

由新息含義式（1），給定系統(tǒng)不確定性的量化參數(shù)γ：

其中，γ為估計(jì)量殘差與理論殘差期望的比值，將γ作為濾波是否發(fā)散的判據(jù)，γ的理論比值接近1［15］，當(dāng)γ異常大時(shí)，則判定濾波發(fā)散。新息期望如式（5）：

則根據(jù)式（4）與式（6）可得參數(shù)γ。當(dāng)γ增大時(shí)，系統(tǒng)不確定性增加，當(dāng)γ減小時(shí)，系統(tǒng)不確定性減小。若預(yù)先給定參數(shù)的上下界，當(dāng)γ<γmin時(shí)，系統(tǒng)不確定程度可接受，認(rèn)為系統(tǒng)建模準(zhǔn)確；當(dāng)γmin≤γ≤γmax時(shí)，系統(tǒng)不確定度較大，系統(tǒng)模型較為準(zhǔn)確；當(dāng)γ>γmax時(shí)，量測被認(rèn)為失去真實(shí)性，系統(tǒng)模型不準(zhǔn)確。由于γmin與γmax一般根據(jù)經(jīng)驗(yàn)給定，同樣的經(jīng)驗(yàn)值在不同情況下可能有不同效果，在此引入平滑加權(quán)殘差平方（Smoothed Weighted Residual Squared，SWRS）思想［16］，定義系統(tǒng)不確定度Uk，并給出量化式如式（7）所示：

定義系統(tǒng)初始不確定度U1=1，α為自適應(yīng)因子，取α∈(0，1)，取γ0=max[1?α，γ]。Uk在自適應(yīng)因子α的作用下被膨脹化；殘差項(xiàng)rTkrk在量化參數(shù)γ0作權(quán)值的處理下保證Uk的準(zhǔn)確性。即Uk利用新息的統(tǒng)計(jì)特性，擁有表示系統(tǒng)模型不準(zhǔn)確度的含義，并為噪聲的在線估計(jì)提供計(jì)算基礎(chǔ)。區(qū)別于只對狀態(tài)模型進(jìn)行評估的方法，引入的新息序列包含狀態(tài)與量測的信息，可以較全面地表示此刻系統(tǒng)模型準(zhǔn)確度。當(dāng)系統(tǒng)工作不穩(wěn)定時(shí)，γ偏離1，系統(tǒng)殘差項(xiàng)較大，此時(shí)的Uk在權(quán)值作用下被修正；當(dāng)系統(tǒng)工作穩(wěn)定時(shí)，γ接近1，誤差項(xiàng)量值較小，Uk保持穩(wěn)定并趨于平滑。Uk越小代表系統(tǒng)模型越準(zhǔn)確，Uk越大，代表系統(tǒng)模型越不準(zhǔn)確，從而可以將Uk作為調(diào)整模型的依據(jù)，作用于增強(qiáng)濾波。

3 IAE算法

調(diào)整系統(tǒng)模型的過程實(shí)際上為對噪聲進(jìn)行實(shí)時(shí)估計(jì)的過程，由于UKF要求量測噪聲先驗(yàn)已知，所以本文基于新息的統(tǒng)計(jì)特性，引入滑動(dòng)窗口的新息自適應(yīng)估計(jì)方法。定義滑動(dòng)窗口的寬度區(qū)間N∈[Nmin，Nmax]，滑動(dòng)窗口的IAE算法步驟描述為：

Step 1：計(jì)算狀態(tài)的一步預(yù)測量與狀態(tài)協(xié)方差的一步預(yù)測量；

Step 2：根據(jù)Nk?1計(jì)算修正量測噪聲矩陣系數(shù)；

Step 3：梯度檢測量測噪聲矩陣系數(shù)在線更新Nk；

Step 4：計(jì)算濾波增益Kk，更新狀態(tài)估計(jì)與狀態(tài)協(xié)方差。

滑動(dòng)窗口IAE算法相比較于傳統(tǒng)IAE算法，增加了滑動(dòng)窗口N的計(jì)算選取環(huán)節(jié)。對計(jì)算的準(zhǔn)確性描述，N值越大，C?rk的估計(jì)值越準(zhǔn)確，同時(shí)自適應(yīng)靈敏度降低，所以N值的選取需要在準(zhǔn)確度與跟蹤靈敏度中取得平衡，從而更為準(zhǔn)確；對計(jì)算復(fù)雜度進(jìn)行量化分析，滑動(dòng)窗口主要增加了梯度檢測函數(shù)和N值的實(shí)時(shí)計(jì)算，梯度檢測函數(shù)中涉及到對角陣矩陣的求逆運(yùn)算，可以將其轉(zhuǎn)換為除法運(yùn)算。所以，相較于傳統(tǒng)新息自適應(yīng)算法，滑動(dòng)窗口IAE算法所增加的計(jì)算量并不大，并且滑動(dòng)窗口的優(yōu)勢可以在濾波靈敏度與準(zhǔn)確度中做出較好的取舍［17］。具體表現(xiàn)在序列長度為N的采樣區(qū)間內(nèi)，歷元新息方差的N值隨著每一次離散遞推的完成都會發(fā)生變化，以下給出滑動(dòng)窗口IAE的具體推導(dǎo)過程。

根據(jù)新息序列自適應(yīng)濾波，假設(shè)條件為系統(tǒng)噪聲穩(wěn)定，量測噪聲隨時(shí)間段變化，且各段噪聲互不相關(guān)、強(qiáng)度未知，同時(shí)滑動(dòng)采樣的有限N個(gè)新息具備遍歷性，并且式（8）表示的新息方差Crk為關(guān)于r的相關(guān)函數(shù)，此時(shí)濾波的量測噪聲方差R更新如式（9），針對R的梯度檢測函數(shù)見文獻(xiàn)［17］。

根據(jù)中心極限定律，采用高斯概率密度函數(shù)表示觀測向量Z的條件概率密度函數(shù)如式（10），記R對角線上的系數(shù)如式（11）：

對式（10）取對數(shù)得：

式中m為觀測量維數(shù)，求ln|Crk|對r的偏導(dǎo)：

根據(jù)極大似然準(zhǔn)則對式（12）取最大值，則：得到：對式（8）求r的偏導(dǎo)：

已知R，記r=[q11，q22，…，qmm]為Q對角線上的噪聲系數(shù)，且R與r獨(dú)立，將式（16）代入式（15）得：則k時(shí)刻的增益矩陣為：

由Pk/k?1?Pk=Kk Hk Pk/k?1，Δxk=Kkrk整理得：

再由Pk/k?1=Φk/k?1Pk/k?1+Qk?1得到系統(tǒng)過程噪聲Q的新息序列表達(dá)如式（22）：

在系統(tǒng)中同時(shí)調(diào)整系統(tǒng)過程噪聲Q和觀測噪聲矩陣R，與單獨(dú)調(diào)整Q或R陣的條件是相同的，以下給出證明，先將式（16）代入式（15）得：

單獨(dú)調(diào)整R時(shí)，假設(shè)Rii=rii，Q與r獨(dú)立時(shí)，整理式（23）為：

式（24）成立的條件為式（25）：

單獨(dú)調(diào)整Q時(shí)，假設(shè)Qii=rii，R與r獨(dú)立時(shí)，整理式（23）得：其中，Hj HTj為二次型，必定為非負(fù)定矩陣，所以式（26）成立的條件依舊為式（25）。在同時(shí)調(diào)整Q與R時(shí)，假設(shè)rii=mRii，rii=nQii，m>0，n>0時(shí)，整理式（23）得：

基于以上敘述，結(jié)合所定義的系統(tǒng)不確定度U，針對Q陣定義模糊隸屬度函數(shù)如式（28），γ與σ關(guān)系如式（29）：

模糊控制器的輸入為Uk(k=1，2，3，…，n)，輸出為σ，σ包含兩個(gè)模糊集分別為D=減少；I=增加。對式（28）與式（29）描述的模糊規(guī)則進(jìn)行描述，當(dāng)Uk≤0.1時(shí)，表示系統(tǒng)不確定度較小，此時(shí)的Q不變；當(dāng)0.1

4 模糊IAE-UKF濾波器

量測更新往往是根據(jù)頻率較低的GPS信息，一旦觀測量受到干擾，不僅不能進(jìn)行誤差抑制，而且可能會引入更多的誤差。針對量測信號丟失或突變的情況，目前較為廣泛的處理方式有兩種，分別為檢測、構(gòu)造及適應(yīng)、調(diào)整。前者力求構(gòu)造一個(gè)新的序列去代替原信號，后者使得系統(tǒng)先驗(yàn)知識能夠適應(yīng)系統(tǒng)的時(shí)變特性。

本文基于后者的處理思想，設(shè)計(jì)了一種基于量化系統(tǒng)不確定性的系統(tǒng)模型調(diào)整方法。針對低成本組合導(dǎo)航系統(tǒng)，建立組合導(dǎo)航系統(tǒng)為ψ角誤差模型［7］，如式（30）。取系統(tǒng)狀態(tài)向量為x=[ψ，δp，δv，εa，εg]T，分別為失準(zhǔn)角誤差、位置誤差、速度誤差、三軸加速度計(jì)漂移、三軸陀螺儀漂移。

式中：ωen為載體在導(dǎo)航系中相對于地理系的角速度，ωie為地球自轉(zhuǎn)角速度，f為加速計(jì)測量得到的比力，fb為加速度計(jì)在載體坐標(biāo)系下的比力，Re為地球半徑，加速度計(jì)與陀螺儀建模為一階馬爾可夫過程。取系統(tǒng)量測向量如式（31），實(shí)現(xiàn)GPS/INS的低成本組合導(dǎo)航系統(tǒng)。

由以上敘述內(nèi)容，給出模糊IAE-UKF算法流程框圖，如圖1所示，在UKF濾波框架［19］中嵌入模糊控制與IAE算法，克服系統(tǒng)模型不準(zhǔn)確的問題。圖1中新息量由藍(lán)色框標(biāo)注，紅色虛線框內(nèi)為引入不確定度的模糊控制系統(tǒng)，模糊規(guī)則由式（28）與式（29）給出，形成了一種并行在線調(diào)整GPS/INS組合導(dǎo)航系統(tǒng)模型噪聲方差與量測噪聲方差的模糊IAE-UKF濾波器（彩圖見期刊電子版）。

圖1 模糊IAE-UKF算法框圖Fig.1 Fuzzy IAE-UKF algorithm diagram

5 試驗(yàn)與分析

為了驗(yàn)證上述所提方法在車載GPS/INS組合導(dǎo)航系統(tǒng)中的有效性與優(yōu)越性，設(shè)計(jì)了車載組合導(dǎo)航的測試，整個(gè)系統(tǒng)以INS為主，通過航位推算獲得融合信息，試驗(yàn)設(shè)備如圖2所示。采用GPS衛(wèi)星板卡與IMU慣性測量單元組合系統(tǒng)，傳感器參數(shù)如表1所示。

圖2 試驗(yàn)設(shè)備圖Fig.2 experimental equipment

表1 傳感器參數(shù)Tab.1 Sensor parameters

圖3 行駛軌跡Fig.3 Vehicle track

采集了兩組數(shù)據(jù)，第一組數(shù)據(jù)衛(wèi)星信號良好；第二組數(shù)據(jù)衛(wèi)星信號穩(wěn)定性較差；且兩組數(shù)據(jù)都存在較大初始姿態(tài)誤差。第一組數(shù)據(jù)的行駛軌跡如圖3所示。組合導(dǎo)航解的參考值由衛(wèi)星板卡通過差分（Real-Time Kinematic，RTK）方式獲取，其精度在開闊地段精度可達(dá)到厘米級，具體參數(shù)見表2。從而使用RTK數(shù)據(jù)進(jìn)行對比，從誤差的角度分析所設(shè)計(jì)方法的優(yōu)越性。圖4與圖5給出了速度誤差曲線，顯示了非線性濾波算法UKF較線性濾波算法KF的優(yōu)越性。當(dāng)存在較大的不可交換誤差時(shí)，認(rèn)為載體機(jī)動(dòng)性較高。此時(shí)慣性傳感器通常有較大的角速度與比力值，狀態(tài)量的可觀性因系統(tǒng)的時(shí)變特性也會產(chǎn)生變化，對濾波收斂速度產(chǎn)生一定影響，量測不穩(wěn)定則影響組合導(dǎo)航解的準(zhǔn)確性。

表2 組合位置與速度的誤差均值Tab.2 Mean error of combined position and speed

圖4 北向速度曲線Fig.4 North speed curve

圖5 東向速度曲線Fig.5 Earth speed curve

采用第二組數(shù)據(jù)進(jìn)行量測不穩(wěn)定時(shí)的方法驗(yàn)證，行駛軌跡如圖6所示。圖7～圖8給出了不同算法下的組合位置誤差，圖9～圖10給出了組合速度誤差。表1列出了組合位置與速度的誤差均值，表2列出了組合位置與速度的均方根值（Root Mean Square，RMS）。

圖6 行駛軌跡Fig.6 Vehicle track

圖7 經(jīng)度誤差Fig.7 Longitude error

圖8 緯度誤差Fig.8 Latitude error

圖9 東向速度誤差Fig.9 Eastward speed error

圖10 北向速度誤差Fig.10 Northbound speed error

表3 組合位置與速度的RMSTab.3 RMS of combined position and speed

由圖7～圖8，模糊IAE-UKF算法精度較高，位置誤差得以明顯抑制。由圖9～圖10，組合速度誤差在整個(gè)濾波過程中較為平緩，表明模糊IAE-UKF算法受載體機(jī)動(dòng)性影響較小，精度優(yōu)于傳統(tǒng)UKF與IAE-UKF算法。由表1～表3的統(tǒng)計(jì)數(shù)據(jù)分析得，IAE_UKF與模糊IAEUKF算法使得位置誤差與速度誤差下降在20%以上，其中模糊IAE-UKF算法能夠使得誤差下降超過60%，驗(yàn)證了文章改進(jìn)方法的有效性與優(yōu)越性。

為了更進(jìn)一步說明設(shè)計(jì)方法在量測異常情況下的優(yōu)越性，在第二組數(shù)據(jù)中設(shè)置兩個(gè)階段的GPS信號丟失，第一階段為100～107 s，第二階段為200～205 s。圖11給出了GPS信號丟失情況下的經(jīng)度誤差，圖12給出了GPS信號丟失情況下的東向速度誤差。表明模糊IAE-UKF方法在GPS信號丟失時(shí)能夠較塊收斂，且能適應(yīng)由外部量測異常導(dǎo)致不確定性增大時(shí)的系統(tǒng)。圖13給出了量測正常與量測異常下的不確定度對比。由圖13可知，量測正常時(shí)，U大范圍處于（0，1）內(nèi)；量測出現(xiàn)異常時(shí)，U值越大，代表此刻系統(tǒng)模型的不準(zhǔn)確性越大，從而能夠根據(jù)U值對系統(tǒng)噪聲進(jìn)行在線調(diào)整。

圖11 經(jīng)度誤差Fig.11 Longitude error

圖12 東向速度誤差Fig.12 Eastward speed error

圖13 系統(tǒng)不確定度UFig.13 Uncertainty of the system U

5 結(jié) 論

文章以提高低成本GPS/INS組合導(dǎo)航系統(tǒng)的濾波精度為目標(biāo)，在初值姿態(tài)誤差較大時(shí)，使用非線性濾波算法克服組合導(dǎo)航系統(tǒng)的不確定性。首先給出了系統(tǒng)不確定性的含義，其次利用新息含義式給定量化參數(shù)γ與U的權(quán)值函數(shù)，對不確定度進(jìn)行量化，引入滑動(dòng)窗口IAE方法，利用梯度檢測函數(shù)確定滑動(dòng)窗口長度N；最后設(shè)計(jì)輸入為U輸出為σ的模糊控制系統(tǒng)，生成可自適應(yīng)調(diào)整噪聲方差陣Q與R的方法，嵌入到UKF濾波器中，構(gòu)建了模糊IAE-UKF濾波器。

模糊IAE-UKF濾波器同時(shí)考慮Q與R對模型的影響，是一種帶有過程噪聲方差估計(jì)器與量測噪聲方差估計(jì)器的濾波器，實(shí)現(xiàn)在線調(diào)整系統(tǒng)模型，抑制系統(tǒng)不確定性對組合導(dǎo)航解的影響。最后通過三組半物理仿真試驗(yàn)驗(yàn)證了提出方法的有效性，為系統(tǒng)的應(yīng)用提供一定的參考價(jià)值。