魏德軒，曹 樂，韓學法

（上海工程技術大學 電子電氣工程學院，上海 201620）

0 引言

隨著無人機、無人車的發(fā)展，低成本組合導航系統(tǒng)的應用需求越來越多，對組合導航算法的精度、魯棒性有了更高的要求。全球衛(wèi)星導航系統(tǒng)（global navigation satellite system, GNSS）與捷聯(lián)慣性導航系統(tǒng)（strapdown inertial navigation system,SINS）兩種導航系統(tǒng)優(yōu)勢互補、成本低，在軍用和民用領域有著廣泛應用，是組合導航技術的熱門研究方向。

目前針對GNSS/SINS 組合導航算法有基于人工智能領域的神經(jīng)網(wǎng)絡算法[1-2]和基于貝葉斯原理的傳統(tǒng)信息融合算法?；谏窠?jīng)網(wǎng)絡的組合導航算法，需要利用大量數(shù)據(jù)進行訓練，效果受訓練集數(shù)據(jù)影響較大。傳統(tǒng)信息融合算法不需要大量數(shù)據(jù)進行訓練，實時性好，其中卡爾曼濾波（Kalman filtering, KF）算法由于計算量小，對計算性能要求低，其應用更為廣泛。在經(jīng)典卡爾曼濾波組合導航算法中，綜合利用了歷史所有GNSS 的觀測值，對SINS 進行校正，當濾波模型準確時，濾波的增益計算回路會逐漸收斂，新的觀測值對狀態(tài)估計的修正作用逐漸減小。因此，為了使狀態(tài)估計能較好地適應觀測值的變化，文獻[3]開發(fā)了在線自適應遺忘因子濾波算法；文獻[4]為了降低計算量，利用新息協(xié)方差計算漸消因子，提高了實時性；文獻[5]為了提高濾波算法的精度和魯棒性，設計了漸消記憶指數(shù)加權的新息協(xié)方差估計方法，并引入了多重漸消因子以實現(xiàn)對預測誤差協(xié)方差陣的調節(jié)；漸消卡爾曼濾波（fading Kalman filter, FKF）效果較為依賴GNSS 觀測值，然而GNSS 在城市高樓區(qū)、林蔭道等場所易受干擾，噪聲統(tǒng)計特性復雜[6]。為了降低GNSS 噪聲的影響，文獻[7]在低成本GNSS/INS 組合導航中，使用高精度實時動態(tài)差分（real-time kinematic, RTK）GNSS 定位，雖然有效提高了導航定位精度，但是RTK 定位需要基站，應用受到限制；為了能夠實時獲取GNSS 觀測信息的噪聲統(tǒng)計特性，文獻[8]基于高斯白噪聲假設，設計了基于χ2( )1 分布的濾波異常檢測，對噪聲的統(tǒng)計特性進行實時檢測，實現(xiàn)了魯棒漸消容積卡爾曼濾波算法；文獻[9]利用最大相關熵的方法，對噪聲進行預處理，得到了噪聲的先驗信息，在消除脈沖噪聲的影響后，再進行魯棒自適應濾波，提高了濾波精度；上述算法涉及大量矩陣運算，要求矩陣非奇異，計算復雜，對組合導航系統(tǒng)的運算性能要求較高。隨著不確定度理論的發(fā)展，在導航定位領域也逐漸使用不確定度描述定位特性[10-11]，文獻[12]結合測量不確定度理論，對GNSS模型進行了建模分析，利用3 個參數(shù)對7 個影響GNSS 信號質量的因素進行量化，得到相應的噪聲統(tǒng)計特性，以此為基礎改進了GNSS 觀測噪聲模型，提高了導航精度；但是這種方法需要事先采集大量GNSS 數(shù)據(jù)，對噪聲統(tǒng)計特性進行建模分析，雖然實時應用受限，但是證明了GNSS 模型精度越高，導航定位精度越高。

基于上述分析，針對FKF 組合導航算法對于觀測噪聲突變而難以準確建模，從而導致定位精度較低、魯棒性差的問題，本文以動態(tài)測量不確定度理論為基礎，對GNSS 觀測數(shù)據(jù)進行不確定度建模實時計算，定義了監(jiān)測系數(shù)表示觀測噪聲先驗信息，并據(jù)此對FKF 的組合導航算法進行改進，以提高濾波算法精度和魯棒性。

1 基于FKF 的組合導航算法

1.1 組合導航系統(tǒng)方程

本文GNSS/SINS 導航系統(tǒng)采用松組合的結構方式，卡爾曼濾波器采用基于誤差狀態(tài)量的間接式結構，選用北東地坐標系為導航坐標系。低成本陀螺儀角度隨機游走誤差較大，使用互補濾波算法輸出的姿態(tài)角進行導航信息融合，組合導航系統(tǒng)框圖如圖1 所示。

圖1 組合導航框圖

在k時刻，導航系統(tǒng)狀態(tài)空間模型為

式中：Qk為非負的系統(tǒng)噪聲序列方差陣；kR為正定的觀測噪聲序列方差陣；δkj為克羅內克（Kronecker）δ函數(shù)，當k=j時，δkj=1，當k≠j時，δkj=0。

誤差狀態(tài)卡爾曼濾波算法的預測和修正過程[13-14]為

1.2 漸消卡爾曼濾波算法

針對系統(tǒng)模型失準和噪聲統(tǒng)計特性與實際環(huán)境不相符兩種狀況，F(xiàn)KF 的兩種實現(xiàn)方式如圖2 所示，分別是漸消因子法和膨脹系數(shù)法。

圖2 FKF 實現(xiàn)方式

1.2.1 漸消因子法

通過增強k時刻GNSS 的量測值的權重，使得狀態(tài)估計能更好地適應新的量測值的變化，此時漸消卡爾曼濾波算法的實現(xiàn)方式[3,5]為

當系統(tǒng)模型失配時，實際的新息協(xié)方差矩陣與計算出的理論值不相等，致使式（8）不成立。此時通過式（5）計算得到kλ，實時調整Pk,k1-對Kk的影響，使式（8）保持成立，進而達到抑制濾波發(fā)散的目的。主要的思想是通過提高新觀測量的權重抑制濾波發(fā)散，提高濾波精度。但是當觀測噪聲變大時，反而會導致導航誤差增大，濾波精度降低甚至濾波發(fā)散。

1.2.2 膨脹系數(shù)法

針對噪聲模型失配的問題，文獻[15-16]提出了通過“膨脹”觀測噪聲矩陣來降低濾波增益的方案，當檢測到量測噪聲異常時，通過膨脹系數(shù)矩陣降低觀測量的權重，調整公式為

式（13）意味著，當觀測噪聲的增大導致濾波精度下降時，新息矢量的協(xié)方差也會變大，對噪聲矩陣進行“膨脹”，進而削弱觀測量的修正作用，提高系統(tǒng)模型預測值的權重，避免因觀測噪聲的影響導致濾波發(fā)散，從而提高濾波器的魯棒性。

2 基于不確定度理論改進的FKF 低成本組合導航算法

FKF 的兩種實現(xiàn)方式原理不同：第一種算法是在系統(tǒng)模型失準、但噪聲模型符合時，可以提高濾波精度；第二種算法是在系統(tǒng)模型較為準確、但觀測值受到異常干擾時，可以在損失一定精度的條件下，保證濾波穩(wěn)定，具有較強的魯棒性。為了實現(xiàn)兩種方式的有效結合，提高算法的自適應性、精度和魯棒性，本文利用動態(tài)測量不確定度理論對GNSS 觀測值建模分析。GNSS 定位信號的不確定度大小與接收機噪聲、電離層效應、大氣效應、星歷誤差、衛(wèi)星時鐘誤差、多徑效應和枝葉衰減等因素直接相關，直接測量這些誤差因素的不確定度是不切實際的[12]，并且這些因素在車輛運動時，可能會發(fā)生較大的變化，無法將其作為靜態(tài)常數(shù)。動態(tài)測量不確定度理論將GNSS 測量過程看做序列參數(shù)測量過程，可以按圖3 所示分解為確定性成分與隨機性成分[17]，分別利用多項式擬合和自回歸模型（autoregressive model, AR）進行建模計算。

圖3 不確定度分析框圖

為了使算法既能在不確定度發(fā)生微弱擾動時保持穩(wěn)定，又能同時敏感地檢測到觀測噪聲的變化，本文定義了監(jiān)測系數(shù)μ，結合變化量ΔS對不確定度進行分析后，選擇FKF 的實現(xiàn)方式，改進后的組合導航算法實現(xiàn)過程如圖4 所示。對GNSS 測量數(shù)據(jù)進行建模時，針對長為N的數(shù)據(jù)，觀測數(shù)據(jù)X(i) 可表示為

圖4 改進后的組合導航算法系統(tǒng)框圖

式中：X0(i)為真值；ε(i) 為測量誤差。用不確定度的理論將X0(i) 和ε(i) 分別分解為確定性成分和隨機性成分，即

式中：F0(i) 為確定性變化成分；Y0(i) 為隨機性變化成分；εs(i)為系統(tǒng)誤差；εr(i)為隨機誤差。各成分性質不同，根據(jù)圖3 有

式中：F(i) 代表確定性成分；Y(i) 代表隨機性成分。實際的測量數(shù)據(jù)通常由確定性成分和隨機性成分組成，即

式中：0C、1C、2C為多項式擬合系數(shù)。確定性成分的A 類標準不確定度可根據(jù)式（19）計算。

計算隨機性成分的不確定度時，將殘差v(i) 記為新序列Y(i) ，由于殘差v(i) 均值為零，可以看作是零均值的隨機過程。序列Y(i) 長度固定且較短，本文選擇AR 模型進行短期預測提取隨機成分，其模型為

式中：m為模型的階次；mw為待估計參數(shù)；εar(i)為估計的誤差項，通常將其作為高斯白噪聲，其均值、方差和相關性滿足

式中：SN(i) 、SE(i) 、SD(i) 分別為北向、東向和地向測量值的不確定度；SGPS(i) 為GNSS 測量點的總不確定度；通過式（27）可得到GNSS 量測值的不確定度，為了降低觀測數(shù)據(jù)中異方差的影響，對不確定度數(shù)據(jù)進行對數(shù)變換。相同噪聲方差下的不確定度存在微弱擾動，僅通過不確定度的變化選擇組合導航算法會降低算法魯棒性。因此本文通過式（28）定義μ為相鄰時刻不確定度的監(jiān)測系數(shù)，ΔS為相鄰時刻不確定度的變化量，即

3 實驗與結果分析

3.1 數(shù)據(jù)集及數(shù)據(jù)處理

本文使用基蒂（Kitti）自動駕駛數(shù)據(jù)集中馬拉加市區(qū)第二紀元的數(shù)據(jù)驗證算法。路徑軌跡如圖5（a）所示，將RTK 校正修復后的全球定位系統(tǒng)（global positioning system,GPS）數(shù)據(jù)作為載體運動真值，輸出頻率1 Hz。慣性測量單元使用輸出頻率為100 Hz 的xSense MTi，3 軸加速度的靜態(tài)誤差偏差約為0.05 m/s2，3 軸瞬時角速度誤差約為0.4 (°)/s[18]。載體坐標系為前左上，車載GPS 與慣性測量單元（inertial measurement unit, IMU）傳感器在車體的安裝位置如圖5(b)所示，具體位置坐標如表1 所示。

圖5 數(shù)據(jù)集采集

為使GPS 數(shù)據(jù)與IMU 數(shù)據(jù)嚴格時間同步，對GPS 數(shù)據(jù)進行三次樣條插值后按IMU 數(shù)據(jù)輸出時刻抽取 GPS 數(shù)據(jù)，仿真時整體疊加方差為5 m 的高斯白噪聲模擬GPS 信號精度，加入了兩段方差為10 m 的高斯白噪聲模擬突變噪聲。圖6為原始緯度、經(jīng)度及海拔高度進行三次樣條插值后，提取的頻率為10 Hz 的數(shù)據(jù)。圖7 為將GPS數(shù)據(jù)加噪聲后轉換為北向、東向、地向坐標系下的位置坐標。

圖6 原始數(shù)據(jù)

圖7 噪聲數(shù)據(jù)

3.2 結果分析

由式（27）可知，此時得到的不確定度為每個測量點的綜合不確定度，數(shù)據(jù)中所加噪聲及不確定度計算結果如圖8（a）、圖8（b）所示，進行對數(shù)變換后，計算得到的監(jiān)測系數(shù)μ如圖8（c）所示，能準確反映數(shù)據(jù)統(tǒng)計特性的突變。

圖8 數(shù)據(jù)處理

由于低成本陀螺儀誤差漂移較大，在組合導航系統(tǒng)中采用互補濾波進行姿態(tài)解算，此時姿態(tài)信息與比力、地磁關聯(lián)較大，受GPS 量測噪聲影響較小，無法體現(xiàn)本文中算法的效果。因此通過圖9、圖10 和表2、表3 列出各方向的位置誤差和速度誤差結果進行對比。

表2 三種算法位置最大誤差單位：m

表3 三種算法速度最大誤差單位：m/s

圖9 位置誤差

圖10 速度誤差

從圖9 和圖10 可以看出，誤差狀態(tài)KF 對噪聲有一定的抑制作用，但是地向位置誤差有發(fā)散的趨勢，證明KF 算法魯棒性不強且精度較低，最大誤差約為3.30 m。當使用FKF 時，實際噪聲與建模觀測噪聲匹配時精度有明顯提高，但是當觀測噪聲增大時，反而更容易受到干擾，最大誤差為3.20 m，魯棒性較差。改進的UNFKF 在沒有突變噪聲干擾時，精度與FKF 相當。噪聲突變時濾波精度降低，是因為噪聲突變等價于觀測信息的修正作用減小，IMU 的誤差無法被校正，相比之下精度必然有所下降，最大誤差為2.10 m，但濾波精度仍然比KF、FKF 高35%。當噪聲穩(wěn)定后，能繼續(xù)保持濾波收斂，證明魯棒性較高。在圖10 速度誤差圖中，改進的算法在幾種算法中北向、東向速度的最大誤差沒有明顯降低，但是各向速度誤差波動較小，并且載體總速度的最大誤差相比KF降低了32%。

4 結束語

本文針對 FKF 算法在低成本組合導航應用中魯棒性差、精度低的問題進行了研究，通過對FKF算法的實現(xiàn)方式進行分析，根據(jù)其原理的區(qū)別引入了動態(tài)測量不確定度建模的方法，對觀測數(shù)據(jù)進行建模，實時計算不確定度，為了降低不確定度微小擾動的干擾，提高分析結果的準確度，定義了監(jiān)測系數(shù)并結合不確定度變化量判斷觀測噪聲統(tǒng)計特性的變化趨勢，并根據(jù)觀測噪聲的變化選擇FKF 的實現(xiàn)方式，最后使用Kitti 數(shù)據(jù)集進行仿真驗證。結果表明：算法在異常量測噪聲的干擾下能夠保持較強的魯棒性和自適應性，且收斂速度快，位置誤差精度提高35%，總體速度誤差降低32%。下一步將研究針對各個維度不同噪聲統(tǒng)計特性下的不確定度的多漸消因子的自適應改進算法。