徐 俊，王茂森，戴勁松，王大鵬

（南京理工大學(xué) 機械工程學(xué)院，江蘇 南京 210094）

1 引言

近年來無人機（UAV）編隊飛行已經(jīng)成為智能控制領(lǐng)域的一個研究熱點。無人機編隊飛行，即多架無人機進行某種隊形排列組織模式和任務(wù)分配的組織模式以適應(yīng)任務(wù)的要求，它既包括隊形的產(chǎn)生，保持和變化，也包括飛行任務(wù)的規(guī)劃和組織[1]。多UAV編隊飛行可以彌補單個無人機執(zhí)行任務(wù)效率低，安全冗余差等缺點，可以執(zhí)行通訊中繼、高精度定位對地攻擊、多角度3維立體成像等復(fù)雜任務(wù)[2]。通過多無人機的協(xié)作，無人機組群飛行有效的延伸無人機的任務(wù)執(zhí)行范圍，增強了任務(wù)的執(zhí)行效能和冗余保障，其綜合性能與單機相比成幾何級的增強，應(yīng)用范圍和任務(wù)使命都有了拓展。

國內(nèi)目前對無人機編隊飛行的研究基本上聚集在協(xié)同控制方法的研究和實驗室仿真階段。例如文獻(xiàn)[3]對多飛行平臺分散化協(xié)同控制系統(tǒng)結(jié)構(gòu)以及分散化的規(guī)劃等技術(shù)問題進行了深入研究。文獻(xiàn)[4]提出了一種分布式編隊控制器的分散化設(shè)計方法，并進行了仿真實驗。在提出了一種基于GPS坐標(biāo)控制的編隊算法并對算法進行了實驗驗證。搭建了穩(wěn)定的多節(jié)點無線通訊鏈路，解決了數(shù)據(jù)沖撞以及干擾問題。

最后對無人機群規(guī)劃的路徑和實際飛行路徑進行了比較以及對各部分硬軟件進行測試，從而驗證本系統(tǒng)的精確性和穩(wěn)定性和可行性。

2 編隊飛行控制系統(tǒng)總體設(shè)計

編隊飛行控制系統(tǒng)設(shè)計主要分為一下幾個部分：一是無人機端的飛行控制系統(tǒng)設(shè)計，二是無線通訊鏈路的搭建。三是編隊算法的實現(xiàn)。

2.1 控制系統(tǒng)任務(wù)規(guī)劃

地面站主要負(fù)責(zé)整個系統(tǒng)的統(tǒng)籌管理，包括隊形目標(biāo)函數(shù)的產(chǎn)生，各個目標(biāo)航點的計算，對各架無人機的狀態(tài)包括姿態(tài)、高度、速度、位置等的監(jiān)測。

2.2 控制系統(tǒng)的總體結(jié)構(gòu)

讓編隊模塊任務(wù)在電腦端的地面站處理之后，無人機端的任務(wù)負(fù)擔(dān)得到減小，可以讓更多的資源分配給姿態(tài)檢測、控制、導(dǎo)航、通訊等更緊急的任務(wù)。從而保證這些任務(wù)執(zhí)行的實時性。

系統(tǒng)的整體結(jié)構(gòu)，如圖1所示。系統(tǒng)由多架無人機、地面站組成，系統(tǒng)之間的通信由ZigBee實現(xiàn)各個節(jié)點之間的組網(wǎng)，通訊協(xié)議采用MAVLink協(xié)議。地面站根據(jù)編隊需要的陣形，計算出在當(dāng)前位置下各個無人機的GPS坐標(biāo)和高度，通過無線發(fā)送給各架無人機，無人機接收到自身的目標(biāo)航點并飛向該航點。同時，每架無人機廣播自身的GPS坐標(biāo)，并接受其他無人機的GPS坐標(biāo)，進行防撞檢測。

圖1 控制系統(tǒng)結(jié)構(gòu)Fig.1 Control System Structure

由于地面站對整個系統(tǒng)的無人機進行監(jiān)管和調(diào)度，其職能類似于空管，能夠全局調(diào)度各架無人機，從而保證了編隊系統(tǒng)的安全性和靈活性。

3 單架無人機的系統(tǒng)設(shè)計

微型四旋翼是一個具有六自由度和4個控制輸入的欠阻尼系統(tǒng)，具有多變量、非線性、強耦合和干擾敏感的特性[5]。為了實現(xiàn)編隊指令的正確執(zhí)行，無人機端的任務(wù)非常多，要同時進行大量的計算，比如傳感器數(shù)據(jù)采集和處理、姿態(tài)計算、姿態(tài)控制、導(dǎo)航、通訊、指令解析等。

3.1 無人機的硬件設(shè)計

為了保證單機系統(tǒng)的可靠性，無人機端的傳感器都選擇了不同型號的兩種同樣同樣功能的傳感器進行數(shù)據(jù)冗余，增強了系統(tǒng)的抗干擾性和魯棒性。

同時為了提高無人機執(zhí)行任務(wù)的實時性，無人機控制系統(tǒng)采用兩顆處理器芯片，主控制器和協(xié)處理器之間通過串口進行數(shù)據(jù)通訊。協(xié)處理器主要負(fù)責(zé)遙控器信號的接收解析和對電機的PWM輸出控制。系統(tǒng)大部分傳感器通過SPI總線和主控制器連接，保證了底層數(shù)據(jù)傳輸?shù)膶崟r性。單機系統(tǒng)的硬件框架圖，如圖2所示。

圖2 單機系統(tǒng)硬件框圖Fig.2 UAV Control System Hardware Structure

3.2 單架無人機的軟件設(shè)計

傳統(tǒng)的無人機系統(tǒng)程序采用while（1）循環(huán)或者中斷的執(zhí)行方式，這種方式往往實時性弱、可維護性差。為了大運算量、多任務(wù)的有序執(zhí)行，系統(tǒng)采用了NuttX操作系統(tǒng)，能為多旋翼飛行器控制系統(tǒng)的開發(fā)提供極大的方便。

無人機端所有的任務(wù)通過任務(wù)進程實現(xiàn)。每個進程設(shè)定一個調(diào)度的時間間隔，該進程則按照這個時間間隔運行，同時給每個進程設(shè)定最大運行時間，若本次進程執(zhí)行時間預(yù)計超過該時間則放棄該次運行。

程序框架圖，如圖3所示。每個進程有兩個執(zhí)行控制量，如（2.5，180）表示該進程沒2.5ms調(diào)度一次，當(dāng)該進程執(zhí)行的預(yù)估時間大于180ms時則放棄該次執(zhí)行，釋放占有的CPU資源。

圖3 程序執(zhí)行流程圖Fig.3 Program Execution Flow Chart

4 編隊系統(tǒng)的通訊鏈路設(shè)計

無人機編隊系統(tǒng)的通訊是鏈接各架無人機和地面站之間的橋梁，各架無人機自身坐標(biāo)點的發(fā)送、地面站指令的發(fā)送都需要一個穩(wěn)定實現(xiàn)全雙工的無線通訊鏈路。

4.1 通訊鏈路的硬件選擇

針對編隊系統(tǒng)多對多通訊的特點，選用了具有自組網(wǎng)功能的ZigBee模塊。同時ZigBee模塊在2.4GHz工作頻率下能提供250kbps的數(shù)據(jù)吞吐率，遠(yuǎn)高于實際編隊系統(tǒng)中的數(shù)據(jù)吞吐率，因此ZigBee模塊能夠滿足編隊系統(tǒng)的無線數(shù)據(jù)傳輸要求。

4.2 編隊系統(tǒng)的通訊協(xié)議設(shè)計

通訊鏈路使用MAVLink（Micro Air Vehicle Link）通訊協(xié)議，MAVLink是一種只包含頭文件信息調(diào)度庫的輕量通訊協(xié)議，主要用于地面站（GCS）和微型無人運載工具間的通信[6]。

為了使MAVLink適應(yīng)編隊系統(tǒng)，對MAVLink的協(xié)議進行了定制，將一號機的MAVLink消息幀起始位標(biāo)志設(shè)置為0xFE，二號機的MAVLink消息幀起始位修改為0xFD，將三號機的MAVLink消息幀起始位標(biāo)志修改為0xFC。這樣在編隊系統(tǒng)中每架無人機可以在多個消息幀中識別出自己的消息。

4.3 通訊鏈路的防沖撞處理

在把每架無人機的MAVLink消息幀的ID區(qū)分開之后理論上各個無人機都能在眾多的消息中識別出發(fā)給自己的消息，但多架無人機一起發(fā)送的時候，會發(fā)生數(shù)據(jù)沖撞，接受端會收到亂碼。因此同一時刻只能由一個發(fā)送設(shè)備占用信道，系統(tǒng)采用了類似于令牌環(huán)形網(wǎng)絡(luò)的數(shù)據(jù)通訊機制。

以三架無人機為例說明通訊鏈路的傳輸流程：將地面站作為參與環(huán)形網(wǎng)絡(luò)的一員，網(wǎng)絡(luò)的發(fā)送按照約定的順序發(fā)送消息，一號機先發(fā)送消息，在消息的中增加自身的發(fā)送OK狀態(tài)標(biāo)志位，一號機發(fā)送完畢之后網(wǎng)絡(luò)中的所有設(shè)備都接收到一號機的消息，獲取了一號機的狀態(tài)信息。每架無人機將消息發(fā)送OK的標(biāo)志位和約定的發(fā)送時序進行比對，發(fā)現(xiàn)是自身約定的時序時則開始發(fā)送否則不發(fā)送。此時二號機接收到消息發(fā)現(xiàn)是約定的時序，二號機開始發(fā)送消息，并將消息中的OK狀態(tài)標(biāo)志位修改為UAV2。接線來是三號機發(fā)送，這樣依次發(fā)送構(gòu)成一個環(huán)形網(wǎng)絡(luò)，且保證了每次發(fā)送的時候只有一個設(shè)備占用信道。

為了提高環(huán)形網(wǎng)絡(luò)的魯棒性，每個設(shè)備通訊程序中引入了一個發(fā)送周期計時模塊，每次發(fā)送完畢計時清零并開始下一次計時，當(dāng)在設(shè)定的計時時間內(nèi)沒有接收到某架無人機消息是則從最后一次接受到消息的OK標(biāo)志位按照約定的順序向后遞推2個設(shè)備，此時該設(shè)備發(fā)送消息，并且該設(shè)備之后在循環(huán)中將監(jiān)聽的設(shè)備改為最后一次接收到的設(shè)備，這樣就保證通訊系統(tǒng)不會因為某個設(shè)備的損壞而通信中斷。

5 隊形編隊算法設(shè)計

考慮到地面站能對所有的無人機進行統(tǒng)一調(diào)度并且筆記本電腦的強大的計算能力和方便的開發(fā)環(huán)境，因此本系統(tǒng)將編隊算法實現(xiàn)放在地面站軟件中。

5.1 經(jīng)緯度坐標(biāo)和距離之間的轉(zhuǎn)換

無人機端獲取位置的方式是依靠GPS模塊。在實際位置估算中結(jié)合加速度計判斷自身狀態(tài)，能以20cm的精度穩(wěn)定自身位置。高精度的GPS為接下來的控制算法提供了基礎(chǔ)[7]。

在編隊算法的實現(xiàn)過程中，運算的單位是米，即陣形是以米為單位控制的，因此需要把計算結(jié)果的米的單位轉(zhuǎn)化成GPS坐標(biāo)發(fā)送給無人機執(zhí)行。

已知當(dāng)前位置經(jīng)緯度為lon1、lat1，目標(biāo)航點經(jīng)緯度為lon2、lat2，將經(jīng)度差dlon轉(zhuǎn)換成經(jīng)度方向的距離差dLon轉(zhuǎn)換公式如下

將緯度差dLat轉(zhuǎn)換成緯度方向的距離差dLat需要有一個換算，因為每個緯度對應(yīng)的緯線的圓周長是不一樣的，假如當(dāng)前的緯度值為lat，則換算公式為：

計算出在本地（118．852108E，32．026415W）一米的長度在經(jīng)度方向上近似等于0．000010°，在緯度方向上一米近似等于0．000009°。

5.2 自由空間的編隊飛行算法

自由空間即沒有約束條件的三維空間，無人機在此空間可以自由飛行。下面說明算法坐標(biāo)系到無人機實際運行的GPS坐標(biāo)點之間的轉(zhuǎn)化。

算法坐標(biāo)系中的坐標(biāo)可以看成是相對于起始點的距離偏移量，因此轉(zhuǎn)化到經(jīng)緯度坐標(biāo)系時只要知道起始點的經(jīng)緯度，再在起始點經(jīng)緯度疊加上相對距離轉(zhuǎn)化成經(jīng)緯度的偏移量就得到了該點在經(jīng)緯度坐標(biāo)系中的GPS坐標(biāo)?？梢缘玫綗o人機的經(jīng)度坐標(biāo)值Lon和其緯度坐標(biāo)值Lat。

式（4）為經(jīng)緯度值和算法求出的坐標(biāo)值之間的轉(zhuǎn)化關(guān)系公式：

從算法坐標(biāo)系我們通過式（5）可以求出無人機的經(jīng)緯度值，按照0．5m為單位可以得到一系列的沿著軌跡的航點，將這些航點依次發(fā)送給一架無人機，無人機沿著這些航點飛行便使得該架無人機按照我們設(shè)定的軌跡飛行。

控制了一架無人機之后還需要控制其余的無人機，當(dāng)多架無人機按照某一個陣形飛行時，各架無人機之間的相對位置可以看著固定不變的[8]。

例如當(dāng)無人機編隊按照等腰三角形陣形飛行，一號機的軌跡確定，二號機相對于1號機的位置差△X1和△Y是保持不變，在此基礎(chǔ)上疊加上位移差得到二號機的GPS坐標(biāo)和三號機的GPS坐標(biāo)，這樣得到陣形中各無人機的坐標(biāo)。

假設(shè)在算法坐標(biāo)系中，主機（一號機）的飛行軌跡方程，如式（5）所示。根據(jù)式（4）我們可以計算出在GPS坐標(biāo)系中主機的軌跡方程。

若按三角形編隊飛行，三號機相對于一號機往西偏移7m，往南方向偏移7m；二號機相對于一號機往東偏移7m，往南偏移7m。1號機GPS坐標(biāo)為（118．852108E，32．026415W）。在算法坐標(biāo)系和GPS坐標(biāo)系中的理論隊形飛行軌跡，如圖4所示。

圖4 算法坐標(biāo)系和GPS坐標(biāo)系編隊路徑圖Fig．4 Algorithm Coordinate System and GPS Coordinate System Formation Path Graph

6 編隊飛行系統(tǒng)實驗與分析

編隊系統(tǒng)以三架四旋翼無人機為測試對象，測試系統(tǒng)的實際飛行的效果。實驗內(nèi)容包括通訊功能測試和編隊飛行測試，測試主機的GPS坐標(biāo)為（118．852108E，32．026415W）。

為了驗證編隊飛行系統(tǒng)的可靠性，使用三架四旋翼無人機進行編隊飛行試驗，設(shè)置1號機為主機，三號機相對于一號機往西偏移7m；二號機相對于一號機往東偏移7m。二號機和三號機跟隨一號機飛行，三架四旋翼無人機組成一字形隊并排形飛行。

一號機的飛行的四段軌跡方程依次為式（6）～式（9）所示。

在各架無人機飛行過程中，記錄無人機實時位置得到實際的航線。實驗現(xiàn)場拍攝圖片，如圖5所示。實際飛行曲線和理論軌跡曲線的對比，如圖6所示。

圖5 一字型編隊飛行Fig.5 Linear Formation Flight

圖6 理論路徑和實際路徑對比圖Fig.6 Comparison of Theoretical and Practical Paths

可以發(fā)現(xiàn)實際的飛行軌跡能夠較好的沿著理論曲線飛行，說明了編隊算法的正確性和可行性。

7 總結(jié)

設(shè)計四旋翼無人機的編隊飛行控制系統(tǒng)，包括單機飛控系統(tǒng)、多節(jié)點通訊鏈路、編隊算法，并對各部分進行了測試，得到了令人滿意的運行效果，驗證其可行性。同時單機采用的NuttX操作系統(tǒng)在保證實時性的同時也為以后的功能擴展提供了良好的平臺。實現(xiàn)三架無人機編隊飛行為以后十架甚至幾十架無人機編隊提供了扎實的基礎(chǔ)。

［1］樊瓊劍，楊忠，方挺.多無人機協(xié)同編隊飛行控制的研究現(xiàn)狀［J］.航空學(xué)報，2009，30（4）：683-691.（Fan Qiong-jian，Yang Zhong，F(xiàn)ang Ting.Research status of coordinated formation flight control for multi-UAVs［J］.Aeronautical Journal，2009，30（4）：683-69.）

［2］洪曄，繆存孝，雷旭升.基于長機—僚機模式的無人機編隊方法及飛行實驗研究［J］.機器人，2010，32（4）：505-509.（Hong Ye，Miu Chun-xiao，Lei Xu-shen.Study and test based on UAV formation method of the master-slave modle ［J］.Robot，2010，32（4）：505-509.）

［3］周銳，吳雯漫，羅廣文．自主多無人機的分散化協(xié)同控制［J］．航空學(xué)報，2008，29（B05）：26-32.（Zhou Rui，Wu Wen-man，Luo Guang-wun.The decentralized coordination control of multiple autonomous UAVs［J］.Aeronautical Journal，2008，29（B05）：26-32.）

［4］何真，陸宇平．無人機編隊隊形保持控制器的分散設(shè)計方法［J］．航空學(xué)報，2008，29（B05）：55-60.（He Zhen，Lu Yu-ping.Design method of decentralized controller for formation keeping machine ［J］.Aeronautical Journal，2008，29（B05）：55-60.）

［5］聶博文，馬宏緒，王劍.微小型四旋翼飛行器的研究現(xiàn)狀與關(guān)鍵技術(shù)［J］.電光與控制，2007，14（6）：113-117.（Nie Bo-wen，Ma Hong-xu，Wang Jian.Research status and key technology of micro four rotor aircraft［J］.Electronics Optics&Control，2007，14（6）：113-117）

［6］孟穎，張貴陽，魏曉馬.基于APM飛控制作的MAVLink通信協(xié)議分析［J］.電子世界，2014（23）：101-103.（Meng Ying，Zhang Gui-yang，Wei Xiao-ma.Analysis of MAVLink communication protocol based on the production of APM flight control［J］.Electronic World，2014（23）：101-103.）

［7］Park，Chulwoo；Kim，Hyoun Jin；Kim，Youdan.Real-time leader-follower UAV formation flight based on modified nonlinear guidence［J］.29th Congress of the International Council of the Aeronautical Sciences，ICAS 2014，9（7）.

［8］高玉東，郗曉寧，王威.GPS技術(shù)用于編隊衛(wèi)星狀態(tài)整體確定方案設(shè)計［J］.宇航學(xué)報，2006，27（2）：205-209.（Gao Yu-dong，Xi Xiao-bo，Wang Wei.Design of GPS technology for formation satellite state integral determination［J］.Aeronautical Journal，2006，27（2）：205-209.）