基于幾何拓撲約束的多飛行器高精度協同相對導航方法

摘 要：多飛行器編隊的多重作業能力強、 可靠性高、 整體效率高， 是未來航空領域發展的重要方向， 而高精度的相對導航是實現多飛行器編隊飛行的關鍵。 為了實現高精度相對導航， 現有方法通常采用增加導航傳感器種類或提高導航傳感器測量精度的方式提升相對導航精度， 但這不僅導致相對導航系統成本和體積的增加， 而且會導致系統復雜性的增加。 為此， 本文從編隊整體的幾何拓撲結構出發， 建立編隊幾何拓撲約束模型， 并將其引入對相對慣導誤差的估計中， 提出一種基于幾何拓撲約束的協同相對導航方法。 所提方法通過引入整個編隊的幾何拓撲信息， 能夠在不增加導航傳感器配置的前提下提升相對導航精度。 在此基礎上， 設計得到一種多飛行器高精度協同相對導航方案。 仿真結果表明， 所提方法具有比現有方法更高的相對導航精度， 其相對定姿、 相對定位和相對測速的誤差均方差分別減少了66.48%、 48.73%和69.53%以上， 可以實現多飛行器之間高精度的相對導航。

關鍵詞：多飛行器編隊； 航空領域； 幾何拓撲約束； 協同相對導航； 導航精度

中圖分類號：TJ760; V249

文獻標識碼： A

文章編號：1673-5048（2024）04-0078-08

DOI： 10.12132/ISSN.1673-5048.2024.0007

0 引 言

多飛行器編隊具有作戰能力強、 生存率高、 成本低等諸多優勢， 受到世界各軍事強國的廣泛關注［1-3］。 在多飛行器編隊中， 對于各飛行器的單體絕對導航， 通?？梢圆捎媒萋搼T性導航系統（Strapdown Inertial Navigation System， SINS）/全球導航衛星系統（Global Navigation Sate-llite System， GNSS）組合導航或者SINS/地形匹配組合導航（GNSS拒止環境下）的方式來實現［4-5］。 而多飛行器編隊完成特定任務的前提是編隊構型的保持和重構控制， 編隊控制的基礎是編隊成員之間高精度的相對導航［6-8］。 因此， 本文重點研究多飛行器編隊中各飛行器之間的相對導航。

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

為了實現GNSS拒止環境下的多飛行器相對導航， 文獻［13］提出一種基于SINS/數據鏈的相對導航方法， 這種方法利用數據鏈測距信息校正相對SINS誤差， 實現了高精度的相對導航。 另外， 部分學者將視覺相機引入SINS/數據鏈相對導航系統中， 以進一步提升相對導航系統精度。 文獻［14］提出一種基于SINS/數據鏈/視覺的相對導航方法， 將數據鏈測距信息、 視覺相機測角信息與主、 從機上SINS解算的位置和速度信息進行融合， 實現了主、 從機之間的高精度相對導航。

從以上分析可知， 為了實現多飛行器之間高精度的相對導航， 傳統方法通常采用增加導航傳感器種類或提高導航傳感器測量精度的方式提升相對導航精度， 但這不僅導致相對導航系統成本和體積的增加， 而且會導致系統復雜性的增加。 為此， 本文提出一種基于幾何拓撲約束的協同相對導航方法， 進而設計得到一種多飛行器高精度協同相對導航方案， 從而能夠在不增加導航傳感器配置的前提下提升相對導航精度。

1 問題描述

1.1 飛行器之間的相對關系

如圖1所示為多飛行器編隊中飛行器之間的相對關系示意圖。 飛行器1和飛行器2本體坐標系分別記為b1系和b2系。 b2系可由b1系經過3次歐拉旋轉得到， 其中3個歐拉旋轉角x， y， z稱為相對俯仰角、 相對滾轉角和相對航向角， 方向余弦矩陣Cb2b1稱為相對姿態矩陣， 其與x， y， z之間滿足以下關系：

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

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

飛行器1和飛行器2相對于地心的位置向量分別記為R1和R2。 根據圖1中的向量關系， 飛行器2相對于飛行器1的相對位置向量R12為

R12=R2－R1 （2）

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

U12=dR12dti=dR2dti－dR1dti（3）

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

1.2 傳統多飛行器相對導航方法

多飛行器編隊中， 利用兩架飛行器上的陀螺儀和加速度計輸出， 便可進行相對慣導解算。 但受初始條件誤差、 慣性器件誤差等影響， 相對慣導誤差會隨時間發散。 因此， 傳統的相對導航方法通常是通過引入數據鏈、 視覺相機提供誤差不隨時間累積的相對導航基準信息， 與相對慣導解算的相對導航信息進行匹配， 并采用最優估計算法對相對慣導誤差進行估計與校正， 以抑制相對慣導誤差發散。 相對導航系統的狀態模型和量測模型如下。

（1） 相對導航系統狀態模型

飛行器1與飛行器2之間的相對姿態誤差、 相對速度誤差和相對位置誤差模型分別為［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分別為相對姿態失準角、 相對速度誤差、 相對位置誤差； ωb11為飛行器1上的陀螺儀輸出； Ωb11為ωb11對應的反對稱矩陣； fb22為飛行器2上的加速度計輸出； δωb11， δωb22分別為飛行器1和飛行器2上陀螺儀的測量誤差； δfb11， δfb22分別為飛行器1和飛行器2上加速度計的測量誤差。

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

δωbii=εgi+wgi（7）

δfbii=Δai+wai（8）

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

因此， 綜合式（4）～（8）， 可建立相對導航系統的狀態模型為

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對應的反對稱矩陣； AR和AU分別為Rb112和Ub112對應的反對稱矩陣。

（2） 相對導航系統量測模型

利用數據鏈和視覺相機可以測得飛行器2相對于飛行器1的距離、 俯仰角和方位角， 分別為

d～=d+εd

θ～=θ+εθ

α～=α+εα （10）

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

如圖2所示， 根據測得的距離d～、 俯仰角θ～和方位角α～， 可推算得到相對位置向量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為相對位置向量真實值Rb112的3個分量； 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。

另外， 相對慣導解算的相對位置向量R^b112為

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

將式（11）與式（12）做差， 可建立相對導航系統的量測模型為

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）共同組成了傳統相對導航濾波模型。 傳統方法通過對相對慣導誤差進行估計與校正， 從而可獲得各飛行器之間的相對位置、 相對速度和相對姿態信息。

然而， 相對慣導誤差的估計精度和速度受系統可觀測性及其狀態量可觀測度的影響。 而根據線性系統可觀測性理論， 系統可觀測性及狀態量可觀測度不僅與量測量的選取有關， 而且與狀態量之間的耦合（約束）關系有關。 因此， 當多架飛行器組成編隊飛行時， 可以考慮根據整個編隊在空間域中的幾何拓撲結構， 建立編隊幾何拓撲約束模型， 并將其引入對相對慣導誤差的估計中。 這樣， 通過利用編隊的幾何拓撲約束條件， 提高相對慣導誤差狀態的可觀測度， 也即提高其估計精度， 從而提升各飛行器之間的相對導航精度。

2 多飛行器高精度協同相對導航方案設計

2.1 編隊幾何拓撲約束模型的建立

以3架飛行器組成的多飛行器編隊為例， 各飛行器之間相對導航示意圖如圖3所示。 可見， 飛行器1、 飛行器2和飛行器3分別利用數據鏈和視覺相機對飛行器2、 飛行器3和飛行器1進行相對距離和相對角度觀測， 從而估計出各飛行器上相對導航系統的狀態向量X12、 X23和X31。

2.1.1 相對姿態約束模型

多飛行器編隊中， 飛行器1、 飛行器2與飛行器3之間的相對姿態矩陣滿足

Cb1b3·Cb3b2·Cb2b1=I（14）

式中： 相對姿態矩陣的真實值Cb2b1， Cb3b2， Cb1b3與其解算值Cb2b1′， Cb3b2′， Cb1b3′之間存在以下關系：

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

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

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

式中： Aδ12， Aδ23， Aδ31分別為相對姿態失準角δ12， δ23， δ31對應的反對稱矩陣。

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

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

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

進而， 根據反對稱矩陣與向量之間的對應關系， 將式（18）改寫成向量形式， 可得相對姿態約束模型為

δ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 相對位置約束模型

多飛行器編隊中， 飛行器1、 飛行器2與飛行器3之間的相對位置向量R12， R23， R31構成一個閉合的向量環， 即

R12+R23+R31=0（20）

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

Rb112+Cb1b2Rb223+Cb1b3Rb331=0（21）

式中： 相對位置向量的真實值Rb112， Rb223， Rb331與其解算值R^b112， R^b223， R^b331之間存在以下關系：

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）

再將相對姿態矩陣的真實值Cb1b2， Cb1b3與其解算值Cb1′b2， Cb1b3′之間的關系式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對應的反對稱矩陣； AR^31為R^b331對應的反對稱矩陣。

進而， 將相對姿態約束模型式（19）代入式（26）， 可得相對位置約束模型為

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 相對速度約束模型

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

U12+U23+U31=0（28）

推導得到相對速度約束模型為

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對應的反對稱矩陣； AU^31為U^b331對應的反對稱矩陣； M3=U^b112+Cb1′b2U^b223+Cb1b3′U^b331－AU^23M1。

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

ε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表示狀態向量X中的狀態分量。

因此， 綜合式（19）、 （27）、 （29）以及式（30）～（32）， 可構建編隊幾何拓撲約束模型為

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。

由此可見， 編隊幾何拓撲約束模型式（33）能夠反映出不同飛行器上相對導航系統的狀態向量， 即X12、 X23和X31之間所滿足的約束關系。 因此， 若將該約束關系引入對狀態向量的估計中， 則相當于對狀態向量施加約束條件。 而根據線性系統可觀測性理論， 對狀態向量施加約束條件能有效提高狀態向量的可觀測度。 這樣， 通過將該約束關系引入對狀態向量的估計中， 便可提高狀態向量的可觀測度， 從而提高狀態向量的估計精度。

2.2 基于幾何拓撲約束的協同相對導航方法

以飛行器1上相對導航系統狀態向量X12的估計為例。 在對X12的估計過程中， 時間更新過程與標準Kalman濾波算法相同。 為了充分利用編隊在空間域中所滿足的幾何拓撲約束關系， 將編隊幾何拓撲約束模型式（33）引入對X12的量測更新中， 則量測更新方程可改寫為

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為狀態向量的估計值； X^12， k|k－1為狀態向量的一步預測值； K12， k為濾波增益矩陣； Z12， k為量測向量； H12， k為量測矩陣； P12， k|k－1為一步預測均方誤差陣； γP12， k|k－11+P12， k|k－1F（X^12， k|k－1－X-12， k|k－1）為幾何拓撲約束項； ·F為Frobenius范數； γ為幾何拓撲約束系數； X-12， k|k－1為由編隊幾何拓撲約束模型所確定的預測值， 即

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上相對導航系統狀態向量X23和X31的一步預測值， 可由數據鏈傳送至飛行器1上。

式（34）表明， 在對狀態向量X12進行量測更新時， 一方面利用量測向量Z12， k（包括飛行器1與飛行器2之間的距離和相對角度信息）對狀態一步預測值X^12， k|k－1進行修正； 另一方面， 還對狀態向量X12施加了編隊幾何拓撲約束條件。 這樣， 通過綜合利用兩架飛行器之間的距離和相對角度信息， 以及整個編隊在空間域中的幾何拓撲信息， 便可有效提高狀態向量X12的估計精度。

2.3 高精度協同相對導航方案

根據所提的協同相對導航方法， 設計得到一種多飛行器高精度協同相對導航方案， 其方案框圖如圖4所示。

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

3 仿真驗證

3.1 仿真條件

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

多飛行器編隊中， 各飛行器均配置相同精度的導航傳感器。 其中， 陀螺儀零偏為1 （°）/h， 陀螺儀隨機游走為0.2 （°）/h； 加速度計零偏為1 mg， 加速度計隨機游走為0.2 mg/Hz。 數據鏈的測距噪聲標準差為3 m， 視覺相機的測角噪聲標準差為1′。

3.2 仿真結果與分析

如圖6～7所示為傳統方法和所提方法的相對導航誤差曲線， 而這兩種方法相對導航誤差的均值和均方差如表1所示。

由圖6～7可知， 傳統方法和所提方法利用數據鏈和視覺相機提供的誤差不隨時間累積的相對導航基準信息， 與相對慣導解算的相對導航信息匹配， 實現了對相對慣導誤差的估計與校正， 可得到誤差不隨時間發散的相對導航結果。 另外， 結合表1中的數據可知， 相較于傳統方法， 所提方法的相對定姿、 相對定位以及相對測速

精度均有顯著提升。 可見， 所提方法通過將整個編隊所滿足的幾何拓撲約束關系引入對相對慣導誤差的估計中， 可有效提高相對慣導誤差的估計精度， 從而顯著提升飛行器之間的相對導航精度。

為了進一步驗證所提方法在不同幾何拓撲結構下的

有效性與先進性， 多飛行器編隊采用不同的幾何拓撲結構進行相對導航。 如圖8所示為多飛行器編隊所采用的幾何拓撲結構， 而表2為不同幾何拓撲結構下， 所提方法與傳統方法的相對導航誤差對比。

由表2中所列數據可知， 無論多飛行器編隊采用哪種幾何拓撲結構， 相較于傳統方法， 所提方法的相對定姿、 相對定位以及相對測速誤差的均值和均方差均明顯減小， 這充分驗證了所提方法在不同幾何拓撲結構下的有效性與先進性。

4 結 論

為了在不增加導航傳感器配置的前提下提升多飛行器之間的相對導航精度， 本文提出一種多飛行器高精度協同相對導航方法。 經過理論分析與仿真驗證， 所得到的結論如下：

（1） 傳統方法通過對相對慣導誤差進行估計與校正， 可獲得各飛行器之間的相對位置、 相對速度和相對姿態。 然而， 由于沒有利用編隊的幾何拓撲信息， 因此相對慣導誤差的可觀測度較低， 從而導致傳統方法的相對導航精度不高。

（2） 所提方法將編隊所滿足的幾何拓撲約束關系引入對相對慣導誤差的估計中， 可有效提高相對慣導誤差的可觀測度， 進而提高其估計精度， 最終提升多飛行器之間的相對導航精度。

仿真結果表明， 在不增加導航傳感器配置的前提下， 所提方法具有比傳統方法更高的相對導航精度， 能夠滿足多飛行器高精度相對導航的需求。

參考文獻：

［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］ 李清華， 高影， 王振桓， 等. 一種動態分組的多節點協同定位編隊構型優化方法［J］. 中國慣性技術學報， 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］ 程傳奇， 郝向陽， 張振杰， 等. 魯棒性地形匹配/慣性組合導航算法［J］. 中國慣性技術學報， 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］ 孫博文， 王大軼， 周海銀， 等. 自主光學相對導航中序列圖像最優構建方法［J］. 宇航學報， 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的智能導彈協同作戰相對導航系統研究［J］. 戰術導彈技術， 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緊組合相對導航算法研究［J］. 現代導航， 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的小型無人機相對導航算法研究［J］. 電子測量技術， 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］ 劉梓軒， 孫永榮， 曾慶化， 等. 基于測距測角信息的DG-IEKF相對導航算法［J］. 導航與控制， 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