張 勃 肖 雄 劉朝熙 胡向陽 黃瑾瑜 李 琪

（中國民航大學(xué)航空工程學(xué)院，天津300300）

隨著無人機技術(shù)應(yīng)用的日益廣泛，諸多生產(chǎn)活動中，關(guān)于多機協(xié)同作業(yè)的無人機編隊飛行的需求日漸增多，因此研究民航業(yè)相關(guān)領(lǐng)域中該類技術(shù)的應(yīng)用具有良好的發(fā)展前景。編隊化飛行的核心在于精確定位，雖然目前基于GPS、北斗導(dǎo)航系統(tǒng)等技術(shù)的編隊飛行能滿足室外場合作業(yè)，但面對有室內(nèi)需求的飛行任務(wù)卻無法獲得高效穩(wěn)定的定位數(shù)據(jù)。為解決室內(nèi)編隊飛行需求，本文采用基于UWB 定位技術(shù)進行室內(nèi)無人機編隊飛行算法研究，借由能搭載UWB 模塊的四旋翼無人機模擬搭建了室內(nèi)場合下的編隊化飛行系統(tǒng)。

1 UWB 定位技術(shù)原理分析

1.1 UWB 的測距原理

UWB 定位技術(shù)通過極窄的高速脈沖接收傳輸數(shù)據(jù)，傳輸帶寬高，適合局域網(wǎng)絡(luò)通信或短途導(dǎo)航。其測距原理是雙向飛行時間方法,即每個模塊會發(fā)出一個獨立的信號,通過信號的飛行時間來對模塊間的距離進行計算。

比如存在兩個模塊A 和B, 首先由A 發(fā)出一條信號并記錄發(fā)射時的時間TA1,記模塊B 收到信號下時間為TB1,此時B 在時間TB2 發(fā)出一個響應(yīng)信號, 記模塊A 收到響應(yīng)信號時間為TA2。即可計算出飛行時間得出距離。

記距離為S，光速為C，飛行時間為T

即可得出兩模塊之間的距離

1.2 基于UWB 的TDOA 定位方法

基于TDOA 方法的定位主要是利用基站接收到信號之間的時間差來確定標(biāo)簽位置，標(biāo)簽發(fā)出的信號到兩基站的時間差為常數(shù)說明了標(biāo)簽所處的位置一定在這以這兩點為焦點的雙曲線上，故確定雙曲線的交點，即確定好標(biāo)簽位置，因此TDOA定位算法又被稱為雙曲線算法。擁有四個基站則會有四條雙曲線，四條雙曲線的交點即標(biāo)簽的位置。

如圖1 所示：UWB 定位基站的坐標(biāo)分別為A（X1，Y1，Z1）、B（X2，Y2，Z2）、C（X3，Y3，Z3）、D（X4，Y4，Z4），基站A、B、C、D 在安裝部署時位置固定且坐標(biāo)已知，所求定位標(biāo)簽的坐標(biāo)為（X，Y，Z）。求解坐標(biāo)（X，Y）的方程組如下公式所示：

圖1

1.2.1 其中四個基站A、B、C、D 的距離分別為r1，r2，r3，r4，接收到信號的時間分別為t1，t2，t3，t4：

1.2.2 因標(biāo)簽發(fā)出的信號到每兩個基站間的距離差為常數(shù),故可得到一條雙曲線，所以有幾個基站即可得到幾條雙曲線,而雙曲線的交點即為所需的標(biāo)簽位置。

TDOA 不需要標(biāo)簽與基站之間進行重復(fù)的通信, 只需要標(biāo)簽發(fā)送一次信號,因而工作效率更為高效。

2 室內(nèi)無人機編隊系統(tǒng)架構(gòu)與關(guān)鍵技術(shù)

2.1 編隊系統(tǒng)架構(gòu)

定位系統(tǒng)由四個定位基站以及一個定位標(biāo)簽組成，四個定位基站上裝有UWB 定位通信模塊，定位標(biāo)簽可以通過TODA算法解算出相對的空間坐標(biāo)，從而得知標(biāo)簽的相對位置。飛行平臺為四旋翼無人機，無人機上搭載APM2.6 飛控，通過單片機將接收標(biāo)簽返回的數(shù)據(jù)并給無人機的飛控傳遞相應(yīng)的信號，無人機的飛控接收到信號后通過調(diào)整4 個電機的轉(zhuǎn)速來控制無人機的飛行姿態(tài)。標(biāo)簽返回的數(shù)據(jù)可以通過Nassistant 程序來接收顯示無人機的飛行軌跡（即引導(dǎo)標(biāo)簽的運行軌跡）。

2.2 領(lǐng)航-跟隨法

跟隨-領(lǐng)航法是目前比較成熟的一種無人機編隊方法，通過對無人機進行編隊，將對無人機編隊時對多個無人機控制轉(zhuǎn)換成對單個無人機運動的情況進行研究。該方法將無人機編隊看作一個剛性體，領(lǐng)航飛行器作為固定參照目標(biāo)，其余無人機跟隨其保持隊形。其優(yōu)勢在于簡少了對多無人機系統(tǒng)的控制，簡化了研究對象的復(fù)雜度，通過改變領(lǐng)航者與跟隨者之間距離和角度可達到改變飛行隊形的目的，其局限性主要體現(xiàn)在：編隊系統(tǒng)對領(lǐng)航無人機的依賴性過強從而導(dǎo)致容錯率低，抗干擾能力弱并且引導(dǎo)標(biāo)簽不能得到跟隨機的實際坐標(biāo)。

2.3 領(lǐng)航-跟隨控制邏輯

基于領(lǐng)航跟隨法原理，選取引導(dǎo)標(biāo)簽充當(dāng)虛擬領(lǐng)航者，通過Nassistant 可視化程序可觀察標(biāo)簽坐標(biāo)和航向角大小，應(yīng)用地面站設(shè)定跟隨無人機的期望距離和相對角度，各個跟隨者的坐標(biāo)可以準(zhǔn)確定位。利用飛行器上搭載的UWB 模塊可實時接收無人機間相對距離以及飛行角度，因此只需根據(jù)多次飛行實驗即可以確定在目標(biāo)期望下兩機相對角度與實際距離下誤差的允許范圍，從而達到編隊控制的目的。圖2 為在Nassistant 可視化程序下模擬出的引導(dǎo)標(biāo)簽及其跟隨機的移動軌跡，其中藍線為引導(dǎo)標(biāo)簽軌跡，紅線為跟隨者軌跡。

圖2 Nassistant 可視化程序

2.4 具體實驗步驟

步驟一：通過改寫飛控APM2.6 開源程序設(shè)計搭載引導(dǎo)標(biāo)簽的飛行器飛行路線，基于Nassistant 可視化程序選擇合適的跟隨飛行器與引導(dǎo)標(biāo)簽進行信息交流，設(shè)置該跟隨無人機為跟隨者A。

步驟二：跟隨飛行器基于UWB 模塊傳輸定位開始跟隨領(lǐng)航標(biāo)簽，并與各個跟隨者之間進行信號交流，獲得彼此定位坐標(biāo)，飛行參數(shù)，各跟隨無人機從不同初始位置出發(fā)，基于跟隨者A 進行統(tǒng)一編隊飛行規(guī)劃。

步驟三：按照引導(dǎo)標(biāo)簽傳輸?shù)娘w行路徑，分配給各個跟隨機不同位置信息，在跟隨領(lǐng)航標(biāo)簽運動的同時保證編隊的穩(wěn)定，一旦跟隨者A 與引導(dǎo)標(biāo)簽間距離符合允許誤差的安全半徑，通過地面站保證各個跟隨機進入減速狀態(tài)，保證編隊之間的相對飛行速度。

步驟四：當(dāng)引航標(biāo)簽與跟隨飛行器達到飛控中預(yù)定距離時，編隊飛行系統(tǒng)內(nèi)開始保持相對靜止?fàn)顟B(tài)，各個飛行器間距離保持一定，速度相同。

在編隊飛行規(guī)劃中，基于mission planer 地面站多次觀察引導(dǎo)標(biāo)簽與跟隨飛行器的安全距離，當(dāng)兩者相對距離過大或過小時，跟隨飛行器基于可視安全距離進行多次距離調(diào)整，實現(xiàn)無人機的編隊飛行控制。（圖3）

圖3

3 系統(tǒng)軟硬件設(shè)計與實現(xiàn)

3.1 無人機的結(jié)構(gòu)與控制

應(yīng)用“十”型無人機，中央方塊部分為無人機電源裝置及飛控裝置，4 個頂點分別對應(yīng)無人機的4 個電機，應(yīng)用單片機可以實現(xiàn)接收處理UWB 標(biāo)簽返回的數(shù)據(jù)并且可以通過相應(yīng)坐標(biāo)數(shù)據(jù)再對無人機飛控傳輸相應(yīng)指令，使無人機能夠按照既定規(guī)劃路線飛行。（圖4）

圖4

3.2 UWB 定位標(biāo)簽與單片機串口通信實現(xiàn)方式

3.2.1 UWB 定位標(biāo)簽與單片機的信號接收

實現(xiàn)UWB 定位標(biāo)簽與單片機間的有效通信，首先要對兩串口進行相關(guān)設(shè)置。由于標(biāo)簽發(fā)射的位置信號的波特率固定，只需將信號采樣模式發(fā)送給計算機即可實現(xiàn)串口參數(shù)設(shè)置。單片機主要通過發(fā)射端和接收端處理信號，因此要保持它的波特率處于對應(yīng)狀態(tài)。

3.2.2 UWB 標(biāo)簽與單片機間傳輸數(shù)據(jù)的優(yōu)化

對傳統(tǒng)通訊協(xié)議進行修善以保證單片機和標(biāo)簽間的通訊的穩(wěn)定性和有效性?；谠形恢眯畔鬏敹ㄎ怀绦蜻M行二次加工優(yōu)化，使單片機在完成初始化、通過中斷或掃描接受發(fā)射數(shù)據(jù)信息等過程中，不需要人工電腦和遙控器的操作，僅僅通過各種命令幀格式即可實現(xiàn)整個編隊之間的通訊交流。

3.2.3 在UWB 定位下的Nassistant 程序參數(shù)檢測

運用基于UWB 技術(shù)的Nassistant 程序檢測無人機在室內(nèi)空間中的坐標(biāo)參數(shù)，可以很大程度上提高精度、較小誤差。

（1）無人飛行器按照“十”字形行駛，其兩個電機通過上升、下降方式進行角度調(diào)整，另外兩個按照對滾轉(zhuǎn)方式操控調(diào)整。

（2）通過mission planner 地面站提前設(shè)置好飛行器的I、D值為0，在無人機進行上下行駛操作過程中逐漸加大P 的數(shù)值。

（3）為保證無人機飛行器飛行平穩(wěn)順利，在無人機各參數(shù)比例調(diào)整測試中要適時調(diào)整，確認P 值，符合測試要求。

（4）在無人機飛行器飛行測試期間，要避免變速誤差、防止等幅振蕩等，因而要合理設(shè)置飛行器參數(shù)D，保證測試飛行的平穩(wěn)，通過與Nassistant 程序上UWB 模塊等位置參數(shù)進行合理規(guī)劃，參照具體規(guī)劃要求進行適當(dāng)調(diào)整，保證飛行質(zhì)量、完成飛行任務(wù)。

4 結(jié)論

衛(wèi)星定位導(dǎo)航下的室內(nèi)信號及精度都不是十分理想，很難達到預(yù)期定位要求。本文通過研究基于UWB 技術(shù)下室內(nèi)無人機定位，并且通過單片機與定位標(biāo)簽的通訊，使無人機能夠按照既定航線進行編隊飛行。這對以后無人機在室內(nèi)精確定位并編隊飛行具有一定借鑒意義。