協(xié)方差交叉融合的慣性/衛(wèi)星/雷達(dá)組合導(dǎo)航研究

周 峰 梁祿揚(yáng) 林 平

北京航天自動(dòng)控制研究所，北京 100854

0 引言

未來(lái)戰(zhàn)爭(zhēng)是體系與體系的對(duì)抗，機(jī)群協(xié)同編隊(duì)作為一種新型的空中力量將越來(lái)越受重視[1]，特別是有人駕駛飛機(jī)與無(wú)人機(jī)混合編隊(duì)將成為一種新的作戰(zhàn)編隊(duì)樣式。有人駕駛飛機(jī)和無(wú)人機(jī)的導(dǎo)航設(shè)備通常選用慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)，慣性導(dǎo)航系統(tǒng)具有自主、隱蔽及短航時(shí)高精度的特點(diǎn)，衛(wèi)星導(dǎo)航系統(tǒng)具有定位精度高、誤差不隨時(shí)間積累的特點(diǎn)，將兩者融合得到高精度高可靠的組合導(dǎo)航系統(tǒng)已廣泛應(yīng)用于各種飛機(jī)。然而，在復(fù)雜對(duì)抗的實(shí)戰(zhàn)環(huán)境下，衛(wèi)星導(dǎo)航信號(hào)容易被干擾，慣性導(dǎo)航定位誤差隨時(shí)間而積累，對(duì)單個(gè)飛機(jī)來(lái)講，若沒有其他輔助導(dǎo)航系統(tǒng)對(duì)其進(jìn)行修正，定位誤差的迅速增長(zhǎng)使其無(wú)法長(zhǎng)時(shí)間使用。通過相對(duì)測(cè)量手段，有效利用協(xié)同編隊(duì)成員之間的相對(duì)定位信息，實(shí)現(xiàn)自身的導(dǎo)航定位，不但能夠有效降低整個(gè)編隊(duì)導(dǎo)航系統(tǒng)的成本，還能提高協(xié)同編隊(duì)機(jī)群的抗干擾能力，提升實(shí)戰(zhàn)化作戰(zhàn)水平。

國(guó)內(nèi)外已有學(xué)者開展了機(jī)群編隊(duì)相對(duì)導(dǎo)航方面的研究，文獻(xiàn)[2-3]基于編隊(duì)成員相互測(cè)距的方法，實(shí)現(xiàn)機(jī)群組網(wǎng)定位，該文獻(xiàn)通過建立簡(jiǎn)化的誤差模型，以編隊(duì)成員輸出計(jì)算得到的距離與測(cè)量距離之差作為觀測(cè)值，采用卡爾曼濾波對(duì)這些誤差狀態(tài)進(jìn)行估計(jì)。該方法需要建立一個(gè)高維數(shù)的狀態(tài)估計(jì)模型，并且由于各成員兩兩之間均有數(shù)據(jù)通信，因此計(jì)算量和機(jī)載數(shù)據(jù)鏈通信負(fù)荷較大。文獻(xiàn)[4]在假設(shè)慣性導(dǎo)航誤差近似呈零均值高斯分布條件下，提出了一種利用成像導(dǎo)引頭對(duì)航路上任意未知地標(biāo)被動(dòng)觀測(cè)的彈群定位誤差協(xié)同修正方法保持導(dǎo)航精度。文獻(xiàn)[5]給出了自主編隊(duì)飛行計(jì)劃(Autonomous Formation Flight Project，AFFP)中所采用的基于 INS/GPS 的相對(duì)導(dǎo)航系統(tǒng)，該系統(tǒng)主要應(yīng)用于有人駕駛飛機(jī)和無(wú)人機(jī)的相對(duì)導(dǎo)航狀態(tài)獲取、相對(duì)導(dǎo)航和編隊(duì)飛行控制技術(shù)，該方法自主性比較差，依賴于GPS信號(hào)的可用性，一旦GPS信號(hào)受到干擾，相對(duì)導(dǎo)航精度下降很快。

本文采用有人駕駛飛機(jī)(長(zhǎng)機(jī))和無(wú)人機(jī)(僚機(jī))組成機(jī)群協(xié)同編隊(duì)模式，長(zhǎng)機(jī)裝有高精度組合導(dǎo)航系統(tǒng)，僚機(jī)采用低成本慣導(dǎo)和衛(wèi)星導(dǎo)航的組合導(dǎo)航系統(tǒng)，并裝有雷達(dá)測(cè)角測(cè)距裝置，在協(xié)同編隊(duì)飛行過程中能夠測(cè)量相對(duì)長(zhǎng)機(jī)的角度和距離。假設(shè)僚機(jī)與長(zhǎng)機(jī)機(jī)載數(shù)據(jù)鏈系統(tǒng)時(shí)間同步，則能夠?qū)崟r(shí)接收長(zhǎng)機(jī)的導(dǎo)航定位信息。提出慣性/衛(wèi)星/雷達(dá)協(xié)方差信息融合卡爾曼濾波器，通過雷達(dá)相對(duì)定位與慣性/衛(wèi)星組合導(dǎo)航信息融合技術(shù)，解決了機(jī)群協(xié)同作戰(zhàn)在衛(wèi)星導(dǎo)航信號(hào)拒止條件下的導(dǎo)航精度下降問題，提升了實(shí)戰(zhàn)條件下僚機(jī)的導(dǎo)航精度與可靠性，計(jì)算簡(jiǎn)便易于工程實(shí)現(xiàn)。

1 慣性/衛(wèi)星組合導(dǎo)航卡爾曼濾波器

僚機(jī)導(dǎo)航系統(tǒng)采用基于偽距偽距率的緊組合導(dǎo)航方案，組合導(dǎo)航卡爾曼濾波器建模時(shí)用到的坐標(biāo)系有北東地導(dǎo)航坐標(biāo)系(用n表示)、慣性坐標(biāo)系(用i表示)、地球坐標(biāo)系(用e表示)和機(jī)體坐標(biāo)系(用b表示)。

1.1 系統(tǒng)狀態(tài)方程

卡爾曼濾波器的系統(tǒng)狀態(tài)變量為

其中狀態(tài)量分別為慣導(dǎo)系統(tǒng)3個(gè)方向的速度誤差、姿態(tài)誤差、加速度計(jì)常值誤差和陀螺常值誤差?？柭鼮V波器位置、速度和姿態(tài)誤差方程為

(1)

(2)

(3)

1.2 觀測(cè)量

計(jì)算INS偽距、偽距率時(shí)，第i顆可見導(dǎo)航衛(wèi)星對(duì)應(yīng)INS偽距、偽距率可由以下公式計(jì)算：

(4)

(5)

式中：Xs，Ys，Zs，Vxs，Vys和Vzs分別表示第i顆可見導(dǎo)航衛(wèi)星的位置、速度在導(dǎo)航坐標(biāo)系中的分量。

將INS偽距、偽距率與接收機(jī)輸出的偽距、偽距率作差作為量測(cè)量：

(6)

(7)

2 雷達(dá)相對(duì)定位卡爾曼濾波器

雷達(dá)相對(duì)定位卡爾曼濾波器建模時(shí)，使用僚機(jī)開始相對(duì)導(dǎo)航時(shí)刻長(zhǎng)機(jī)質(zhì)心為坐標(biāo)原點(diǎn)的北東地地理坐標(biāo)系，簡(jiǎn)稱相對(duì)導(dǎo)航坐標(biāo)系。

2.1 系統(tǒng)狀態(tài)方程

選取長(zhǎng)機(jī)與僚機(jī)相對(duì)位置分量和相對(duì)速度分量作為狀態(tài)向量，

(8)

式中：xmt，ymt和zmt為長(zhǎng)機(jī)在相對(duì)導(dǎo)航坐標(biāo)系下的位置分量，Vxmt，Vymt和Vzmt為其速度分量；xdt，ydt和zdt為僚機(jī)在相對(duì)導(dǎo)航坐標(biāo)系下的位置分量，Vxdt，Vydt和Vzdt為其速度分量。

狀態(tài)方程為

(9)

(10)

(11)

2.2 觀測(cè)量

僚機(jī)通過雷達(dá)對(duì)長(zhǎng)機(jī)進(jìn)行觀測(cè)測(cè)量[6]，可以獲得僚機(jī)與長(zhǎng)機(jī)的相對(duì)距離ρ，相對(duì)方位角qT，相對(duì)俯仰角qD。

(12)

由狀態(tài)變量表示的量測(cè)變量函數(shù)關(guān)系為：

(13)

3 慣性/衛(wèi)星/雷達(dá)協(xié)方差交叉航跡融合

3.1 協(xié)方差交叉航跡融合

將雷達(dá)相對(duì)定位的結(jié)果與慣性/衛(wèi)星組合導(dǎo)航結(jié)果在北東地導(dǎo)航坐標(biāo)系下采用協(xié)方差交叉航跡融合，慣性/衛(wèi)星/雷達(dá)協(xié)方差交叉航跡融合包括2個(gè)濾波器，分別為前兩章給出的GPS/INS緊組合卡爾曼濾波器和主動(dòng)雷達(dá)定位卡爾曼濾波器。航跡融合的結(jié)構(gòu)圖如圖1所示。

圖1 慣性/衛(wèi)星/雷達(dá)航跡融合框圖

將雷達(dá)相對(duì)定位濾波器輸出的結(jié)果轉(zhuǎn)換到北東地導(dǎo)航坐標(biāo)系下，設(shè)為x1，作為局部航跡1；選取慣性衛(wèi)星緊組合濾波器輸出的位置x2為局部航跡2，使用協(xié)方差交叉融合卡爾曼濾波器對(duì)局部航跡1和局部航跡2進(jìn)行融合，該濾波器的實(shí)際精度高于每個(gè)局部濾波器的精度，應(yīng)用協(xié)方差交叉融合方法的卡爾曼濾波器[7-8]為

(14)

(15)

(16)

其中權(quán)系數(shù)β∈[0,1]，且滿足

(17)

式中：P1和P2分別為局部航跡1和局部航跡2的誤差協(xié)方差矩陣，tr為矩陣的跡，權(quán)系數(shù)β可根據(jù)式(17)用Fibinacci法[9]最優(yōu)化問題搜索求解得到。這里β=0.31。

當(dāng)衛(wèi)星導(dǎo)航信號(hào)受到干擾，慣性/衛(wèi)星組合導(dǎo)航輸出的位置由于慣性器件的誤差會(huì)產(chǎn)生漂移，定位誤差變大，局部航跡1和局部航跡2隨時(shí)間增加差異會(huì)變大，此時(shí)協(xié)方差交叉融合方法的卡爾曼濾波器輸出的結(jié)果誤差變大，需要重新設(shè)定權(quán)系數(shù)β。

3.2 航跡融合決策

慣性/衛(wèi)星/雷達(dá)協(xié)方差交叉融合方法在使用時(shí)需考慮傳感器測(cè)量信息的狀態(tài)，本文提出使用決策邏輯判斷來(lái)調(diào)整融合權(quán)系數(shù)。決策邏輯根據(jù)決策統(tǒng)計(jì)距離D和決策樹進(jìn)行航跡融合選擇，決策樹的結(jié)構(gòu)如圖2所示。門限T是根據(jù)兩條局部航跡相距的最大允許程度確定的，若統(tǒng)計(jì)距離D小于門限T，就意味著兩個(gè)局部傳感器所給出的兩條航跡非常接近，采用最優(yōu)協(xié)方差交叉融合算法生成航跡；如果統(tǒng)計(jì)距離D大于門限T，就需要優(yōu)化協(xié)方差交叉融合算法參數(shù)，從而生成系統(tǒng)航跡。

圖2 多模型融合決策樹

設(shè)P1和P2分別為估計(jì)x1和x2的誤差協(xié)方差陣。統(tǒng)計(jì)距離D定義為局部航跡之間的距離[9]：

D=(x1-xTF)T(P1+PTF)-1(x1-xTF)

(18)

由于：

P1+PTF=P1(P1+P2)-1(P1+2P2)

(19)

x1-xTF=P1(P1+P2)-1(x1-x2)

(20)

則：

D=(x1-x2)T(P1+P2)-1P1(P1+2P2)-1(x1-x2)

(21)

統(tǒng)計(jì)距離D實(shí)際上是度量局部航跡1和局部航跡2接近程度的一個(gè)量。在衛(wèi)星導(dǎo)航信號(hào)正常的情況下，兩條航跡很接近，兩個(gè)濾波器正常工作，可以采用最優(yōu)權(quán)系數(shù)β計(jì)算系統(tǒng)的航跡輸出。若D>T，則表示經(jīng)過慣性/衛(wèi)星組合導(dǎo)航濾波器校正后的輸出誤差較大或者發(fā)散，此時(shí)使用新的權(quán)系數(shù)β′=0.93，降低慣性/衛(wèi)星組合導(dǎo)航輸出的航跡在融合后輸出中的比重，確保融合后的航跡精度。

4 仿真結(jié)果與分析

4.1 仿真條件

在北東地導(dǎo)航坐標(biāo)系下描述長(zhǎng)機(jī)與僚機(jī)的運(yùn)動(dòng)，長(zhǎng)機(jī)的位置方程為：

rx=50t+5cos(0.01t)

(22)

ry=1000sin(0.04t)

(23)

rz=-5t

(24)

僚機(jī)的位置方程為：

rx=50t+5cos(0.01t)+2cos(0.001t)-1000

(25)

ry=1000sin(0.04t)+2cos(0.001t)-1500

(26)

rz=-5t+4cos(0.001t)

(27)

僚機(jī)雷達(dá)測(cè)角誤差為0.3°，測(cè)距誤差為15m。僚機(jī)慣導(dǎo)采用低成本IMU，誤差如表1所示。

表1 子慣導(dǎo)IMU的誤差

4.2 仿真結(jié)果

協(xié)同機(jī)群起飛后，僚機(jī)90s開始雷達(dá)測(cè)量，慣性/衛(wèi)星/雷達(dá)協(xié)方差交叉融合卡爾曼濾波航跡輸出結(jié)果如圖3～ 5所示。從圖中可以看出，經(jīng)融合后的定位精度在1.5m以內(nèi)。曲線中的鋸齒形波動(dòng)主要由雷達(dá)測(cè)角誤差引起，通過與慣性衛(wèi)星組合導(dǎo)航結(jié)果融合后，該波動(dòng)明顯減小。

圖3 航跡融合后x方向位置誤差

圖4 航跡融合后y方向位置誤差

圖5 航跡融合后z方向位置誤差

起飛后120s～240s之間衛(wèi)星導(dǎo)航信號(hào)丟失，經(jīng)航跡決策判斷慣性/衛(wèi)星/雷達(dá)協(xié)方差交叉融合使用新的權(quán)重系數(shù)，航跡輸出的仿真曲線如圖6～ 8所示。從圖中可以看出，衛(wèi)星導(dǎo)航信號(hào)丟失后，所提出的慣性/衛(wèi)星/雷達(dá)協(xié)方差交叉融合卡爾曼濾波器的x和z方向輸出有效地抑制了位置誤差變大。當(dāng)衛(wèi)星導(dǎo)航信號(hào)恢復(fù)后，定位精度立即恢復(fù)到衛(wèi)星信號(hào)丟失前的水平。

圖6 航跡融合結(jié)果與慣性/衛(wèi)星x方向位置誤差比較

圖7 航跡融合結(jié)果與慣性/衛(wèi)星y方向位置誤差比較

圖8 航跡融合結(jié)果與慣性/衛(wèi)星z方向位置誤差比較

5 結(jié)論

在機(jī)群協(xié)同編隊(duì)中，僚機(jī)使用基于協(xié)方差交叉融合的慣性/衛(wèi)星/雷達(dá)定位方法，具有高精度高可靠的特點(diǎn)，特別在衛(wèi)星導(dǎo)航受干擾不可用的情況下，能夠有效地抑制慣性導(dǎo)航誤差漂移，提升了機(jī)群協(xié)同編隊(duì)的作戰(zhàn)性能。