基于聯(lián)邦濾波算法的無人機(jī)集群分層協(xié)同導(dǎo)航

谷旭平, 唐大全

(海軍航空大學(xué)航空作戰(zhàn)勤務(wù)學(xué)院, 山東 煙臺 264001)

0 引 言

隨著無人機(jī)控制和導(dǎo)航技術(shù)的不斷精進(jìn),無人機(jī)在軍事行動、應(yīng)急救災(zāi)、智能農(nóng)業(yè)、環(huán)境監(jiān)測、災(zāi)難響應(yīng)、地形勘測、商業(yè)運(yùn)輸、安全監(jiān)控等軍民領(lǐng)域有著廣闊的應(yīng)用前景。由于單無人機(jī)的任務(wù)性能和抗毀傷能力有限,研究重心逐漸向無人機(jī)集群過渡。無人機(jī)集群具有生存率高、成本低、效率高等優(yōu)點(diǎn)。

無人機(jī)導(dǎo)航是關(guān)于如何規(guī)劃無人機(jī)安全、快速到達(dá)目標(biāo)任務(wù)的過程。無人機(jī)必須掌握其位置、速度、航向、出發(fā)點(diǎn)以及目標(biāo)位置。目前主流的導(dǎo)航方法為慣性導(dǎo)航、衛(wèi)星導(dǎo)航、視覺導(dǎo)航,但都各有不足。因此,提高集群導(dǎo)航精度,必須采取合適的無人機(jī)導(dǎo)航。

為了彌補(bǔ)各個(gè)導(dǎo)航系統(tǒng)的缺陷,提高導(dǎo)航精度,多采用相對導(dǎo)航技術(shù)。文獻(xiàn)[20]提出一種適用于編隊(duì)飛行的基于慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)和全球衛(wèi)星導(dǎo)航系統(tǒng)(global positioning system,GPS)的相對導(dǎo)航系統(tǒng)。文獻(xiàn)[21]提出一種基于GPS/INS/無線電測距的無人機(jī)相對導(dǎo)航。文獻(xiàn)[22]采用基于聯(lián)合濾波器的多源容錯(cuò)組合導(dǎo)航系統(tǒng)。該系統(tǒng)不僅彌補(bǔ)了單一導(dǎo)航系統(tǒng)的不足,而且具有很強(qiáng)的容錯(cuò)能力,可以在出現(xiàn)故障時(shí)及時(shí)隔離故障系統(tǒng),使整個(gè)導(dǎo)航系統(tǒng)仍能提供可靠精準(zhǔn)的導(dǎo)航信息。文獻(xiàn)[23]采用的無人機(jī)相對導(dǎo)航系統(tǒng)由載波相位差分GPS,INS和超寬帶技術(shù)組成。該系統(tǒng)可以在GPS受到挑戰(zhàn)時(shí),仍能提供精準(zhǔn)的導(dǎo)航信息。文獻(xiàn)[24]采用基于超寬帶輔助的無人機(jī)編隊(duì)相對導(dǎo)航方式。該方案可以在單依靠GPS的情況下,達(dá)到比較精準(zhǔn)的相對導(dǎo)航效果。文獻(xiàn)[25]提出一種通過主從式編隊(duì)飛行結(jié)構(gòu)的可視化算法來解決主從式飛機(jī)的姿態(tài)估計(jì)和跟蹤問題的方法。文獻(xiàn)[26]提出一種在GPS受限的條件下基于超寬帶信息的間接協(xié)同定位技術(shù)。文獻(xiàn)[27]描述了一種適用于主從結(jié)構(gòu)的無人機(jī)編隊(duì)控制方法,并設(shè)計(jì)了一個(gè)級聯(lián)回路調(diào)整主從無人機(jī)之間的相對距離和角度,以提高協(xié)同導(dǎo)航的穩(wěn)定性。文獻(xiàn)[28]提出一種在主從式編隊(duì)模式下,基于雙目視覺傳感器感知無人機(jī)相對方位的方法。

傳統(tǒng)的相對導(dǎo)航技術(shù),在導(dǎo)航信息受到干擾時(shí),導(dǎo)航精度容易發(fā)散。并且傳統(tǒng)單主從式導(dǎo)航結(jié)構(gòu),在長機(jī)發(fā)生故障時(shí),導(dǎo)航性能會受到嚴(yán)重沖擊。為了解決上述問題,本文建立一種分層協(xié)同導(dǎo)航模型,并基于聯(lián)邦濾波(federated filter,FF)算法,提出了分層協(xié)同導(dǎo)航算法,提高集群導(dǎo)航的可靠性。

分層協(xié)同導(dǎo)航的優(yōu)點(diǎn)之一是靈活性,但增加連接性并不會增加集群中所有成員的導(dǎo)航性能,比如導(dǎo)航精度較高的無人機(jī)融合導(dǎo)航精度較低的無人機(jī)導(dǎo)航信息,會導(dǎo)致導(dǎo)航精度下降。因此,基于圖論的全連通融合導(dǎo)航算法,具有較大通信和計(jì)算負(fù)擔(dān),不適用于高動態(tài)變化的無人機(jī)集群導(dǎo)航,而分層協(xié)同導(dǎo)航解決了這一難題。

分層協(xié)同導(dǎo)航根據(jù)集群中無人機(jī)導(dǎo)航精度,將其分為長機(jī)層和僚機(jī)層,通過長機(jī)選擇策略,僚機(jī)可通過機(jī)載數(shù)據(jù)鏈融合長機(jī)的相對導(dǎo)航信息,提高集群導(dǎo)航信息的利用率,進(jìn)而提高集群整體的導(dǎo)航性能。同時(shí)，利用長機(jī)選擇策略,減少了精度較低以及非視距誤差的測量,減少通信和計(jì)算負(fù)擔(dān)。通過仿真,比較分層協(xié)同導(dǎo)航算法與傳統(tǒng)的單主從式協(xié)同導(dǎo)航算法在導(dǎo)航精度,抗干擾能力方面的優(yōu)劣,進(jìn)一步驗(yàn)證分層協(xié)同導(dǎo)航算法在未來大規(guī)模無人機(jī)集群導(dǎo)航方面的優(yōu)勢,通過不同算法的仿真比較,進(jìn)一步驗(yàn)證了FF算法的合理性。

1 分層協(xié)同導(dǎo)航系統(tǒng)設(shè)計(jì)

1.1 分層協(xié)同導(dǎo)航結(jié)構(gòu)

傳統(tǒng)的單主從式協(xié)同導(dǎo)航結(jié)構(gòu),僚機(jī)與長機(jī)的協(xié)同關(guān)系是固定的,僚機(jī)利用傳感器從長機(jī)獲取相對導(dǎo)航信息。當(dāng)長機(jī)故障時(shí),系統(tǒng)的可靠性會受到影響。因此，單主從式導(dǎo)航結(jié)構(gòu)不能滿足大規(guī)模集群導(dǎo)航條件。為增強(qiáng)無人機(jī)集群導(dǎo)航的可靠性,設(shè)計(jì)如圖1所示的分層協(xié)同導(dǎo)航。

圖1 分層協(xié)同導(dǎo)航Fig.1 Hierarchical cooperative navigation

如圖1(a)所示的分層協(xié)同導(dǎo)航架構(gòu),擺脫了以往單主從模式固定的長僚機(jī)搭配。根據(jù)導(dǎo)航精度將無人機(jī)集群分為長機(jī)層和僚機(jī)層,僚機(jī)可以獲得長機(jī)層的相對導(dǎo)航信息。如果某一架長機(jī)故障,其他長機(jī)正常,僚機(jī)仍可以獲得其他長機(jī)的協(xié)同導(dǎo)航信息,因此該分層協(xié)同導(dǎo)航結(jié)構(gòu)具有較好的可靠性和魯棒性。如果給出至少一架長機(jī)的相對導(dǎo)航信息,就可以確定僚機(jī)的位置。這意味著,該協(xié)同導(dǎo)航結(jié)構(gòu)具有靈活性的特點(diǎn),即只需要一架長機(jī),就可以進(jìn)行協(xié)同定位,這對于長僚機(jī)之間通信部分出現(xiàn)故障的情況,具有良好的魯棒性。該分層協(xié)同導(dǎo)航結(jié)構(gòu)中,一架僚機(jī)可以獲得多架長機(jī)的協(xié)同導(dǎo)航信息,與傳統(tǒng)的單主從式協(xié)同導(dǎo)航結(jié)構(gòu)相比,該結(jié)構(gòu)更能保證故障條件下僚機(jī)的導(dǎo)航性能,并且在大規(guī)模無人機(jī)集群條件下,更能充分利用多機(jī)協(xié)同測量信息,提高集群導(dǎo)航信息的利用率。

如圖1(b)所示的長機(jī)選擇策略考慮到長機(jī)之間的相互遮掩,避免了測量信號的相互干擾。根據(jù)導(dǎo)航系統(tǒng)提示,可以掌握僚機(jī)與其他長機(jī)之間的方位信息,對導(dǎo)航誤差引起的方向不確定性進(jìn)行估計(jì)。以某一僚機(jī)為中心,估算其他長機(jī)在球坐標(biāo)系的遮掩區(qū)域大小,對可以進(jìn)行長僚機(jī)測量的長機(jī)進(jìn)行區(qū)分,建立長機(jī)選擇策略優(yōu)先級,減少非視距誤差對導(dǎo)航性能的影響。如圖1(b)所示,根據(jù)長僚機(jī)之間的方位信息,僚機(jī)可以獲得5架長機(jī)的協(xié)同導(dǎo)航信息;但當(dāng)考慮長機(jī)遮掩區(qū)域的影響時(shí),僚機(jī)只能獲得3架長機(jī)的協(xié)同導(dǎo)航信息。

1.2 協(xié)同導(dǎo)航系統(tǒng)設(shè)計(jì)方案

圖2為無人機(jī)集群協(xié)同導(dǎo)航系統(tǒng)設(shè)計(jì)方案,集群中每一架UAV都搭載一定精度的INS。僚機(jī)搭載激光測距測角設(shè)備和多普勒測速儀等相對導(dǎo)航設(shè)備,用于測量長僚機(jī)之間的相對方位、相對距離和相對速度。僚機(jī)融合處理自身導(dǎo)航設(shè)備以及機(jī)載數(shù)據(jù)鏈提供的長機(jī)層的導(dǎo)航信息,建立測量模型,并通過濾波估計(jì)INS誤差,實(shí)現(xiàn)導(dǎo)航信息的校正。通過融合長機(jī)層的導(dǎo)航信息,提高集群導(dǎo)航信息的利用率,進(jìn)而提高集群整體的導(dǎo)航性能。

圖2 分層協(xié)同導(dǎo)航系統(tǒng)總體方案Fig.2 Overall scheme of hierarchical cooperative navigation system

2 相對導(dǎo)航矢量模型設(shè)計(jì)

2.1 相對導(dǎo)航矢量測量模型

僚機(jī)測距與測速傳感器測得第架長機(jī)與第架僚機(jī)的相對速度與距離為

(1)

(2)

僚機(jī)激光測角傳感器測得第架長機(jī)與第架僚機(jī)的相對角度為

(3)

分別為相應(yīng)角度測量誤差。

圖3 僚機(jī)相對導(dǎo)航測量Fig.3 Follower relative navigation measurement

(4)

將式(1)和式(3)分別代入式(4)得到:

(5)

由于,,都很小,因此得到相對速度測量誤差模型:

(6)

載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的變換矩陣為

(7)

(8)

則根據(jù)式(6)和式(7)可以得到相對導(dǎo)航速度在導(dǎo)航坐標(biāo)系的分量為

(9)

(10)

2.2 相對導(dǎo)航矢量計(jì)算模型

221 相對速度計(jì)算模型

長機(jī)與僚機(jī)導(dǎo)航系統(tǒng)解算的速度為

(11)

(12)

長機(jī)與僚機(jī)導(dǎo)航系統(tǒng)解算速度相減得到兩機(jī)相對速度計(jì)算值:

(13)

222 相對距離計(jì)算模型

(14)

(15)

式中:,,和,,分別為長機(jī)與僚機(jī)的經(jīng)度、緯度和高度真實(shí)值；,,和,,分別為相應(yīng)誤差。

依據(jù)式(14)和式(15)解算出長機(jī)與僚機(jī)的位置在地球坐標(biāo)系的分量為

(16)

(17)

式中:為地球卯酉圈半徑；為橢圓度。

由于,,都很小,因此得到長機(jī)的位置誤差模型為

(18)

同理得到僚機(jī)的位置誤差模型為

(19)

地球坐標(biāo)系到導(dǎo)航坐標(biāo)系的轉(zhuǎn)換矩陣為

(20)

(21)

則長機(jī)與僚機(jī)的相對距離計(jì)算值在導(dǎo)航坐標(biāo)系下的分量為

(22)

3 分層協(xié)同導(dǎo)航模型

3.1 系統(tǒng)的狀態(tài)方程

選擇僚機(jī)的INS誤差模型作為狀態(tài)方程:

()=(,-1)(-1)+(,-1)(-1)

(23)

式中:為INS誤差的狀態(tài)轉(zhuǎn)移矩陣;為INS的噪聲輸入矩陣;為INS的噪聲矢量,其誤差協(xié)方差為;為INS的誤差矢量,定義為

(24)

3.2 導(dǎo)航系統(tǒng)觀測方程

式(9)和式(13)相減得

(25)

則相對速度測量方程為

(26)

式(10)和式(22)相減得

(27)

則相對距離測量方程為

(28)

由式(26)和式(28)聯(lián)立得到觀測方程為

(29)

則量測噪聲的誤差協(xié)方差矩陣為

(30)

式中: 和分別為長機(jī)提供的速度與位置協(xié)方差;和分別為相對速度與距離傳感器的噪聲協(xié)方差；為相對角度傳感器的噪聲協(xié)方差,并且對于僚機(jī),相應(yīng)的相互獨(dú)立。

為了方便描述,令

(31)

(32)

=[,]

(33)

假設(shè)無人機(jī)集群中有架長機(jī),則僚機(jī)的觀測方程為

(34)

則僚機(jī)的誤差協(xié)方差矩陣為

(35)

4 分層協(xié)同導(dǎo)航算法

4.1 FF算法

面對未來大規(guī)模無人機(jī)集群任務(wù),若采用集中式的信息融合,計(jì)算和通信負(fù)擔(dān)重,并且容錯(cuò)性差。而FF算法作為一種新型的分散化濾波方法,降低了算法的復(fù)雜性,提高了算法的容錯(cuò)性與可靠性,而且FF算法易于實(shí)現(xiàn),信息分配方式靈活,計(jì)算量小。

圖4 FF結(jié)構(gòu)框架Fig.4 Structure framework of FF

FF器的工作流程分為4個(gè)步驟。

(36)

時(shí)間更新。子濾波器與主濾波器的時(shí)間更新相互獨(dú)立,其中=1,2,…,,,則時(shí)間更新方程為

(37)

量測更新。量測更新只在子濾波器中進(jìn)行,即=1,2,…,,則量測更新方程為

(38)

(39)

信息融合。將各個(gè)局部濾波器的局部估計(jì)值進(jìn)行融合,得到全局最優(yōu)估計(jì),即

(+1|+1)=

(40)

(41)

4.2 分層協(xié)同導(dǎo)航算法流程

根據(jù)前文所建立的濾波模型,以FF算法為基礎(chǔ),設(shè)計(jì)了本文的分層協(xié)同導(dǎo)航濾波算法,具體步驟如下。

確定無人機(jī)集群中長機(jī)數(shù)量;

依據(jù)式(23)、式(36)和式(37)進(jìn)行信息分配與時(shí)間更新;

依據(jù)式(9)和式(10)以及測得的測距測速測角信息,獲得相對導(dǎo)航測量信息;

根據(jù)長僚機(jī)導(dǎo)航系統(tǒng)提供的位置速度信息,依據(jù)式(13)和式(22)獲得相對導(dǎo)航計(jì)算信息;

依據(jù)式(29)建立協(xié)同導(dǎo)航觀測模型,其中式(30)代表的誤差協(xié)方差矩陣()是根據(jù)測角誤差,測距誤差,測速誤差以及長機(jī)的導(dǎo)航系統(tǒng)的位置誤差和速度誤差 確定;

根據(jù)式(38)～式(41)進(jìn)行濾波狀態(tài)的更新,以及局部傳感器的信息融合。

5 仿真分析

5.1 仿真設(shè)置

為了驗(yàn)證算法的有效性以及協(xié)同導(dǎo)航模型的正確性,進(jìn)行數(shù)據(jù)仿真。參與本次仿真的無人機(jī)共25架,其中長機(jī)7架,仿真時(shí)間為3 000 s,長機(jī)的定位誤差為2 m,仿真中INS以及相對導(dǎo)航設(shè)備的相關(guān)參數(shù)如表1所示。如無特別提示,仿真都是在該條件下進(jìn)行。此外,進(jìn)行100次蒙特卡羅實(shí)驗(yàn),研究不同導(dǎo)航因素對導(dǎo)航性能的影響。同時(shí)對傳統(tǒng)的單主從式導(dǎo)航(single leader-follower, SLF)算法和分層協(xié)同導(dǎo)航(hierarchical collaborative fusion, HCF)算法進(jìn)行仿真比較。

表1 導(dǎo)航傳感器參數(shù)

5.2 相對導(dǎo)航矢量精度分析

對不同測距、測速、測角精度下的無人機(jī)集群導(dǎo)航進(jìn)行仿真,分析相對測距、測速和測角精度對僚機(jī)1導(dǎo)航性能的影響,如圖5～圖7所示。

圖5 不同測距精度下的位置誤差Fig.5 Position error under different ranging accuracies

圖6 不同測速精度下的位置誤差Fig.6 Position error under different velocity accuracies

圖7 不同測角精度下的位置誤差Fig.7 Position error under different angular accuracies

圖5(a)、圖6(a)和圖7(a)分別給出了僚機(jī)在3 m和13 m的測距精度,1 m/s和11 m/s的測速精度,1.0°和0.1°測角精度下僚機(jī)的定位誤差曲線,并對SLF算法與HCF算法進(jìn)行比較。圖5(b)、圖6(b)和圖7(b)顯示了在不同測距、測速和測角精度下的僚機(jī)位置均方根誤差(root mean square eror, RMSE)。

如圖5(a)、圖6(a)和圖7(a)所示,提高測距、測速和測角精度,可以減小SLF算法與HCF算法的定位誤差。在相同的測距、測速和測角精度下,HCF算法的定位誤差比SLF算法要小。如圖5(b)、圖6(b)和圖7(b)所示,隨著相對導(dǎo)航測量精度的提高,僚機(jī)的定位RMSE減小。在相同導(dǎo)航測量精度下,HCF算法比SLF算法的定位精度提高了18%～27%。根據(jù)曲線的增長趨勢可知,HCF算法受相對導(dǎo)航測量精度的影響較小。從圖5(b)、圖6(b)和圖7(b)可以看出,與SLF算法相比,HCF對導(dǎo)航精度的要求較低,也即為了滿足一定的導(dǎo)航性能,HCF算法降低了相對測量精度的要求。

5.3 集群中長機(jī)數(shù)量對導(dǎo)航性能的影響

圖8顯示了無人機(jī)集群中長機(jī)的比例對導(dǎo)航精度的影響。圖8(a)顯示了長機(jī)占比10%和30%的情況下,SLF算法與HCF算法的定位精度誤差。圖8(b)顯示了長機(jī)占比為10%～60%時(shí),僚機(jī)的定位RMSE變化。

圖8 不同長機(jī)比例下的位置誤差Fig.8 Position error under different proportions of leader

從圖8(a)可以看出在,對于HCF算法,提高長機(jī)占比可以提高導(dǎo)航精度,但對于SLF算法來說,效果不是很明顯。并且在長機(jī)占比相同的情況下,HCF算法的定位精度更好。這也說明HCF算法較SLF算法更能充分利用無人機(jī)集群中協(xié)同導(dǎo)航信息,提高整體的導(dǎo)航性能。從圖8(b)可以看出,在長機(jī)占比相同的情況下,HCF算法較SLF算法具有更低的定位RMSE。隨著長機(jī)比例的增加,無人機(jī)集群的定位RMSE逐漸降低,但下降趨于平緩,長機(jī)占比從10%增加到30%的定位精度的提高要明顯大于長機(jī)占比從30%到50%的。因此可以看出,提高長機(jī)比例可以降低定位誤差,但當(dāng)長機(jī)占比達(dá)到45%左右時(shí),效果不是很明顯,此時(shí)再增加長機(jī)占比,只會增加導(dǎo)航成本。

5.4 導(dǎo)航性能的穩(wěn)定性分析

假設(shè)給僚機(jī)1提供導(dǎo)航信息的長機(jī)在1 500～2 000 s內(nèi)受損,無法提供導(dǎo)航信息。圖9(a)顯示了僚機(jī)1在長機(jī)故障情況下的定位誤差,圖9(b)顯示了長機(jī)1故障下自身的定位誤差,圖9(c)顯示了在不同數(shù)量長機(jī)故障時(shí)的集群定位RMSE變化。如圖9(a)所示,在1 500～2 000 s內(nèi),由于給僚機(jī)1提供導(dǎo)航信息的長機(jī)發(fā)生故障,對于HCF算法,僚機(jī)1仍能保持定位精度,但是對于SLF算法而言,定位誤差迅速發(fā)散。如圖9(b)所示,對于SLF算法,當(dāng)長機(jī)1故障時(shí),定位誤差迅速發(fā)散,但對于HCF算法,長機(jī)1可以利用無人機(jī)集群中其他長機(jī)的導(dǎo)航信息實(shí)現(xiàn)定位誤差校正,保持定位精度的穩(wěn)定性。如圖9(c)所示,隨著長機(jī)故障數(shù)量的增加,集群整體定位RMSE增加。但在相同的長機(jī)故障數(shù)量下,HCF算法比SLF算法具有更小的定位RMSE,并且在長機(jī)故障數(shù)較多的情況下尤為明顯。從曲線的增長率可以看出,HCF算法較SLF算法受長機(jī)故障數(shù)影響較小,也就減小了長機(jī)故障對集群導(dǎo)航性能的影響。所以HCF算法比SLF算法更能保證導(dǎo)航性能的穩(wěn)定性。

圖9 長機(jī)故障引起的導(dǎo)航誤差Fig.9 Navigation error associated with leader plane failure

5.5 算法性能測試

為驗(yàn)證本文FF算法的性能,選取卡爾曼濾波(Kalman filter,KF)算法與粒子濾波(particle filter,PF)算法進(jìn)行仿真比較,仿真結(jié)果如圖10所示,仿真時(shí)間如表2所示。

圖10 不同濾波算法比較Fig.10 Comparison of different filtering algorithms

表2 不同算法仿真時(shí)間對比

在濾波初始條件相同的條件下,有重置的FF算法和集中式融合算法是等價(jià)的。從仿真結(jié)果看出,在仿真精度上,FF和KF的濾波精度要稍好于PF算法。但是在仿真時(shí)間上,由于FF算法的各子濾波器的濾波運(yùn)算是并行的,這使得運(yùn)算速度大大提高,運(yùn)算時(shí)間僅為集中式融合算法的二分之一,由仿真結(jié)果可以看出無論是在仿真精度還是在運(yùn)行時(shí)間上,FF算法都具有很大優(yōu)勢,進(jìn)一步驗(yàn)證了FF算法在分層協(xié)同導(dǎo)航方面的優(yōu)勢。

6 結(jié) 論

為了改善傳統(tǒng)單主從式無人機(jī)協(xié)同導(dǎo)航的性能,提高集群對導(dǎo)航信息的利用率以及集群導(dǎo)航性能的穩(wěn)定性,本文基于FF算法,提出了一種無人機(jī)集群分層協(xié)同導(dǎo)航算法,建立了無人機(jī)集群分層協(xié)同導(dǎo)航模型。仿真結(jié)果顯示,與傳統(tǒng)的SLF算法相比,HCF算法提高了集群導(dǎo)航信息的利用率以及定位精度,并且受相對導(dǎo)航測量精度影響較小,提高了協(xié)同導(dǎo)航系統(tǒng)的容錯(cuò)能力以及穩(wěn)定性,保證了協(xié)同導(dǎo)航系統(tǒng)整體的導(dǎo)航性能,通過不同濾波算法的比較,驗(yàn)證了FF算法的合理性。