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

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

1 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4.1 通訊鏈路的硬件選擇

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

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

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

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

4.3 通訊鏈路的防沖撞處理

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

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

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

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

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

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

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

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

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

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

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

5.2 自由空間的編隊(duì)飛行算法

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

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

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

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

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

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

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

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

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

6 編隊(duì)飛行系統(tǒng)實(shí)驗(yàn)與分析

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

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

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

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

圖5 一字型編隊(duì)飛行Fig.5 Linear Formation Flight

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

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

7 總結(jié)

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

［1］樊瓊劍，楊忠，方挺.多無人機(jī)協(xié)同編隊(duì)飛行控制的研究現(xiàn)狀［J］.航空學(xué)報(bào)，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ī)—僚機(jī)模式的無人機(jī)編隊(duì)方法及飛行實(shí)驗(yàn)研究［J］.機(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］周銳，吳雯漫，羅廣文．自主多無人機(jī)的分散化協(xié)同控制［J］．航空學(xué)報(bào)，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］何真，陸宇平．無人機(jī)編隊(duì)隊(duì)形保持控制器的分散設(shè)計(jì)方法［J］．航空學(xué)報(bào)，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ù)用于編隊(duì)衛(wèi)星狀態(tài)整體確定方案設(shè)計(jì)［J］.宇航學(xué)報(bào)，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.）