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

（1.蘭州交通大學自動控制研究所，甘肅蘭州730070；2.蘭州交通大學甘肅省高原交通信息工程及控制重點實驗室，甘肅蘭州730070；3.蘭州交通大學交通運輸學院，甘肅蘭州730070）

1 引 言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 IAE算法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 模糊IAE-UKF濾波器

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

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

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

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

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

5 試驗與分析

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

圖2 試驗設備圖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所示。組合導航解的參考值由衛(wèi)星板卡通過差分（Real-Time Kinematic，RTK）方式獲取，其精度在開闊地段精度可達到厘米級，具體參數(shù)見表2。從而使用RTK數(shù)據(jù)進行對比，從誤差的角度分析所設計方法的優(yōu)越性。圖4與圖5給出了速度誤差曲線，顯示了非線性濾波算法UKF較線性濾波算法KF的優(yōu)越性。當存在較大的不可交換誤差時，認為載體機動性較高。此時慣性傳感器通常有較大的角速度與比力值，狀態(tài)量的可觀性因系統(tǒng)的時變特性也會產(chǎn)生變化，對濾波收斂速度產(chǎn)生一定影響，量測不穩(wě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ù)進行量測不穩(wě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，組合速度誤差在整個濾波過程中較為平緩，表明模糊IAE-UKF算法受載體機動性影響較小，精度優(yōu)于傳統(tǒng)UKF與IAE-UKF算法。由表1～表3的統(tǒng)計數(shù)據(jù)分析得，IAE_UKF與模糊IAEUKF算法使得位置誤差與速度誤差下降在20%以上，其中模糊IAE-UKF算法能夠使得誤差下降超過60%，驗證了文章改進方法的有效性與優(yōu)越性。

為了更進一步說明設計方法在量測異常情況下的優(yōu)越性，在第二組數(shù)據(jù)中設置兩個階段的GPS信號丟失，第一階段為100～107 s，第二階段為200～205 s。圖11給出了GPS信號丟失情況下的經(jīng)度誤差，圖12給出了GPS信號丟失情況下的東向速度誤差。表明模糊IAE-UKF方法在GPS信號丟失時能夠較塊收斂，且能適應由外部量測異常導致不確定性增大時的系統(tǒng)。圖13給出了量測正常與量測異常下的不確定度對比。由圖13可知，量測正常時，U大范圍處于（0，1）內(nèi)；量測出現(xiàn)異常時，U值越大，代表此刻系統(tǒng)模型的不準確性越大，從而能夠根據(jù)U值對系統(tǒng)噪聲進行在線調(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組合導航系統(tǒng)的濾波精度為目標，在初值姿態(tài)誤差較大時，使用非線性濾波算法克服組合導航系統(tǒng)的不確定性。首先給出了系統(tǒng)不確定性的含義，其次利用新息含義式給定量化參數(shù)γ與U的權(quán)值函數(shù)，對不確定度進行量化，引入滑動窗口IAE方法，利用梯度檢測函數(shù)確定滑動窗口長度N；最后設計輸入為U輸出為σ的模糊控制系統(tǒng)，生成可自適應調(diào)整噪聲方差陣Q與R的方法，嵌入到UKF濾波器中，構(gòu)建了模糊IAE-UKF濾波器。

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