潘禮規(guī)，尹佳琪，徐春光

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

1 引言

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

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

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

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

2 相對定位模型

2.1 方案設(shè)計

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

圖1 天線安裝示意圖

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

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

2.2 數(shù)學模型

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

(1)

(2)

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

(3)

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

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

(4)

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

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

(5)

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

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

rij=θijλi/2π

(6)

記定位模型的非線性向量函數(shù)為：

Z=h(X)

(7)

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

(8)

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

3 相對定位解算

3.1 基本迭代求解

(9)

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

3.1.11MA-1FA模型

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

(10)

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

(11)

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

(12)

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

3.1.2NMA-1FA模型

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

(13)

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

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

(14)

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

3.2 導航濾波估計

(15)

3.2.1狀態(tài)更新

任意無人機3個天線狀態(tài)方程表達形式為：

(16)

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

3.2.2觀測更新

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

3.2.3參數(shù)更新

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

(17)

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

(18)

4 幾何構(gòu)型評估

4.1 權(quán)系數(shù)陣

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

(19)

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

(20)

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

(21)

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

(22)

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

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

(23)

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

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

(24)

4.2 精度因子

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

(25)

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

(26)

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

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

(27)

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

5 仿真實驗

5.1 基本數(shù)據(jù)

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

表1 各觀測時段內(nèi)速度參數(shù)Table 1 Velocity parameters during each observation period

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

5.2 初始條件

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

5.3 結(jié)果與分析

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

6 結(jié)論

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

1) 隨機擇機策略實現(xiàn)了觀測時段內(nèi)的多構(gòu)型融合，解決了單一構(gòu)型限制，適用于大規(guī)模集群無人機飛行環(huán)境；

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

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