鄢家鑫,賀曉華,周澤波,林國(guó)泉

(1. 電子科技大學(xué)航空航天學(xué)院,四川 成都 611731;2. 中國(guó)空空導(dǎo)彈研究院,河南 洛陽(yáng) 471009; 3.航空制導(dǎo)武器航空科技重點(diǎn)實(shí)驗(yàn)室,河南 洛陽(yáng) 471009)

0 引言

與單無(wú)人機(jī)作業(yè)相比,多無(wú)人機(jī)協(xié)同作業(yè)能完成許多更加復(fù)雜的任務(wù),因而具有更高的應(yīng)用價(jià)值[1]。在多無(wú)人機(jī)協(xié)同作業(yè)的應(yīng)用中,對(duì)于無(wú)人機(jī)的導(dǎo)航精度有著較高的要求,而位置信息作為無(wú)人機(jī)實(shí)現(xiàn)導(dǎo)航控制所必需的關(guān)鍵信息,其精度對(duì)于多無(wú)人機(jī)進(jìn)行協(xié)同作業(yè)而言就顯得十分重要。在多無(wú)人機(jī)系統(tǒng)中,除了無(wú)人機(jī)自身的傳感器信息外,還可以利用相鄰無(wú)人機(jī)的信息輔助自身進(jìn)行導(dǎo)航[3],這種導(dǎo)航方式稱為協(xié)同導(dǎo)航。

協(xié)同導(dǎo)航通過(guò)利用相鄰節(jié)點(diǎn)的觀測(cè)信息與自身的觀測(cè)信息進(jìn)行融合以達(dá)到增強(qiáng)自身定位性能的目的[5],因此其本質(zhì)還是多元數(shù)據(jù)融合。從其核心算法的角度分類,大致可以分為基于優(yōu)化理論[7]、基于概率圖模型[8]以及基于濾波估計(jì)[9]的三大類協(xié)同導(dǎo)航算法[10]。文獻(xiàn)[11]通過(guò)節(jié)點(diǎn)之間共享衛(wèi)星偽距觀測(cè)信息、節(jié)點(diǎn)間相對(duì)位置信息以及節(jié)點(diǎn)自身的位置估計(jì)信息,采用最小二乘法進(jìn)行融合估計(jì)以實(shí)現(xiàn)協(xié)同定位;然而文中將所有數(shù)據(jù)集中處理進(jìn)行全局的聯(lián)合估計(jì),因此是一個(gè)集中式的結(jié)構(gòu),另外該方法忽略了觀測(cè)值的方差信息。文獻(xiàn)[12]中采用基于聯(lián)合樹(shù)的方法實(shí)現(xiàn)了衛(wèi)星之間的協(xié)同定軌,聯(lián)合樹(shù)推理算法屬于貝葉斯網(wǎng)絡(luò)中的精確推理算法,其采用樹(shù)結(jié)構(gòu)對(duì)網(wǎng)絡(luò)進(jìn)行建模,然后根據(jù)消息傳遞算法求取各變量的邊緣概率。文獻(xiàn)[13]提出了一種基于粒子濾波器的新型混合全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system,GNSS)地面定位算法,稱之為混合協(xié)同粒子濾波器,該算法融合了來(lái)自衛(wèi)星和地面接收器的測(cè)距數(shù)據(jù),它是完全分布式的,并且可以提高定位的可用性和準(zhǔn)確性;然而粒子濾波計(jì)算量較大限制了它的實(shí)際應(yīng)用場(chǎng)景。

本文提出了一種基于GNSS/UWB的多無(wú)人機(jī)協(xié)同定位技術(shù),超寬帶技術(shù)(ultra wide band,UWB)作為一種具備高精度、低成本、體積小等特點(diǎn)的距離測(cè)量傳感器,十分適合作為無(wú)人機(jī)之間高精度的相互觀測(cè)傳感器[15]。將UWB測(cè)距信息與GNSS信息融合進(jìn)行協(xié)同導(dǎo)航,可以有效解決傳統(tǒng)單體導(dǎo)航中存在的問(wèn)題。

1 消息傳遞算法與迭代濾波

1.1 因子圖與消息傳遞算法

對(duì)于處理多變量的復(fù)雜全局函數(shù)問(wèn)題,通?？梢岳靡蚴椒纸獾姆绞綄⑵浞纸鉃槿舾蓚€(gè)更簡(jiǎn)單的局部函數(shù)的乘積形式。假設(shè)一個(gè)多變量全局函數(shù)g(x1,…,xn)可以被因式分解為幾個(gè)局部函數(shù)的乘積形式:

(1)

式中:J表示局部函數(shù)的集合,Xj表示全局變量{x1,…,xn}的子集,fj(Xj)表示以Xj為變量的局部函數(shù)。

在因子圖中,每個(gè)變量xj都有唯一的變量節(jié)點(diǎn)與之對(duì)應(yīng),每個(gè)局部函數(shù)fj都有唯一的因子節(jié)點(diǎn)與之對(duì)應(yīng),并且使用邊連接與之相關(guān)聯(lián)的變量節(jié)點(diǎn)。以一個(gè)簡(jiǎn)單的因子圖為例,在圖中,空心圓“”表示變量節(jié)點(diǎn),實(shí)心方塊“”表示因子節(jié)點(diǎn)。令g(x1,x2,x3,x4,x5)表示一個(gè)含5變量的函數(shù),并假設(shè)g可以表示為5個(gè)因子的乘積,其表達(dá)式如下:

g(x1,x2,x3,x4,x5)=
fA(x1)fB(x2)fC(x1,x2,x3)fD(x3,x4)fE(x3,x5)。

(2)

式(2)對(duì)應(yīng)的因子圖如圖1所示。

圖1 式(2)的因子圖表示Fig.1 Factor graph representation of equation(2)

可以通過(guò)因式分解和分布律來(lái)計(jì)算每一個(gè)邊緣密度函數(shù)。以g1(x1)為例:

(3)

式中:～{xi}表示不包括xi的變量集合。

但是在計(jì)算所有的邊緣函數(shù)時(shí)會(huì)存在大量的子問(wèn)題被重復(fù)計(jì)算,計(jì)算效率不高。通過(guò)和積算法用消息來(lái)表示各種計(jì)算中的子問(wèn)題,將邊緣密度函數(shù)的計(jì)算過(guò)程看作是消息傳遞的過(guò)程,建立了一種直觀高效的計(jì)算邊緣密度的方法。

如圖2所示,μx→f(x)表示由變量節(jié)點(diǎn)x發(fā)往因子節(jié)點(diǎn)f的消息,μf→x(x)表示由因子節(jié)點(diǎn)f發(fā)往變量節(jié)點(diǎn)x的消息,n(v)表示節(jié)點(diǎn)v鄰居的集合。

圖2 和積算法Fig.2 Sum-product algorithm

和積算法中兩種消息的計(jì)算由式(4)、式(5)表示。

變量到局部因子的消息:

(4)

局部因子到變量的消息:

(5)

式中:n(x)/{f}表示變量x除去f之外的鄰居因子節(jié)點(diǎn)的集合,n(f)/{x}表示因子節(jié)點(diǎn)f除去x之外的鄰居變量節(jié)點(diǎn)的集合,～{x}表示除x之外所有變量的集合,f(X)為局部函數(shù),X=n(f)表示局部函數(shù)中自變量的集合。邊緣密度函數(shù)為

(6)

1.2 迭代濾波算法

對(duì)于非線性問(wèn)題,通常情況下EKF只保留到一階項(xiàng)。因此不可避免地會(huì)損失精度,若使用當(dāng)前時(shí)刻濾波估計(jì)的結(jié)果作為新的展開(kāi)點(diǎn),再次對(duì)系統(tǒng)進(jìn)行近似展開(kāi),并再次進(jìn)行濾波估計(jì),則有助于提高估計(jì)的精度。

首先,按EKF方法進(jìn)行預(yù)濾波,此時(shí)無(wú)需對(duì)均方誤差矩陣進(jìn)行測(cè)量更新

(7)

(8)

使用平滑值對(duì)一步狀態(tài)預(yù)測(cè)進(jìn)行修正:

(9)

(10)

于是,可以得到迭代濾波的公式:

(11)

若迭代前后的差值小于閾值則認(rèn)為迭代收斂,并輸出當(dāng)前結(jié)果。其流程如圖3所示。

2 基于消息傳遞的多無(wú)人機(jī)協(xié)同位置估計(jì)

2.1 多無(wú)人機(jī)協(xié)同定位模型

(12)

無(wú)人機(jī)i在k時(shí)刻對(duì)GPS衛(wèi)星的偽距方程可表示為

(13)

無(wú)人機(jī)i在k時(shí)刻對(duì)BDS衛(wèi)星的偽距方程可表示為

(14)

除此之外,無(wú)人機(jī)還與鄰居無(wú)人機(jī)之間具備相對(duì)距離觀測(cè)的能力,它們之間的距離觀測(cè)方程可表示為

(15)

于是,無(wú)人機(jī)群體在k時(shí)刻的定位問(wèn)題可以描述為在給定所有可用觀測(cè)值條件下的后驗(yàn)概率:

(16)

假設(shè)無(wú)人機(jī)之間的狀態(tài)相互獨(dú)立,且觀測(cè)值之間也相互獨(dú)立,則可以根據(jù)齊次馬爾可夫假設(shè)以及觀測(cè)獨(dú)立假設(shè)得到

(17)

(18)

對(duì)式(16)進(jìn)行因式分解。為了簡(jiǎn)化書寫,以下 和ρM,B統(tǒng)一用ρM表示,其分解過(guò)程如下:

(19)

式中:

(20)

將式(20)代入式(19)有

(21)

(22)

至此,已經(jīng)得出全局概率密度的因式分解形式,若要求解無(wú)人機(jī)i的邊緣密度,則需對(duì)式(22)進(jìn)行積分:

(23)

式中:M/i表示集合M中除去變量i后剩余變量的集合。

2.2 基于消息傳遞的多無(wú)人機(jī)協(xié)同位置估計(jì)

式(22)給出了全局概率密度函數(shù)的因子分解形式,根據(jù)無(wú)人機(jī)之間的狀態(tài)相互獨(dú)立與觀測(cè)值之間相互獨(dú)立的假設(shè),式(22)可進(jìn)一步寫成

(24)

式中:n(i)表示無(wú)人機(jī)i的鄰居無(wú)人機(jī)的集合。若令

(25)

則有

(26)

于是,可得到式(26)對(duì)應(yīng)的因子圖如圖4所示。

圖4 多無(wú)人機(jī)協(xié)同定位因子圖模型Fig.4 Multi-UAV cooperative positioning factor map model

圖中,fi表示無(wú)人機(jī)i從k-1時(shí)刻到k時(shí)刻的轉(zhuǎn)移因子,ρi,sB表示k時(shí)刻無(wú)人機(jī)i可觀測(cè)的BDS衛(wèi)星集合的因子節(jié)點(diǎn),ρi,sG表示k時(shí)刻無(wú)人機(jī)i可觀測(cè)GPS衛(wèi)星集合的因子節(jié)點(diǎn),ri,j表示k時(shí)刻無(wú)人機(jī)i與鄰居j之間的距離觀測(cè)因子。

因?yàn)闇y(cè)量噪聲會(huì)對(duì)觀測(cè)值的可信度產(chǎn)生影響,所以有必要對(duì)其隨機(jī)模型進(jìn)行分析。通?？梢约僭O(shè)測(cè)量噪聲的隨機(jī)模型服從零均值的高斯分布。衛(wèi)星高度角越大,衛(wèi)星信號(hào)上的噪聲越小,因此可以采用高度角來(lái)描述偽距觀測(cè)噪聲的隨機(jī)模型,采用基于高度角的正弦函數(shù)形式的隨機(jī)模型來(lái)描述:

(27)

式中:σρ為偽距測(cè)量噪聲的標(biāo)準(zhǔn)差;a,b均為常數(shù),通常分別設(shè)置為0.003和0.000 2;e是偽距與載波誤差比,通?？筛鶕?jù)經(jīng)驗(yàn)設(shè)置。

對(duì)于無(wú)人機(jī)間相對(duì)距離觀測(cè)值,鄰居無(wú)人機(jī)的位置精度也會(huì)對(duì)距離觀測(cè)量的可信度造成影響。假設(shè)(xi,yi,zi)為待估無(wú)人機(jī)的坐標(biāo),(xn,yn,zn)為鄰居無(wú)人機(jī)的坐標(biāo),鄰居無(wú)人機(jī)在x,y,z軸上的定位精度分別為σn(x),σn(y),σn(z),則其在距離上引入噪聲的標(biāo)準(zhǔn)差可表示為

(28)

考慮傳感器自身噪聲以及鄰居無(wú)人機(jī)位置引入的噪聲后,無(wú)人機(jī)間距離觀測(cè)值的方差可表示為

(29)

式中:σUWB表示傳感器自身的測(cè)距噪聲標(biāo)準(zhǔn)差。

結(jié)合多無(wú)人機(jī)協(xié)同定位因子圖模型以及消息傳遞算法式可得到無(wú)人機(jī)節(jié)點(diǎn)i的后驗(yàn)概率密度函數(shù)為

(30)

由于假設(shè)所有觀測(cè)噪聲服從零均值高斯分布,所以因子圖上所有的消息均服從高斯分布。

在消息傳遞過(guò)程中也僅需傳遞均值和方差這兩個(gè)參數(shù)信息即可。圖5給出了一個(gè)簡(jiǎn)化多無(wú)人機(jī)協(xié)同定位因子圖模型。

對(duì)于從k-1時(shí)刻到k時(shí)刻的轉(zhuǎn)移因子節(jié)點(diǎn)傳遞給無(wú)人機(jī)變量節(jié)點(diǎn)的消息μf→Xi,其概率密度函數(shù)的均值和方差由狀態(tài)一步預(yù)測(cè)公式(10)以及均方誤差一步預(yù)測(cè)公式(11)給出。

對(duì)于衛(wèi)星因子節(jié)點(diǎn)傳遞給無(wú)人機(jī)變量節(jié)點(diǎn)的消息μs→Xi,其概率密度函數(shù)為

(31)

式中:A為歸一化因子,Xi表示無(wú)人機(jī)i的坐標(biāo)向量為未知量,Xs表示衛(wèi)星坐標(biāo)向量是已知的,‖·‖表示歐氏距離,bi是接收機(jī)鐘差也是未知的。均值為偽距觀測(cè)值ρi,s,方差為σρ,其計(jì)算方法如式(27)所示。

對(duì)于距離因子節(jié)點(diǎn)傳遞給鄰接無(wú)人機(jī)變量節(jié)點(diǎn)的消息μr→Xi,其概率密度函數(shù)為

(32)

式中:A為歸一化因子;Xi表示無(wú)人機(jī)i的坐標(biāo)向量為未知量;Xn(i)表示鄰居無(wú)人機(jī)n(i)的坐標(biāo)向量,其值為鄰居無(wú)人機(jī)傳遞給該因子節(jié)點(diǎn)的消息μXn(i)→r的均值;ri,n(i)為距離觀測(cè)值,是該密度函數(shù)的均值;σr為方差,其計(jì)算方法如式(29)所示。

對(duì)于無(wú)人機(jī)變量節(jié)點(diǎn)傳遞給鄰接距離因子節(jié)點(diǎn)的消息μXn(i)→r,其概率密度函數(shù)的均值和方差可通過(guò)參數(shù)估計(jì)方法獲得?？紤]到消息服從高斯分布,觀測(cè)方程為非線性方程,且消息傳遞算法是一個(gè)迭代近似的過(guò)程,因此本文采用迭代濾波估計(jì)方法對(duì)此類消息的均值和方差進(jìn)行估計(jì)。迭代濾波估計(jì)方法的流程如1.2節(jié)所示。

(33)

式中:n(n(i))表示n(i)的鄰居節(jié)點(diǎn),/i表示不包含i節(jié)點(diǎn)。

對(duì)于無(wú)人機(jī)變量節(jié)點(diǎn)自身的后驗(yàn)概率密度函數(shù)如式(30)所示,可以看出它也是多個(gè)鄰接因子節(jié)點(diǎn)消息的乘積形式,因此該概率密度函數(shù)的均值和方差也可采用迭代濾波估計(jì)方法來(lái)獲取。

基于上述說(shuō)明,可以給出基于消息傳遞的多無(wú)人機(jī)協(xié)同位置估計(jì)算法流程圖,如圖6所示。

圖6 多無(wú)人機(jī)協(xié)同位置估計(jì)算法流程圖Fig.6 Flow chart of multi-UAV cooperative position estimation algorithm

3 仿真驗(yàn)證

為了驗(yàn)證本文所提出的分布式多無(wú)人機(jī)協(xié)同定位算法的有效性,首先設(shè)計(jì)了一組仿真實(shí)驗(yàn),用于對(duì)比本文算法與傳統(tǒng)單無(wú)人機(jī)獨(dú)立定位算法的定位效果。

在本實(shí)驗(yàn)中,設(shè)置了3個(gè)目標(biāo)節(jié)點(diǎn),其真實(shí)位置坐標(biāo)如表1所示。

表1 目標(biāo)節(jié)點(diǎn)真實(shí)坐標(biāo)Tab.1 Real coordinates of target nodes

同時(shí)還設(shè)置了10個(gè)錨節(jié)點(diǎn),其位置坐標(biāo)如表2所示。

表2 錨的位置坐標(biāo)Tab.2 Anchor position at coordinates

仿真實(shí)驗(yàn)假設(shè):目標(biāo)節(jié)點(diǎn)以及錨節(jié)點(diǎn)均處于靜止?fàn)顟B(tài),目標(biāo)節(jié)點(diǎn)之間均可相互觀測(cè),目標(biāo)節(jié)點(diǎn)與所有錨節(jié)點(diǎn)之間均可觀測(cè),錨節(jié)點(diǎn)距離觀測(cè)噪聲方差因子為5 m2,目標(biāo)節(jié)點(diǎn)距離觀測(cè)噪聲方差因子為0.01 m2。

采用狀態(tài)的均方根誤差值對(duì)協(xié)同定位以及獨(dú)立定位的結(jié)果進(jìn)行比較,仿真結(jié)果的均方根誤差(RMSE)值如表3所示。

表3 仿真定位結(jié)果的RMSE值Tab.3 RMSE values of simulation positioning results

仿真結(jié)果的絕對(duì)誤差如圖7—圖9所示。

圖7 節(jié)點(diǎn)A不同定位方式的絕對(duì)誤差Fig.7 Absolute error of different positioning methods of node A

圖8 無(wú)人機(jī)B不同定位方式的絕對(duì)誤差Fig.8 Absolute error of different positioning methods of UAV B

圖9 無(wú)人機(jī)C不同定位方式的絕對(duì)誤差Fig.9 Absolute error of different positioning methods of UAV C

圖中,實(shí)線代表的是協(xié)同定位的結(jié)果,點(diǎn)劃線代表的是獨(dú)立定位的結(jié)果,不難看出,本文提出的分布式多節(jié)點(diǎn)機(jī)協(xié)同定位方法的定位結(jié)果要優(yōu)于傳統(tǒng)單節(jié)點(diǎn)獨(dú)立定位的結(jié)果。另外從表3可以看出,單節(jié)點(diǎn)獨(dú)立定位結(jié)果的均方根誤差是多節(jié)點(diǎn)協(xié)同定位結(jié)果的均方根誤差1.3倍左右。特別地,當(dāng)無(wú)人機(jī)之間協(xié)同定位解算時(shí)發(fā)生交互通信中斷或者不連續(xù)時(shí),各個(gè)無(wú)人機(jī)的定位結(jié)果等同于各自導(dǎo)航系統(tǒng)獨(dú)立定位的結(jié)果。因此相較于獨(dú)立定位或者發(fā)生通信中斷時(shí),本文提出的協(xié)同定位方法的定位精度有著30%左右的提升。

目標(biāo)節(jié)點(diǎn)間相對(duì)距離保持的誤差曲線圖如圖10所示。

圖10 節(jié)點(diǎn)間相對(duì)距離誤差曲線圖Fig.10 Relative distance error curve between nodes

圖中,實(shí)線代表的是協(xié)同定位的結(jié)果,點(diǎn)劃線代表的是獨(dú)立定位的結(jié)果。得益于定位精度的提高,與單節(jié)點(diǎn)定位相比,協(xié)同定位方案下的節(jié)點(diǎn)定位結(jié)果更符合節(jié)點(diǎn)間相對(duì)距離的約束條件。

同時(shí)將本文所提的分布式多節(jié)點(diǎn)協(xié)同定位方法與集中式EKF多節(jié)點(diǎn)協(xié)同定位方法進(jìn)行比較,以RMSE值作為考察對(duì)象。表4給出了對(duì)比仿真定位結(jié)果的RMSE值。

表4 分布式與集中式對(duì)比仿真實(shí)驗(yàn)結(jié)果的RMSE值Tab.4 RMSE values of centralized and distributed simulation results

圖11—圖13給出了集中式算法與分布式算法的對(duì)比結(jié)果。

圖11 節(jié)點(diǎn)A集中式與分布式定位的絕對(duì)誤差Fig.11 Comparison of RMSE for centralized and distributed positioning of node A

圖12 節(jié)點(diǎn)B集中式與分布式定位的絕對(duì)誤差Fig.12 Comparison of RMSE for centralized and distributed positioning of node B

圖13 節(jié)點(diǎn)C集中式與分布式定位的絕對(duì)誤差Fig.13 Comparison of RMSE for centralized and distributed positioning of node C

圖中點(diǎn)劃線代表集中式EKF算法的定位結(jié)果,實(shí)線代表本文所提分布式算法的定位結(jié)果。相比于分布式算法的定位結(jié)果,集中式算法的定位結(jié)果曲線波動(dòng)較小,定位精度要優(yōu)于分布式算法,但從總體來(lái)說(shuō)差異并不巨大。仿真結(jié)果表明,集中式算法的定位精度優(yōu)于分布式算法的定位精度。這是由于集中式算法是根據(jù)全局信息進(jìn)行最優(yōu)估計(jì),而分布式算法則是根據(jù)局部信息進(jìn)行最優(yōu)估計(jì),并且通過(guò)消息傳遞機(jī)制實(shí)現(xiàn)對(duì)全局最優(yōu)估計(jì)的近似,因此不可避免地會(huì)存在一定程度的精度丟失,然而考慮到集中式算法在實(shí)際應(yīng)用中的局限性以及分布算法的優(yōu)越性,該10%的定位精度差距是可以接受的。

4 結(jié)論

本文提出了多無(wú)人機(jī)協(xié)同定位的因子圖模型,并根據(jù)消息傳遞算法進(jìn)行無(wú)人機(jī)間信息的傳遞,最后使用迭代濾波算法對(duì)無(wú)人機(jī)的位置進(jìn)行估計(jì),實(shí)現(xiàn)了基于無(wú)人機(jī)間距離觀測(cè)以及衛(wèi)星距離觀測(cè)的多無(wú)人機(jī)分布式協(xié)同定位。仿真結(jié)果表明該分布式協(xié)同定位算法有著接近于集中式協(xié)同定位算法的定位精度,并且優(yōu)于單點(diǎn)獨(dú)立定位算法的定位精度。后續(xù)將在搭載了高性能嵌入式計(jì)算機(jī)Jetson xavier NX,GPS,UWB,ZigBee,Pixhawk等主要電子設(shè)備的多架四旋翼平臺(tái)上進(jìn)行算法驗(yàn)證。