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

賴瑋清，萬(wàn)九卿

北京航空航天大學(xué) 自動(dòng)化科學(xué)與電氣工程學(xué)院，北京 100191

在如今的戰(zhàn)場(chǎng)環(huán)境下，單個(gè)作戰(zhàn)裝備往往難以勝任復(fù)雜多變的任務(wù)，集群協(xié)同模式正逐漸成為未來(lái)戰(zhàn)場(chǎng)重要的作戰(zhàn)手段［1］。集群成員節(jié)點(diǎn)間準(zhǔn)確的相對(duì)定位，包括相對(duì)位置和相對(duì)姿態(tài)的估計(jì)，是飛行器集群任務(wù)規(guī)劃與編隊(duì)控制的基礎(chǔ)，也是集群完成協(xié)同探測(cè)、感知、搜索與打擊任務(wù)的前提條件。

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

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

圖1 基于慣導(dǎo)/數(shù)據(jù)鏈的飛行器集群相對(duì)定位Fig.1 Relative positioning of aircraft group based on inertial navigation/data link

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

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

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

1 問(wèn)題模型

1.1 坐標(biāo)系與狀態(tài)變量定義

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

第k時(shí)刻集群中第i個(gè)飛行器的運(yùn)動(dòng)由如式（1）所示的10 維狀態(tài)向量描述：

1.2 慣性器件模型

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

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

1.3 數(shù)據(jù)鏈測(cè)距模型

每個(gè)飛行器節(jié)點(diǎn)均搭載數(shù)據(jù)鏈信號(hào)發(fā)射與接收終端，通過(guò)偽碼與載波相位測(cè)量，可實(shí)現(xiàn)彈間測(cè)距?？紤]雙向單程測(cè)距法［38］，節(jié)點(diǎn)間距離測(cè)量為：

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

節(jié)點(diǎn)間數(shù)據(jù)鏈測(cè)距模型為：

1.4 飛行器集群分布式相對(duì)定位

根據(jù)每個(gè)飛行器節(jié)點(diǎn)的初始狀態(tài)以及慣性器件的輸出，利用捷聯(lián)慣導(dǎo)更新算法，可遞推獲得每個(gè)飛行器運(yùn)動(dòng)狀態(tài)軌跡，從而實(shí)現(xiàn)集群相對(duì)定位。然而，由于存在初始狀態(tài)誤差和慣性器件誤差，慣導(dǎo)輸出的飛行器運(yùn)動(dòng)軌跡會(huì)逐漸遠(yuǎn)離真實(shí)狀態(tài)。本文研究的目標(biāo)是：利用數(shù)據(jù)鏈提供的節(jié)點(diǎn)間距離量測(cè)，對(duì)慣導(dǎo)輸出進(jìn)行修正，實(shí)現(xiàn)節(jié)點(diǎn)相對(duì)定位誤差的抑制。如圖2所示，飛行器集群相對(duì)定位主要包含如下4 個(gè)基本過(guò)程。

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

1） 慣導(dǎo)狀態(tài)更新

每個(gè)飛行器根據(jù)陀螺和加計(jì)讀數(shù)，利用捷聯(lián)慣導(dǎo)算法更新自身的運(yùn)動(dòng)狀態(tài)。慣導(dǎo)狀態(tài)更新包括姿態(tài)更新、速度更新和位置更新過(guò)程，具體更新公式請(qǐng)參閱文獻(xiàn)［39］。

2） 誤差狀態(tài)聯(lián)合分布更新

飛行器真實(shí)運(yùn)動(dòng)狀態(tài)與慣導(dǎo)計(jì)算狀態(tài)之間的偏差可用誤差狀態(tài)δXik表示，誤差狀態(tài)為隨機(jī)變量。由于存在數(shù)據(jù)鏈相對(duì)測(cè)距，各飛行器節(jié)點(diǎn)運(yùn)動(dòng)狀態(tài)相互耦合，故需考慮集群節(jié)點(diǎn)誤差狀態(tài)的聯(lián)合分布，其中M為集群節(jié)點(diǎn)數(shù)目。誤差狀態(tài)聯(lián)合分布的更新算法包括慣性器件噪聲驅(qū)動(dòng)的時(shí)間更新和數(shù)據(jù)鏈相對(duì)測(cè)距量測(cè)更新。

3） 慣導(dǎo)狀態(tài)修正

量測(cè)更新后的誤差狀態(tài)均值用于修正慣導(dǎo)狀態(tài)，得到飛行器運(yùn)動(dòng)狀態(tài)估計(jì)，修正后須將誤差均值重置為0。

4） 相對(duì)定位

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

由以上分析可見(jiàn)，每個(gè)飛行器節(jié)點(diǎn)可獨(dú)立進(jìn)行慣導(dǎo)狀態(tài)的更新與修正，本身就是分布式過(guò)程。然而，各飛行器誤差狀態(tài)之間相互耦合，須設(shè)計(jì)分布式算法進(jìn)行集群誤差狀態(tài)聯(lián)合分布的時(shí)間更新與量測(cè)更新。節(jié)點(diǎn)間數(shù)據(jù)鏈的通信功能為分布式相對(duì)定位提供了基礎(chǔ)。在分布式系統(tǒng)中，通過(guò)每個(gè)節(jié)點(diǎn)執(zhí)行相同的局部信息處理算法，同時(shí)通過(guò)數(shù)據(jù)鏈和相鄰節(jié)點(diǎn)進(jìn)行信息交互，最終實(shí)現(xiàn)集群相對(duì)運(yùn)動(dòng)狀態(tài)的全局估計(jì)。

2 分布式誤差狀態(tài)更新算法

2.1 誤差狀態(tài)及其聯(lián)合分布的參數(shù)化描述

誤差狀態(tài)表示飛行器真實(shí)運(yùn)動(dòng)狀態(tài)Xk與慣導(dǎo)計(jì)算狀態(tài)之間的偏差，第i個(gè)飛行器節(jié)點(diǎn)的誤差狀態(tài)由如下15 維隨機(jī)向量描述：

本文假定誤差為小量，在慣導(dǎo)狀態(tài)附近，誤差狀態(tài)傳播方程和觀測(cè)方程可以近似為線性模型。假定初始誤差狀態(tài)服從高斯分布，則誤差狀態(tài)在線性系統(tǒng)作用下始終保持為高斯分布。將k時(shí)刻集群誤差狀態(tài)聯(lián)合分布記為，該聯(lián)合分布亦為高斯分布。

本文采用標(biāo)準(zhǔn)型和規(guī)范型2 種方式描述集群誤差狀態(tài)聯(lián)合分布，分別用于誤差狀態(tài)分布的時(shí)間更新和量測(cè)更新：

式中：高斯標(biāo)準(zhǔn)型參數(shù)包括均值向量μk與協(xié)方差矩陣Σk；高斯規(guī)范型參數(shù)包括信息矢量ηk與精度矩陣Λk；均值向量μk和信息矢量ηk均由M個(gè)15×1 維子矢量構(gòu)成，第i個(gè)子矢量對(duì)應(yīng)于第i個(gè)飛行器節(jié)點(diǎn)；協(xié)方差陣Σk和精度矩陣Λk均由M×M個(gè)15×15 維矩陣子塊構(gòu)成。在標(biāo)準(zhǔn)型描述下進(jìn)行時(shí)間更新，可由高斯分布線性傳播模型直接對(duì)協(xié)方差陣的每個(gè)子塊獨(dú)立進(jìn)行時(shí)間更新，計(jì)算簡(jiǎn)潔且便于分布式實(shí)現(xiàn)；在規(guī)范型描述下進(jìn)行量測(cè)更新，量測(cè)更新歸結(jié)為對(duì)精度矩陣的各個(gè)子塊和信息矢量子塊的修正，計(jì)算過(guò)程簡(jiǎn)單，同時(shí)所需信息僅包括與相鄰節(jié)點(diǎn)間的相對(duì)測(cè)距，便于分布式實(shí)現(xiàn)。

飛行器集群誤差狀態(tài)聯(lián)合分布的更新過(guò)程如圖3所示，第i個(gè)飛行器節(jié)點(diǎn)僅維護(hù)并更新均值向量子塊或信息矢量子塊，以及協(xié)方差陣子塊或精度矩陣子塊。誤差狀態(tài)的更新過(guò)程可分為3 個(gè)模塊：① 分布式時(shí)間更新，在高斯標(biāo)準(zhǔn)型描述下進(jìn)行，詳見(jiàn)第2.2 節(jié)；② 分布式量測(cè)更新，在高斯規(guī)范型描述下進(jìn)行，詳見(jiàn)第2.3 節(jié)；③ 分布式矩陣求逆，實(shí)現(xiàn)標(biāo)準(zhǔn)型和規(guī)范型之間的參數(shù)轉(zhuǎn)換，詳見(jiàn)第3 節(jié)。誤差狀態(tài)在每個(gè)時(shí)刻均進(jìn)行時(shí)間更新，當(dāng)觀測(cè)到來(lái)時(shí)，利用路徑和算法將標(biāo)準(zhǔn)型轉(zhuǎn)換為規(guī)范型描述，然后進(jìn)行量測(cè)更新，再將更新后的規(guī)范型描述轉(zhuǎn)換為標(biāo)準(zhǔn)型，繼續(xù)進(jìn)行時(shí)間更新。

圖3 集群誤差狀態(tài)更新過(guò)程Fig.3 Aircraft group error states update process

2.2 誤差狀態(tài)分布的時(shí)間更新

2.2.1 誤差狀態(tài)傳播模型

根據(jù)慣導(dǎo)微分方程和慣性器件誤差模型，忽略二階小量，可得每個(gè)飛行器節(jié)點(diǎn)的線性離散誤差狀態(tài)傳播方程如式（8）所示［39］：

式中：I表示單位陣；Δt表示采樣時(shí)間。狀態(tài)轉(zhuǎn)移矩陣中的各個(gè)子塊M以及姿態(tài)變換陣可根據(jù)當(dāng)前時(shí)刻飛行器自身的慣導(dǎo)狀態(tài)計(jì)算。式（9）中的各矩陣子塊計(jì)算公式請(qǐng)參閱文獻(xiàn)［39］。

2.2.2 分布式誤差狀態(tài)時(shí)間更新算法

根據(jù)誤差狀態(tài)傳播方程，可得分布式誤差狀態(tài)時(shí)間更新算法。誤差狀態(tài)分布的時(shí)間更新在標(biāo)準(zhǔn)型描述下進(jìn)行，由于誤差均值在每次量測(cè)更新后都被置零，因此只需要考慮協(xié)方差矩陣的時(shí)間更新。將集群誤差狀態(tài)聯(lián)合分布協(xié)方差陣中對(duì)應(yīng)于(i，j)節(jié)點(diǎn)的子塊記為，第i個(gè)飛行器節(jié)點(diǎn)上存儲(chǔ)的協(xié)方差陣子塊為，其中M表示集群成員節(jié)點(diǎn)集合。第i個(gè)飛行器節(jié)點(diǎn)上協(xié)方差子塊更新公式為

2.3 誤差狀態(tài)分布的量測(cè)更新

2.3.1 誤差狀態(tài)觀測(cè)模型

飛行器節(jié)點(diǎn)(i，j)之間的相對(duì)距離與飛行器位置之間的關(guān)系為

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

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

式中：

其中各項(xiàng)偏導(dǎo)數(shù)可根據(jù)式（13）和式（14）以及求導(dǎo)鏈?zhǔn)椒▌t計(jì)算：

式（18）中各項(xiàng)偏導(dǎo)數(shù)的計(jì)算公式請(qǐng)參閱文獻(xiàn)［39］。

將式（15）移項(xiàng)并寫(xiě)成矩陣形式，可得數(shù)據(jù)鏈相對(duì)測(cè)距線性化觀測(cè)模型為

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

2.3.2 分布式誤差狀態(tài)量測(cè)更新算法

第k-1 時(shí)刻時(shí)間更新后的誤差狀態(tài)聯(lián)合分布為

將聯(lián)合分布精度矩陣中對(duì)應(yīng)于(i，j)節(jié)點(diǎn)的子塊記為，信息矢量中對(duì)應(yīng)于i節(jié)點(diǎn)的子矢量記為。記第k時(shí)刻量測(cè)矢量為，對(duì)應(yīng)的雅可比矩陣為Hk，由貝葉斯公式可知，量測(cè)更新后誤差狀態(tài)聯(lián)合分布為

當(dāng)?shù)趉時(shí)刻節(jié)點(diǎn)(i，j)之間具有量測(cè)矢量時(shí)，精度矩陣子塊和信息矢量子塊可進(jìn)行如式（23）和（24）所示的更新：

式中：n(i)表示節(jié)點(diǎn)i的鄰居節(jié)點(diǎn)集合，即所有與節(jié)點(diǎn)i之間存在數(shù)據(jù)鏈聯(lián)結(jié)的節(jié)點(diǎn)集合。在分布式量測(cè)更新過(guò)程中，每個(gè)飛行器節(jié)點(diǎn)i需要收集相鄰節(jié)點(diǎn)的慣導(dǎo)狀態(tài)以計(jì)算觀測(cè)雅可比，再根據(jù)相對(duì)距離量測(cè)利用上式進(jìn)行精度矩陣與信息矢量子塊的更新。

3 基于路徑和原理的誤差狀態(tài)聯(lián)合分布參數(shù)轉(zhuǎn)換

誤差狀態(tài)聯(lián)合分布的高斯標(biāo)準(zhǔn)型和規(guī)范型參數(shù)之間存在如式（25）所示的轉(zhuǎn)換關(guān)系：

實(shí)現(xiàn)2 種描述轉(zhuǎn)換的關(guān)鍵在于協(xié)方差陣（或精度矩陣）的求逆，需要將所有矩陣子塊匯聚到中央節(jié)點(diǎn)上進(jìn)行集中運(yùn)算。本文基于路徑和原理，設(shè)計(jì)矩陣的分布式求逆算法，從而實(shí)現(xiàn)聯(lián)合分布標(biāo)準(zhǔn)型和規(guī)范型參數(shù)的分布式轉(zhuǎn)換。在分布式算法中，通過(guò)每個(gè)飛行器節(jié)點(diǎn)的局部運(yùn)算和節(jié)點(diǎn)間的信息交互，可直接完成矩陣子塊和之間的轉(zhuǎn)換。

3.1 路徑和原理

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

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

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

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

3.2 主對(duì)角線子塊的分布式求逆方法

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

3.3 非對(duì)角線子塊的分布式求逆算法

算法1 基于路徑和算法的逆矩陣主對(duì)角線子塊計(jì)算：DistInvDD搜索圖G 上自節(jié)點(diǎn)α 出發(fā)回到節(jié)點(diǎn)α 上的環(huán)路集合：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調(diào)用DistInvDD 計(jì)算：Rˉ-1 μr μr(G {α，μ2， …， μr-1})計(jì)算：Ac=Ac·Rˉ-1 μr μr(G {α，μ2， …， μr-1})·Rˉμr μr-1 end計(jì)算：B-1αα(G)=B-1αα(G)+Aγ End對(duì)矩陣塊B-1αα(G)求逆：Rˉ-1 αα(G)=inv(B-1 αα(G))

4 基于集群分解的快速相對(duì)定位

利用上述分布式算法，可以得到集群中每個(gè)節(jié)點(diǎn)狀態(tài)的精確推理結(jié)果。然而，隨著圖模型規(guī)模的擴(kuò)大，路徑和算法的計(jì)算量會(huì)迅速增加，限制了上述算法在大規(guī)模集群上的應(yīng)用。針對(duì)這一問(wèn)題，本節(jié)給出一種基于集群分解的相對(duì)定位近似推理方法。

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

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

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

融合后的誤差狀態(tài)估計(jì)可用于對(duì)慣導(dǎo)狀態(tài)的修正。基于集群分解的分布式相對(duì)定位快速推理算法如算法3所示。

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

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

5 仿真驗(yàn)證

5.1 仿真設(shè)置

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

每個(gè)飛行器初始狀態(tài)誤差服從零均值高斯分布，各分量標(biāo)準(zhǔn)差如表1所示。

表1 初始狀態(tài)誤差Table 1 Initial state error

每個(gè)飛行器的加速度計(jì)、陀螺儀誤差參數(shù)以及飛行器間數(shù)據(jù)鏈測(cè)距標(biāo)準(zhǔn)差如表2所示。根據(jù)表1 給出的初始狀態(tài)誤差以及表2 給出的陀螺常值漂移與加計(jì)常值偏值，設(shè)定每個(gè)飛行器的初始協(xié)方差陣。根據(jù)表2 給出的角度隨機(jī)游走與速度隨機(jī)游走，設(shè)定誤差狀態(tài)傳播的噪聲驅(qū)動(dòng)矩陣。根據(jù)表2 給出的數(shù)據(jù)鏈觀測(cè)誤差設(shè)定觀測(cè)方差陣。在設(shè)定時(shí)需將表中數(shù)據(jù)轉(zhuǎn)換為國(guó)際標(biāo)準(zhǔn)單位。

表2 各傳感器參數(shù)Table 2 Parameters of each sensor

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

5.2 仿真結(jié)果

5.2.1 相對(duì)定位均方根誤差

將4 種算法應(yīng)用到仿真數(shù)據(jù)上，計(jì)算相對(duì)位置和相對(duì)姿態(tài)估計(jì)誤差如式（32）所示：

對(duì)于包含M個(gè)節(jié)點(diǎn)的集群，共有N=組相對(duì)定位結(jié)果，計(jì)算k時(shí)刻集群相對(duì)定位均方根誤差為

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

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

圖6 相對(duì)姿態(tài)均方根誤差Fig.6 Relative attitude root mean square error

5.2.2 蒙特卡洛仿真

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

統(tǒng)計(jì)L次仿真結(jié)果，計(jì)算總均方根誤差的均值和標(biāo)準(zhǔn)差為

取L=30，相對(duì)位置總均方根誤差均值和標(biāo)準(zhǔn)差結(jié)果如表3所示，相對(duì)姿態(tài)總均方根誤差均值和標(biāo)準(zhǔn)差結(jié)果如表4所示。

表3 相對(duì)位置總均方根誤差均值和標(biāo)準(zhǔn)差Table 3 Mean of relative position total root mean square error and standard deviation

表4 相對(duì)姿態(tài)總均方根誤差均值和標(biāo)準(zhǔn)差Table 4 Mean of relative attitude total root mean square error and standard deviation

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

5.2.3 運(yùn)行時(shí)間

在本文算法中，慣導(dǎo)更新頻率為100 Hz，誤差狀態(tài)時(shí)間更新頻率為100 Hz，誤差狀態(tài)量測(cè)更新頻率為10 Hz。表5 給出了4 種算法各環(huán)節(jié)單節(jié)點(diǎn)的平均耗時(shí)，計(jì)算機(jī)主頻為3.20 GHz，實(shí)現(xiàn)語(yǔ)言為C++。

表5 算法運(yùn)行時(shí)間Table 5 Algorithm running time

由表5 可見(jiàn)，4 種算法中慣導(dǎo)更新計(jì)算都是在各節(jié)點(diǎn)上獨(dú)立完成，運(yùn)行時(shí)間差別不大。GBP 算法和STGBP 算法在每個(gè)節(jié)點(diǎn)進(jìn)行局部時(shí)間更新，耗時(shí)最短，而其余3 種算法需要對(duì)集群全部節(jié)點(diǎn)聯(lián)合分布進(jìn)行時(shí)間更新，計(jì)算時(shí)間稍長(zhǎng)。由于量測(cè)更新過(guò)程中涉及矩陣求逆運(yùn)算，因此耗時(shí)遠(yuǎn)大于其他環(huán)節(jié)。GBP 算法、STGBP 算法以及路徑和算法中只涉及15×15 維矩陣求逆。然而，STGBP 算法因?yàn)樵诿恳粫r(shí)刻需要遍歷支撐樹(shù)并對(duì)多個(gè)支撐樹(shù)拓?fù)涞慕Y(jié)果進(jìn)行信息融合，因此耗時(shí)長(zhǎng)于GBP 算法。在路徑和算法中，通過(guò)集群分解控制了子圖的規(guī)模，從而減小了計(jì)算負(fù)擔(dān)。表5 最后一列給出了4 種算法在2 500 s 數(shù)據(jù)集上的總運(yùn)行時(shí)間，由表5 可見(jiàn)，路徑和算法運(yùn)算速度高于集中式ESKF 算法。

6 結(jié) 論

本文提出一種基于路徑和原理的飛行器集群分布式協(xié)同定位方法，根據(jù)慣性器件測(cè)量和數(shù)據(jù)鏈相對(duì)測(cè)距實(shí)現(xiàn)集群節(jié)點(diǎn)間相對(duì)位姿的分布式估計(jì)。該算法適用于任意集群拓?fù)浣Y(jié)構(gòu)，避免了信念傳播算法在環(huán)狀拓?fù)鋾r(shí)存在的收斂性問(wèn)題。同時(shí)，該算法分別在高斯標(biāo)準(zhǔn)型和規(guī)范型描述下對(duì)集群誤差狀態(tài)聯(lián)合分布進(jìn)行時(shí)間更新和量測(cè)更新，利用路徑和求逆原理實(shí)現(xiàn)了2 種描述間的轉(zhuǎn)換。全部更新過(guò)程和參數(shù)轉(zhuǎn)換過(guò)程只包含集群中每個(gè)節(jié)點(diǎn)自身的局部信息處理和節(jié)點(diǎn)間的信息交互，因此是完全分布式的。為進(jìn)一步提升算法運(yùn)算速度，給出了基于集群分解快速推理方法。在六自由度長(zhǎng)航時(shí)飛行器集群仿真數(shù)據(jù)上進(jìn)行了驗(yàn)證，結(jié)果表明，本文算法的相對(duì)定位精度明顯優(yōu)于現(xiàn)有的分布式信念傳播算法。