SINS/BDS緊組合系統(tǒng)中自適應(yīng)兩階段擴(kuò)展卡爾曼濾波

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

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

0 引言

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

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

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

1.1 狀態(tài)方程

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

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

(1)

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

偏差向量

系統(tǒng)噪聲向量

偏差噪聲向量

1.2 量測(cè)方程

(2)

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

2.1 兩階段卡爾曼濾波

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

(3)

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

(4)

無(wú)偏差狀態(tài)估計(jì)方程為

(5)

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

(6)

(7)

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

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

ρ-r0=AX+V

轉(zhuǎn)換為

ρ-rI=AIX+V

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

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

(8)

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

2.3 量測(cè)噪聲方差陣在線估計(jì)

(9)

(10)

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

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

(11)

由于

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

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

(12)

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

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

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

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

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

3 仿真驗(yàn)證

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

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

圖2 跑車(chē)實(shí)驗(yàn)實(shí)物圖Fig.2 Picture of vehicle experiment

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

圖4 緊組合導(dǎo)航系統(tǒng)位置曲線比較Fig.4 Comparison of position curves in tightly integrated navigation system

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

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

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

4 結(jié)論

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