基于UWB 定位的室內無人機編隊飛行規劃

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

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

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

1 UWB 定位技術原理分析

1.1 UWB 的測距原理

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

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

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

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

1.2 基于UWB 的TDOA 定位方法

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

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

圖1

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

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

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

2 室內無人機編隊系統架構與關鍵技術

2.1 編隊系統架構

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

2.2 領航-跟隨法

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

2.3 領航-跟隨控制邏輯

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

圖2 Nassistant 可視化程序

2.4 具體實驗步驟

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

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

步驟三：按照引導標簽傳輸的飛行路徑，分配給各個跟隨機不同位置信息，在跟隨領航標簽運動的同時保證編隊的穩定，一旦跟隨者A 與引導標簽間距離符合允許誤差的安全半徑，通過地面站保證各個跟隨機進入減速狀態，保證編隊之間的相對飛行速度。

步驟四：當引航標簽與跟隨飛行器達到飛控中預定距離時，編隊飛行系統內開始保持相對靜止狀態，各個飛行器間距離保持一定，速度相同。

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

圖3

3 系統軟硬件設計與實現

3.1 無人機的結構與控制

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

圖4

3.2 UWB 定位標簽與單片機串口通信實現方式

3.2.1 UWB 定位標簽與單片機的信號接收

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

3.2.2 UWB 標簽與單片機間傳輸數據的優化

對傳統通訊協議進行修善以保證單片機和標簽間的通訊的穩定性和有效性。基于原有位置信息傳輸定位程序進行二次加工優化，使單片機在完成初始化、通過中斷或掃描接受發射數據信息等過程中，不需要人工電腦和遙控器的操作，僅僅通過各種命令幀格式即可實現整個編隊之間的通訊交流。

3.2.3 在UWB 定位下的Nassistant 程序參數檢測

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

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

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

（3）為保證無人機飛行器飛行平穩順利，在無人機各參數比例調整測試中要適時調整，確認P 值，符合測試要求。

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

4 結論

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