SINS/BDS緊組合系統中自適應兩階段擴展卡爾曼濾波

臧中原，董 亮，毋 蒙，蔣 凱，楊 健

(上海航天控制技術研究所，上海 201109)

0 引言

為獲得更高精度的定位結果，將多種導航定位技術組合，已成為導航定位的熱門研究方向。捷聯慣性導航系統(Strap-down Inertial Navigation System, SINS ) 是一種自主導航系統，具有短期精度高、自主性強、抗干擾能力強、數據更新率高等特點，可提供豐富的導航信息(包括位置、速度、姿態),缺點是其誤差隨時間累積,不適用于長時間單獨導航。作為最主要的全球衛星導航系統( Global Navigation Satellite System , GNSS )之一，北斗衛星導航系統( BeiDou Navigation Satellite System , BDS )已全面建成[1]。BDS具有無累積誤差和長期穩定性, 但易受干擾,接收機數據更新率低,無姿態信息。因此，為了提高系統的導航性能，將BDS和SINS兩種系統融合，取長補短，充分發揮各自的優勢，同時提高系統的導航精度。相較于采用位置/速度組合的SINS/BDS松組合導航系統，采用BDS接收機更原始信息—偽距/偽距率組合的SINS/BDS緊組合導航系統，具有更高的導航精度和可靠性。

在SINS/BDS組合導航系統中，由于未知隨機常值偏差(如陀螺零偏、加速度計偏值、BDS接收機鐘差等)的存在，已知該組合建立的非線性系統模型不準確和噪聲不確定，從而導致濾波器的性能下降，甚至導致濾波器發散。為此，需要對系統狀態和偏差進行同時估計，文獻[2-3]將偏差項增擴為狀態，提出了增廣狀態卡爾曼濾波器(Augmented State Kalman Filter，ASKF)，增加了系統的維數，造成計算量增大、估計時間加長，從而影響濾波器的實時性，而且容易發散。針對線性系統的上述問題，B. Friedland和M. B. Ignagni相繼提出了兩階段卡爾曼濾波(Two-stage Kalman Filter，TKF)算法及其改進算法，將狀態與偏差分為兩個卡爾曼濾波器進行估算，再利用偏差對狀態進行修正。該算法適用于偏差項較多的情況，它能減少系統的狀態維數，從而減少計算量，提高解算速度，并且不影響定位精度。此外，一些學者將TKF算法推廣到非線性系統中，提出了兩階段擴展卡爾曼濾波器(Two-stage Extended Kalman Filter，TEKF)[4]和二階段H∞濾波[5]。TEKF的實現過程與EKF[6]相似，都需要準確已知先驗信息。然而在實際情況下，系統模型參數與實際物理過程有一定的偏差，系統的偏差與噪聲方差以及量測噪聲方差無法事先確定。所以，本文提出了一種自適應兩階段擴展卡爾曼濾波(Adaptive Two-stage Extended Kalman Filter，ATEKF)算法，即利用估算的幾何精度因子(Geometric Dilu-tion of Precision，GDOP)值和新息對TEKF中的量測噪聲方差進行在線估計的自適應濾波方法，并應用到SINS/BDS緊組合導航系統中，通過實際系統驗證了該自適應算法的有效性。

1 SINS/BDS緊組合系統模型

1.1 狀態方程

慣性導航系統選取東北天地理坐標系作為導航坐標系，記為n；載體坐標系記為b，其x沿載體橫軸向右，y沿載體縱軸向前，z沿載體豎軸向上。組合系統的狀態方程如下[7]。

假設SINS/BDS組合導航系統，SINS的誤差模型具有下列形式

(1)

其中，狀態向量

偏差向量

系統噪聲向量

偏差噪聲向量

1.2 量測方程

(2)

2 自適應兩階段卡爾曼濾波

2.1 兩階段卡爾曼濾波

將式(1)、式(2)合在一起構成組合系統的狀態方程和量測方程[8-9]，寫成下列離散化式

(3)

考慮到SINS/BDS緊組合導航系統的狀態方程是線性的，且式(3)非線性系統的信息完全已知，TEKF的耦合差分方程如下

(4)

無偏差狀態估計方程為

(5)

類似地，偏差狀態估計方程為

(6)

(7)

2.2 緊組合系統中的GDOP估算方法

在SINS/BDS緊組合導航系統中，以往求解GDOP值通常利用BDS接收機的定位結果。但在實際應用中，由于可見衛星的變化，會造成選星過程的重復，從而增大了計算量，使運算的時間成本增加，影響求解GDOP值的實時性。針對這種不足，本文提出了利用修正后的SINS的位置，并結合衛星位置估算GDOP值，從而完成選星過程。參考文獻[7]的2.1節和文獻[10]的5.4節中求解GDOP的方法，使用經過修正后的SINS的位置，將偽距觀測方程由下式

ρ-r0=AX+V

轉換為

ρ-rI=AIX+V

相應的幾何矩陣AI表示為

則以修正后的SINS位置求得的幾何精度因子定義為GDOPI，可以求得

(8)

這里可見衛星數選為4顆。在組合導航系統中，經過修正后的SINS位置誤差較小，因此可以使用修正后的SINS位置輸出求解GDOP，進而完成選星。該方法充分利用了組合導航系統中的已知量，顯著縮短了選星時間。

2.3 量測噪聲方差陣在線估計

(9)

(10)

已知，觀測量的誤差εδρ與位置誤差、鐘差的誤差ΔX=[εδx,εδy,εδz,εδtu]T之間的關系式

ΔX=(ATA)-1ATεδρ

(11)

由于

E(ΔXΔXT)=(ATA)-1E(εδρ(εδρ)T)=σ2GI

再結合式(11)可得

(12)

進一步，由式(10)和式(12)可得

對于偽距率觀測誤差方差與速度誤差和鐘差率誤差的估計方差同樣存在上述關系式。

此外，通常SINS導航解算出的位置和速度，分別是在大地坐標系和地理坐標系下的。因此，在對量測噪聲方差陣Rk進行估計之前，需要將權系數矩陣GI由ECEF坐標系轉換到大地坐標系下，記為GI,λLh，產生相應的精度因子為GDOPI,λLh；或由ECEF坐標系轉換到地理坐標系下，記為GI,ENU，產生相應的精度因子GDOPI,ENU。具體表達式如下

其中：Rk(·)表示偽距和偽距率對應的量測噪聲方差陣中的相關項元素； tr(·)表示大地坐標系和地理坐標系對應的權系數陣對角線元素之和。

圖1 自適應TEKF的流程圖Fig.1 Flow chart of the ATEKF

3 仿真驗證

為了驗證本文提出的基于ATEKF的SINS/BDS緊組合導航算法的正確性和有效性，進行了跑車數據離線仿真實驗。將本文提出的ATEKF的SINS/BDS緊組合導航算法與傳統卡爾曼濾波的SINS/BDS導航方法進行離線仿真，對定位、測速精度作比較。

首先，在進行跑車實驗的過程中采集BDS接收機數據和慣性測量數據，陀螺儀和加速度計的數據2.5ms存儲一次，BDS接收機數據100ms存儲一次。跑車試驗過程中的慣性測量單元(Inertial Measure-ment Unit，IMU)和BDS接收機的參數設置為：陀螺零偏2(°)/h(1σ)，零偏穩定性2(°)/h(1σ)；加速度計零偏0.5mg(1σ)，零偏穩定性1×10-4g(1σ)。BDS接收機位置誤差為10m(1σ)、速度誤差為1m/s(1σ)；鐘差和鐘漂分別取30m(1σ)和0.3m/s(1σ)；偽距和偽距率初始誤差取1m(1σ)和0.5m/s(1σ)。跑車實驗實物圖如圖2所示，Google地圖顯示的跑車實驗二維軌跡如圖3所示。用跑車實驗中采集的離線數據進行離線仿真實驗，實驗時長為700s，實驗結果如圖4和圖5所示。

圖2 跑車實驗實物圖Fig.2 Picture of vehicle experiment

圖3 基于MATLAB顯示的SINS/BDS組合導航跑車軌跡Fig.3 The vehicle track of SINS/BDS integrated navigation system on MATLAB

圖4 緊組合導航系統位置曲線比較Fig.4 Comparison of position curves in tightly integrated navigation system

圖5 緊組合導航系統速度曲線比較Fig.5 Comparison of velocity curves in tightly integrated navigation system

在圖4和圖5中，藍線表示傳統方法的仿真曲線，紅線表示本文方法的仿真曲線。從仿真結果可以得出：本文方法的精度優于傳統方法；位置和速度誤差都有改善，曲線較為平滑，誤差減小，導航效果穩定。傳統方法和本文方法的誤差比較如表1所示。

表1 傳統方法和本文方法的誤差比較

4 結論

本文結合新息和估算的GDOP值，對量測噪聲方差陣Rk進行在線估計，從而實現了自適應TEKF。仿真實驗結果表明，該方法具有較好的效果，速度誤差和位置誤差均明顯減小，系統的精度得到了明顯提高，濾波穩定性也得到了改善。最后，通過采集跑車實驗數據，對本文中設計的ATEKF的SINS/BDS緊組合導航系統進行了離線仿真實驗驗證，相較于傳統方法，定位測速精度均得到提高。