基于因子圖的無人機集群分布式協同導航方法

陳明星，熊 智,2，劉建業,2，王 融,2，熊 駿

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

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

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

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

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

1 集群無人機協同導航方案

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

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

2 基于圖優化的協同導航方法

2.1 系統模型與問題描述

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

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

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

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

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

根據文獻[13]和文獻[14]，可以得到：

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

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

2.2 因子節點模型描述

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

2.2.1 GNSS 因子

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

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

2.2.2 測距因子

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

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

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

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

2.3.1 集中優化

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

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

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

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

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

2.3.2 分布式優化

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

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

同理可得如下方程：

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

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

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

3 仿真分析

3.1 仿真條件設置

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

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

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

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

3.2 仿真結果與分析

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

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

· 不進行優化而僅利用GNSS 定位；

· 僅進行一次優化而不進行消息迭代；

· 進行消息迭代優化。

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

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

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

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

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

表4 定位誤差統計結果Tab.4 Statistics of position error

4 結 論

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