陳明星，熊 智,2，劉建業(yè),2，王 融,2，熊 駿

(1.南京航空航天大學自動化學院導航研究中心，南京 211106；2.先進飛行器導航、控制與健康管理工業(yè)和信息化部重點實驗室，南京，211106)

近年來，無人機集群技術(shù)得到了行業(yè)內(nèi)的廣泛關(guān)注，與單個無人機相比，無人機集群通過無人機之間的信息交互、相互協(xié)作能夠完成更加復雜多樣的任務，擁有功能分布化、體系生存率高、效率高等優(yōu)勢，具備極大的潛在應用價值[1-3]。

在無人機集群飛行過程中，高精度的位置信息是高效可靠地執(zhí)行各項任務的關(guān)鍵所在。對于密集集群無人機系統(tǒng)來說，使用的無人機一般為旋翼飛行器，受到成本和載荷的限制，為所有無人機配備高精度導航設備是非常困難的[4]。因此，可通過無人機之間信息交互進行協(xié)同導航，從而提升無人機群的定位精度[5-8]。

國內(nèi)外學者對無人載體協(xié)同導航進行了針對性的研究。文獻[9]提出一種融合慣性、磁力計、可用衛(wèi)星偽距、合作無人機位置、單目相機信息的協(xié)同導航方法，有效提高了無人機群在GPS 受限情況下的導航性能，但所使用的傳感器數(shù)量較多。文獻[10]提出了一種基于集成超寬帶(UWB)和GNSS 的車載協(xié)同定位方案，能獲得比分米級更好的定位精度。文獻[11]提出了一種混合式協(xié)同定位方法，結(jié)合卡爾曼濾波和置信傳播算法的優(yōu)勢，實現(xiàn)了無人載體網(wǎng)絡的協(xié)同定位。文獻[12]提出了一種基于因子圖和積算法的多AUV協(xié)同導航方法，試驗結(jié)果表明其定位精度優(yōu)于EKF 和UKF 算法，但其是一種集中式的處理方法。

本文提出了一種基于圖優(yōu)化的密集集群無人機分布式協(xié)同導航方法，結(jié)合無人機自身的GNSS 信息和無人機之間的距離信息建立了協(xié)同導航因子圖，并通過消息迭代實現(xiàn)了因子圖的分布式處理，最后通過仿真驗證了所提出方法的有效性。

1 集群無人機協(xié)同導航方案

在密集集群的情況下，為了防止無人機之間發(fā)生碰撞，無人機的位置精度顯得尤為重要。Real-time kinematic (RTK) 載波相位差分技術(shù)雖然能達到厘米級的定位精度，但為集群中所有無人機全部配備RTK設備的成本太高。本方案中，為了獲得更高的定位精度，為少量無人機配備高精度RTK 設備，其他無人機配備普通的衛(wèi)星導航設備，在飛行過程中，無人機可通過數(shù)據(jù)鏈與相鄰無人機進行信息的交互，進而實現(xiàn)協(xié)同信息的融合與定位結(jié)果的優(yōu)化。協(xié)同導航方案如圖1所示。

圖1 無人機群協(xié)同導航方案Fig.1 Cooperative navigation scheme for UAV swarm

2 基于圖優(yōu)化的協(xié)同導航方法

2.1 系統(tǒng)模型與問題描述

假設所有無人機可通過衛(wèi)星導航設備接收到自身的位置信息，同時可以跟相鄰的無人機進行信息交互。x表示無人機的位置狀態(tài)，可定義集群無人機的位置變量為：

其中，Z為無人機群接收到的所有量測值，包括GNSS信息和無線測距信息。由此可得位置變量X的最大后驗估計為：

以圖2所示的6 架無人機為例，對各無人機的空間相關(guān)性進行建模，可以得到如圖3所示的因子圖模型。因子圖是一種雙邊圖，其包含兩種類型的節(jié)點：因子節(jié)點fi,j和變量節(jié)點xi。

圖2 無人機群協(xié)同示意圖Fig.2 Diagram of cooperative navigation for UAV swarm

圖3 無人機群協(xié)同因子圖Fig.3 Cooperative factor graph for UAV swarm

根據(jù)文獻[13]和文獻[14]，可以得到：

式中，Xi,j表示變量集合X中與因子fk相關(guān)的變量，k為因子的編號。對于高斯噪聲分布，假設每個因子fk都具備如下形式[13]：

其中，hk(?)為測量函數(shù)，zk為真實的量測值，∑k為量測誤差的協(xié)方差矩陣，為馬氏距離。只需要將式(5)取對數(shù)，就可以將求最大后驗估計問題轉(zhuǎn)化為最小化非線性最小二乘的和的問題：

2.2 因子節(jié)點模型描述

本文中的傳感器主要包括GNSS 設備和無線測距設備，本節(jié)將推導這兩種傳感器測量模型的因子公式。

2.2.1 GNSS 因子

對于GNSS 的位置量測，一般可建模為：

式中，p為載體的真實位置，n GNSS為GNSS 測量噪聲。因此，GNSS 因子可表示為：

2.2.2 測距因子

無人機i和無人機j的無線測距值可建模為如下形式：

其中，di,j為i和j之間的真實距離，nwireless為測距噪聲。而觀測函數(shù)h(Xi,j)為：

綜上，測距因子可以表示為：

2.3 基于消息迭代策略的協(xié)同導航分布式處理方法

2.3.1 集中優(yōu)化

通過泰勒展開將最小二乘目標函數(shù)線性化：

為了將不同的代價函數(shù)合并，需要對上式中的雅可比矩陣和預測誤差進行“白化”，即將馬氏范數(shù)轉(zhuǎn)換為2-范數(shù)，從而消去觀測噪聲協(xié)方差矩陣∑k：

結(jié)合無人機群所有的GNSS 位置觀測和無線測距值進行集中優(yōu)化，得到如下標準最小二乘問題：

將式(15)對Δ 求偏微分并使其等于零，得到：

式(16)可通過高斯牛頓法進行求解，具體過程可參考文獻[15]。

2.3.2 分布式優(yōu)化

對于密集集群無人機來說，集中優(yōu)化時雅可比矩陣A 的規(guī)模與無人機數(shù)量相關(guān)。假設無人機數(shù)量為n，測距信息數(shù)量為m，則A 的維數(shù)將達到(3n+m)×3n，因此，集中優(yōu)化的計算量非常龐大，且非常依賴中心節(jié)點，中心節(jié)點的故障將會導致整個系統(tǒng)癱瘓，魯棒性不強。

為了解決集中優(yōu)化計算資源不足、過度依賴中心節(jié)點的問題，可以將2.3.1 節(jié)所述的因子圖集中優(yōu)化問題分散到所有無人機上進行分布式計算。每架無人機僅處理自身的GNSS 信息和相鄰無人機之間的協(xié)同信息，并通過消息迭代的方式實現(xiàn)無人機群的位置優(yōu)化，消息迭代的內(nèi)容就是每架無人機的位置估計值和位置協(xié)方差。以圖2中的無人機1 為例，在每次優(yōu)化的初始時刻，可通過機載GNSS 設備獲取自身位置信息，并利用無線網(wǎng)絡播發(fā)位置信息和GNSS 位置量測協(xié)方差信息；當接收到相鄰無人機的位置、位置協(xié)方差和距離量測之后，結(jié)合自身位置x1和相鄰無人機位置x2、x3、x4可以建立局部狀態(tài)、局部雅可比矩陣A1以及局部誤差向量b1，從而得到如式(17)所示方程，并由此構(gòu)建局部因子圖。

同理可得如下方程：

通過高斯牛頓法可對x1的位置進行初步更新優(yōu)化，對A1進行QR 分解A1=QR，優(yōu)化后的X1的誤差協(xié)方差可由式(19)求得：

從上述誤差協(xié)方差矩陣中取出相應的行和列即可獲得x1的協(xié)方差。將更新后精度得到提高的位置信息及位置協(xié)方差播發(fā)出去，同時接收其他無人機播發(fā)的更新后的消息，結(jié)合自身和接收到的信息構(gòu)建新的因子圖，這就是消息迭代過程。重復因子圖優(yōu)化和消息迭代過程，直至滿足迭代退出條件為止?？稍O置閾值來檢測前后兩次迭代的位置之差，以此判斷是否可退出迭代。算法流程如表1所示。

表1 基于消息迭代的協(xié)同導航分布式處理方法流程Tab.1 Process of distributed processing method for cooperative navigation based on message iteration

3 仿真分析

3.1 仿真條件設置

利用MATLAB 仿真無人機群的運動，驗證算法的可行性。仿真條件如下：在200 m×100 m×200 m的范圍內(nèi)遍布18 架無人機，這些無人機均配備衛(wèi)星導航接收機、超寬帶測距模塊(UWB)與通信設備，其中3 架無人機配備高精度的RTK 設備，量測噪聲均考慮為高斯噪聲，UWB 考慮晶振誤差和信號處理時間的存在設置了晶振誤差比例系數(shù)[4][5]，所有無人機可與自身100 m 范圍內(nèi)的相鄰無人機測距和通信。無人機群的初始位置如圖4所示，其中一架無人機的運動軌跡如圖5所示，為方便顯示無人機之間的相對位置和距離，圖4以(118 °，32 °，0 m)為坐標原點，三維尺度全部轉(zhuǎn)化成m。表2列出了無人機所搭載設備的仿真參數(shù)。

圖4 無人機群初始位置Fig.4 Initial position of UAV swarm

圖5 無人機1 運動軌跡Fig.5 Trajectory of UAV 1

表2 傳感器配置與仿真參數(shù)Tab.2 Sensor configuration and simulation parameters

3.2 仿真結(jié)果與分析

基于上述仿真條件，對配備低精度GNSS 設備的15架無人機的定位精度進行分析。將每架無人機100m范圍內(nèi)的其他無人機稱為可交互無人機，15 架無人機的可交互無人機個數(shù)及其中的高精度無人機個數(shù)如表3所示。

為了綜合分析可交互無人機個數(shù)對優(yōu)化精度的影響，選取無人機1、無人機7、無人機12 和無人機13來進行對比。對下列三種情況進行仿真，仿真結(jié)果如圖6所示。

· 不進行優(yōu)化而僅利用GNSS 定位；

· 僅進行一次優(yōu)化而不進行消息迭代；

· 進行消息迭代優(yōu)化。

藍線表示無優(yōu)化而僅GNSS 定位下的仿真結(jié)果，綠線表示僅進行一次優(yōu)化而不做消息迭代下的仿真結(jié)果，紅線表示迭代優(yōu)化的仿真結(jié)果。

表3 可交互無人機個數(shù)Tab.3 Number of interactive UAVs

圖6 定位誤差對比曲線Fig.6 Comparison on position error

由圖6可以看出，本文所提出的方法的定位精度相比純GNSS 定位得到明顯提升，為了定量地分析三種情況下的定位誤差，對無人機定位均方根誤差(RMSE)進行了統(tǒng)計，結(jié)果如表4所示。表4中的位置估計誤差的計算公式如式(20)所示：

從表4可以看出，通過與相鄰無人機進行信息交互和測距，可以提升無人機群的定位水平。即使在僅進行一次優(yōu)化而不進行消息迭代的情況下，無人機的定位精度也得到了提高。優(yōu)化后的定位精度與可交互無人機的數(shù)量存在正相關(guān)關(guān)系，對比無人機7 和無人機13 可以看出，在僅進行一次優(yōu)化時，由于無人機13 存在兩架高精度的可交互無人機，因此其優(yōu)化效果要優(yōu)于無人機7，而隨著迭代的進行，可交互無人機的定位精度不斷提高，無人機7 的定位精度逐漸接近無人機13 的定位精度。迭代優(yōu)化后定位精度相比于未優(yōu)化時無人機1、無人機7、無人機12 和無人機13分別提升了3.37、4.53、7.38、6.35 倍。

表4 定位誤差統(tǒng)計結(jié)果Tab.4 Statistics of position error

4 結(jié) 論

本文以無人機密集集群時的協(xié)同導航為背景，研究了基于圖優(yōu)化的分布式協(xié)同導航方法，該方法將無人機的位置狀態(tài)抽象成變量節(jié)點，將GNSS 傳感器的位置量測和測距傳感器的距離量測抽象成因子節(jié)點，建立了無人機群協(xié)同導航的因子圖，并通過消息迭代的方式實現(xiàn)了因子圖的分布式優(yōu)化。仿真結(jié)果表明，基于圖優(yōu)化的密集集群無人機分布式導航方法能夠?qū)崿F(xiàn)定位解算過程的分布式處理，且可以明顯地提升無人機的定位精度，減少編隊中高精度導航設備的配置數(shù)量，適用于大規(guī)模無人機密集編隊應用場景。本文假設量測噪聲滿足高斯分布，下一步將對非高斯量測噪聲和相對測距存在非視距誤差條件下的無人機群分布式導航方法進行深入研究。