一種長機故障情況下機群編隊協同定位算法

劉曉洋,徐勝紅

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

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

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

1 問題描述與方案設計

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

圖1 協同導航系統定位算法結構

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

選作偽長機的僚機i所對應的Di應滿足以下關系：

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

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

2 系統數學模型

2.1 僚機部分系統方程

選取地理坐標系為導航坐標系(n系)，僚機 的INS誤差狀態方程為

(1)

式中，狀態量Xi為

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

系統噪聲Wi為

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

僚機i狀態方程為自身INS與偽長機INS誤差狀態方程的疊加，即

(2)

如圖1所示，在導航坐標系(n系)中，僚機i量測值Zi是由僚機i通過數據鏈收到的偽長機INS信息與自身INS信息和僚機測量的相對導航信息依次相減得到的，即

(3)

(4)

結合式(3)和式(4)可得僚機狀態量與量測值的關系為

(5)

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

(6)

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

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

(7)

式中,

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

(8)

式中，γ、θ、φ分別為僚機真實橫滾、俯仰和航向角，由于慣性導航系統解算的姿態角誤差極小，用慣性導航系統解算出的姿態角可以近似替代真實姿態角，即

(9)

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

(10)

INS輸出的三維位置信息為經、緯、高，相對導航傳感器輸出的三維位置信息為三個方向的距離，常規單位是m，為了與相對導航距離信息量綱匹配，對INS輸出的經度和緯度進行了轉換

(11)

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

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

(12)

式中,

2.2 偽長機部分系統模型

偽長機濾波器的狀態量是各編隊成員狀態的組合

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

(13)

依據式(13)可以構造偽長機的量測方程:

ZG=HG·XG+VG

(14)

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

3 濾波算法

機群編隊協同導航系統的聯邦濾波器由僚機和偽長機濾波器兩部分組成，濾波算法的運行流程如圖3所示。

圖3 濾波算法流程

1) 狀態重置

利用偽長機濾波器狀態估計重置各僚機濾波器的狀態量，即

(15)

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

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

各僚機部分濾波器獨立地進行時間更新和量測更新。

時間更新

(16)

(17)

量測更新

(18)

(19)

(20)

3) 信息融合

利用聯邦卡爾曼濾波中公共狀態估計的融合算法[7]，將各僚機部分濾波器得到的偽長機狀態估計和其對應的協方差陣融合，得到除長機部分濾波器以外的全局估計。

(21)

(22)

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

偽長機部分濾波器不進行時間更新，狀態一步預測和協方差一步預測值由僚機濾波器狀態估計提供，并且偽長機狀態部分由融合后的結果替換。

(23)

(24)

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

(25)

(26)

4 仿真分析

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

表1 INS和相對導航傳感器的相關誤差參數

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

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

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

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

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

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

5 結論

1) 在建立協同導航系統模型的基礎上，提出了基于同性能INS誤差特性的機群編隊協同定位算法。

2) 該算法對原有主從式協同導航模型改動較小，使機群編隊故障前后能夠迅速反應，保證了編隊系統的穩定性。

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