四旋翼無人機的編隊飛行控制系統設計

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

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

1 引言

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

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

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

2 編隊飛行控制系統總體設計

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

2.1 控制系統任務規劃

地面站主要負責整個系統的統籌管理，包括隊形目標函數的產生，各個目標航點的計算，對各架無人機的狀態包括姿態、高度、速度、位置等的監測。

2.2 控制系統的總體結構

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

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

圖1 控制系統結構Fig.1 Control System Structure

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

3 單架無人機的系統設計

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

3.1 無人機的硬件設計

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

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

圖2 單機系統硬件框圖Fig.2 UAV Control System Hardware Structure

3.2 單架無人機的軟件設計

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

無人機端所有的任務通過任務進程實現。每個進程設定一個調度的時間間隔，該進程則按照這個時間間隔運行，同時給每個進程設定最大運行時間，若本次進程執行時間預計超過該時間則放棄該次運行。

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

圖3 程序執行流程圖Fig.3 Program Execution Flow Chart

4 編隊系統的通訊鏈路設計

無人機編隊系統的通訊是鏈接各架無人機和地面站之間的橋梁，各架無人機自身坐標點的發送、地面站指令的發送都需要一個穩定實現全雙工的無線通訊鏈路。

4.1 通訊鏈路的硬件選擇

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

4.2 編隊系統的通訊協議設計

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

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

4.3 通訊鏈路的防沖撞處理

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

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

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

5 隊形編隊算法設計

考慮到地面站能對所有的無人機進行統一調度并且筆記本電腦的強大的計算能力和方便的開發環境，因此本系統將編隊算法實現放在地面站軟件中。

5.1 經緯度坐標和距離之間的轉換

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

在編隊算法的實現過程中，運算的單位是米，即陣形是以米為單位控制的，因此需要把計算結果的米的單位轉化成GPS坐標發送給無人機執行。

已知當前位置經緯度為lon1、lat1，目標航點經緯度為lon2、lat2，將經度差dlon轉換成經度方向的距離差dLon轉換公式如下

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

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

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

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

算法坐標系中的坐標可以看成是相對于起始點的距離偏移量，因此轉化到經緯度坐標系時只要知道起始點的經緯度，再在起始點經緯度疊加上相對距離轉化成經緯度的偏移量就得到了該點在經緯度坐標系中的GPS坐標。可以得到無人機的經度坐標值Lon和其緯度坐標值Lat。

式（4）為經緯度值和算法求出的坐標值之間的轉化關系公式：

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

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

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

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

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

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

6 編隊飛行系統實驗與分析

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

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

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

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

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

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

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

7 總結

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

［1］樊瓊劍，楊忠，方挺.多無人機協同編隊飛行控制的研究現狀［J］.航空學報，2009，30（4）：683-691.（Fan Qiong-jian，Yang Zhong，Fang 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］周銳，吳雯漫，羅廣文．自主多無人機的分散化協同控制［J］．航空學報，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］．航空學報，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］聶博文，馬宏緒，王劍.微小型四旋翼飛行器的研究現狀與關鍵技術［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通信協議分析［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技術用于編隊衛星狀態整體確定方案設計［J］.宇航學報，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.）