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

（1. 南京航空航天大學(xué) 自動(dòng)化學(xué)院，南京 210016；2. 南京理工大學(xué) 機(jī)械工程學(xué)院，南京 210094）

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

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

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

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

1 無人機(jī)相對(duì)導(dǎo)航方案

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

圖1 無人機(jī)近距離相對(duì)導(dǎo)航方案Fig.1 Schematic of close relative navigation

2 超寬帶測距輔助的相對(duì)導(dǎo)航方法

2.1 相對(duì)導(dǎo)航量測方程

2.1.1 相對(duì)差分量測方程

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

圖2 無人機(jī)相對(duì)差分原理Fig.2 Schematic of relative differential pseudorange

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

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

同時(shí)，為了滿足相對(duì)速度估計(jì)，引入雙差變化率：

2.1.2 UWB量測方程

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

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

在SDS-TWR中，B接收到A的響應(yīng)信號(hào)后，等待一段處理時(shí)間再返回另一個(gè)測距信息。A節(jié)點(diǎn)接收信號(hào)后記錄信號(hào)接收時(shí)間，得到雙向測距結(jié)果。

在該模式下，測量得到的信號(hào)傳輸時(shí)間（TOF，Time of Flight）為：

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

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

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

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

2.2 相對(duì)導(dǎo)航狀態(tài)方程

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

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

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

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

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

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

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

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

3 仿真分析

3.1 仿真初始條件

無人機(jī)A按照如圖4所示的軌跡飛行，無人機(jī)B對(duì)A進(jìn)行跟隨飛行，單機(jī)均采用GPS/INS緊組合定位，飛行過程中進(jìn)行實(shí)時(shí)相對(duì)導(dǎo)航，飛行時(shí)長1 h。

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

圖4 無人機(jī)A飛行軌跡Fig.4 Trajectory of UAV A

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

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

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

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

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

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

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

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

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

表2 相對(duì)導(dǎo)航結(jié)果對(duì)比Tab.2 Comparison of relative navigation

圖7 Y方向相對(duì)導(dǎo)航誤差對(duì)比Fig.7 Comparison of relative navigation errors in Y direction

圖8 Z方向相對(duì)導(dǎo)航誤差對(duì)比Fig.8 Comparison of relative navigation errors in Z direction

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

表3 相對(duì)導(dǎo)航結(jié)果對(duì)比（4可見星）Tab.3 Comparison of relative navigation

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

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

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

4 結(jié) 論

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