馬 霞,王永勝

(1.中國(guó)西南電子技術(shù)研究所,成都 610036;2.北京市9236信箱,北京 100036)

1 引 言

捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)是利用慣性元件在載體內(nèi)部感測(cè)運(yùn)載體的運(yùn)動(dòng)加速度,通過(guò)積分運(yùn)算,從而求出導(dǎo)航參數(shù)以確定載體速度、位置、姿態(tài)等導(dǎo)航信息。SINS由加速度計(jì)和陀螺儀構(gòu)成,其缺點(diǎn)是陀螺儀存在著漂移誤差,致使定位精度隨時(shí)間不斷降低,以導(dǎo)航時(shí)間2 h計(jì)算,在1 h內(nèi)導(dǎo)航誤差約為0.8 nmile,2 h以上時(shí)導(dǎo)航誤差約為2 nmile。GPS系統(tǒng)具有高精度的定位及授時(shí)能力,能夠全天候、連續(xù)實(shí)時(shí)地提供高精度的三維速度和位置信息,且其定位能力誤差不會(huì)隨著時(shí)間的推移而發(fā)散。但GPS也有弱點(diǎn),如飛機(jī)做高機(jī)動(dòng)時(shí)定位不準(zhǔn),數(shù)據(jù)率低(約1 Hz)等。塔康(TACAN)[1]是一種無(wú)線電導(dǎo)航系統(tǒng),具有近程導(dǎo)航定位精度高特點(diǎn)。本文研究了SINS/GNSS/TACAN機(jī)載綜合導(dǎo)航定位算法,利用各導(dǎo)航定位子系統(tǒng)提供定位信息,采用廣義聯(lián)邦濾波技術(shù)完成信息融合。仿真實(shí)驗(yàn)表明,該系統(tǒng)具有較高定位精度。

2 廣義聯(lián)邦濾波器信息融合設(shè)計(jì)

根據(jù)聯(lián)邦濾波器設(shè)計(jì)特點(diǎn)[2],本文根據(jù)SINS、GPS和TACAN 3種導(dǎo)航源特點(diǎn)設(shè)計(jì)出了如圖1所示的機(jī)載綜合導(dǎo)航原理框圖。

圖1 機(jī)載綜合導(dǎo)航設(shè)計(jì)原理框圖Fig.1 Schematic block diagram of airborne integrated navigation system

共同參考系統(tǒng)選用SINS系統(tǒng),SINS輸出一方面給主濾波器,另一方面輸出給各子濾波器。在子濾波器1中完成SINS和GPS局部濾波,并把濾波局部估計(jì)值 X1(k)及其協(xié)方差P1(k)給主濾波器;在子濾波器2中完成SINS和TACAN局部濾波,并把濾波局部估計(jì)值 X2(k)及其協(xié)方差P2(k)給主濾波器;在主濾波器中完成子濾波器與主濾波器估計(jì)值進(jìn)行全局最優(yōu)估計(jì),進(jìn)行子濾波器的公共狀態(tài)矢量融合和時(shí)間更新,輸出可靠、準(zhǔn)確的導(dǎo)航參數(shù)誤差的全局最優(yōu)估計(jì)量。根據(jù)分散化全局濾波公式[3],得到全局最優(yōu)估計(jì)為

根據(jù)聯(lián)邦濾波器信息分配策略不同,本文選用有重置聯(lián)邦濾波器結(jié)構(gòu)。重置中把由子濾波器與主濾波器合成的全局估計(jì)值 X(k)及其相應(yīng)的協(xié)方差陣 P(k)再反饋回到子濾波器(圖1中子濾波器和主濾波器之間選用雙向箭頭),以重置子濾波器的估計(jì),即:

式中,βi(i=1,2)稱為“信息分配因子”。FLAC(模糊邏輯自適應(yīng)控制器)用來(lái)在線實(shí)時(shí)估計(jì)加權(quán)因子α(k),增強(qiáng)濾波的魯棒性。

圖1中機(jī)載綜合導(dǎo)航系統(tǒng)對(duì)子導(dǎo)航源SINS實(shí)施閉環(huán)負(fù)反饋校正,實(shí)現(xiàn)動(dòng)態(tài)校正(比力方程積分初始條件重新設(shè)置)、動(dòng)態(tài)對(duì)準(zhǔn)(姿態(tài)矩陣微分方程積分初始條件重新設(shè)置)和動(dòng)態(tài)標(biāo)定(陀螺和加速度計(jì)隨機(jī)誤差補(bǔ)償)的目的,提高導(dǎo)航系統(tǒng)性能。

3 坐標(biāo)系的定義及坐標(biāo)轉(zhuǎn)換

對(duì)于SINS,觀測(cè)量來(lái)源于慣性加速度計(jì)及陀螺儀。加速度計(jì)給出比力向量fi在載體坐標(biāo)系(b)三軸上的分量;后者觀測(cè)的是載體坐標(biāo)系相對(duì)于慣性坐標(biāo)系旋轉(zhuǎn)角速度向量在b系三軸上的3個(gè)分量。

GPS觀測(cè)量一般是在地心直角坐標(biāo)系中的值。TACAN觀測(cè)量是目標(biāo)飛機(jī)相對(duì)于塔康信標(biāo)臺(tái)的斜距和反方位角。

針對(duì)圖1中子濾波器1中狀態(tài)方程和觀測(cè)方程都選用地心直角坐標(biāo)系,則SINS需要一系列坐標(biāo)轉(zhuǎn)換,即從載體坐標(biāo)系(b)到地心直角坐標(biāo)系(e)坐標(biāo)轉(zhuǎn)換,有關(guān)坐標(biāo)轉(zhuǎn)換公式見(jiàn)文獻(xiàn)[4]。

子濾波器2中狀態(tài)坐標(biāo)系選用直角坐標(biāo)系,而觀測(cè)方程中輸入量選擇大地線長(zhǎng)度、飛機(jī)方位角和飛機(jī)氣壓高度。

利用塔康的信標(biāo)位置及塔康設(shè)備測(cè)得的飛機(jī)相對(duì)于塔康地面臺(tái)的斜距、反方位角和飛機(jī)實(shí)測(cè)氣壓高度求飛機(jī)大地線長(zhǎng)度和方位角。

式中,R為塔康所在地地球半徑,D為飛機(jī)到塔康的斜距,H表示飛機(jī)大地線高度,h表示塔康信標(biāo)臺(tái)高度。

利用塔康的信標(biāo)位置及捷聯(lián)慣性導(dǎo)航提供載體位置,求出飛機(jī)大地線長(zhǎng)度和方位角。具體坐標(biāo)系轉(zhuǎn)換見(jiàn)文獻(xiàn)[5]。

4 SINS/GNSS/TACAN機(jī)載綜合導(dǎo)航定位算法

系統(tǒng)濾波采用卡爾曼濾波平方根分解算法[6,7],通過(guò)對(duì)慣性導(dǎo)航系統(tǒng)誤差傳遞和誤差分析[2],可以建立加速度計(jì)和陀螺儀誤差源誤差狀態(tài)空間模型,得到系統(tǒng)連續(xù)狀態(tài)方程,經(jīng)過(guò)離散化后再應(yīng)用于卡爾曼濾波方程中。

4.1 主濾波器

主濾波器接收慣性導(dǎo)航數(shù)據(jù)輸出,慣性導(dǎo)航系統(tǒng)狀態(tài)方程為其中,取濾波器慣性系統(tǒng)狀態(tài)向量為

式中,δXSINS、δYSINS、δZSINS為載機(jī)的位置誤差,δVXSINS、δVYSINS、δVZSINS為載機(jī)的速 度誤差,φx、φy、φz為載機(jī)的姿態(tài)角誤差 ,δfax、δfay、δfaz為載機(jī)的加速度誤差,δVθx、δVθy、δVθz為載機(jī)的姿態(tài)角速度誤差。其中FSINS為連續(xù)系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣,GSINS為系統(tǒng)噪聲矩陣,WSINS為系統(tǒng)噪聲,FSINS=

μ為地球引力常數(shù),即 μ=3.986005×1014m3/s2;

為比力矢量的反對(duì)稱陣;

F4=A-C-be;F6=-A-C-be其中A-C-be表示從載體坐標(biāo)系b到地心直角坐標(biāo)系e變換矩陣[4];

g-a-m、b-m分別為加速度計(jì)和陀螺儀常數(shù)誤差。

4.2 局部濾波器1

GPS/SINS子濾波器采用位置和速度進(jìn)行組合方式,卡爾曼濾波的量測(cè)量為慣性導(dǎo)航SINS的相應(yīng)輸出信息與GPS導(dǎo)航信息相減,得到測(cè)量輸入值:

則GPS/SINS子濾波器的狀態(tài)方程為式(4),量測(cè)方程為

4.3 局部濾波器2

SINS/TACAN子濾波器采用大地基線長(zhǎng)、載飛到塔康方位角和載機(jī)的大氣高度進(jìn)行組合方式。根據(jù)圖2中球面三角形N1P1P2可以得到大地線長(zhǎng)和大地線方位角誤差模型。慣性導(dǎo)航SINS的相應(yīng)輸出信息與TACAN導(dǎo)航信息相減,得到測(cè)量輸入值:

式中,SSINS為 SINS計(jì)算出大地線長(zhǎng)度,STACAN為T(mén)ACAN計(jì)算出大地線長(zhǎng)度,αSINS為SINS計(jì)算出大地線方位,αTACAN為T(mén)ACAN計(jì)算出大地線方位。

量測(cè)方程為

α為捷聯(lián)慣導(dǎo)提供大地線反方位角度,B和L為捷聯(lián)慣導(dǎo)提供飛機(jī)的緯度和經(jīng)度。

圖2 大地線示意圖Fig.2 Illustration of geodesic line

4.4 SINS/GNSS/TACAN機(jī)載綜合濾波算法

對(duì)式(4)進(jìn)行離散化后得到卡爾曼濾波系統(tǒng)狀態(tài)方程為

卡爾曼濾波系統(tǒng)觀測(cè)方程為

組合導(dǎo)航融合算法則采用式(1)和式(2)。

融合規(guī)則如下:

(1)假設(shè)SINS數(shù)據(jù)一直有效,即SINS設(shè)備一直工作;

(2)當(dāng)GPS數(shù)據(jù)有效、TACAN有效時(shí),選用三者組合導(dǎo)航濾波算法,即規(guī)則(1)、(2)。

(3)當(dāng)GPS數(shù)據(jù)有效、TACAN無(wú)效時(shí),則局部濾波器1作為全局輸出;

(4)當(dāng)GPS數(shù)據(jù)無(wú)效、TACAN有效時(shí),則局部濾波器2作為全局輸出;

(5)當(dāng)GPS數(shù)據(jù)無(wú)效、TACAN無(wú)效時(shí),則SINS的濾波輸出結(jié)果作為全局輸出。

5 仿真研究與結(jié)果分析

仿真條件為:SINS數(shù)據(jù)輸出率為50 Hz,加速度計(jì)常值偏置誤差在載機(jī)坐標(biāo)系三維上分別為0.00436、0.00258、0.0009;陀螺常值偏置誤差在 3個(gè)姿態(tài) 上誤差 分別為 0.063288°/h、0.01489°/h、0.07588°/h;GPS數(shù)據(jù)輸出率為1 Hz,在地心坐標(biāo)系下常值誤差分別為2 m、-11 m、-6 m,相關(guān)的隨機(jī)誤差分別為5 m、33.1266 m、17 m;TACAN數(shù)據(jù)輸出率為1 Hz,TACAN地面臺(tái)的經(jīng)度為108.8549°,緯度為34.1099°,高度為140 m,從飛機(jī)到塔康地面臺(tái)的斜距偏置誤差為36 m,反方位角偏置誤差為0.95°;仿真中主濾波器濾波周期為6 s,每隔6 s進(jìn)行一次閉環(huán)反饋矯正。

仿真結(jié)果見(jiàn)圖3。從圖3中看出,系統(tǒng)運(yùn)行了1 h 25 min,SINS/GNSS/TACAN機(jī)載綜合后導(dǎo)航有效地抑制了慣導(dǎo)誤差隨時(shí)間累積,同時(shí)定位精度也得到了很大提高,在地心直角坐標(biāo)中三維上誤差分別為3.3680 m、25.5134 m、5.6288 m,CEP 誤差為26.3431 m,滿足導(dǎo)航定位精度要求。

圖3 系統(tǒng)運(yùn)行1 h 25 min載機(jī)軌跡和誤差圖Fig.3 Airborne trace and error figure duringrunning system for 1 hour 25 minutes

6 結(jié) 論

將GPS和TANCAN作為SINS誤差校準(zhǔn)參考源,在1s、2s、3s、4s、5s時(shí)采用 SINS/TANCAN 子濾波器結(jié)果校準(zhǔn)SINS誤差,在主濾波器濾波周期即6 s時(shí),選用三者(SINS/GNSS/TACAN)綜合導(dǎo)航濾波結(jié)果校準(zhǔn)SINS誤差。仿真結(jié)果表明,此系統(tǒng)能有效地校準(zhǔn)慣導(dǎo)漂移誤差,使慣性導(dǎo)航誤差控制在一定誤差范圍之內(nèi),滿足了定位誤差要求。在實(shí)際戰(zhàn)場(chǎng)環(huán)境中,GPS、TACAN某些時(shí)候受到干擾不可用,由于系統(tǒng)采用了聯(lián)邦濾波器,可以很好隔離不可用導(dǎo)航源,使得系統(tǒng)具有更好的容錯(cuò)性和抗干擾性。

[1]張忠興,李曉明,張景偉,等.無(wú)線電導(dǎo)航理論與系統(tǒng)[M].西安:陜西科學(xué)技術(shù)出版社,1998:1-8.ZHANG Zhong-xing,LI Xiao-min,ZHANG Jing-wei,et al.Radio Short-range NavigationTheory andSystem[M].Xi′an:Shaanxi Science and Technology Press,1998.(in Chinese)

[2]張國(guó)良,曾靜.組合導(dǎo)航原理與技術(shù)[M].西安:西安交通大學(xué)出版社,2008:134-136.ZHANG Guo-liang,ZENG Jing.The principle and Technology of Integrated Naiagation[M].Xi′an:Xi′an Jiaotong Universityvi Press,2008:134-136.(in Chinese)

[3]顧啟泰,王頌.聯(lián)邦濾波器的最優(yōu)性[J].清華大學(xué)報(bào)告(自然科學(xué)版),2003,43(11):1460-1463.GU Qi-tai,WANG Song.Optimized federated filter[J].Jowrnarl of Tsinghua University(Science&Technology Edition),2003,43(11):1460-1463.(in Chinese)

[4]左安元,劉剛.一種機(jī)載定位坐標(biāo)轉(zhuǎn)換方法誤差分析及仿真[J].電訊技術(shù),2006,46(6):58-62.ZUO An-yuan,LIU Gang.Error Analysis andSimulation of a Coordinate Conversion Scheme for Airborne Position[J].Telecommunication Engineering,2006,46(6):58-62.(in Chinese)

[5]呂曉華,楊國(guó)平.大型GIS中方位距離的量算方法[J].測(cè)繪學(xué)院學(xué)報(bào),2004,21(2):138-140.LU Xiao-hua,YANG Guo-ping.Algorithms of the Measurements of Azimuth and Distance in Large Area GIS[J].Journal of GeomaticsScience and Technology,2004,21(2):138-140.(in Chinese)

[6]Carlson N A.Federated Square Root Filter for Decentralized Parrallel Processes[J].IEEE Transactions on Aerospace and Electronic Systems,1990,AES-26(3):517-525.

[7]張樹(shù)春,胡廣大.平方根濾波及其在目標(biāo)跟蹤方面的應(yīng)用[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2008,40(5):700-704.ZHANG Shu-chun,HU Guang-da.Square-root filtering and its applicationsin target tracking[J].Journal of Harbin Institute of T echnology,2008,40(5):700-704.(in Chinese)