無人機集群多構型融合相對定位性能分析

潘禮規，尹佳琪，徐春光

(1.中山大學 航空航天學院, 廣東 深圳 518107； 2.中國航天科工信息技術研究院, 北京 100144)

1 引言

近年來，無人機憑借體積小、成本低以及高度靈活性等優點受到廣泛關注[1]。無人機集群概念的提出，進一步促進了無人機技術的發展，借助個體間的交互，實現群體行為的全局一致性協同，可實現單架無人機無法執行的作戰任務，譬如集群中某架無人機發生故障時，可由集群編隊中的其他無人機執行故障無人機的功能，從而提高整個系統的抗干擾能力和效能[2-3]。隨著無人機集群技術的快速發展，無人機集群在農業植保、森林防火、智慧城市、物流運輸、戰場偵察與作戰、目標搜索與追蹤等方面將發揮巨大的作用[4-10]。

在復雜環境中，大規模集群無人機近距離飛行時，高精度、高可靠的導航與定位信息是實現高動態飛行的重要保障[11]。目前，諸多學者對相關技術進行了大量研究。文獻[12]和文獻[13]針對傳統單主從式協同導航存在的精度發散問題，提出了分層式協同導航方案。其中，文獻[12]以相對導航和慣性導航信息作為量測量，利用卡爾曼濾波算法估計校正慣性導航誤差，使精度滿足較長時間導航要求。文獻[13]則通過增加長機數量，提高集群內部信息利用率，基于聯邦濾波算法改善了導航性能。在導航定位過程中，無人機間的相對定位，特別是無依托的自主相對定位，對提高無人機集群的定位精度和抗干擾能力至關重要。

集群內部不同目標間的相對定位不僅應用于無人機集群，在其他集群場景中也大量存在，譬如衛星星座間的相對定位等，研究人員對相關方法開展了大量研究[14-15]，其研究結論對改進無人機集群間相對定位算法具有借鑒意義。文獻[14]考慮了故障星對星座幾何構型的影響，針對剔除故障星觀測數據進行定位解算時存在GDOP值突變的問題，采用加權最小二乘法提高了數據利用率，并改善了接收機自主完好性監測性能。文獻[15]仿真研究了LEO星座對GNSS精密單點定位性能的增強作用，相比于單GPS系統，LEO星座的參與，可降低GDOP值、減小PPP濾波矩陣條件數以及加快PPP收斂時間，并且顯著縮短了模糊度首次固定所需時間。文獻[16]利用測距與站址坐標估計目標位置，3架無人機繞圓編隊飛行，在編隊成正三角形時，可獲得最小GDOP值，提高了定位精度。上述文獻通過改善觀測幾何減小精度因子值，進一步提高了定位精度。在大規模集群系統中，固定構型實現的數據鏈較為單一，多構型組合能夠使集群具有更為靈活的應變能力，有望提高整個集群系統的抗摧毀能力。

針對大規模集群無人機相對定位問題，基于隨機擇機策略提出一種迭代估值定位解算方法，引入衛星星座間相對定位精度評估常用的精度因子作為評估準則，分析了觀測時段內多構型融合對定位性能的影響，利用擴展卡爾曼濾波算法對無人機狀態信息進行估計。

2 相對定位模型

2.1 方案設計

采用課題組研究團隊提出的一種無人機集群相對定位技術，該技術在無人機機背上安裝3個無線電發射/接收裝置，每個天線均可實時檢測和發射不同頻率電波，通過機間觀測，構建觀測模型并進行導航定位解算，則可獲得無人機狀態信息。無人機機身上的天線安裝示意圖如圖1所示。

圖1 天線安裝示意圖

在無人機集群飛行環境中，隨機選取一定數量無人機對某一架無人機進行定位解算，從無人機在主機圍成區域內飛行，研究觀測時段內多構型融合對飛行器定位性能的影響。圖2顯示了8架主飛行器組成立方體隊形示意圖。

圖2 8架主機組成立方體隊形示意圖

2.2 數學模型

每架無人機配備的3個無線電信號收/發裝置，在其自身機體坐標系的位置分別為(a,0,0)、(0,0,b)和(0,0,-b)。根據相對位姿參考系，選取某一架主機機體坐標系作為其他無人機的導航坐標系，則從機3個天線位置信息可通過式(1)進行計算，即：

(1)

(2)

將式(2)代入式(1)，整理得：

(3)

式(3)中：(x,y,z)表示從機位置坐標；(xj,yj,zj)表示從機天線位置。

假設一主機機體坐標系作為其他無人機的導航坐標系，則該主機3個天線位置坐標為：

(4)

式(4)中，Xi0表示領航主機天線坐標，且i=1,2,3。

結合式(3)與式(4)，可得2架無人機機間距離表達式為：

(5)

式(5)中：i表示主機第i個天線；j表示從機第j個天線。

以載波相位作為量測值，其相位角與波長、距離有如下關系，即：

rij=θijλi/2π

(6)

記定位模型的非線性向量函數為：

Z=h(X)

(7)

式(7)中：Z表示相位轉換至距離的觀測向量；h(X)表示非線性向量函數。

(8)

式(8)中，v為觀測模型噪聲，滿足均值為零的高斯白噪聲序列。

3 相對定位解算

3.1 基本迭代求解

(9)

式(9)中：ΔX表示狀態向量修正量；ΔZ表示觀測向量估值偏差；RZ表示量測誤差方差陣；PX表示定位誤差方差陣；H表示非線性向量函數h(X)對狀態向量X的Jacobian矩陣。

3.1.11MA-1FA模型

結合式(8)所示觀測方程，可知觀測量Z的估值偏差ΔZ具體形式為：

(10)

式(10)中，矩陣H表示非線性向量函數h(X)對狀態向量X的Jacobian矩陣，表達式為：

(11)

式(11)中，矩陣Aj具體如下：

(12)

式(12)中：下標i表示主飛行器第i個天線；下標j表示從飛行器第j個天線。

3.1.2NMA-1FA模型

上述給出了單架主機對一從飛行器的定位估值方法，若是多架無人機對一架無人機進行觀測定位解算，則全局觀測向量Z為多架無人機觀測向量的組合，有：

(13)

式(13)中，Zk表示第k架無人機對從無人機的觀測向量，其中k=1,2,…,N。

相應地，非線性向量函數h(X)對狀態向量X的Jacobian矩陣為：

(14)

式(14)中，Hk表示第k架無人機對從無人機的觀測矩陣，具體如式(11)和式(12)所示。

3.2 導航濾波估計

(15)

3.2.1狀態更新

任意無人機3個天線狀態方程表達形式為：

(16)

式(16)中：uk-1表示輸入矩陣；Bk-1表示控制矩陣，取Bk-1=τI9×9，其中τ=tk-tk-1表示觀測采樣間隔；Φk|k-1表示狀態轉移矩陣，取Φk|k-1=I9×9。

3.2.2觀測更新

多架無人機對單架無人機的觀測向量和觀測矩陣如式(13)、式(14)所示，將狀態更新方程和觀測更新方程代入遞推方程式(15)，可濾波估計出無人機的位置狀態信息。

3.2.3參數更新

利用上述方法可估計出飛行器3個天線的位置信息，再根據天線在機身上固有的安裝位置關系，進一步得到飛行器機體坐標系位置信息，即：

(17)

相應地，結合式(3)，可進一步獲得該飛行器在導航坐標系中的相對姿態估值，有：

(18)

4 幾何構型評估

4.1 權系數陣

式(8)給出了無人機非線性定位方程，僅考慮位置狀態參數時，線性化處理后得；

(19)

式(19)中，Δxj表示天線j的狀態修正量。式(19)的最小二乘解為：

(20)

考慮測量誤差項εr時，式(20)的最小二乘解為：

(21)

將式(20)代入式(21)，則有：

(22)

式(22)表示測量誤差與定位誤差之間的關系。

Rεr=E((εr-E(εr))(εr-E(εr))T)=

(23)

式(23)中，I表示9×9的單位矩陣。

因此，簡化的定位誤差向量協方差陣為：

(24)

4.2 精度因子

在導航學中，精度因子(dilution of precision,DOP)作為評估定位性能的重要指標，指測量誤差放大至定位誤差的倍數，與衛星的幾何分布有關[18]。將其遷移至無人機集群定位系統中，也可作為集群定位性能的評估準則。根據本文狀態向量形式，式(24)左右兩邊對角元素的關系為：

(25)

三維空間定位誤差標準差定義為：

(26)

式(26)中，PDOP表示空間位置精度因子。

相應地，各方向上的精度因子計算如下：

(27)

式(27)中：(σX,σY,σZ)表示各方向上的定位誤差標準差；XDOP、YDOP和ZDOP分別表示X、Y和Z軸方向上的精度因子。

5 仿真實驗

5.1 基本數據

采用數值仿真方法對上述模型進行仿真實驗。為便于分析觀測時段內多種幾何構型對相對定位性能的影響，假設主飛行器處于穩定飛行狀態，從飛行器相對于領航無人機的初始位置為(20,30,20) m，在觀測時段內其速度狀態如表1所示。

表1 各觀測時段內速度參數Table 1 Velocity parameters during each observation period

已知每架無人機天線安裝參數為a=1.0 m，b=1.0 m，且每個天線發射的電磁波波長分別為λ1=5 m，λ2=6 m，λ3=7 m。選取某一主飛行器機體坐標系作為其他無人機的導航坐標系，所有主飛行器均布置在邊長為200 m的立方體頂點處，且姿態保持一致。仿真時間為50 s，每秒隨機選擇3架(RD-3MA)/5架(RD-5MA)主機進行觀測定位，采樣頻率取20 Hz，分析觀測時段內多構型融合對定位性能的影響。

5.2 初始條件

取初始時刻無人機位置狀態誤差為(-1.335 00, -1.621 60, -1.682 19) m，其位置初始協方差陣為 diag(1.873 06,3.147 71,3.703 52)。結合初始時刻飛行器姿態角與天線安裝參數，可得3個天線狀態參數組成的初始時刻狀態向量與協方差陣。考慮相位量測值服從高斯分布，其轉換至距離觀測量噪聲為0.15(3σ) m。利用迭代估值方法和擴展卡爾曼濾波算法，在初始條件下可估計出無人機位姿參數。

5.3 結果與分析

結合初始條件，從飛行器隨機接收3架/5架主機的觀測數據，利用迭代估值方法，可估計出飛行器在各自觀測時段內的精度因子值和位置估值偏差，結果如圖3—圖8所示。

圖3 各個時段主機觀測情況(RD-3MA)

圖3與圖4顯示了各個觀測時段內主機觀測情況。從圖3與圖4可看出，整個觀測時段內，各主機被選中的概率基本一致，任意一架主機觀測數據均可能作為從機的定位數據來源。

圖4 各個時段主機觀測情況(RD-5MA)

圖5 各個觀測時段DOP值(RD-3MA)

圖6 各個觀測時段DOP值(RD-5MA)

圖7 位置估值偏差曲線(RD-3MA)

圖5與圖6顯示了各個觀測時段內精度因子值變化情況。從圖5與圖6可看出，RD-3MA策略下的DOP值存在劇增的情況，說明選取的3MA組成的幾何構型不利于進行定位解算，會導致觀測矩陣性質極度不穩定。而RD-5MA策略下各個觀測時段的DOP值均較小，3個方向的DOP值均小于1，減弱了量測誤差對定位精度的影響，提高了定位性能。圖7與圖8給出了觀測時段內位置估值偏差曲線。從圖7與圖8可看出，RD-3MA策略在DOP值極高的觀測時段，其位置估值偏差也較大，定位誤差達到米級，而RD-5MA策略下的DOP值均較小，相應的定位精度較高，可獲得厘米精度等級。

上述結果說明，隨機選擇5架主機可獲得較為穩定的觀測幾何，其定位精度更高，在8架主機中，隨機選取至少5架主機(RD-NMA,N≥5)進行觀測定位，可估計出精度因子和位置估值偏差，結果如圖9—圖11所示。此外，利用蒙特卡洛進行100次實驗，均方根誤差變化曲線如圖12所示。

圖9 各個時段主機觀測情況(RD-NMA)

圖9給出了各個時段內主機觀測情況。從圖9可看出，在整個觀測時段內，共選取了271機次，比RD-5MA策略多21機次，對全部觀測時段選用主機情況進行統計可知，各主機被選用的概率在10.7%～14.0%。圖10給出了各個觀測時段內精度因子值變化情況。從圖10可看出，各方向上的DOP值均小于1，同樣削弱了觀測誤差對定位精度的影響。此外，相比于RD-5MA選取策略，RD-NMA策略下DOP值小于0.4的次數較多，但總體相差不大，說明RD-NMA策略獲得的幾何構型相對較好，由于主機數量有限，使得效果提升不明顯。針對大規模集群無人機執行飛行任務，RD-NMA策略理論上可顯著提升定位性能，充分發揮集群內部個體的優勢。圖11顯示了位置估值偏差曲線。從圖10與圖11中可看出，在DOP值稍高時段，位置偏差同步增大，與圖8類似，其定位精度可達厘米量級。圖12顯示了位置偏差均方根誤差變化曲線。

圖10 各個觀測時段DOP值(RD-NMA)

圖11 位置估值偏差曲線(RD-NMA)

圖12 位置偏差RMSE值(RD-NMA)

從圖12中可看出，RMSE維持在較低水平，且隨時間推移在較小范圍內穩定波動，無劇烈震蕩趨勢。此外，對觀測時段內的RMSE值進行統計分析，可知3個方向上的RMSE值均為0.017 m，三維RMSE為0.030 m，可以滿足無人機集群相對定位精度要求。

在擴展卡爾曼濾波框架下，選取固定3架主機(UAV1～UAV3,ST-3MA)和隨機3架主機(RD-3MA)進行濾波估計，可獲得位置和姿態估值偏差，結果如圖13—圖16所示。

圖13 位置估值偏差曲線(ST-3MA)

圖14 位置估值偏差曲線(RD-3MA)

圖13與圖14顯示了2種擇機策略的位置估值偏差曲線。從圖13與圖14可看出，考慮了狀態更新與觀測更新的權重分配，2種擇機策略均大大提高了狀態估計精度，達到了毫米精度量級。相比于ST-3MA策略，RD-3MA策略可獲得更高的定位精度，且收斂速度更快，其位置誤差可收斂至±0.002 m。

圖15與圖16顯示了2種擇機策略的姿態估值偏差曲線。從圖13—圖16可看出，隨著位置估計精度的提高，姿態估值效果也顯著提升，RD-3MA策略的姿態角誤差可收斂到±0.2°，可滿足集群無人機相對姿態精度要求。將ST-3MA與RD-3MA2種擇機策略的位置估值誤差進行統計分析，其均方根誤差統計結果如表2所示。

圖15 姿態估值偏差曲線(ST-3MA)

圖16 姿態估值偏差曲線(RD-3MA)

表2 2種擇機策略的均方根誤差

表2給出了2種擇機策略的均方根誤差統計結果。從表2可看出，相比于ST-3MA策略，RD-3MA策略在3個方向上的RMSE值分別減小了39.8%、65.7%和42.9%，三維RMSE則減小了50%，顯著提高了無人機集群定位精度。

6 結論

針對大規模集群無人機相對定位問題，提出了隨機擇機策略進行觀測定位，在傳統精度因子基礎上，提出了適用于本文定位方法的精度因子評估準則，結合濾波方法給出了高精度位姿狀態估計，結果表明：

1) 隨機擇機策略實現了觀測時段內的多構型融合，解決了單一構型限制，適用于大規模集群無人機飛行環境；

2) 隨著觀測主機數量增多，幾何構型愈加穩定，精度因子無劇烈震蕩變化，且均方根誤差維持在較低水平，顯著提高了定位精度；

3) 在擴展卡爾曼濾波框架下，隨機擇機策略可估計出穩定且高精度的位置狀態信息，為相對姿態信息的高精度解算提供了重要依據。