陳杰敏+吳發(fā)林+耿澄浩+徐珊

摘 要： 四旋翼無人機在民用及軍用領域都發(fā)揮著越來越重要的作用。 為了完成某些特定任務， 需要由多架四旋翼組成的編隊保持適當隊形飛行。 與單架四旋翼執(zhí)行任務相比， 四旋翼編隊具有能增加任務成功率、 提高整體抗干擾性能、 擴大監(jiān)控范圍等優(yōu)點。 本文基于主從式編隊結構， 結合信息拓撲理論， 把四旋翼編隊描述為二階一致性系統(tǒng)， 設計編隊控制器來實現(xiàn)四旋翼編隊的穩(wěn)定飛行。 主機和從機均采用PID控制， 主機跟蹤預設軌跡， 從機跟蹤編隊控制器計算出的軌跡跟蹤指令。 最后通過仿真分析了控制算法對四旋翼編隊隊形生成及隊形保持的控制效果。

關鍵詞： 四旋翼無人機； 編隊飛行； 信息拓撲理論； 一致性理論； 主從式編隊

中圖分類號： V249.1 文獻標識碼： A 文章編號： 1673-5048（2017）06-0025-07[SQ0]

0 引 言

四旋翼無人機（以下簡稱四旋翼）是一種有四個螺旋槳且螺旋槳分別呈十字交叉型的飛行器。 在過去的數(shù)十年中， 傳感器及電子元器件的微型化、 低廉化推動了四旋翼的快速發(fā)展。 四旋翼在軍事領域可用于巡邏偵察、 定點攻擊等方面， 在民用領域可用于遙感測繪、 農業(yè)植保、 通信中繼、 航拍航測、 短途運輸?shù)确矫妫?具有廣泛的應用前景和實際用途， 成為商業(yè)公司和科研單位的研究熱點[1]。 隨著無線傳感網絡等技術的創(chuàng)新應用， 由多架無人機組成的編隊在民用及軍事等方面都展現(xiàn)出越來越廣泛的用途， 如能進行空中集群表演、 組建軍事通信網絡、 構建網絡化軍事打擊編隊等。

無人機編隊飛行， 是指在三維空間中， 多架無人機按照一定的隊形進行排列， 使其在飛行過程中保持隊形不變或者相對位置在一定范圍內變動， 并能根據外部情況和任務需求進行動態(tài)調整， 以保持編隊的協(xié)同一致性。

無人機編隊飛行控制方式主要有主從法[2]、 虛擬結構法[3-4]、 行為控制法[5-6]等。 主從法結構中， 其中一個飛行器被指定為主機， 其余飛行器則被指定為從機。 從機以一定的偏移量跟蹤主機的方向及位置。 虛擬結構法把整個編隊視為單一的虛擬結構， 每架無人機是虛擬結構中相對位置固定的一點。 當編隊移動時， 每架無人機跟蹤隊形中固定點的運動即可。 行為控制法是將主要控制目標分解為一些預期行為， 并基于控制輸入的加權平均為每一個行為設計單獨的控制輸入， 能處理諸如隊形保持、 碰撞規(guī)避、 中心聚攏等行為。

多智能體集群行為可定義為：集群系統(tǒng)中的多智能體通過局部信息相互作用， 并遵循三條原則：（1） 避免群體成員之間的碰撞；（2） 智能體與其鄰居的速度盡量匹配；（3） 各智能體盡量向鄰居智能體靠近， 并保持一定的群聚性。 多無人機編隊飛行也屬于多智能系統(tǒng)的研究范疇， 基于該理論研究無人機編隊飛行系統(tǒng)是近年來的研究熱點， 特別是對信息拓撲理論思想的運用， 將編隊系統(tǒng)看成是通信與感知信息流構成的通信拓撲， 利用一致性問題的研究方法進行研究， 得到了許多學者的青睞。 控制器的設計與系統(tǒng)性能分析可利用編隊飛行器間的信息交互數(shù)學模型， 按照代數(shù)圖論的方法進行研究。 如薛瑞彬等[7]考慮編隊系統(tǒng)存在通訊時延的情況下， 利用一致性理論設計了分布式編隊控制算法， 并對系統(tǒng)穩(wěn)定性予以理論和仿真證明。 周稼康等[8]針對主從結構的編隊飛行系統(tǒng)， 基于一致性算法分析了通信拓撲結構為有向圖時系統(tǒng)控制器參數(shù)的選取。 文獻[9-10]研究了基于一致性理論的無人機時變編隊的控制算法， 并在四旋翼硬件平臺上進行了驗證。 文獻[11]將存在多個時延的線性時變拓撲結構轉化為一致性問題進行研究分析， 并將結果用于編隊飛行系統(tǒng)的研究中。

為改進傳統(tǒng)主從飛行編隊信息交流單一的不足， 本文將信息拓撲理論應用于無人機主從編隊， 增加從機與從機之間的信息交互， 從而使編隊在飛行過程中隊形保持更加穩(wěn)定， 且隊形變換更加多樣。

1 四旋翼模型建立

在大地坐標系E及載體坐標系B下建立四旋翼模型。 四旋翼模型坐標表示如圖1所示。 四旋翼質量為m， 對應的轉動慣量矩陣I。 位置及姿態(tài)角分別記為r=[xyz]T和η=[θψ]T。 在載體坐標系B下定義角速度ω=[pqr]T。

為了簡化控制方案設計， 根據以下假設， 對四旋翼模型進行合理簡化[12]。 首先， 四旋翼為對稱剛體， 其質心與載體坐標系原點重合， 轉動慣量矩陣簡化為I=diag[IxxIyyIzz]。 其次， 四旋翼工作在懸浮狀態(tài)， 其橫滾角及俯仰角θ均較小[13]（c≈1， cθ≈1， s≈， sθ≈θ）， 因此， 載體角速度ω可認為與姿態(tài)角變化率相等， 并且忽略陀螺力矩。

根據牛頓歐拉法， 四旋翼平移動力學及旋轉動力學方程如下：

式中： g為重力加速度； ez=[001]T為大地坐標系下的單位向量； TB為載體坐標系下四個旋翼產生的總推力； REB∈SO（3）為從載體坐標系B向大地坐標系E變換的旋轉矩陣； τ為作用在四旋翼上的控制力矩。 τ和TB組成系統(tǒng)的4個輸入u， 通過改變τ和TB來改變旋翼的轉速， 從而控制四旋翼向期望位置運動。 旋轉矩陣RBE、 4個旋翼的轉速與τ和TB的變換關系分別如式（3）～（4）所示：

2 四旋翼控制

為了更好地驗證編隊控制器性能， 底層四旋翼控制采用成熟的PID控制算法， 內環(huán)控制姿態(tài)， 外環(huán)控制位置， 控制框圖見圖2。

3 編隊控制

3.1 信息拓撲理論

為了實現(xiàn)垂直起降無人機群的編隊飛行或者保證剛體姿態(tài)同步， 必須利用局部信息交換來設計控制方案。 因此， 無人機之間需要發(fā)送狀態(tài)信息。 本文用加權圖來描述組成員之間的信息交換。 在此給出信息拓撲理論的一些標準定義和特性。

加權圖G由三元組（V， E， A）組成， 其中V={v1， v2， ...， vn}是節(jié)點或頂點集合， 用于描述集群中飛行器集合。 EV×V是成對節(jié)點的集合， 稱為邊。 A=[aij]∈Rn×n是加權鄰接矩陣。 邊（i， j）∈E表示第i個系統(tǒng)接收來自第j個系統(tǒng)的信息， 且j與i相鄰。 通常， 在圖中利用從節(jié)點vj指向節(jié)點vi的有向線段， 表示后者能夠直接獲取前者的信息， 并稱vi是vj的父節(jié)點， vj是vi的子節(jié)點。 若vi與vj之間可相互獲取信息， 則兩者互為父子節(jié)點， 滿足（vi， vj）∈E（vj， vi）∈E， 且兩者之間的有向線段為雙向， 稱為雙向邊， 一般用雙向箭頭線段或者不帶箭頭的線段表示。 若G中所有的邊均為雙向邊， 則G為無向圖， 可視為有向圖的特例。

加權圖的加權鄰接矩陣定義：當且僅當（i， j）∈E時， aij>0；當且僅當（i， j）E時， aij=0。 如果系統(tǒng)間的互連是雙向的， 那么加權圖G是無向的， E中的節(jié)點對是無序的， 即（i， j）∈E（j， i）∈E， 且A是對稱的， 即aij=aji。 在單向互連接的情況下， G是一個有向圖， E包含有序節(jié)點對， A不一定是對稱的。 在信息拓撲理論中， 有向邊（i， j）用一個由節(jié)點j指向節(jié)點i的有向連接線（箭頭）來表示。 在無向圖中， 使用沒有箭頭的連接線。

如果有向圖中任意兩個完全不同節(jié)點之間存在一條有向路徑， 則該圖稱為是強連通的。 這里， 有向路徑是有向圖中的有向邊序列， 其形式是（i1， i2）， （i2， i3）， …， 其中il∈V。 記任意節(jié)點vi的鄰居集為Ni（G）=j∈V（G）（i， j）∈E（G）， 若流入節(jié)點vi的信息個數(shù)等于其流向其他所有節(jié)點的個數(shù)， 即節(jié)點vi的入度degini=∑i∈Njaij與出度degouti=∑j∈Niaij相等時， 稱圖G是平衡的。 顯然， 任意無向圖是平衡的。 若A中的非零元素均為1， 對于任意節(jié)點vi， 有∑nj=1aij=∑nj=1aji成立， 顯然圖G是平衡的， 此時稱G為不加權圖。 如果用無向邊替代（有向圖G中的）有向邊而獲得的無向圖G'是連通的， 那么稱有向圖G是弱連通的。 更多關于信息拓撲理論的介紹及有關圖論的例子請參考文獻[14]。

利用拓撲結構中的邊來表示無人機的通信或控制鏈路， 雙向邊則可用于表示兩架無人機間可雙向進行交互。 由于編隊系統(tǒng)中的任意無人機均可能對這個系統(tǒng)產生影響， 故而可利用信息將其拓撲為無向連通圖或者有向強連通圖， 但編隊飛行的無人機之間獲取相互信息的能力不同， 相互之間不一定能夠獲取彼此信息， 故而信息交互可能是單向的， 特別是在主從結構的編隊飛行中， 其信息拓撲一般為有向圖。

3.2 一致性編隊理論

一致性理論是根據多智能體間的狀態(tài)信息流通， 按照網絡拓撲的思想構造一致性算法， 使得智能體與鄰居智能體的狀態(tài)誤差保持一致， 并使得多智能體的狀態(tài)信息與期望信息保持一致。 利用一致性理論對編隊系統(tǒng)協(xié)同控制問題進行研究， 可歸納為將編隊系統(tǒng)轉化為網絡拓撲結構， 通過一致性算法使得無人機的狀態(tài)信息與期望信息保持一致， 從而實現(xiàn)多無人機自主編隊運動控制。

對一致性理論分布式編隊飛行控制的研究， 常采用二階一致性算法進行分析。

設雙積分連續(xù)系統(tǒng)為

式中： i=1， 2， …， n， 其中n為系統(tǒng)中多智能體個數(shù)； xi∈Rm， vi∈Rm分別為第i個智能體的位置和速度矢量。 式（17）的一致性問題可歸納為給定任意系統(tǒng)初始值xi（0）， vi（0）與i=1， 2， …， n， 設計控制器ui， 使當t→∞時， |xi-xj|→0， （i， j=1， 2， …， n）和|vi-vj|→0， （i， j=1， 2， …， n）成立。 解決該問題的方法是設計如下二階一致性算法[15]：

式中： β>0， aij為加權鄰接矩陣A中第i行第j列元素。

3.3 多四旋翼一致性編隊飛行控制方案

由N架四旋翼組成編隊， 根據其之間的信息通信關系， 把飛行編隊建模為有向圖G， 有向圖節(jié)點vi表示第i架四旋翼。 信息流通方向由有向圖G中的箭頭方向表示。 節(jié)點v1表示主機， 其余節(jié)點表示從機。 主機把自身的狀態(tài)發(fā)送給從機， 從機之間信息拓撲關系可為任意有向圖。

四旋翼編隊生成問題描述為各無人機在零時刻靜止于地面且無隊形， 設計拓撲結構G， 并根據主機的位姿信息設計從機vi的期望跟蹤指令， 使得ri-rd→0， ψi-ψd→0， 下標d表示對應的期望值。

在主從編隊模式中， 主機位置為編隊一致性算法的平衡點。 定義主機與從機的期望位置偏差為相對位置偏差， 記為R*i=[x*i y*i z*i]T。 通過改變通信拓撲結構和設置不同的R*i即可設計出不同的編隊形狀。

在主從編隊模式下， 給出一致性編隊算法如下：

式中： xdL和vdL分別為編隊主機的位置和速度； α表示主機與從機的鄰接關系。 當從機i能接收到主機的狀態(tài)信息時， α>0， 否則， α=0。 v·dL為主機的加速度信息。

因此， 四旋翼一致性編隊飛行控制方法的計算步驟如下：

（1）根據編隊中無人機之間的通信關系， 設計編隊信息拓撲結構。 信息拓撲結構決定了對應的鄰接矩陣。

（2）根據飛行編隊的期望隊形， 設計相對位置偏差矩陣R*i=[x*i y*i z*i]T。

（3）設計主機飛行軌跡， 通過PID控制使主機跟蹤期望軌跡飛行。

（4）從機接收主機的位置和速度信息， 以及其他有信息交互的從機的位置和速度， 通過式（19）計算出其下一時刻的加速度， 從而計算出下一時刻的期望位置。

（5）通過計算得到的從機期望位置， 由PID控制器來控制從機， 從而使從機跟蹤期望位置飛行。

通過以上步驟， 可以實現(xiàn)四旋翼編隊保持期望的隊形飛行。

4 仿真與分析

4.1 仿真參數(shù)設置

在Matlab/Simulink環(huán)境中搭建四旋翼模型， 模擬5架微小型四旋翼在無障礙物環(huán)境下的編隊飛行。 具體參數(shù)為：m=0.58 kg， Ixx=Iyy=0.114 7 kg/m2， Izz=0.052 2 kg/m2。 一致性編隊算法中， α=1， β=2， v1為主機， vi（i=2， 3， 4， 5）為從機。 隊形為正方形， 主機位于中心， 從機位于正方形四角， 相對位置偏差矩陣設為

5架四旋翼均從原點出發(fā)， 4架從機根據一致性編隊協(xié)議及相對位置偏差， 按照編隊控制器計算出的期望位置指令， 向各自的期望點運動。 編隊飛行軌跡為上升的S形曲線， 主機期望飛行軌跡曲線方程為x=0.1t， y=20cos（πt/50）-20， z=0.1t。 仿真時間設為200 s， 仿真步長為0.001 s。

4.2 仿真結果分析

圖4為四旋翼編隊飛行三維軌跡圖。 從圖中可

以看出， 通過一致性理論編隊控制算法來控制編隊， 無人機起飛后， 能自主形成期望隊形， 并保持穩(wěn)定隊形飛行。 圖5為xy平面的飛行軌跡， 可以

的位置。 5架無人機形成預設隊形后， 從機能跟隨看出， 在初始階段， 從機能自主飛往各自期望

保持相對隊形不變協(xié)同飛行。 圖6為5架無人機在z方向的位置變化圖， 可以看出在2.5 s后， 從機與主機的高度收斂為一致。

以從機1為例， 分析從機的飛行軌跡。 圖7表示從機1的編隊控制器根據式（18）計算出的u隨時間的變化， 再根據u計算從機1的期望跟蹤位

置。 圖8中表示從機1的實際位置和姿態(tài)角隨時間的變化。 可以看出， 其位置能跟蹤期望位置指令飛行， 其三維姿態(tài)角在0.5 s左右即可達到小角度飛行的穩(wěn)定狀態(tài)。 分析其位置誤差， 如圖9所示， 相

對位置偏差R*1=[50 0]T， 所以零時刻x方向與期望位置的誤差為5 m， y方向與期望位置的誤差為0 m。 從機1啟動后， x方向誤差從5 m逐漸收斂， 在7 s后收斂為0附近。 y方向

誤差從0 m先上升至0.8 m， 在8 s后收斂到0附近。 通過分析可得， 本文中采取的一致性算法能起到

較好的誤差收斂效果。 其他從機的相關誤差特性與從機1類似， 不再贅述。

5 結 論

本文采用二階一致性理論及信息拓撲理論設計微小型四旋翼主從式編隊控制算法， 編隊中的單架四旋翼控制采用PID控制器。 通過Matlab/Simulink仿真驗證了該算法的有效性。 在本文中的算法暫未考慮通信時延、 外部噪聲擾動等不確定性因素， 下一階段將以上因素考慮在模型內， 以提高該算法在實際工程應用中的價值。

參考文獻：

[1] Floreano D， Wood R J. Science， Technology and the Future of Small Autonomous Drones[J]. Nature， 2015， 521（7553）： 460-466.

[2] Roldo V， Cunha R， Cabecinhas D， et al. A Leader-Following Trajectory Generator with Application to Quadrotor Formation Flight[J]. Robotics & Autonomous Systems， 2014， 62（10）： 1597-1609.

[3] Davidi A， Berman N， Arogeti S. Formation Flight Using Multiple Integral Backstepping Controllers[C]∥ IEEE 5th International Conference on Cybernetics and Intelligent Systems （CIS）， 2011： 317-322.

[4] Ren W， Beard R. Decentralized Scheme for Spacecraft Formation Flying via the Virtual Structure Approach[J]. Journal of Guidance， Control， and Dynamics， 2004， 27（1）： 73-82.

[5] Balch T， Arkin R C. Behavior-Based Formation Control for Multirobot Teams[J]. IEEE Transactions on Robotics and Automation， 1998， 14（6）： 926-939.

[6] Xu Dongdong， Zhang Xingnan， Zhu Zhangqing， et al. Behavior-Based Formation Control of Swarm Robots[J]. Mathematical Problems in Engineering， 2014（1）： 1214-1225.

[7] 薛瑞彬， 宋建梅， 張民強. 具有時延及聯(lián)合連通拓撲的多飛行器分布式協(xié)同編隊飛行控制研究[J]. 兵工學報， 2015， 36（3）： 492-502.

Xue Ruibin， Song Jianmei， Zhang Minqiang. Research on Distributed Multi-Vehicle Coordinated Formation Flight Control With Coupling Tine-Delay and Jointy-Connected Topologiex[J]. Acta Armamentarii， 2015， 36（3）： 492-502.（in Chinese）

[8] 周稼康， 胡慶雷， 馬廣富， 等. 基于一致性算法的衛(wèi)星編隊姿軌耦合的協(xié)同控制[J]. 系統(tǒng)工程與電子技術， 2011， 33（4）： 825-832.

Zhou Jiakang， Hu Qinglei， Ma Guangfu， et al. Cooperative Attitude and Translation Controll of Satellite Formation Flying Using Consensus Algorithrn[J]. Systems Engineering and Electroniscs， 2011， 33（4）： 825-832.

（in Chinese）

[9] Dong Xiwang， Yu Bocheng， Shi Zongying， et al. Time-Varying Formation Control for Unmanned Aerial Vehicles： Theories and Applications[J]. IEEE Transactions on Control Systems Technology， 2014， 23（1）： 340-348.

[10] Yu Bocheng， Dong Xiwang， Shi Zongying， et al. Formation Control for Quadrotor Swarm Systems： Algorithms and Experiments[C]∥32nd Chinese Control Conference （CCC）， 2013： 7099-7104.

[11] Yang Xiaogang， Xi Jianxiang， Wu Jinying， et al. Consensus Transformation for Multi-Agent Systems with Topology Variances and Time-Varying Delays[J]. Neurocomputing， 2015， 168： 1059-1064.

[12] Mohd Basri M A， Husain A R， Danapalasingam K A. Stabilization and Trajectory Tracking Control for Underactuated Quadrotor Helicopter Subject to Wind-Gust Disturbance[J]. Sadhana， 2015， 40（5）： 1531-1553.

[13] Mellinger D W. Trajectory Generation and Control for Quadrotors[D]. Philadelphia： University of Pennsylvania， 2012.

[14] Jungnickel D. Graphs， Networks and Algorithms[M]. Springer， 2005.

[15] Ren W， Atkins E. Distributed Multi-Vehicle Coordinated Control via Local Information Exchange[J]. International Journal of Robust and Nonlinear Control， 2007， 17（10-11）： 1002-1033.

Consensus-Based Formation Control Approach for Quadrotor UAVs

Chen Jiemin， Wu Falin， Geng Chenghao， Xu Shan

（School of Instrumentation Science and Opto-Electronics Engineering， Beihang University， Beijing 100191， China） Abstract： Quadrotors are playing an important role in numerous civilian and military applications. In many applications， a team of quadrotors needs to execute a mission while maintaining a specific shape. Compared with conventional system， formation flight can improve the probability of success， increase the anti-interference performance and expand the region of surveillance. In this paper， the quadrotor formation is described as a second-order consensus system based on leader-follower structure and information topology theory， in order to design a formation controller for steady formation flight. The leader and the follower quadrotors are controlled by PID laws. The leader follows the desired trajectory while the followers follow the instruction calculated by the formation controller. A simulation is carried out to analyze the control effect of formation gathering and keeping of quadrotors with control algorithm.

Key words： quadrotor UAVs； formation flight； information topology theory； consensus-based theory； leader-follower formation