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

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

0 引言

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

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

1 SINS/BDS緊組合系統(tǒng)模型

1.1 狀態(tài)方程

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

假設(shè)SINS/BDS組合導航系統(tǒng)，SINS的誤差模型具有下列形式

(1)

其中，狀態(tài)向量

偏差向量

系統(tǒng)噪聲向量

偏差噪聲向量

1.2 量測方程

(2)

2 自適應(yīng)兩階段卡爾曼濾波

2.1 兩階段卡爾曼濾波

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

(3)

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

(4)

無偏差狀態(tài)估計方程為

(5)

類似地，偏差狀態(tài)估計方程為

(6)

(7)

2.2 緊組合系統(tǒng)中的GDOP估算方法

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

ρ-r0=AX+V

轉(zhuǎn)換為

ρ-rI=AIX+V

相應(yīng)的幾何矩陣AI表示為

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

(8)

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

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

(9)

(10)

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

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

(11)

由于

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

再結(jié)合式(11)可得

(12)

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

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

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

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

圖1 自適應(yīng)TEKF的流程圖Fig.1 Flow chart of the ATEKF

3 仿真驗證

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

首先，在進行跑車實驗的過程中采集BDS接收機數(shù)據(jù)和慣性測量數(shù)據(jù)，陀螺儀和加速度計的數(shù)據(jù)2.5ms存儲一次，BDS接收機數(shù)據(jù)100ms存儲一次。跑車試驗過程中的慣性測量單元(Inertial Measure-ment Unit，IMU)和BDS接收機的參數(shù)設(shè)置為：陀螺零偏2(°)/h(1σ)，零偏穩(wěn)定性2(°)/h(1σ)；加速度計零偏0.5mg(1σ)，零偏穩(wěn)定性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所示。用跑車實驗中采集的離線數(shù)據(jù)進行離線仿真實驗，實驗時長為700s，實驗結(jié)果如圖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 緊組合導航系統(tǒng)位置曲線比較Fig.4 Comparison of position curves in tightly integrated navigation system

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

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

表1 傳統(tǒng)方法和本文方法的誤差比較

4 結(jié)論

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