基于路徑和算法的飛行器集群分布式相對定位

賴瑋清，萬九卿

北京航空航天大學 自動化科學與電氣工程學院，北京 100191

在如今的戰場環境下，單個作戰裝備往往難以勝任復雜多變的任務，集群協同模式正逐漸成為未來戰場重要的作戰手段［1］。集群成員節點間準確的相對定位，包括相對位置和相對姿態的估計，是飛行器集群任務規劃與編隊控制的基礎，也是集群完成協同探測、感知、搜索與打擊任務的前提條件。

集群節點間的相對定位可通過對各節點絕對運動狀態的估計間接獲得。由于純慣導的定位誤差隨時間不斷累積，而衛星定位信號易受屏蔽或干擾，因此基于慣導/數據鏈組合的集群協同相對定位逐漸成為研究的熱點。現有的數據鏈設備兼具機間通訊和相對測距功能，已被廣泛應用于飛行器集群組網［2-4］。利用數據鏈的相對距離測量可對慣導估計結果進行修正，進而提升集群相對定位精度。由于數據鏈只能提供相對測量，因此慣導/數據鏈組合不能保證節點絕對定位精度。相對定位的可觀性與集群節點運動方式有關，文獻［5-6］給出了相對定位可觀性的充分必要條件。

針對如圖1所示的慣導/數據鏈組合集群協同定位問題，研究者提出了許多集中式相對定位方法，主要包括最小二乘法［7-9］，擴展卡爾曼濾波方法［10］，非線性優化方法［11］和圖優化算法［12］等。集中式算法需要將集群內全部節點的觀測信息匯聚到中心節點集中處理，因此，集中式系統的規模和可靠性往往受到中心節點的制約，中心節點的運算和通信負擔會隨集群規模迅速增加，且中心節點的毀傷會導致整個系統的失效。另一方面，分布式系統不存在中心節點，每個節點只處理局部量測信息并與相鄰節點通訊。各節點運行同樣的信息處理算法和信息交互協議，可方便地增刪集群節點或改變集群拓撲結構。同時，任何節點的毀傷都不會顯著影響系統的性能。和集中式系統相比，分布式系統具有更高的靈活性、可擴展性和魯棒性。

圖1 基于慣導/數據鏈的飛行器集群相對定位Fig.1 Relative positioning of aircraft group based on inertial navigation/data link

在分布式集群協同定位算法中，基于概率圖模型的消息傳遞類算法得到了廣泛的關注，主要包括信念傳播算法［13］，變分消息傳遞算法［14］，廣義近似消息傳遞算法等［15］，其中信念傳播算法在分布式集群協同定位中的應用最為廣泛。信念傳播算法根據數據鏈拓撲結構建立概率圖模型描述集群狀態的聯合分布，通過圖模型節點間的消息傳遞計算每個節點狀態的邊緣分布，利用圖模型節點與集群物理節點的對應關系，實現集群分布式推理。由于數據鏈觀測模型的非線性，使得信念傳播算法中的概率分布難以進行參數化表達，因此目前的研究中通常采用加權粒子群來近似表達消息概率分布［16-23］。對于高維狀態變量，描述其概率分布需要的粒子數極為龐大，極大地限制了這類方法的應用。高斯信念傳播［24-28］算法通過對觀測模型線性化，建立誤差狀態的線性模型，在初始高斯分布假設下，誤差狀態分布在更新過程中始終為高斯分布，可用均值向量和協方差矩陣參數化表達。在高斯信念傳播算法中，消息的更新和傳遞歸結為均值和協方差參數的更新和傳遞，使得系統的計算和通信負擔大為降低。

然而，信念傳播算法的收斂性與概率圖模型的結構有關。當概率圖模型為無環結構時，例如樹結構或鏈結構，信念傳播算法迭代過程能夠確保收斂到正確的邊緣分布。對于慣導/數據鏈集群協同定位，相應的圖模型通常存在環結構，此時信念傳播算法不能保證收斂［13，29］。對于有環圖上的高斯信念傳播算法收斂性問題已有大量研究成果［27，30-36］，當收斂條件滿足時，高斯信念傳播算法迭代過程可確保收斂，且邊緣分布均值可以收斂到精確解，但一般情況下收斂后的邊緣分布協方差是不準確的［30］。在慣導/數據鏈協同定位推理過程中，每一時刻節點狀態分布的協方差陣誤差會傳導到下一時刻，導致下一時刻均值和協方差的計算誤差，該誤差會隨時間不斷累積，最終對定位精度造成不利影響。

針對上述問題，本文提出一種面向飛行器集群慣導/數據鏈組合協同定位的分布式推理框架。在該框架中，每個飛行器節點各自獨立進行慣導解算以估計自身運動狀態，利用誤差狀態均值對該估計進行反饋修正。在慣導估計狀態附近對慣導誤差傳播模型和數據鏈觀測模型線性化，得到集群節點誤差狀態的線性高斯模型。采用標準型和規范型2 種形式描述誤差狀態高斯分布，在高斯標準型描述下進行誤差狀態分布的時間更新，在高斯規范型描述下利用數據鏈相對測距進行誤差狀態分布的量測更新。基于矩陣求逆的路徑和原理［37］，設計分布式矩陣求逆算法，實現高斯分布標準型和規范型之間的轉換。對于任意飛行器集群拓撲結構，該推理框架都能確保獲得精確解。同時，該推理框架是完全分布式的，只包含每個集群節點的局部信息處理和相鄰結點間通過數據鏈的消息傳遞。提出基于集群分解的相對定位近似推理算法，提高了推理計算速度。進行了飛行器集群六自由度長航時協同相對定位仿真，結果表明，本文方法的相對位置和相對姿態估計精度明顯優于現有分布式高斯信念傳播算法。

1 問題模型

1.1 坐標系與狀態變量定義

本文用到的坐標系包括：地心慣性系oixiyizi；地心地固系oexeyeze；導航系onxnynzn，即“東-北-天”地理坐標系；本體系obxbybzb，即“右-前-上”本體坐標系。

第k時刻集群中第i個飛行器的運動由如式（1）所示的10 維狀態向量描述：

1.2 慣性器件模型

對于陀螺器件，考慮的誤差包括陀螺隨機常值漂移和角度隨機游走，陀螺輸出模型為

對于加速度計器件，考慮的誤差包括加計隨機常值偏值和速度隨機游走，加速度計輸出模型為：

1.3 數據鏈測距模型

每個飛行器節點均搭載數據鏈信號發射與接收終端，通過偽碼與載波相位測量，可實現彈間測距。考慮雙向單程測距法［38］，節點間距離測量為：

式中：T代表偽距測量；t和r分別為終端發射和接收延時，可由設備標校得到；c為光速。

節點間數據鏈測距模型為：

1.4 飛行器集群分布式相對定位

根據每個飛行器節點的初始狀態以及慣性器件的輸出，利用捷聯慣導更新算法，可遞推獲得每個飛行器運動狀態軌跡，從而實現集群相對定位。然而，由于存在初始狀態誤差和慣性器件誤差，慣導輸出的飛行器運動軌跡會逐漸遠離真實狀態。本文研究的目標是：利用數據鏈提供的節點間距離量測，對慣導輸出進行修正，實現節點相對定位誤差的抑制。如圖2所示，飛行器集群相對定位主要包含如下4 個基本過程。

圖2 飛行器集群分布式相對定位過程Fig.2 Distributed relative positioning process of aircraft cluster

1） 慣導狀態更新

每個飛行器根據陀螺和加計讀數，利用捷聯慣導算法更新自身的運動狀態。慣導狀態更新包括姿態更新、速度更新和位置更新過程，具體更新公式請參閱文獻［39］。

2） 誤差狀態聯合分布更新

飛行器真實運動狀態與慣導計算狀態之間的偏差可用誤差狀態δXik表示，誤差狀態為隨機變量。由于存在數據鏈相對測距，各飛行器節點運動狀態相互耦合，故需考慮集群節點誤差狀態的聯合分布，其中M為集群節點數目。誤差狀態聯合分布的更新算法包括慣性器件噪聲驅動的時間更新和數據鏈相對測距量測更新。

3） 慣導狀態修正

量測更新后的誤差狀態均值用于修正慣導狀態，得到飛行器運動狀態估計，修正后須將誤差均值重置為0。

4） 相對定位

相對定位包括相對位置和相對姿態的估計，節點(i，j)間的相對位置記為，將pi和pj由經緯高地理坐標系變換到地心直角坐標系后相減得到Δpij，Δpij為三維矢量，其中表示坐標變換后的相減運算。節點(i，j)間的相對姿態記為，節點i的姿態四元數與節點j的姿態四元數的共軛相乘后對應的等效旋轉矢量即為Δ?ij，Δ?ij為三維矢量，其中?表示四元數的乘積運算。

由以上分析可見，每個飛行器節點可獨立進行慣導狀態的更新與修正，本身就是分布式過程。然而，各飛行器誤差狀態之間相互耦合，須設計分布式算法進行集群誤差狀態聯合分布的時間更新與量測更新。節點間數據鏈的通信功能為分布式相對定位提供了基礎。在分布式系統中，通過每個節點執行相同的局部信息處理算法，同時通過數據鏈和相鄰節點進行信息交互，最終實現集群相對運動狀態的全局估計。

2 分布式誤差狀態更新算法

2.1 誤差狀態及其聯合分布的參數化描述

誤差狀態表示飛行器真實運動狀態Xk與慣導計算狀態之間的偏差，第i個飛行器節點的誤差狀態由如下15 維隨機向量描述：

本文假定誤差為小量，在慣導狀態附近，誤差狀態傳播方程和觀測方程可以近似為線性模型。假定初始誤差狀態服從高斯分布，則誤差狀態在線性系統作用下始終保持為高斯分布。將k時刻集群誤差狀態聯合分布記為，該聯合分布亦為高斯分布。

本文采用標準型和規范型2 種方式描述集群誤差狀態聯合分布，分別用于誤差狀態分布的時間更新和量測更新：

式中：高斯標準型參數包括均值向量μk與協方差矩陣Σk；高斯規范型參數包括信息矢量ηk與精度矩陣Λk；均值向量μk和信息矢量ηk均由M個15×1 維子矢量構成，第i個子矢量對應于第i個飛行器節點；協方差陣Σk和精度矩陣Λk均由M×M個15×15 維矩陣子塊構成。在標準型描述下進行時間更新，可由高斯分布線性傳播模型直接對協方差陣的每個子塊獨立進行時間更新，計算簡潔且便于分布式實現；在規范型描述下進行量測更新，量測更新歸結為對精度矩陣的各個子塊和信息矢量子塊的修正，計算過程簡單，同時所需信息僅包括與相鄰節點間的相對測距，便于分布式實現。

飛行器集群誤差狀態聯合分布的更新過程如圖3所示，第i個飛行器節點僅維護并更新均值向量子塊或信息矢量子塊，以及協方差陣子塊或精度矩陣子塊。誤差狀態的更新過程可分為3 個模塊：① 分布式時間更新，在高斯標準型描述下進行，詳見第2.2 節；② 分布式量測更新，在高斯規范型描述下進行，詳見第2.3 節；③ 分布式矩陣求逆，實現標準型和規范型之間的參數轉換，詳見第3 節。誤差狀態在每個時刻均進行時間更新，當觀測到來時，利用路徑和算法將標準型轉換為規范型描述，然后進行量測更新，再將更新后的規范型描述轉換為標準型，繼續進行時間更新。

圖3 集群誤差狀態更新過程Fig.3 Aircraft group error states update process

2.2 誤差狀態分布的時間更新

2.2.1 誤差狀態傳播模型

根據慣導微分方程和慣性器件誤差模型，忽略二階小量，可得每個飛行器節點的線性離散誤差狀態傳播方程如式（8）所示［39］：

式中：I表示單位陣；Δt表示采樣時間。狀態轉移矩陣中的各個子塊M以及姿態變換陣可根據當前時刻飛行器自身的慣導狀態計算。式（9）中的各矩陣子塊計算公式請參閱文獻［39］。

2.2.2 分布式誤差狀態時間更新算法

根據誤差狀態傳播方程，可得分布式誤差狀態時間更新算法。誤差狀態分布的時間更新在標準型描述下進行，由于誤差均值在每次量測更新后都被置零，因此只需要考慮協方差矩陣的時間更新。將集群誤差狀態聯合分布協方差陣中對應于(i，j)節點的子塊記為，第i個飛行器節點上存儲的協方差陣子塊為，其中M表示集群成員節點集合。第i個飛行器節點上協方差子塊更新公式為

2.3 誤差狀態分布的量測更新

2.3.1 誤差狀態觀測模型

飛行器節點(i，j)之間的相對距離與飛行器位置之間的關系為

式中：(xk，yk，zk)表示飛行器節點的位置矢量在地心直角坐標系三軸的投影，地心直角坐標與導航系地理坐標(Lk，λk，hk)之間的關系為

其中：e為地球橢圓偏心率。

式中：

其中各項偏導數可根據式（13）和式（14）以及求導鏈式法則計算：

式（18）中各項偏導數的計算公式請參閱文獻［39］。

將式（15）移項并寫成矩陣形式，可得數據鏈相對測距線性化觀測模型為

其中：0表示1×3 維零向量。

2.3.2 分布式誤差狀態量測更新算法

第k-1 時刻時間更新后的誤差狀態聯合分布為

將聯合分布精度矩陣中對應于(i，j)節點的子塊記為，信息矢量中對應于i節點的子矢量記為。記第k時刻量測矢量為，對應的雅可比矩陣為Hk，由貝葉斯公式可知，量測更新后誤差狀態聯合分布為

當第k時刻節點(i，j)之間具有量測矢量時，精度矩陣子塊和信息矢量子塊可進行如式（23）和（24）所示的更新：

式中：n(i)表示節點i的鄰居節點集合，即所有與節點i之間存在數據鏈聯結的節點集合。在分布式量測更新過程中，每個飛行器節點i需要收集相鄰節點的慣導狀態以計算觀測雅可比，再根據相對距離量測利用上式進行精度矩陣與信息矢量子塊的更新。

3 基于路徑和原理的誤差狀態聯合分布參數轉換

誤差狀態聯合分布的高斯標準型和規范型參數之間存在如式（25）所示的轉換關系：

實現2 種描述轉換的關鍵在于協方差陣（或精度矩陣）的求逆，需要將所有矩陣子塊匯聚到中央節點上進行集中運算。本文基于路徑和原理，設計矩陣的分布式求逆算法，從而實現聯合分布標準型和規范型參數的分布式轉換。在分布式算法中，通過每個飛行器節點的局部運算和節點間的信息交互，可直接完成矩陣子塊和之間的轉換。

3.1 路徑和原理

設有M×M個子塊組成矩陣R，用Rαβ表示表示其(α，β)子塊，(α，β∈{1，…，M})。為簡便起見，將R-1的(α，β)子塊記為。構建與R對應的有向圖G=(V，E)，其中V={α：α=1，…，M}表示節點集合，E={(α，β)：α，β=1， …，M}表示邊集合，圖G 的邊(α，β)與矩陣R的子塊Rβα一一對應。當且僅當Rβα≠0 時，(α，β)∈E，邊(α，β)的權重為Rβα。圖G 上的一個自α到ω的長度為l 的行走w定義為由l 條連續的邊組成的序列(αμ2)(μ2μ3)…(μlω)，簡記為(αμ2μ3…μlω)，圖G 上的自α到ω的全部行走的集合記為WG；αω。一個行走w的權重為組成行走的所有邊的權重的乘積。如果一個行走經過的所有節點互不相同，則該行走稱為一條路徑p，圖G上的起點為α，終點為ω的全部路徑集合記為PG；αω。如果一條路徑的起點和終點相同，則該路徑稱為一個環路c，圖G 上的起點和終點為α的全部環路集合記為CG；α。

由矩陣乘法運算法則，矩陣R的l 次冪Rl的(ω，α)子塊可按式（26）計算：

對于有限圖G，M＜∞，路徑集合PG；αω與環路集合CG；α均為有限集。用表示圖G 對應的矩陣的逆矩陣的第(α，β) 個子塊，用G{α，β，…}表示去掉節點{α，β，…}∈V 以及相應邊后的子圖，文獻［37］給出了逆矩陣各子塊的計算公式，主對角線子塊如式（28）所示：

式中：c=(μ1，μ2， …，μl(c)，μ1) 為圖G 上一條以μ1≡α為起點和終點的閉合環路。非對角線子塊如式（29）所示：

3.2 主對角線子塊的分布式求逆方法

集群中的第α個飛行器節點負責計算逆矩陣主對角線子塊。首先根據集群中每個飛行器的鄰居節點信息，利用深度優先或廣度優先路徑搜索算法［40］，可得圖G 上以α為起點和終點的全部環路集合CG；α。對于每一條環路c ∈CG；α，由式（28）可知，飛行器節點α將任務分配給環路c上的各個節點μj，每個節點μj負責計算子圖上的逆矩陣主對角線子塊。直至子圖中只包含一個節點時，該遞歸過程結束。主對角線子塊的分布式求逆算法如算法1所示。

3.3 非對角線子塊的分布式求逆算法

算法1 基于路徑和算法的逆矩陣主對角線子塊計算：DistInvDD搜索圖G 上自節點α 出發回到節點α 上的環路集合：CG；α初始化：B-1 αα(G)=0 For c=(α，μ2，…， μl( )c，α)∈CG；αα，do令：Ac=( -1)l( )c +1 Rˉμ1 μl( )c For r=l(c)：-1：2，do調用DistInvDD 計算：Rˉ-1 μr μr(G {α，μ2， …， μr-1})計算：Ac=Ac·Rˉ-1 μr μr(G {α，μ2， …， μr-1})·Rˉμr μr-1 end計算：B-1αα(G)=B-1αα(G)+Aγ End對矩陣塊B-1αα(G)求逆：Rˉ-1 αα(G)=inv(B-1 αα(G))

4 基于集群分解的快速相對定位

利用上述分布式算法，可以得到集群中每個節點狀態的精確推理結果。然而，隨著圖模型規模的擴大，路徑和算法的計算量會迅速增加，限制了上述算法在大規模集群上的應用。針對這一問題，本節給出一種基于集群分解的相對定位近似推理方法。

將整個飛行器集群記為M，假設集群中有M個成員節點。 將集群M 分解為M個子群Ml，l=1，…，M，子群Ml由成員節點l及其相鄰節點組成，即。該分解過程滿足2 個基本條件：① 覆蓋所有成員節點，即M=；② 每個成員節點可能同時屬于不同的子群，即子群之間存在相互重疊。如圖4所示，飛行器集群M 包括5 個成員節點，結點間的連線對應于飛行器間的數據鏈。按上述規則，可將集群M 分解為5 個子群M1，…， M5。

圖4 集群的分解Fig.4 Aircraft group decomposition

根據節點i的鄰居信息n(i)，每個節點可以獨立構建一個子群Mi=i∪n(i)，同時可以確定自身所屬的子群集合Ml，l∈i∪n(i)。可以看到，按上述方式構建的子群集合必然滿足集群分解的2 個基本條件。在每個子群上獨立進行分布式推理得到和，即第l個子群上對節點i的誤差狀態均值與方差估計。將所有包含節點i的子群對節點i的誤差狀態估計進行信息融合［39］，即

融合后的誤差狀態估計可用于對慣導狀態的修正。基于集群分解的分布式相對定位快速推理算法如算法3所示。

算法3 基于集群分解的分布式推理For 集群中每個成員節點 i=1：M，do獨立進行慣導解算，得到慣導狀態Xˉik；根據n(i)， 構建子群Mi=i ∪n(i)，并確定節點i所屬子群Ml，l ∈i ∪n(i)；For 每個子群 Ml.l=1：M，do由式（12）進行時間更新，得到Σij k，l由算法1，算法2 計算Λij k，l由式（23）和式（24）進行量測更新，得到Λijk，l 和ηik，l由算法1，算法2 計算Σij k，l 和μik，l End由式（30）和式（31）進行信息融合，計算μi k利用μi k 對慣導狀態Xˉik 修正，得到估計X?ik End

在基于集群分解的推理過程中， 每個子群上運行第2 節和第3 節所述的分布式精確推理算法，可確保收斂。但由于沒有利用到整個集群觀測信息，因此只能得到誤差狀態的次優估計。另一方面，由于每個子群規模較小，因而極大地減少了路徑和求逆的計算量，提升了算法運行速度。

5 仿真驗證

5.1 仿真設置

為了驗證所提算法的性能，仿真生成包含10 個飛行器節點的集群六自由度飛行軌跡，仿真飛行時長為2 500 s。10 個飛行器以800 m/s 的平均速度編隊飛行，根據文獻［5-6］給出的可觀性條件，在每個飛行器3 個軸向上添加不同頻率正弦機動，使得集群內任意2 個節點的相對運動在3 個軸向上互不相關。每個飛行器的姿態根據當前時刻速度軸指向確定，根據慣導反演算法［39］生成加速度計和陀螺儀讀數。在仿真過程中，任意2 個節點間的數據鏈以一定的概率發生故障，每次故障時間服從20～50 s 之間的均勻分布。當數據鏈正常時，可以在2 個節點之間傳遞信息，同時根據節點位置給出對應節點對之間的相對距離量測。因此，仿真過程中集群拓撲結構在隨時間不斷變化。

每個飛行器初始狀態誤差服從零均值高斯分布，各分量標準差如表1所示。

表1 初始狀態誤差Table 1 Initial state error

每個飛行器的加速度計、陀螺儀誤差參數以及飛行器間數據鏈測距標準差如表2所示。根據表1 給出的初始狀態誤差以及表2 給出的陀螺常值漂移與加計常值偏值，設定每個飛行器的初始協方差陣。根據表2 給出的角度隨機游走與速度隨機游走，設定誤差狀態傳播的噪聲驅動矩陣。根據表2 給出的數據鏈觀測誤差設定觀測方差陣。在設定時需將表中數據轉換為國際標準單位。

表2 各傳感器參數Table 2 Parameters of each sensor

本文實現了4 種算法的性能對比：① 集中式卡爾曼濾波算法（Error-State Kalman Filter，ESKF）［41］，利用卡爾曼濾波更新誤差狀態，在線性高斯模型假設下可獲得最優狀態估計，該算法需將全部數據匯集到中心節點統一處理；② 分布式高斯信念傳播算法（Gaussian beliefs propaga-tion， GBP）［26］，在線性高斯模型假設下，針對誤差狀態聯合分布建立因子圖模型，通過因子節點與變量節點間的消息傳遞更新高斯模型參數，該算法僅包含節點局部信息處理和節點間信息交互；③ 支撐樹高斯信念傳播算法（Spanning Tree Gaussian Beliefs Propagation，STGBP），針對每一時刻數據鏈拓撲，遍歷其全部支撐樹，在每個支撐樹結構上運行GBP 推理算法，再將各支撐樹上的推理結果求平均用于相對定位；④ 基于路徑和（Path-Sum）的分布式近似推理算法，如本文第4 節所述，通過集群分解實現誤差狀態更新的快速近似推理。

5.2 仿真結果

5.2.1 相對定位均方根誤差

將4 種算法應用到仿真數據上，計算相對位置和相對姿態估計誤差如式（32）所示：

對于包含M個節點的集群，共有N=組相對定位結果，計算k時刻集群相對定位均方根誤差為

式中：α=E，N，U 表示東、北、天三軸方向。

集群相對定位均方根誤差結果如圖5 和圖6所示，顯示了ESKF、GBP、STGBP 以及路徑和Path-Sum 算法的結果。由圖5 和圖6 可見：① 集中式ESKF 的相對定位估計誤差在經過短時間震蕩后迅速收斂，在1 000 s 后相對位置和姿態均方根誤差分別保持在5 m 和6′以內，全時段均方根誤差最小；② 分布式GBP 的估計誤差曲線出現大幅震蕩，最大相對位置均方根誤差可達800 m，相對姿態均方根誤差可達250′，主要原因是集群數據鏈中大量的環狀結構使得GBP 算法在迭代過程中往往無法收斂至精確解，從而導致算法的不穩定；③ 分布式STGBP 的估計誤差曲線同樣出現震蕩，最大相對位置均方根誤差可達450 m，相對姿態均方根誤差可達230′，STGBP 相對定位結果略優于GBP，主要是由于在樹結構上的GBP 算法可以確保收斂，但和本文算法相比仍有明顯差距，其主要原因在于：為了去除環結構，在STGBP 支撐樹中人為斷掉了原有的部分數據鏈，同時也拋棄了相應的相對測距觀測，這使得STGBP 無法利用相對測距閉環約束，從而降低了其相對定位精度；④本文給出的路徑和算法的估計誤差曲線與ESKF接近，在1 000 s 后相對位置和姿態均方根誤差分別保持在15 m 和6′以內，明顯優于GBP 算法和STGBP 算法。

圖6 相對姿態均方根誤差Fig.6 Relative attitude root mean square error

5.2.2 蒙特卡洛仿真

為進一步驗證算法的魯棒性，對算法進行蒙特卡洛仿真。計算第τ次仿真的集群相對定位總均方根誤差為

統計L次仿真結果，計算總均方根誤差的均值和標準差為

取L=30，相對位置總均方根誤差均值和標準差結果如表3所示，相對姿態總均方根誤差均值和標準差結果如表4所示。

表3 相對位置總均方根誤差均值和標準差Table 3 Mean of relative position total root mean square error and standard deviation

表4 相對姿態總均方根誤差均值和標準差Table 4 Mean of relative attitude total root mean square error and standard deviation

由表3 和表4 可見：① 集中式ESKF 算法的三軸相對定位估計誤差具有最小的均值和標準差，表現出良好的估計精度和魯棒性；② 分布式GBP 算法相對位置誤差均值和標準最大；③ 分布式STGBP 算法的表現優于GBP 算法，仍與路徑和算法存在較大差距；④ 路徑和算法的表現接近于集中式ESKF，在30 次蒙特卡洛仿真中，三軸相對位置估計誤差均值在10 m 以內，標準差在2 m 以內，三軸相對姿態估計誤差均值在5′以內，標準差在1′以內，估計精度和魯棒性顯著優于GBP 和STGBP 算法。

5.2.3 運行時間

在本文算法中，慣導更新頻率為100 Hz，誤差狀態時間更新頻率為100 Hz，誤差狀態量測更新頻率為10 Hz。表5 給出了4 種算法各環節單節點的平均耗時，計算機主頻為3.20 GHz，實現語言為C++。

表5 算法運行時間Table 5 Algorithm running time

由表5 可見，4 種算法中慣導更新計算都是在各節點上獨立完成，運行時間差別不大。GBP 算法和STGBP 算法在每個節點進行局部時間更新，耗時最短，而其余3 種算法需要對集群全部節點聯合分布進行時間更新，計算時間稍長。由于量測更新過程中涉及矩陣求逆運算，因此耗時遠大于其他環節。GBP 算法、STGBP 算法以及路徑和算法中只涉及15×15 維矩陣求逆。然而，STGBP 算法因為在每一時刻需要遍歷支撐樹并對多個支撐樹拓撲的結果進行信息融合，因此耗時長于GBP 算法。在路徑和算法中，通過集群分解控制了子圖的規模，從而減小了計算負擔。表5 最后一列給出了4 種算法在2 500 s 數據集上的總運行時間，由表5 可見，路徑和算法運算速度高于集中式ESKF 算法。

6 結 論

本文提出一種基于路徑和原理的飛行器集群分布式協同定位方法，根據慣性器件測量和數據鏈相對測距實現集群節點間相對位姿的分布式估計。該算法適用于任意集群拓撲結構，避免了信念傳播算法在環狀拓撲時存在的收斂性問題。同時，該算法分別在高斯標準型和規范型描述下對集群誤差狀態聯合分布進行時間更新和量測更新，利用路徑和求逆原理實現了2 種描述間的轉換。全部更新過程和參數轉換過程只包含集群中每個節點自身的局部信息處理和節點間的信息交互，因此是完全分布式的。為進一步提升算法運算速度，給出了基于集群分解快速推理方法。在六自由度長航時飛行器集群仿真數據上進行了驗證，結果表明，本文算法的相對定位精度明顯優于現有的分布式信念傳播算法。