引入系統不確定度的模糊IAE-UKF組合導航

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

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

1 引 言

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

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

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

2 系統不確定度

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

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

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

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

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

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

由新息含義式（1），給定系統不確定性的量化參數γ：

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

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

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

3 IAE算法

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

Step 1：計算狀態的一步預測量與狀態協方差的一步預測量；

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

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

Step 4：計算濾波增益Kk，更新狀態估計與狀態協方差。

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

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

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

對式（10）取對數得：

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

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

已知R，記r=[q11，q22，…，qmm]為Q對角線上的噪聲系數，且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得到系統過程噪聲Q的新息序列表達如式（22）：

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

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

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

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

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

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

4 模糊IAE-UKF濾波器

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

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

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

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

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

5 試驗與分析

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

圖2 試驗設備圖Fig.2 experimental equipment

表1 傳感器參數Tab.1 Sensor parameters

圖3 行駛軌跡Fig.3 Vehicle track

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

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

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

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

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

圖6 行駛軌跡Fig.6 Vehicle track

圖7 經度誤差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算法受載體機動性影響較小，精度優于傳統UKF與IAE-UKF算法。由表1～表3的統計數據分析得，IAE_UKF與模糊IAEUKF算法使得位置誤差與速度誤差下降在20%以上，其中模糊IAE-UKF算法能夠使得誤差下降超過60%，驗證了文章改進方法的有效性與優越性。

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

圖11 經度誤差Fig.11 Longitude error

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

圖13 系統不確定度UFig.13 Uncertainty of the system U

5 結 論

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

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