一種長機(jī)故障情況下機(jī)群編隊協(xié)同定位算法

劉曉洋,徐勝紅

(海軍航空大學(xué) a.研究生管理大隊； b.控制工程系， 山東 煙臺 264001)

面對現(xiàn)代戰(zhàn)爭日益復(fù)雜的作戰(zhàn)環(huán)境，機(jī)群編隊協(xié)同作戰(zhàn)這種新型空戰(zhàn)模式受到了越來越多的關(guān)注[1]。協(xié)同定位作為協(xié)同作戰(zhàn)的關(guān)鍵技術(shù)之一，其定位精度的高低直接影響整個編隊協(xié)同作戰(zhàn)效果。目前，機(jī)群協(xié)同定位主要采用高低精度搭配的主從式結(jié)構(gòu)，然而在編隊執(zhí)行任務(wù)時長機(jī)很可能在敵方的干擾作用下失去與僚機(jī)的通信，無法為編隊協(xié)同定位提供高精度的定位信息，導(dǎo)致機(jī)群定位效果不佳。因此，如何在長機(jī)故障情況下實現(xiàn)協(xié)同定位已成為急需解決的重要問題。

針對機(jī)群編隊缺少誤差有界絕對導(dǎo)航信息的問題，文獻(xiàn)[2]提出了一種基于CEC與捷聯(lián)慣性導(dǎo)航(SINS)組合的相對導(dǎo)航算法，提高了平臺的導(dǎo)航精度，彌補(bǔ)了SINS誤差隨航時積累的不足，但該算法無法滿足動平臺編隊協(xié)同作戰(zhàn)的要求。文獻(xiàn)[3]在無GPS條件下，提出了一種基于機(jī)間相互測距信息并利用幾何圖形平移旋轉(zhuǎn)來估計各飛機(jī)導(dǎo)航誤差的方法。該方法有效延緩了慣導(dǎo)位置誤差的發(fā)散速度，但沒有考慮飛機(jī)高度信息，只是在平面內(nèi)進(jìn)行了機(jī)群協(xié)同定位，且該算法需要機(jī)群隊形的先驗知識，有較高的局限性。文獻(xiàn)[4]通過編隊成員INS位置信息和機(jī)間測距信息作差得到量測值來提高各成員INS的位置精度，但該方法建立的模型維數(shù)過高，且各成員兩兩之間均要求通信，因此對計算機(jī)和數(shù)據(jù)鏈的要求較高。本文在對多平臺INS輸出信息進(jìn)行最優(yōu)綜合實現(xiàn)INS誤差修正[5]的基礎(chǔ)上，提出了一種分布式機(jī)群編隊協(xié)同定位算法，在保證長機(jī)失聯(lián)前后編隊協(xié)同導(dǎo)航系統(tǒng)模型變動較小的情況下抑制編隊整體定位誤差發(fā)散。

1 問題描述與方案設(shè)計

協(xié)同導(dǎo)航系統(tǒng)定位算法結(jié)構(gòu)如圖1所示，長機(jī)搭載精度較高的INS/GPS組合導(dǎo)航系統(tǒng)，其他成員設(shè)為僚機(jī)，除搭載較低精度的INS之外，均配備有光電測距測角傳感器等相對導(dǎo)航設(shè)備，用以測量與長機(jī)之間相對距離和方位等相對信息。長機(jī)通過機(jī)載數(shù)據(jù)鏈廣播高精度導(dǎo)航信息，為僚機(jī)提供量測信息。當(dāng)長機(jī)因被擊落等原因與編隊失去聯(lián)系后，僚機(jī)無法獲得高精度的量測信息，若僅依靠搭載的低精度INS則定位誤差會迅速發(fā)散。

圖1 協(xié)同導(dǎo)航系統(tǒng)定位算法結(jié)構(gòu)

此時，可選擇其中的某一架僚機(jī)作為相對導(dǎo)航參考點，視其為偽長機(jī)。偽長機(jī)的選取可依據(jù)與其他無人機(jī)的距離關(guān)系，假設(shè)僚機(jī)i與僚機(jī)j之間的直線距離為dij，則僚機(jī)i與編隊其他成員的距離和Di可以表示為

選作偽長機(jī)的僚機(jī)i所對應(yīng)的Di應(yīng)滿足以下關(guān)系：

Di=min(D1,D2,…,Dn)

選定偽長機(jī)后，其他僚機(jī)一方面以偽主機(jī)為基準(zhǔn)點測量相對距離和相對方位等相對導(dǎo)航信息作為僚機(jī)卡爾曼濾波器的量測信息，另一方面通過機(jī)載數(shù)據(jù)鏈向偽長機(jī)提供通過僚機(jī)卡爾曼濾波器得到的狀態(tài)估計。偽長機(jī)與長機(jī)類似，通過機(jī)載數(shù)據(jù)鏈為僚機(jī)提供量測信息，不同之處在于，除了提供量測信息外，偽長機(jī)卡爾曼濾波器還反饋其狀態(tài)估計給僚機(jī)用于重置僚機(jī)狀態(tài)。自此，整個機(jī)群編隊構(gòu)成了一個卡爾曼濾波閉環(huán)系統(tǒng)。為了更加簡潔地說明算法，本文以3機(jī)為例建立系統(tǒng)模型。

2 系統(tǒng)數(shù)學(xué)模型

2.1 僚機(jī)部分系統(tǒng)方程

選取地理坐標(biāo)系為導(dǎo)航坐標(biāo)系(n系)，僚機(jī) 的INS誤差狀態(tài)方程為

(1)

式中，狀態(tài)量Xi為

εbxiεbyiεbziεrxiεryiεrzi▽xi▽yi▽zi]T

系統(tǒng)噪聲Wi為

其中，δλi、δφi、δhi為三維位置誤差，δvEi、δvNi、δvUi為三維速度誤差，εbxi、εbyi、εbzi為3個陀螺儀隨機(jī)常值漂移，φEi、φNi、φUi為平臺坐標(biāo)系與導(dǎo)航坐標(biāo)系之間誤差產(chǎn)生的3個平臺失準(zhǔn)角，εrxi、εryi、εrzi為3個陀螺儀一階馬爾科夫漂移，▽xi、▽yi、▽zi為3個加速度計一階馬爾科夫漂移。

僚機(jī)i狀態(tài)方程為自身INS與偽長機(jī)INS誤差狀態(tài)方程的疊加，即

(2)

如圖1所示，在導(dǎo)航坐標(biāo)系(n系)中，僚機(jī)i量測值Zi是由僚機(jī)i通過數(shù)據(jù)鏈?zhǔn)盏降膫伍L機(jī)INS信息與自身INS信息和僚機(jī)測量的相對導(dǎo)航信息依次相減得到的，即

(3)

(4)

結(jié)合式(3)和式(4)可得僚機(jī)狀態(tài)量與量測值的關(guān)系為

(5)

由于系統(tǒng)模型是在n系下建立的,而相對導(dǎo)航的測量是以機(jī)體坐標(biāo)系(b系)為基準(zhǔn)，因此需要對相對導(dǎo)航坐標(biāo)轉(zhuǎn)換。在b系中，僚機(jī)測量的相對導(dǎo)航信息存在誤差，通過激光傳感器測得的相對距離、仰角和方位角均可分為真實值與誤差值兩部分，將如圖2所示沿著b系三個坐標(biāo)軸方向分解，有

(6)

圖2 僚機(jī)相對速度(位置)測量示意圖

假設(shè)相對導(dǎo)航測量誤差比較小，則有cos(ωα)≈1、cos(ωβ)≈1、sin(ωα)≈1、sin(ωβ)≈1，忽略高階小量，即φvωα≈0、φvωβ≈0、φαωβ≈0，式(6)可以寫成以下形式：

(7)

式中,

b系到n系的變換矩陣[6]為

(8)

式中，γ、θ、φ分別為僚機(jī)真實橫滾、俯仰和航向角，由于慣性導(dǎo)航系統(tǒng)解算的姿態(tài)角誤差極小，用慣性導(dǎo)航系統(tǒng)解算出的姿態(tài)角可以近似替代真實姿態(tài)角，即

(9)

則由式(7)可以得到相對導(dǎo)航在n系的誤差值為

(10)

INS輸出的三維位置信息為經(jīng)、緯、高，相對導(dǎo)航傳感器輸出的三維位置信息為三個方向的距離，常規(guī)單位是m，為了與相對導(dǎo)航距離信息量綱匹配，對INS輸出的經(jīng)度和緯度進(jìn)行了轉(zhuǎn)換

(11)

結(jié)合式(4)、(5)、(10)和(11)，僚機(jī)i量測方程為

Zi(t)=Hi(t)Xi(t)+Vi(t)

(12)

式中,

2.2 偽長機(jī)部分系統(tǒng)模型

偽長機(jī)濾波器的狀態(tài)量是各編隊成員狀態(tài)的組合

由INS的誤差服從正態(tài)分布特性可知，多套INS系統(tǒng)在同一位置工作時通過對其輸出加權(quán)平均可以提高定位精度。

(13)

依據(jù)式(13)可以構(gòu)造偽長機(jī)的量測方程:

ZG=HG·XG+VG

(14)

式中，ZG=0，VG=0，HG= [I3×303×15I3×303×15I3×303×15]。

3 濾波算法

機(jī)群編隊協(xié)同導(dǎo)航系統(tǒng)的聯(lián)邦濾波器由僚機(jī)和偽長機(jī)濾波器兩部分組成，濾波算法的運行流程如圖3所示。

圖3 濾波算法流程

1) 狀態(tài)重置

利用偽長機(jī)濾波器狀態(tài)估計重置各僚機(jī)濾波器的狀態(tài)量，即

(15)

這樣可以保證式(13)的約束能反饋給僚機(jī)，抑制僚機(jī)濾波狀態(tài)估計誤差發(fā)散。

2) 僚機(jī)濾波器時間更新和量測更新

各僚機(jī)部分濾波器獨立地進(jìn)行時間更新和量測更新。

時間更新

(16)

(17)

量測更新

(18)

(19)

(20)

3) 信息融合

利用聯(lián)邦卡爾曼濾波中公共狀態(tài)估計的融合算法[7]，將各僚機(jī)部分濾波器得到的偽長機(jī)狀態(tài)估計和其對應(yīng)的協(xié)方差陣融合，得到除長機(jī)部分濾波器以外的全局估計。

(21)

(22)

4) 偽長機(jī)部分波器量測更新

偽長機(jī)部分濾波器不進(jìn)行時間更新，狀態(tài)一步預(yù)測和協(xié)方差一步預(yù)測值由僚機(jī)濾波器狀態(tài)估計提供，并且偽長機(jī)狀態(tài)部分由融合后的結(jié)果替換。

(23)

(24)

由于長機(jī)部分濾波器不進(jìn)行時間更新，因此其無需估計均方誤差。這樣主濾波器只需計算簡化后的濾波增益和狀態(tài)估計，即使主濾波器包含所有狀態(tài)也能保證較低的計算量。

(25)

(26)

4 仿真分析

為了驗證本文提出算法的有效性，進(jìn)行了計算機(jī)數(shù)字仿真分析。在仿真計算中慣性導(dǎo)航和相對導(dǎo)航設(shè)備相關(guān)參數(shù)如表1所示。飛行航跡包括起飛、爬升、轉(zhuǎn)向、勻速直線運動和降落等狀態(tài)，仿真時間1 000 s。以北向為例，某一次仿真實驗中3架飛機(jī)INS解算誤差和濾波估計誤差如圖4所示。

表1 INS和相對導(dǎo)航傳感器的相關(guān)誤差參數(shù)

圖4為長機(jī)故障后編隊各成員僅依靠自身慣性導(dǎo)航系統(tǒng)和采用本文所提協(xié)同導(dǎo)航算法后的北向定位誤差。可以看出利用所提算法，各成員定位誤差明顯小于僅依賴INS時的誤差，即協(xié)同定位誤差的發(fā)散速度明顯小于獨立定位時發(fā)散速度。但圖4僅為INS誤差有正有負(fù)時的情況，若所有編隊成員INS誤差均為正或負(fù)，則最后結(jié)果只能說明協(xié)同定位使得各成員位置估計誤差圓半徑介于INS解算誤差圓半徑的最大值與最小值之間。即便如此，對于機(jī)群來說，其整體定位誤差也優(yōu)于各成員獨自定位。

圖4 編隊各成員北向INS解算和濾波估計誤差

由圖4還可以看出，各成員協(xié)同定位估計誤差曲線基本重疊，說明通過協(xié)同定位算法各成員位置估計誤差大小極為接近。圖5協(xié)同定位誤差曲線局部放大進(jìn)一步說明了這個結(jié)論。表2列出了仿真最后一步3架飛機(jī)INS解算誤差值和協(xié)同定位估計誤差值，直觀印證了上述結(jié)果。

本文以3架飛機(jī)為例進(jìn)行算法仿真驗證，由于INS解算誤差有正有負(fù)，隨著編隊成員數(shù)量增加，算法抑制定位誤差發(fā)散的效果會越來越顯著。

圖5 編隊各成員北向濾波估計誤差局部圖

表2 編隊各成員INS解算和估計誤差

5 結(jié)論

1) 在建立協(xié)同導(dǎo)航系統(tǒng)模型的基礎(chǔ)上，提出了基于同性能INS誤差特性的機(jī)群編隊協(xié)同定位算法。

2) 該算法對原有主從式協(xié)同導(dǎo)航模型改動較小，使機(jī)群編隊故障前后能夠迅速反應(yīng)，保證了編隊系統(tǒng)的穩(wěn)定性。

3) 計算機(jī)仿真結(jié)果表明各成員定位估計誤差為所有INS輸出誤差均值，算法起到了抑制編隊整體定位誤差發(fā)散的作用。