一種基于改進自適應卡爾曼濾波的GNSS/INS組合導航算法

吳曉倩, 盧秀山, 王勝利, 王鳴鶴, 柴大帥

(1.山東科技大學測繪科學與工程學院，青島 266000；2.山東科技大學海洋科學與工程學院，青島 266000；3.中國礦業大學環境與測繪學院, 徐州 221000)

慣性導航系統(inertial navigation system,INS)具有自主性強、隱蔽性高、抗干擾能力強以及提供多種導航參數(位置、速度、航向和姿態等)的特點，但是其精度與慣性測量器件直接相關，導航誤差隨著時間積累[1]。全球衛星導航系統(global navigation satellite system，GNSS)具有全天候、全球性、實時導航功能，但是其自主性差、抗干擾能力弱，信號更新頻率較低[2]。因此在實際應用中常將這兩種導航方式結合起來，優勢互補，提高導航定位精度，并隨著低成本傳感器和計算機計算技術的發展，GNSS/INS組合導航得到廣泛應用[3]。

常用的GNSS/INS組合導航數據融合方法為卡爾曼濾波(Kalman filter，KF)。1960年，R.E. Kalman首次提出卡爾曼濾波，發展至今已經相對完善[4]。卡爾曼濾波要求動力學模型、量測噪聲和系統噪聲的統計特性精確并且為已知值，但在實際應用中很難得到，為解決這個問題提出了自適應卡爾曼濾波(adaptive Kalman filter，AKF)[5-6]，在濾波進行的同時，利用觀測數據連續在線估計和校正噪聲統計特性、動力學模型參數，以提高濾波的精確度，減小估計誤差。

Sage-Husa自適應濾波[7]增加了噪聲計算，計算量大量增加，并且對于具有更高階次的系統，隨著濾波的進行噪聲陣可能會出現負定性的情況，導致濾波發散；AKF的新息協方差陣是通過最大似然估計得到的，存在誤差積累且抑制效果有限；對于含有不同類型參數的狀態向量，單一因子自適應濾波會降低可靠參數的預報信息使用率，同時也會增加不可靠參數預報信息的貢獻值[8]。基于上述問題，在分析KF算法和AKF算法的基礎上，提出一種改進的自適應濾波算法(modified adaptive Kalman filter，MAKF),利用預測殘差建立統計量，采用自適應因子直接調節位置向量和速度向量，以減少其他參數對濾波的平衡作用；由預測殘差的概率密度得到的馬氏距離的平方，再進行假設檢驗，模型異常時使用改進的自適應濾波，否則使用標準卡爾曼濾波，避免了計算次數累計導致的自適應卡爾曼濾波發散，提高了計算效率。

1 卡爾曼濾波算法模型

1.1 標準卡爾曼濾波

標準卡爾曼濾波使用噪聲的統計特性，將系統觀測量作為輸入，將要估計值的參數或系統狀態作為輸出，根據時間更新和觀測更新算法估計出所需信號[9]。假設離散線性系統方程和量測方程分別為

Xk=Φk,k-1Xk-1+ωk-1

(1)

Zk=HkXk+νk

(2)

狀態一步預測為

(3)

狀態一步預測均方誤差為

(4)

濾波增益為

(5)

式中：Pk,k-1為預測狀態協方差陣。

狀態估計為

(6)

式(6)中：Kk為濾波增益矩陣。

狀態估計均方誤差為

Pk=(I-KkHk)Pk,k-1

(7)

式(7)中：Pk為方差陣；I為單位矩陣。

1.2 自適應卡爾曼濾波

AKF是根據實時觀測信息自適應地調整先驗信息的貢獻，合理平衡先驗信息和實時觀測信息對模型參數估計的貢獻[11-12]。

(8)

引入單因子αk(0<αk≤1)控制異常擾動和狀態模型誤差的影響，自適應濾波風險函數為[13]

(9)

式(9)中：min表示自適應濾波風險函數取最小值。

則自適應卡爾曼濾波解為

(10)

(11)

(12)

采用預測殘差向量構造的誤差判別統計量：

(13)

自適應因子αk為

(14)

式(14)中：c的取值為1.0～1.5。

綜上可知，單因子自適應濾波解與標準卡爾曼濾波解類似，計算簡單，自適應因子由預測殘差向量確定，在濾波之前不需要計算狀態向量參考值，且不需要觀測量數量一定大于狀態參數數量。

2 改進的自適應卡爾曼濾波

2.1 利用預測殘差的概率密度建立馬氏距離

(15)

式(15)中：a為量測向量的維數。

由式(15)可以建立判別模型誤差的判別量，即：

(16)

式(16)中：Mk為馬氏距離；γk服從自由度為a的卡方分布。

通過假設檢驗，對于任意給定的顯著性水平值α，其對應的分位點為χα，則γk的概率為

P(γk<χα)=1-α

(17)

2.2 改進自適應因子

在卡爾曼濾波中，包含不同種類參數的狀態向量，各類參數的動力學模型的可靠性各不相同，使用統一的自適應調節因子，對于可靠的預測狀態參數，有可能會減少其使用效率，由于其他可靠參數的平衡作用，對于較大誤差的預測參數權重不能降低。為解決這類問題，在單因子AKF的基礎上進行如下改進：

所以得到：

(18)

由于使用的單因子判別統計量是由預測殘差向量計算得到，而與預測殘差向量直接有關的是位置向量和速度向量，所以直接用由預測殘差向量得到的判別統計量判斷速度向量和位置向量是否可靠，并對其進行調節。

綜上所述，MAKF首先在所有歷元進行假設檢驗，如果馬氏距離的平方γk小于顯著性水平值α對應的分位點χα，執行MAKF算法，否則執行KF算法，計算程序如圖1所示。

圖1 改進后算法計算程序

3 算例分析

使用車載移動測量系統采集一組GNSS和INS數據對算法進行驗證。設備為NovAtel ProPak6雙系統(GPS和BDS)接收機以及Span-LCI類型的慣導。IMU采樣率為200 Hz，GNSS采樣率為5 Hz，組合采樣率為5 Hz；初始坐標精確已知，觀測環境良好。算例中，相應參數由經驗設定：陀螺儀相關時間為600 s，初始方差為1.0°/h，加速度計相關時間為600 s，初始方差為50.0 μg；初始位置誤差分別為1.0、1.0、1.0 m；初始速度誤差分別為0.01、0.01、001 m/s；初始平臺失準角誤差分別為100.0、100.0、100.0 s；采用商業軟件Inertial Explorer 8.6(IE)緊組合解算的位置、速度結果作為真值；臨界值c=1.0。行車軌跡如圖2所示。

圖2 行車軌跡

為驗證改進算法的有效性，提出如下3種方案：① 標準卡爾曼濾波(KF)；②單因子自適應卡爾曼濾波(AKF)；③改進的自適應卡爾曼濾波(MAKF)。

圖3～圖5所示為利用KF、AKF和MAKF在東方向、北方向和天方向位置誤差對比，由圖3～圖5可知，在整個濾波過程中，相對于KF，AKF和MAKF穩定性更好，這是因為KF采用固定的系統噪聲陣和量測噪聲陣，當其與實際系統有較大差異時會導致誤差增大，甚至發散。AKF利用預測殘差構造自適應因子，當載體處于非平衡狀態時，利用自適應因子調節狀態預報向量和觀測向量的權陣，平衡二者對狀態估值的貢獻，可較好的控制動力學模型擾動的影響。

圖3 標準卡爾曼濾波位置誤差

表1所示為3種方案的均方根，可以更直觀地比較濾波的性能。與KF相比，AKF在東北天方向位置估計精度分別提高了0.168、0.148 2、0.175 7 m，而MAKF在東北天方向位置估計精度分別提高了0.173 3、0.153 6、0.186 4 m。由此可見，AKF和MAKF精度均高于KF，其中MAKF略有優勢。

表2所示為3種算法的計算次數，其中m為觀測向量維數；n為狀態向量維數。由表2可知，AKF和MAKF較KF計算次數都有增加，但MAKF計算次數增加的幅度小，這是因為MAKF通過利用預測殘差的概率密度建立馬氏距離的平方值進行假設檢驗，在模型異常時應用改進的自適應因子調節速度向量和位置向量，否則應用標準卡爾曼濾波，提高了計算效率。

表1 3種方案均方根對比

圖4 單因子自適應卡爾曼濾波位置誤差

圖5 改進自適應卡爾曼濾波位置誤差

表2 3種不同算法的計算量

4 結論

基于AKF和馬氏距離，提出MAKF算法，通過預測殘差的概率密度建立馬氏距離的平方值，進行假設檢驗，避免了自適應卡爾曼濾波隨著計算次數累計導致濾波發散，提高了計算效率；利用預測殘差構造的自適應因子調節速度向量和位置向量，避免了其他參數的平衡作用，提高精度。使用實測車載數據對本文算法進行性能評估，實驗結果表明，該方法能能夠提高GNSS/INS組合導航性能，在實際應用中有一定的參考價值。