基于幾何拓?fù)浼s束的多飛行器高精度協(xié)同相對(duì)導(dǎo)航方法

摘 要：多飛行器編隊(duì)的多重作業(yè)能力強(qiáng)、 可靠性高、 整體效率高， 是未來(lái)航空領(lǐng)域發(fā)展的重要方向， 而高精度的相對(duì)導(dǎo)航是實(shí)現(xiàn)多飛行器編隊(duì)飛行的關(guān)鍵。 為了實(shí)現(xiàn)高精度相對(duì)導(dǎo)航， 現(xiàn)有方法通常采用增加導(dǎo)航傳感器種類或提高導(dǎo)航傳感器測(cè)量精度的方式提升相對(duì)導(dǎo)航精度， 但這不僅導(dǎo)致相對(duì)導(dǎo)航系統(tǒng)成本和體積的增加， 而且會(huì)導(dǎo)致系統(tǒng)復(fù)雜性的增加。 為此， 本文從編隊(duì)整體的幾何拓?fù)浣Y(jié)構(gòu)出發(fā)， 建立編隊(duì)幾何拓?fù)浼s束模型， 并將其引入對(duì)相對(duì)慣導(dǎo)誤差的估計(jì)中， 提出一種基于幾何拓?fù)浼s束的協(xié)同相對(duì)導(dǎo)航方法。 所提方法通過引入整個(gè)編隊(duì)的幾何拓?fù)湫畔ⅲ?能夠在不增加導(dǎo)航傳感器配置的前提下提升相對(duì)導(dǎo)航精度。 在此基礎(chǔ)上， 設(shè)計(jì)得到一種多飛行器高精度協(xié)同相對(duì)導(dǎo)航方案。 仿真結(jié)果表明， 所提方法具有比現(xiàn)有方法更高的相對(duì)導(dǎo)航精度， 其相對(duì)定姿、 相對(duì)定位和相對(duì)測(cè)速的誤差均方差分別減少了66.48%、 48.73%和69.53%以上， 可以實(shí)現(xiàn)多飛行器之間高精度的相對(duì)導(dǎo)航。

關(guān)鍵詞：多飛行器編隊(duì)； 航空領(lǐng)域； 幾何拓?fù)浼s束； 協(xié)同相對(duì)導(dǎo)航； 導(dǎo)航精度

中圖分類號(hào)：TJ760; V249

文獻(xiàn)標(biāo)識(shí)碼： A

文章編號(hào)：1673-5048（2024）04-0078-08

DOI： 10.12132/ISSN.1673-5048.2024.0007

0 引 言

多飛行器編隊(duì)具有作戰(zhàn)能力強(qiáng)、 生存率高、 成本低等諸多優(yōu)勢(shì)， 受到世界各軍事強(qiáng)國(guó)的廣泛關(guān)注［1-3］。 在多飛行器編隊(duì)中， 對(duì)于各飛行器的單體絕對(duì)導(dǎo)航， 通?？梢圆捎媒萋?lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System， SINS）/全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Sate-llite System， GNSS）組合導(dǎo)航或者SINS/地形匹配組合導(dǎo)航（GNSS拒止環(huán)境下）的方式來(lái)實(shí)現(xiàn)［4-5］。 而多飛行器編隊(duì)完成特定任務(wù)的前提是編隊(duì)構(gòu)型的保持和重構(gòu)控制， 編隊(duì)控制的基礎(chǔ)是編隊(duì)成員之間高精度的相對(duì)導(dǎo)航［6-8］。 因此， 本文重點(diǎn)研究多飛行器編隊(duì)中各飛行器之間的相對(duì)導(dǎo)航。

近年來(lái)， 國(guó)內(nèi)外學(xué)者開展了相對(duì)導(dǎo)航方面的研究。 文獻(xiàn)［9］提出一種SINS/GNSS松組合方法， 利用GNSS位置/速度的差分信息校正相對(duì)SINS誤差， 以實(shí)現(xiàn)相對(duì)導(dǎo)航。 基于此， 文獻(xiàn)［10］提出一種SINS/GNSS緊組合方法， 利用偽距/偽距率差分信息代替GNSS位置和速度差分信息， 估計(jì)從機(jī)的SINS誤差， 獲得了比松組合精度更高的相對(duì)導(dǎo)航結(jié)果。 為了進(jìn)一步提升相對(duì)導(dǎo)航精度， 文獻(xiàn)［11］將具有厘米級(jí)測(cè)距精度的超寬帶（Ultra-Wide Band， UWB）技術(shù)引入相對(duì)導(dǎo)航中， 提出一種適用于小型無(wú)人機(jī)編隊(duì)的相對(duì)導(dǎo)航方法， 將GNSS載波相位差分信息、 UWB測(cè)距信息和SINS解算的位置信息相融合， 并利用UWB測(cè)距信息建立搜索空間輔助求解整周模糊度， 實(shí)現(xiàn)了高精度的相對(duì)定位。 但GNSS易受干擾或遮擋， 因而SINS/GNSS相對(duì)導(dǎo)航系統(tǒng)的自主性和可靠性無(wú)法保障［12］。

為了實(shí)現(xiàn)GNSS拒止環(huán)境下的多飛行器相對(duì)導(dǎo)航， 文獻(xiàn)［13］提出一種基于SINS/數(shù)據(jù)鏈的相對(duì)導(dǎo)航方法， 這種方法利用數(shù)據(jù)鏈測(cè)距信息校正相對(duì)SINS誤差， 實(shí)現(xiàn)了高精度的相對(duì)導(dǎo)航。 另外， 部分學(xué)者將視覺相機(jī)引入SINS/數(shù)據(jù)鏈相對(duì)導(dǎo)航系統(tǒng)中， 以進(jìn)一步提升相對(duì)導(dǎo)航系統(tǒng)精度。 文獻(xiàn)［14］提出一種基于SINS/數(shù)據(jù)鏈/視覺的相對(duì)導(dǎo)航方法， 將數(shù)據(jù)鏈測(cè)距信息、 視覺相機(jī)測(cè)角信息與主、 從機(jī)上SINS解算的位置和速度信息進(jìn)行融合， 實(shí)現(xiàn)了主、 從機(jī)之間的高精度相對(duì)導(dǎo)航。

從以上分析可知， 為了實(shí)現(xiàn)多飛行器之間高精度的相對(duì)導(dǎo)航， 傳統(tǒng)方法通常采用增加導(dǎo)航傳感器種類或提高導(dǎo)航傳感器測(cè)量精度的方式提升相對(duì)導(dǎo)航精度， 但這不僅導(dǎo)致相對(duì)導(dǎo)航系統(tǒng)成本和體積的增加， 而且會(huì)導(dǎo)致系統(tǒng)復(fù)雜性的增加。 為此， 本文提出一種基于幾何拓?fù)浼s束的協(xié)同相對(duì)導(dǎo)航方法， 進(jìn)而設(shè)計(jì)得到一種多飛行器高精度協(xié)同相對(duì)導(dǎo)航方案， 從而能夠在不增加導(dǎo)航傳感器配置的前提下提升相對(duì)導(dǎo)航精度。

1 問題描述

1.1 飛行器之間的相對(duì)關(guān)系

如圖1所示為多飛行器編隊(duì)中飛行器之間的相對(duì)關(guān)系示意圖。 飛行器1和飛行器2本體坐標(biāo)系分別記為b1系和b2系。 b2系可由b1系經(jīng)過3次歐拉旋轉(zhuǎn)得到， 其中3個(gè)歐拉旋轉(zhuǎn)角x， y， z稱為相對(duì)俯仰角、 相對(duì)滾轉(zhuǎn)角和相對(duì)航向角， 方向余弦矩陣Cb2b1稱為相對(duì)姿態(tài)矩陣， 其與x， y， z之間滿足以下關(guān)系：

Cb2b1=RY（y）RX（x）RZ（－z）=cosycosz+sinysinxsinz－cosysinz+sinysinxcosz－sinycosxcosxsinzcosxcoszsinxsinycosz－cosysinxsinz－sinysinz－cosysinxcoszcosycosx（1）

式中： Ri（·）， i=X， Y， Z為基元旋轉(zhuǎn)矩陣。

飛行器1和飛行器2相對(duì)于地心的位置向量分別記為R1和R2。 根據(jù)圖1中的向量關(guān)系， 飛行器2相對(duì)于飛行器1的相對(duì)位置向量R12為

R12=R2－R1 （2）

將式（2）相對(duì)于地心赤道慣性系i求導(dǎo)， 則可得飛行器2相對(duì)于飛行器1的相對(duì)速度向量U12為

U12=dR12dti=dR2dti－dR1dti（3）

可見， 式（1）～（3）分別描述了飛行器之間的相對(duì)姿態(tài)、 相對(duì)位置和相對(duì)速度關(guān)系。 相對(duì)導(dǎo)航便是指各成員之間通過直接或間接的相對(duì)測(cè)量、 信息交換與融合處理， 確定某架飛行器相對(duì)于其他飛行器的相對(duì)姿態(tài)、 相對(duì)位置和相對(duì)速度。

1.2 傳統(tǒng)多飛行器相對(duì)導(dǎo)航方法

多飛行器編隊(duì)中， 利用兩架飛行器上的陀螺儀和加速度計(jì)輸出， 便可進(jìn)行相對(duì)慣導(dǎo)解算。 但受初始條件誤差、 慣性器件誤差等影響， 相對(duì)慣導(dǎo)誤差會(huì)隨時(shí)間發(fā)散。 因此， 傳統(tǒng)的相對(duì)導(dǎo)航方法通常是通過引入數(shù)據(jù)鏈、 視覺相機(jī)提供誤差不隨時(shí)間累積的相對(duì)導(dǎo)航基準(zhǔn)信息， 與相對(duì)慣導(dǎo)解算的相對(duì)導(dǎo)航信息進(jìn)行匹配， 并采用最優(yōu)估計(jì)算法對(duì)相對(duì)慣導(dǎo)誤差進(jìn)行估計(jì)與校正， 以抑制相對(duì)慣導(dǎo)誤差發(fā)散。 相對(duì)導(dǎo)航系統(tǒng)的狀態(tài)模型和量測(cè)模型如下。

（1） 相對(duì)導(dǎo)航系統(tǒng)狀態(tài)模型

飛行器1與飛行器2之間的相對(duì)姿態(tài)誤差、 相對(duì)速度誤差和相對(duì)位置誤差模型分別為［15］

δ·12=－Ωb11×δ12+δωb11－Cb1b2δωb22（4）

δU·12=（Cb2b1）Tfb22×δ12－ωb11×δU12－δωb11×Ub112－

δfb11+（Cb2b1）Tδfb22（5）

δR·12=δU12－ωb11×δR12－δωb11×Rb112（6）

式中： δ12， δU12， δR12分別為相對(duì)姿態(tài)失準(zhǔn)角、 相對(duì)速度誤差、 相對(duì)位置誤差； ωb11為飛行器1上的陀螺儀輸出； Ωb11為ωb11對(duì)應(yīng)的反對(duì)稱矩陣； fb22為飛行器2上的加速度計(jì)輸出； δωb11， δωb22分別為飛行器1和飛行器2上陀螺儀的測(cè)量誤差； δfb11， δfb22分別為飛行器1和飛行器2上加速度計(jì)的測(cè)量誤差。

飛行器1， 2上陀螺儀和加速度計(jì)的測(cè)量誤差可表示為

δωbii=εgi+wgi（7）

δfbii=Δai+wai（8）

式中： εgi為陀螺儀的零偏； wgi為陀螺儀的隨機(jī)誤差； Δai為加速度計(jì)的零偏； wai為加速度計(jì)的隨機(jī)誤差。

因此， 綜合式（4）～（8）， 可建立相對(duì)導(dǎo)航系統(tǒng)的狀態(tài)模型為

X·=FX+GW（9）

其中，

X=［δ12， x， δ12， y， δ12， z， δU12， x， δU12， y， δU12， z， δR12， x， δR12， y， δR12， z， εg1， x， εg1， y， εg1， z， εg2， x， εg2， y， εg2， z， Δa1， x， Δa1， y， Δa1， z， Δa2， x， Δa2， y， Δa2， z］T

F=

－Ωb1103×303×3I3×3－Cb1b203×303×3

AF－Ωb1103×3AU03×3－I3×3Cb1b2

03×3I3×3－Ωb11AR03×303×303×3

012×3012×3012×3012×3012×3012×3012×3

G=I3×3－Cb1b203×303×3

AU03×3－I3×3Cb1b2

AR03×303×303×3

012×3012×3012×3012×3

W=［wg1， x， wg1， y， wg1， z， wg2， x， wg2， y， wg2， z， wa1， x， wa1， y， wa1， z， wa2， x， wa2， y， wa2， z］T

式中： AF為Cb1b2fb22對(duì)應(yīng)的反對(duì)稱矩陣； AR和AU分別為Rb112和Ub112對(duì)應(yīng)的反對(duì)稱矩陣。

（2） 相對(duì)導(dǎo)航系統(tǒng)量測(cè)模型

利用數(shù)據(jù)鏈和視覺相機(jī)可以測(cè)得飛行器2相對(duì)于飛行器1的距離、 俯仰角和方位角， 分別為

d～=d+εd

θ～=θ+εθ

α～=α+εα （10）

式中： d， θ， α分別為距離、 俯仰角和方位角的真實(shí)值； εd， εθ， εα分別為測(cè)距和測(cè)角誤差。

如圖2所示， 根據(jù)測(cè)得的距離d～、 俯仰角θ～和方位角α～， 可推算得到相對(duì)位置向量R～b112為

R～b112=d～cosθ～cosα～d～cosθ～sinα～d～sinθ～=

（d+εd）cos（θ+εθ）cos（α+εα）（d+εd）cos（θ+εθ）sin（α+εα）（d+εd）sin（θ+εθ）=

ΔxΔyΔy－nxnynz （11）

式中： Δx， Δy， Δz為相對(duì)位置向量真實(shí)值Rb112的3個(gè)分量； nx=d～sinθ～cosα～εθ+d～cosθ～sinα～εα－cosθ～cosα～εd； ny=d～sinθ～sinα～εθ－d～cosθ～cosα～εα－cosθ～sinα～εd； nz=－d～cosθ～εθ－sinθ～εd。

另外， 相對(duì)慣導(dǎo)解算的相對(duì)位置向量R^b112為

R^b112=Rb112+δR12=ΔxΔyΔz+δR12， xδR12， yδR12， z（12）

將式（11）與式（12）做差， 可建立相對(duì)導(dǎo)航系統(tǒng)的量測(cè)模型為

Z=R^b112－R～b112=δR12， xδR12， yδR12， z+nxnynz=HX+V（13）

式中： H=［03×303×3I3×303×12］； V=［nxnynz］T。

這樣， 式（9）和式（13）共同組成了傳統(tǒng)相對(duì)導(dǎo)航濾波模型。 傳統(tǒng)方法通過對(duì)相對(duì)慣導(dǎo)誤差進(jìn)行估計(jì)與校正， 從而可獲得各飛行器之間的相對(duì)位置、 相對(duì)速度和相對(duì)姿態(tài)信息。

然而， 相對(duì)慣導(dǎo)誤差的估計(jì)精度和速度受系統(tǒng)可觀測(cè)性及其狀態(tài)量可觀測(cè)度的影響。 而根據(jù)線性系統(tǒng)可觀測(cè)性理論， 系統(tǒng)可觀測(cè)性及狀態(tài)量可觀測(cè)度不僅與量測(cè)量的選取有關(guān)， 而且與狀態(tài)量之間的耦合（約束）關(guān)系有關(guān)。 因此， 當(dāng)多架飛行器組成編隊(duì)飛行時(shí)， 可以考慮根據(jù)整個(gè)編隊(duì)在空間域中的幾何拓?fù)浣Y(jié)構(gòu)， 建立編隊(duì)幾何拓?fù)浼s束模型， 并將其引入對(duì)相對(duì)慣導(dǎo)誤差的估計(jì)中。 這樣， 通過利用編隊(duì)的幾何拓?fù)浼s束條件， 提高相對(duì)慣導(dǎo)誤差狀態(tài)的可觀測(cè)度， 也即提高其估計(jì)精度， 從而提升各飛行器之間的相對(duì)導(dǎo)航精度。

2 多飛行器高精度協(xié)同相對(duì)導(dǎo)航方案設(shè)計(jì)

2.1 編隊(duì)幾何拓?fù)浼s束模型的建立

以3架飛行器組成的多飛行器編隊(duì)為例， 各飛行器之間相對(duì)導(dǎo)航示意圖如圖3所示。 可見， 飛行器1、 飛行器2和飛行器3分別利用數(shù)據(jù)鏈和視覺相機(jī)對(duì)飛行器2、 飛行器3和飛行器1進(jìn)行相對(duì)距離和相對(duì)角度觀測(cè)， 從而估計(jì)出各飛行器上相對(duì)導(dǎo)航系統(tǒng)的狀態(tài)向量X12、 X23和X31。

2.1.1 相對(duì)姿態(tài)約束模型

多飛行器編隊(duì)中， 飛行器1、 飛行器2與飛行器3之間的相對(duì)姿態(tài)矩陣滿足

Cb1b3·Cb3b2·Cb2b1=I（14）

式中： 相對(duì)姿態(tài)矩陣的真實(shí)值Cb2b1， Cb3b2， Cb1b3與其解算值Cb2b1′， Cb3b2′， Cb1b3′之間存在以下關(guān)系：

Cb2b1=Cb2b1′（I－Aδ12） （15）

Cb3b2=Cb3b2′（I－Aδ23） （16）

Cb1b3=Cb1b3′（I－Aδ31） （17）

式中： Aδ12， Aδ23， Aδ31分別為相對(duì)姿態(tài)失準(zhǔn)角δ12， δ23， δ31對(duì)應(yīng)的反對(duì)稱矩陣。

將式（15）～（17）代入式（14）， 可得

Aδ12+（Cb2b1′）TAδ23Cb2b1′+（Cb3b2′Cb2b1′）TAδ31Cb3b2′Cb2b1′=C1 （18）

式中： C1=I－（Cb1b3′Cb3b2′Cb2b1′）T。

進(jìn)而， 根據(jù)反對(duì)稱矩陣與向量之間的對(duì)應(yīng)關(guān)系， 將式（18）改寫成向量形式， 可得相對(duì)姿態(tài)約束模型為

δ12+（Cb2b1′）Tδ23+（Cb3b2′Cb2b1′）Tδ31=M1（19）

式中： M1=［C32， C13， C21］T為由矩陣C1中的元素組成的向量， Cij（i=1， 2， 3; j=1， 2， 3）為矩陣C1中的第i行、 第j列元素。

2.1.2 相對(duì)位置約束模型

多飛行器編隊(duì)中， 飛行器1、 飛行器2與飛行器3之間的相對(duì)位置向量R12， R23， R31構(gòu)成一個(gè)閉合的向量環(huán)， 即

R12+R23+R31=0（20）

將式（20）投影至b1系， 可得

Rb112+Cb1b2Rb223+Cb1b3Rb331=0（21）

式中： 相對(duì)位置向量的真實(shí)值Rb112， Rb223， Rb331與其解算值R^b112， R^b223， R^b331之間存在以下關(guān)系：

Rb112=R^b112－δR12（22）

Rb223=R^b223－δR23（23）

Rb331=R^b331－δR31（24）

將式（22）～（24）代入式（21）， 可得

δR12+Cb1b2δR23+Cb1b3δR31=R^b112+Cb1b2R^b223+Cb1b3R^b331（25）

再將相對(duì)姿態(tài)矩陣的真實(shí)值Cb1b2， Cb1b3與其解算值Cb1′b2， Cb1b3′之間的關(guān)系式Cb1b2=（I+Aδ12）Cb1′b2， Cb1b3=Cb1b3′（I－Aδ31）， 代入式（25）， 可得

R^b112+Cb1′b2R^b223+Cb1b3′R^b331=δR12+Cb1′b2δR23+Cb1b3′δR31+AR^23δ12－Cb1b3′AR^31δ31（26）

式中： AR^23為Cb1′b2R^b223對(duì)應(yīng)的反對(duì)稱矩陣； AR^31為R^b331對(duì)應(yīng)的反對(duì)稱矩陣。

進(jìn)而， 將相對(duì)姿態(tài)約束模型式（19）代入式（26）， 可得相對(duì)位置約束模型為

M2=δR12+Cb1′b2δR23+Cb1b3′δR31－AR^23（Cb2b1′）Tδ23－［AR^23（Cb3b2′Cb2b1′）T+Cb1b3′AR^31］δ31（27）

式中： M2=R^b112+Cb1′b2R^b223+Cb1b3′R^b331－AR^23M1。

2.1.3 相對(duì)速度約束模型

同理， 還可根據(jù)相對(duì)速度向量U12， U23， U31所滿足的約束條件

U12+U23+U31=0（28）

推導(dǎo)得到相對(duì)速度約束模型為

M3=δU12+Cb1′b2δU23+Cb1b3′δU31－AU^23（Cb2b1′）Tδ23－［AU^23（Cb3b2′Cb2b1′）T+Cb1b3′AU^31］δ31（29）

式中： AU^23為Cb1′b2U^b223對(duì)應(yīng)的反對(duì)稱矩陣； AU^31為U^b331對(duì)應(yīng)的反對(duì)稱矩陣； M3=U^b112+Cb1′b2U^b223+Cb1b3′U^b331－AU^23M1。

另外， 由于不同飛行器上的狀態(tài)向量X12、 X23和X31中含有部分相同的狀態(tài)分量， 因此有

εg1|X12=εg1|X31， Δa1|X12=εa1|X31（30）

εg2|X12=εg2|X23， Δa2|X12=εa2|X23（31）

εg3|X23=εg3|X31， Δa3|X23=εa3|X31（32）

式中： （·）|X表示狀態(tài)向量X中的狀態(tài)分量。

因此， 綜合式（19）、 （27）、 （29）以及式（30）～（32）， 可構(gòu)建編隊(duì)幾何拓?fù)浼s束模型為

X12+Tb1b2X23+Tb1b3X31=M （33）

式中： X12=［δ12δU12δR12εg1εg2Δa1Δa2］T； X23=［δ23δU23δR23εg2εg3Δa2Δa3］T； X31=［δ31δU31δR31εg3εg1Δa3Δa1］T； M=［M1M2M301×12］T； 廣義變換矩陣Tb1b2和Tb1b3為

Tb1b2=

Cb1′b203×303×303×303×303×303×3

－AU^23Cb1′b2Cb1′b203×303×303×303×303×3

－AR^23Cb1′b203×3Cb1′b203×303×303×303×3

03×303×303×303×303×303×303×3

03×303×303×3－I3×303×303×303×3

03×303×303×303×303×303×303×3

03×303×303×303×303×3－I3×303×3；

Tb1b3=Cb1′b2Cb2′b303×303×303×303×303×303×3

－（AU^23Cb1′b2Cb2′b3+Cb1b3′AU^31）Cb1b3′03×303×303×303×303×3

－（AR^23Cb1′b2Cb2′b3+Cb1b3′AR^31）03×3Cb1b3′03×303×303×303×3

03×303×303×303×3－I3×303×303×3

03×303×303×303×303×303×303×3

03×303×303×303×303×303×3－I3×3

03×303×303×303×303×303×303×3。

由此可見， 編隊(duì)幾何拓?fù)浼s束模型式（33）能夠反映出不同飛行器上相對(duì)導(dǎo)航系統(tǒng)的狀態(tài)向量， 即X12、 X23和X31之間所滿足的約束關(guān)系。 因此， 若將該約束關(guān)系引入對(duì)狀態(tài)向量的估計(jì)中， 則相當(dāng)于對(duì)狀態(tài)向量施加約束條件。 而根據(jù)線性系統(tǒng)可觀測(cè)性理論， 對(duì)狀態(tài)向量施加約束條件能有效提高狀態(tài)向量的可觀測(cè)度。 這樣， 通過將該約束關(guān)系引入對(duì)狀態(tài)向量的估計(jì)中， 便可提高狀態(tài)向量的可觀測(cè)度， 從而提高狀態(tài)向量的估計(jì)精度。

2.2 基于幾何拓?fù)浼s束的協(xié)同相對(duì)導(dǎo)航方法

以飛行器1上相對(duì)導(dǎo)航系統(tǒng)狀態(tài)向量X12的估計(jì)為例。 在對(duì)X12的估計(jì)過程中， 時(shí)間更新過程與標(biāo)準(zhǔn)Kalman濾波算法相同。 為了充分利用編隊(duì)在空間域中所滿足的幾何拓?fù)浼s束關(guān)系， 將編隊(duì)幾何拓?fù)浼s束模型式（33）引入對(duì)X12的量測(cè)更新中， 則量測(cè)更新方程可改寫為

X^12， k=X^12， k|k－1+K12， k（Z12， k－H12， kX^12， k|k－1）+

γP12， k|k－11+P12， k|k－1F（X^12， k|k－1－X-12， k|k－1） （34）

式中： X^12， k為狀態(tài)向量的估計(jì)值； X^12， k|k－1為狀態(tài)向量的一步預(yù)測(cè)值； K12， k為濾波增益矩陣； Z12， k為量測(cè)向量； H12， k為量測(cè)矩陣； P12， k|k－1為一步預(yù)測(cè)均方誤差陣； γP12， k|k－11+P12， k|k－1F（X^12， k|k－1－X-12， k|k－1）為幾何拓?fù)浼s束項(xiàng)； ·F為Frobenius范數(shù)； γ為幾何拓?fù)浼s束系數(shù)； X-12， k|k－1為由編隊(duì)幾何拓?fù)浼s束模型所確定的預(yù)測(cè)值， 即

X-12， k|k－1=M－Tb1b2X^23， k|k－1－Tb1b3X^31， k|k－1（35）

式中： X^23， k|k－1和X^31， k|k－1分別為飛行器2和飛行器3上相對(duì)導(dǎo)航系統(tǒng)狀態(tài)向量X23和X31的一步預(yù)測(cè)值， 可由數(shù)據(jù)鏈傳送至飛行器1上。

式（34）表明， 在對(duì)狀態(tài)向量X12進(jìn)行量測(cè)更新時(shí)， 一方面利用量測(cè)向量Z12， k（包括飛行器1與飛行器2之間的距離和相對(duì)角度信息）對(duì)狀態(tài)一步預(yù)測(cè)值X^12， k|k－1進(jìn)行修正； 另一方面， 還對(duì)狀態(tài)向量X12施加了編隊(duì)幾何拓?fù)浼s束條件。 這樣， 通過綜合利用兩架飛行器之間的距離和相對(duì)角度信息， 以及整個(gè)編隊(duì)在空間域中的幾何拓?fù)湫畔ⅲ?便可有效提高狀態(tài)向量X12的估計(jì)精度。

2.3 高精度協(xié)同相對(duì)導(dǎo)航方案

根據(jù)所提的協(xié)同相對(duì)導(dǎo)航方法， 設(shè)計(jì)得到一種多飛行器高精度協(xié)同相對(duì)導(dǎo)航方案， 其方案框圖如圖4所示。

在所設(shè)計(jì)方案中， 一方面利用數(shù)據(jù)鏈測(cè)量的距離d～與視覺相機(jī)測(cè)量的俯仰角θ～和方位角α～計(jì)算得到相對(duì)位置R～b112， 并與相對(duì)慣導(dǎo)解算得到的相對(duì)位置R^b112進(jìn)行匹配， 進(jìn)而將R^b112與R～b112之差作為相對(duì)位置量測(cè)量Z12， k； 另一方面， 基于編隊(duì)的幾何拓?fù)浼s束模型， 利用飛行器2和飛行器3上的狀態(tài)一步預(yù)測(cè)值X^23， k|k－1和X^31， k|k－1， 對(duì)飛行器1上的狀態(tài)向量X12進(jìn)行預(yù)測(cè)， 獲得狀態(tài)預(yù)測(cè)值X-12， k|k－1； 然后， 相對(duì)導(dǎo)航濾波器采用卡爾曼濾波算法對(duì)Z12， k和X-12， k|k－1進(jìn)行綜合處理， 以實(shí)現(xiàn)對(duì)相對(duì)慣導(dǎo)誤差的估計(jì)。 最終， 利用相對(duì)慣導(dǎo)誤差的估計(jì)值對(duì)相對(duì)慣導(dǎo)解算結(jié)果進(jìn)行校正， 以實(shí)現(xiàn)飛行器1與飛行器2之間高精度的協(xié)同相對(duì)導(dǎo)航。

3 仿真驗(yàn)證

3.1 仿真條件

多飛行器編隊(duì)由3架飛行器組成， 各飛行器的初始位置不同， 但飛行中采取相同的機(jī)動(dòng)方式， 其飛行軌跡如圖5所示。 飛行器1～3的初始速度均為30 m/s， 初始俯仰角和滾轉(zhuǎn)角均為0°， 初始偏航角均為45°。 仿真總時(shí)間為1 000 s。

多飛行器編隊(duì)中， 各飛行器均配置相同精度的導(dǎo)航傳感器。 其中， 陀螺儀零偏為1 （°）/h， 陀螺儀隨機(jī)游走為0.2 （°）/h； 加速度計(jì)零偏為1 mg， 加速度計(jì)隨機(jī)游走為0.2 mg/Hz。 數(shù)據(jù)鏈的測(cè)距噪聲標(biāo)準(zhǔn)差為3 m， 視覺相機(jī)的測(cè)角噪聲標(biāo)準(zhǔn)差為1′。

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

如圖6～7所示為傳統(tǒng)方法和所提方法的相對(duì)導(dǎo)航誤差曲線， 而這兩種方法相對(duì)導(dǎo)航誤差的均值和均方差如表1所示。

由圖6～7可知， 傳統(tǒng)方法和所提方法利用數(shù)據(jù)鏈和視覺相機(jī)提供的誤差不隨時(shí)間累積的相對(duì)導(dǎo)航基準(zhǔn)信息， 與相對(duì)慣導(dǎo)解算的相對(duì)導(dǎo)航信息匹配， 實(shí)現(xiàn)了對(duì)相對(duì)慣導(dǎo)誤差的估計(jì)與校正， 可得到誤差不隨時(shí)間發(fā)散的相對(duì)導(dǎo)航結(jié)果。 另外， 結(jié)合表1中的數(shù)據(jù)可知， 相較于傳統(tǒng)方法， 所提方法的相對(duì)定姿、 相對(duì)定位以及相對(duì)測(cè)速

精度均有顯著提升。 可見， 所提方法通過將整個(gè)編隊(duì)所滿足的幾何拓?fù)浼s束關(guān)系引入對(duì)相對(duì)慣導(dǎo)誤差的估計(jì)中， 可有效提高相對(duì)慣導(dǎo)誤差的估計(jì)精度， 從而顯著提升飛行器之間的相對(duì)導(dǎo)航精度。

為了進(jìn)一步驗(yàn)證所提方法在不同幾何拓?fù)浣Y(jié)構(gòu)下的

有效性與先進(jìn)性， 多飛行器編隊(duì)采用不同的幾何拓?fù)浣Y(jié)構(gòu)進(jìn)行相對(duì)導(dǎo)航。 如圖8所示為多飛行器編隊(duì)所采用的幾何拓?fù)浣Y(jié)構(gòu)， 而表2為不同幾何拓?fù)浣Y(jié)構(gòu)下， 所提方法與傳統(tǒng)方法的相對(duì)導(dǎo)航誤差對(duì)比。

由表2中所列數(shù)據(jù)可知， 無(wú)論多飛行器編隊(duì)采用哪種幾何拓?fù)浣Y(jié)構(gòu)， 相較于傳統(tǒng)方法， 所提方法的相對(duì)定姿、 相對(duì)定位以及相對(duì)測(cè)速誤差的均值和均方差均明顯減小， 這充分驗(yàn)證了所提方法在不同幾何拓?fù)浣Y(jié)構(gòu)下的有效性與先進(jìn)性。

4 結(jié) 論

為了在不增加導(dǎo)航傳感器配置的前提下提升多飛行器之間的相對(duì)導(dǎo)航精度， 本文提出一種多飛行器高精度協(xié)同相對(duì)導(dǎo)航方法。 經(jīng)過理論分析與仿真驗(yàn)證， 所得到的結(jié)論如下：

（1） 傳統(tǒng)方法通過對(duì)相對(duì)慣導(dǎo)誤差進(jìn)行估計(jì)與校正， 可獲得各飛行器之間的相對(duì)位置、 相對(duì)速度和相對(duì)姿態(tài)。 然而， 由于沒有利用編隊(duì)的幾何拓?fù)湫畔ⅲ?因此相對(duì)慣導(dǎo)誤差的可觀測(cè)度較低， 從而導(dǎo)致傳統(tǒng)方法的相對(duì)導(dǎo)航精度不高。

（2） 所提方法將編隊(duì)所滿足的幾何拓?fù)浼s束關(guān)系引入對(duì)相對(duì)慣導(dǎo)誤差的估計(jì)中， 可有效提高相對(duì)慣導(dǎo)誤差的可觀測(cè)度， 進(jìn)而提高其估計(jì)精度， 最終提升多飛行器之間的相對(duì)導(dǎo)航精度。

仿真結(jié)果表明， 在不增加導(dǎo)航傳感器配置的前提下， 所提方法具有比傳統(tǒng)方法更高的相對(duì)導(dǎo)航精度， 能夠滿足多飛行器高精度相對(duì)導(dǎo)航的需求。

參考文獻(xiàn)：

［1］ Yin T T， Gu Z， Yan S. Event-Based Formation Control for Multiple Unmanned Aerial Vehicles under Directed Topology［J］. ISA Transactions， 2023， 137： 111-121.

［2］ Yu Z Q， Zhang Y M， Jiang B， et al. A Review on Fault-Tolerant Cooperative Control of Multiple Unmanned Aerial Vehicles［J］. Chinese Journal of Aeronautics， 2022， 35（1）： 1-18.

［3］ 李清華， 高影， 王振桓， 等. 一種動(dòng)態(tài)分組的多節(jié)點(diǎn)協(xié)同定位編隊(duì)構(gòu)型優(yōu)化方法［J］. 中國(guó)慣性技術(shù)學(xué)報(bào)， 2022， 30（6）： 746-751.

Li Qinghua， Gao Ying， Wang Zhenhuan， et al. A Dynamic Grouping Formation Configuration Optimization Method for Multi-Node Cooperative Localization［J］. Journal of Chinese Inertial Technology， 2022， 30（6）： 746-751.（in Chinese）

［4］ Zhang G H， Hsu L T. Intelligent GNSS/INS Integrated Navigation System for a Commercial UAV Flight Control System［J］. Aerospace Science and Technology， 2018， 80： 368-380.

［5］ 程傳奇， 郝向陽(yáng)， 張振杰， 等. 魯棒性地形匹配/慣性組合導(dǎo)航算法［J］. 中國(guó)慣性技術(shù)學(xué)報(bào)， 2016， 24（2）： 202-207.

Cheng Chuanqi， Hao Xiangyang， Zhang Zhenjie， et al. Robust Integrated Navigation Algorithm of Terrain Aided Navigation/INS［J］. Journal of Chinese Inertial Technology， 2016， 24（2）： 202-207.（in Chinese）

［6］ Zhu Y F， Sun Y R， Zhao W， et al. Relative Navigation for Autonomous Aerial Refueling Rendezvous Phase［J］. Optik， 2018， 174： 665-675.

［7］ 孫博文， 王大軼， 周海銀， 等. 自主光學(xué)相對(duì)導(dǎo)航中序列圖像最優(yōu)構(gòu)建方法［J］. 宇航學(xué)報(bào)， 2023， 44（6）： 926-933.

Sun Bowen， Wang Dayi， Zhou Haiyin， et al. Optimal Construction Method for Sequence Images in Autonomous Relative Optical Navigation［J］. Journal of Astronautics， 2023， 44（6）： 926-933.（in Chinese）

［8］ Shoop T， Munoz J， Gunther J， et al. Formation Design for Optimal Relative Navigation in GPS Denied Environments［J］. The Journal of the Astronautical Sciences， 2023， 70（2）： 3.

［9］ 崔乃剛， 王小剛， 郭繼峰. 基于INS/GPS的智能導(dǎo)彈協(xié)同作戰(zhàn)相對(duì)導(dǎo)航系統(tǒng)研究［J］. 戰(zhàn)術(shù)導(dǎo)彈技術(shù)， 2010（2）： 1-5.

Cui Naigang， Wang Xiaogang， Guo Jifeng. Study of Relative Navigation System Based on INS/GPS for Cooperative Intelligent Missile［J］. Tactical Missile Technology， 2010（2）： 1-5.（in Chinese）

［10］ 魏紅波. 基于偽距/偽距率雙差的INS/GNSS緊組合相對(duì)導(dǎo)航算法研究［J］. 現(xiàn)代導(dǎo)航， 2017， 8（2）： 87-92.

Wei Hongbo. Relative Navigation Algorithm of Tightly Integrated INS/GNSS Based on Pseudo-Range/Pseudo-Range Rate Double-Difference［J］. Modern Navigation， 2017， 8（2）： 87-92.（in Chinese）

［11］ 王念曾， 李榮冰， 韓志鳳， 等. 基于慣性/GNSS/UWB的小型無(wú)人機(jī)相對(duì)導(dǎo)航算法研究［J］. 電子測(cè)量技術(shù)， 2019， 42（16）： 94-100.

Wang Nianzeng， Li Rongbing， Han Zhifeng， et al. Research on Relative Navigation Algorithm of Small UAV Based on Inertial/GNSS/UWB［J］. Electronic Measurement Technology， 2019， 42（16）： 94-100.（in Chinese）

［12］ Wang S Z， Zhan X Q， Zhai Y W， et al. Highly Reliable Relative Navigation for Multi-UAV Formation Flight in Urban Environments［J］. Chinese Journal of Aeronautics， 2021， 34（7）： 257-270.

［13］ Cao S， Qin H L， Cong L， et al. Multi-Slots Joint MLE Relative Navigation Algorithm Based on INS/JTIDS/BA for Datalink Network［J］. IEEE Access， 2020， 8： 136795-136807.

［14］ 劉梓軒， 孫永榮， 曾慶化， 等. 基于測(cè)距測(cè)角信息的DG-IEKF相對(duì)導(dǎo)航算法［J］. 導(dǎo)航與控制， 2021， 20（3）： 34-43.

Liu Zixuan， Sun Yongrong， Zeng Qinghua， et al. Relative Navigation Algorithm Based on DG-IEKF under the Range and Angle Information［J］. Navigation and Control， 2021， 20（3）： 34-43.（in Chinese）

［15］ Si F， Zhao Y， Lin Y H， et al. A New Transfer Alignment of Airborne Weapons Based on Relative Navigation［J］. Measurement， 2018， 122： 27-39.

High-Precision Collaborative Relative Navigation Method for

Multiple Aircraft Based on Geometric Topology Constraints

Lu Kewen1， Wang Xinlong1*， Wang Bin2

（1. School of Astronautics， Beihang University， Beijing 100083， China；

2. Beijing Institute of Control & Electronic Technology， Beijing 100038， China）

Abstract： Multiple aircraft formation has strong multi operation capability， high reliability and high overall efficiency， which is an important direction for the future development of aviation field. High-precision relative navigation is crucial for multiple aircraft to achieve formation flight. In order to achieve high-precision relative navigation， the existing methods usually increase the relative navigation accuracy by adding the types of navigation sensors or improving the measurement accuracy of navigation sensors. However， this not only leads to an increase in the cost and volume of the relative navigation system， but also leads to an increase in the system complexity. Therefore， based on the overall geometry topology structure of the formation， a formation geometric topology constraint model is established and introduced into the estimation of the relative inertial navigation errors in this paper. A collaborative relative navigation method based on geometric topology constraints is proposed. By introducing the geometric topology information of the whole formation， the proposed method can improve the relative navigation accuracy without increasing the configuration of navigation sensors. On this basis， a high-precision collaborative relative navigation scheme for multiple aircraft is designed. The simulation results show that the proposed method has higher relative navigation accuracy than the existing method， and the mean squared errors of relative attitude determination， relative positioning and relative velocity measurement are reduced by more than 66.48%， 48.73% and 69.53%， respectively. The proposed method can achieve high-precision relative navigation for multiple aircraft.

Key words： multiple aircraft formation； aviation field； geometric topology constraints； collaborative relative navigation； high precision