超寬帶測距輔助的無人機近距離相對導航方法

熊 駿，熊 智，于永軍，許建新，王云舒

（1. 南京航空航天大學 自動化學院，南京 210016；2. 南京理工大學 機械工程學院，南京 210094）

目前，無人機技術受到軍事、民用等各個應用領域的關注，其中無人機相對導航技術逐漸成為一個研究熱點。在無人機編隊飛行或聯(lián)合任務執(zhí)行等應用場景下，為保證準確的協(xié)同任務執(zhí)行效果，精確的相對導航信息是必不可少的信息源。

相對導航的測量傳感器主要包括激光雷達（LiDAR）、衛(wèi)星導航、視覺測量、超寬帶（UWB）、結構光等[1-6]，其中基于GPS/INS的相對導航方法具有成本低、數(shù)據(jù)輸出頻率高、系統(tǒng)較為可靠等優(yōu)勢，得到了廣泛的應用和研究。然而由于衛(wèi)星導航容易受到干擾和遮蔽等特殊情況的影響，當可用衛(wèi)星數(shù)目較少時，僅依賴 GPS/INS的相對導航系統(tǒng)性能會受到一定程度的削弱[7-9]。

為了獲得更好的相對導航效果，國內外學者進行了針對性的研究，其中一種主流方案是采用相對測量手段對相對導航進行組合與提升。Wang等人采用了VisNav/INS/DGPS的組合方案，通過分布式濾波架構進行信息融合，其仿真結果精度高于僅采用 VisNav/INS或INS/DGPS的相對導航方案[10]。Daniel B. Wilson等人采用了紅外攝像頭和主動式紅外LED標記的組合，對僅依賴GPS/INS的編隊飛行進行了輔助，實驗結果顯示其相對位置估計的均方根誤差（RMSE）在水平方向為1.2m，垂直方向為0.44m[11]。Jason N. Gross等人采用了DGPS/INS/UWB組合進行相對導航，當差分GPS效果較差時，使用UWB相對量測輔助整周模糊度的修復，仿真結果表明UWB輔助下的相對導航系統(tǒng)具有更高的精度和穩(wěn)定性[12]，然而沒有引入相對速度估計，且沒有針對UWB的實際使用情況進行模型優(yōu)化。

本文給出了一種超寬帶測距輔助的無人機近距離相對導航方案，建立了無人機之間雙差、雙差變化率、UWB測距以及基于雙機緊組合定位信息做差的量測方程，并且設計了相對導航狀態(tài)方程，最后針對不同的相對導航配置和衛(wèi)星可用情況進行了仿真和分析。

1 無人機相對導航方案

進行相對導航的無人機均配備慣性測量單元、衛(wèi)星導航接收機、UWB傳感器以實現(xiàn)高精度相對定位。本方案中，為獲得更好的單機定位效果，每架無人機均采用了基于偽距和偽距率的GPS/INS緊組合導航系統(tǒng)。飛行過程中，機載UWB進行無人機之間的相對測量，獲取相對距離和速度信息。同時，機載慣性導航測量數(shù)據(jù)以及衛(wèi)星導航接收機數(shù)據(jù)通過數(shù)據(jù)鏈傳輸?shù)矫考軣o人機的機載計算機，進而完成相對導航信息融合，其相對導航方案如圖1所示。

圖1 無人機近距離相對導航方案Fig.1 Schematic of close relative navigation

2 超寬帶測距輔助的相對導航方法

2.1 相對導航量測方程

2.1.1 相對差分量測方程

以兩架無人機和兩個衛(wèi)星的情況為例，無人機A的衛(wèi)星接收機與衛(wèi)星S1之間的偽距可表示為[13]：

圖2 無人機相對差分原理Fig.2 Schematic of relative differential pseudorange

考慮到無人機是近距離伴飛，基線長度相對衛(wèi)星高度很小，衛(wèi)星S1到無人機A和無人機B的方向余弦矢量差值很小，則有：

同理，無人機A和無人機B的衛(wèi)星接收機與衛(wèi)星S2之間的站際單差可以表示為：

同時，為了滿足相對速度估計，引入雙差變化率：

2.1.2 UWB量測方程

為了減少不同無人機搭載的UWB的晶振偏差，本文采用了對稱雙邊雙向測距（SDS-TWR，Symmetric Double-Sided Two-Way Ranging）算法[14]，可以有效減少由于不同節(jié)點晶振偏差大導致的測量誤差，其測距原理如圖3所示。

圖3 對稱雙邊雙向測距示意圖Fig.3 Schematic of symmetric double-sided two-way ranging

在SDS-TWR中，B接收到A的響應信號后，等待一段處理時間再返回另一個測距信息。A節(jié)點接收信號后記錄信號接收時間，得到雙向測距結果。

在該模式下，測量得到的信號傳輸時間（TOF，Time of Flight）為：

由于空中障礙物較少，且無人機體型不大，因此可以忽略 UWB的非視距誤差（NLOS，Non-line of Sight）。同時，考慮到不同節(jié)點晶振差異引起的時鐘差異，UWB的測量結果可以表示為：

在本方案內，進行相對導航的無人機均搭載UWB傳感器，用于實現(xiàn)無人機之間相對位置和速度進行實時測量，其量測方程分別為：

2.1.3 雙機定位數(shù)據(jù)做差量測模型

每架無人機均采用 GPS/INS緊組合進行絕對定位，根據(jù)無人機自身輸出的導航信息，對兩架無人機A、B緊組合輸出的位置和速度進行做差，可得到基于雙機定位數(shù)據(jù)做差的相對位置和相對速度量測方程：

2.2 相對導航狀態(tài)方程

超寬帶測距輔助的相對導航方法采用擴展卡爾曼濾波器（EKF）作為信息融合手段，濾波器采用兩種量測來增強無人機之間的相對差分定位，一是所有無人機的絕對定位數(shù)據(jù)做差結果二是UWB對基線長度的測量結果

構造狀態(tài)方程如式(17)所示：

2.3 基于UWB/相對差分/雙機定位數(shù)據(jù)做差的觀測方程

由于UWB的相對測量信息為非線性形式，因此對于EKF濾波器構建如下觀測方程：

關于UWB測量信息，以初始狀態(tài)為起始點進行泰勒展開得到：

因此，基于2.1節(jié)推導的觀測模型和上述線性化過程可構建如式(21)(22)所示的觀測向量與觀測噪聲向量：

根據(jù)相對差分(式(7)(8))、UWB 測量(式(12)(13))、雙機定位數(shù)據(jù)做差(式(14)(15))的模型，可定義如式(23)所示的觀測矩陣：

構建量測方程(23)后，結合狀態(tài)方程(17)，采用EKF算法解算狀態(tài)變量，在此不贅述。

3 仿真分析

3.1 仿真初始條件

無人機A按照如圖4所示的軌跡飛行，無人機B對A進行跟隨飛行，單機均采用GPS/INS緊組合定位，飛行過程中進行實時相對導航，飛行時長1 h。

表1列出了無人機所搭載的設備仿真參數(shù)。兩架無人機均配備慣性測量單元、衛(wèi)星導航接收機、UWB傳感器，偽距、偽距率、UWB測量噪聲均考慮為高斯噪聲。

圖4 無人機A飛行軌跡Fig.4 Trajectory of UAV A

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

3.2 仿真結果及分析

圖5以地心地固系（ECEF，Earth-Centered，Earth-Fixed）下X、Y、Z三個方向的相對距離作為對比項，對比了飛行過程中的相對距離真值和本文方法解算得到的相對距離，可用衛(wèi)星數(shù)為 8。具體的相對定位精度如表2的UWB/相對差分部分所示。

圖5 解算值和真值相對軌跡對比Fig.5 Comparison of relative trajectories

為考察不同傳感器配置情況下的相對導航性能，圖6～8對比了三種配置下的相對導航結果，設定可用導航衛(wèi)星數(shù)目為8：

1） UWB+相對差分：本文提出的方法，采用UWB、相對差分和雙機定位數(shù)據(jù)做差值作為量測量；

2）相對差分：傳統(tǒng)相對差分技術，僅采用偽距雙差數(shù)據(jù)做相對狀態(tài)估計；

3）UWB：采用本文算法框架，僅使用 UWB和雙機定位數(shù)據(jù)做差值作為量測量，無相對差分數(shù)據(jù)。

以X、Y、Z（ECEF系）三個方向的相對導航均方根誤差值（RMSE）作為對比項，UWB+相對差分的相對導航精度最高，其魯棒性最佳；相對差分的相對導航精度次之；僅采用UWB作為額外量測信息的相對導航性能最差。具體對比數(shù)據(jù)如表2所示。

表2 相對導航結果對比Tab.2 Comparison of relative navigation

圖7 Y方向相對導航誤差對比Fig.7 Comparison of relative navigation errors in Y direction

圖8 Z方向相對導航誤差對比Fig.8 Comparison of relative navigation errors in Z direction

為考察可用衛(wèi)星數(shù)較少時，本文算法相對于傳統(tǒng)相對差分技術的提升效果，圖9～11對比了兩種方法在較少可用星情況下的相對導航結果。當可見星僅有 4顆時，UWB輔助的相對導航精度明顯高于傳統(tǒng)相對差分技術，在X、Y、Z三個方向（ECEF系）的相對導航精度分別提升了3.2、13.062、2.01倍。具體數(shù)據(jù)如表3所示。

表3 相對導航結果對比（4可見星）Tab.3 Comparison of relative navigation

圖9 X方向相對導航誤差對比（4可見星）Fig.9 Comparison of relative navigation errors in X direction (4 visible satellites)

圖10 Y方向相對導航誤差對比（4可見星）Fig.10 Comparison of relative navigation errors in Y direction (4 visible satellites)

圖11 Z方向相對導航誤差對比（4可見星）Fig.11 Comparison of relative navigation errors in Z direction (4 visible satellites)

4 結 論

本文以無人機之間的相對導航為背景，研究了一種UWB輔助的近距離相對導航方案，給出了無人機之間雙差、雙差變化率、UWB、雙機定位信息做差的量測方程，設計了相對導航狀態(tài)方程，并針對不同的衛(wèi)星可用情況和不同的組合配置情況進行了仿真。仿真結果表明，該方案可以有效提升僅依賴相對差分的相對導航效果，特別是在可見衛(wèi)星數(shù)量較少時，相對導航精度提升明顯，適用于編隊飛行等對無人機相對導航精度和魯棒性有較高要求的應用場景。