基于GNSS/UWB的多無人機協同定位技術

鄢家鑫,賀曉華,周澤波,林國泉

(1. 電子科技大學航空航天學院,四川 成都 611731;2. 中國空空導彈研究院,河南 洛陽 471009; 3.航空制導武器航空科技重點實驗室,河南 洛陽 471009)

0 引言

與單無人機作業相比,多無人機協同作業能完成許多更加復雜的任務,因而具有更高的應用價值[1]。在多無人機協同作業的應用中,對于無人機的導航精度有著較高的要求,而位置信息作為無人機實現導航控制所必需的關鍵信息,其精度對于多無人機進行協同作業而言就顯得十分重要。在多無人機系統中,除了無人機自身的傳感器信息外,還可以利用相鄰無人機的信息輔助自身進行導航[3],這種導航方式稱為協同導航。

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

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

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

1.1 因子圖與消息傳遞算法

對于處理多變量的復雜全局函數問題,通常可以利用因式分解的方式將其分解為若干個更簡單的局部函數的乘積形式。假設一個多變量全局函數g(x1,…,xn)可以被因式分解為幾個局部函數的乘積形式:

(1)

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

在因子圖中,每個變量xj都有唯一的變量節點與之對應,每個局部函數fj都有唯一的因子節點與之對應,并且使用邊連接與之相關聯的變量節點。以一個簡單的因子圖為例,在圖中,空心圓“”表示變量節點,實心方塊“”表示因子節點。令g(x1,x2,x3,x4,x5)表示一個含5變量的函數,并假設g可以表示為5個因子的乘積,其表達式如下:

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

(2)

式(2)對應的因子圖如圖1所示。

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

可以通過因式分解和分布律來計算每一個邊緣密度函數。以g1(x1)為例:

(3)

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

但是在計算所有的邊緣函數時會存在大量的子問題被重復計算,計算效率不高。通過和積算法用消息來表示各種計算中的子問題,將邊緣密度函數的計算過程看作是消息傳遞的過程,建立了一種直觀高效的計算邊緣密度的方法。

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

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

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

變量到局部因子的消息:

(4)

局部因子到變量的消息:

(5)

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

(6)

1.2 迭代濾波算法

對于非線性問題,通常情況下EKF只保留到一階項。因此不可避免地會損失精度,若使用當前時刻濾波估計的結果作為新的展開點,再次對系統進行近似展開,并再次進行濾波估計,則有助于提高估計的精度。

首先,按EKF方法進行預濾波,此時無需對均方誤差矩陣進行測量更新

(7)

(8)

使用平滑值對一步狀態預測進行修正:

(9)

(10)

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

(11)

若迭代前后的差值小于閾值則認為迭代收斂,并輸出當前結果。其流程如圖3所示。

2 基于消息傳遞的多無人機協同位置估計

2.1 多無人機協同定位模型

(12)

無人機i在k時刻對GPS衛星的偽距方程可表示為

(13)

無人機i在k時刻對BDS衛星的偽距方程可表示為

(14)

除此之外,無人機還與鄰居無人機之間具備相對距離觀測的能力,它們之間的距離觀測方程可表示為

(15)

于是,無人機群體在k時刻的定位問題可以描述為在給定所有可用觀測值條件下的后驗概率:

(16)

假設無人機之間的狀態相互獨立,且觀測值之間也相互獨立,則可以根據齊次馬爾可夫假設以及觀測獨立假設得到

(17)

(18)

對式(16)進行因式分解。為了簡化書寫,以下 和ρM,B統一用ρM表示,其分解過程如下:

(19)

式中:

(20)

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

(21)

(22)

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

(23)

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

2.2 基于消息傳遞的多無人機協同位置估計

式(22)給出了全局概率密度函數的因子分解形式,根據無人機之間的狀態相互獨立與觀測值之間相互獨立的假設,式(22)可進一步寫成

(24)

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

(25)

則有

(26)

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

圖4 多無人機協同定位因子圖模型Fig.4 Multi-UAV cooperative positioning factor map model

圖中,fi表示無人機i從k-1時刻到k時刻的轉移因子,ρi,sB表示k時刻無人機i可觀測的BDS衛星集合的因子節點,ρi,sG表示k時刻無人機i可觀測GPS衛星集合的因子節點,ri,j表示k時刻無人機i與鄰居j之間的距離觀測因子。

因為測量噪聲會對觀測值的可信度產生影響,所以有必要對其隨機模型進行分析。通常可以假設測量噪聲的隨機模型服從零均值的高斯分布。衛星高度角越大,衛星信號上的噪聲越小,因此可以采用高度角來描述偽距觀測噪聲的隨機模型,采用基于高度角的正弦函數形式的隨機模型來描述:

(27)

式中:σρ為偽距測量噪聲的標準差;a,b均為常數,通常分別設置為0.003和0.000 2;e是偽距與載波誤差比,通常可根據經驗設置。

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

(28)

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

(29)

式中:σUWB表示傳感器自身的測距噪聲標準差。

結合多無人機協同定位因子圖模型以及消息傳遞算法式可得到無人機節點i的后驗概率密度函數為

(30)

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

在消息傳遞過程中也僅需傳遞均值和方差這兩個參數信息即可。圖5給出了一個簡化多無人機協同定位因子圖模型。

對于從k-1時刻到k時刻的轉移因子節點傳遞給無人機變量節點的消息μf→Xi,其概率密度函數的均值和方差由狀態一步預測公式(10)以及均方誤差一步預測公式(11)給出。

對于衛星因子節點傳遞給無人機變量節點的消息μs→Xi,其概率密度函數為

(31)

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

對于距離因子節點傳遞給鄰接無人機變量節點的消息μr→Xi,其概率密度函數為

(32)

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

對于無人機變量節點傳遞給鄰接距離因子節點的消息μXn(i)→r,其概率密度函數的均值和方差可通過參數估計方法獲得。考慮到消息服從高斯分布,觀測方程為非線性方程,且消息傳遞算法是一個迭代近似的過程,因此本文采用迭代濾波估計方法對此類消息的均值和方差進行估計。迭代濾波估計方法的流程如1.2節所示。

(33)

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

對于無人機變量節點自身的后驗概率密度函數如式(30)所示,可以看出它也是多個鄰接因子節點消息的乘積形式,因此該概率密度函數的均值和方差也可采用迭代濾波估計方法來獲取。

基于上述說明,可以給出基于消息傳遞的多無人機協同位置估計算法流程圖,如圖6所示。

圖6 多無人機協同位置估計算法流程圖Fig.6 Flow chart of multi-UAV cooperative position estimation algorithm

3 仿真驗證

為了驗證本文所提出的分布式多無人機協同定位算法的有效性,首先設計了一組仿真實驗,用于對比本文算法與傳統單無人機獨立定位算法的定位效果。

在本實驗中,設置了3個目標節點,其真實位置坐標如表1所示。

表1 目標節點真實坐標Tab.1 Real coordinates of target nodes

同時還設置了10個錨節點,其位置坐標如表2所示。

表2 錨的位置坐標Tab.2 Anchor position at coordinates

仿真實驗假設:目標節點以及錨節點均處于靜止狀態,目標節點之間均可相互觀測,目標節點與所有錨節點之間均可觀測,錨節點距離觀測噪聲方差因子為5 m2,目標節點距離觀測噪聲方差因子為0.01 m2。

采用狀態的均方根誤差值對協同定位以及獨立定位的結果進行比較,仿真結果的均方根誤差(RMSE)值如表3所示。

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

仿真結果的絕對誤差如圖7—圖9所示。

圖7 節點A不同定位方式的絕對誤差Fig.7 Absolute error of different positioning methods of node A

圖8 無人機B不同定位方式的絕對誤差Fig.8 Absolute error of different positioning methods of UAV B

圖9 無人機C不同定位方式的絕對誤差Fig.9 Absolute error of different positioning methods of UAV C

圖中,實線代表的是協同定位的結果,點劃線代表的是獨立定位的結果,不難看出,本文提出的分布式多節點機協同定位方法的定位結果要優于傳統單節點獨立定位的結果。另外從表3可以看出,單節點獨立定位結果的均方根誤差是多節點協同定位結果的均方根誤差1.3倍左右。特別地,當無人機之間協同定位解算時發生交互通信中斷或者不連續時,各個無人機的定位結果等同于各自導航系統獨立定位的結果。因此相較于獨立定位或者發生通信中斷時,本文提出的協同定位方法的定位精度有著30%左右的提升。

目標節點間相對距離保持的誤差曲線圖如圖10所示。

圖10 節點間相對距離誤差曲線圖Fig.10 Relative distance error curve between nodes

圖中,實線代表的是協同定位的結果,點劃線代表的是獨立定位的結果。得益于定位精度的提高,與單節點定位相比,協同定位方案下的節點定位結果更符合節點間相對距離的約束條件。

同時將本文所提的分布式多節點協同定位方法與集中式EKF多節點協同定位方法進行比較,以RMSE值作為考察對象。表4給出了對比仿真定位結果的RMSE值。

表4 分布式與集中式對比仿真實驗結果的RMSE值Tab.4 RMSE values of centralized and distributed simulation results

圖11—圖13給出了集中式算法與分布式算法的對比結果。

圖11 節點A集中式與分布式定位的絕對誤差Fig.11 Comparison of RMSE for centralized and distributed positioning of node A

圖12 節點B集中式與分布式定位的絕對誤差Fig.12 Comparison of RMSE for centralized and distributed positioning of node B

圖13 節點C集中式與分布式定位的絕對誤差Fig.13 Comparison of RMSE for centralized and distributed positioning of node C

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

4 結論

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