戴邵武，趙超輪，趙國(guó)榮，劉伯彥，羅鑫輝

(海軍航空大學(xué)岸防兵學(xué)院，煙臺(tái) 264001)

0 引言

近年來(lái)，無(wú)人機(jī)行業(yè)發(fā)展迅速，在軍用和民用領(lǐng)域得到了廣泛應(yīng)用。單架無(wú)人機(jī)由于固有的局限性，無(wú)法完成復(fù)雜的任務(wù)，人們便希望通過(guò)多無(wú)人機(jī)間的協(xié)調(diào)、合作，以產(chǎn)生整體大于部分之和的效果。其中，編隊(duì)控制問(wèn)題是多無(wú)人機(jī)系統(tǒng)協(xié)同合作的基礎(chǔ)和難點(diǎn)，也是研究的熱點(diǎn)。根據(jù)信息交互控制策略的不同，多無(wú)人機(jī)協(xié)同控制方式主要分為集中式控制和分布式控制[1-2]。其中，集中式控制策略要求編隊(duì)中至少有1架無(wú)人機(jī)能夠獲取其他所有無(wú)人機(jī)的狀態(tài)信息，通過(guò)集中運(yùn)算得到每架無(wú)人機(jī)的控制指令并發(fā)送給其他無(wú)人機(jī)，該策略控制精度高且易于實(shí)現(xiàn)，但編隊(duì)規(guī)模受限；分布式控制策略則僅需要獲取相鄰幾架無(wú)人機(jī)的狀態(tài)信息便可實(shí)現(xiàn)編隊(duì)，適用于大規(guī)模編隊(duì)，但全局性不強(qiáng)。在具體編隊(duì)控制方法方面，目前主流的方法有領(lǐng)航跟隨法[3]、虛擬結(jié)構(gòu)法[4]、基于行為法、人工勢(shì)場(chǎng)法以及基于一致性理論的方法[5]。以上編隊(duì)控制方法各有優(yōu)劣，適用于不同的任務(wù)需求，也常組合使用以達(dá)到期望的編隊(duì)控制效果[6]。本文采用的集群空間控制方法(Cluster Space Control Method)并非主流控制方法，是由美國(guó)圣塔克拉拉大學(xué)機(jī)器人系統(tǒng)實(shí)驗(yàn)室提出的一種適用于多移動(dòng)機(jī)器人編隊(duì)控制的理論[7]。該方法的突出優(yōu)勢(shì)是編隊(duì)參考點(diǎn)的位置可任意配置，允許編隊(duì)結(jié)構(gòu)全自由度變化，控制精度較高。該方法目前已成功應(yīng)用于無(wú)人車[8]和飛艇[9]等多智能體編隊(duì)控制中，但將該方法應(yīng)用到四旋翼上的研究文獻(xiàn)很少，國(guó)內(nèi)則幾乎沒(méi)有關(guān)于集群空間控制的文獻(xiàn)。

四旋翼無(wú)人機(jī)因可垂直起降、定點(diǎn)懸停、機(jī)動(dòng)靈活等優(yōu)勢(shì)，近年來(lái)備受關(guān)注，應(yīng)用前景廣闊。由于將集群空間控制方法應(yīng)用到四旋翼上的研究還不深入，可供參考的文獻(xiàn)很少，且考慮到對(duì)于無(wú)人機(jī)編隊(duì)控制問(wèn)題，通常先以實(shí)現(xiàn)二維平面編隊(duì)作為基礎(chǔ)[10]，一般選用2～4架無(wú)人機(jī)來(lái)驗(yàn)證所設(shè)計(jì)控制算法的有效性[11-12]。因此，本文研究了二維平面上的四旋翼編隊(duì)控制問(wèn)題，并以3架無(wú)人機(jī)為例說(shuō)明算法的有效性。文獻(xiàn)[8]應(yīng)用集群空間控制方法，設(shè)置平面編隊(duì)的參考原點(diǎn)位置位于3個(gè)機(jī)器人的中心，而本文將編隊(duì)參考原點(diǎn)位置與其中1架無(wú)人機(jī)位置重合，使編隊(duì)結(jié)構(gòu)更為簡(jiǎn)單，以減少運(yùn)算量。

對(duì)于單機(jī)控制，基于四旋翼動(dòng)力學(xué)模型，采用內(nèi)外環(huán)結(jié)構(gòu)和經(jīng)典串級(jí)PID控制實(shí)現(xiàn)位姿控制；對(duì)于編隊(duì)控制，采用集中控制策略，在集群空間控制框架下建立編隊(duì)整體與單機(jī)的運(yùn)動(dòng)學(xué)關(guān)系，集群控制指令由不完全微分PD控制器產(chǎn)生，相較常規(guī)PD控制，其抗高頻干擾能力較強(qiáng)。

1 集群空間控制方法

集群空間控制是一種能夠指定、控制、監(jiān)控多移動(dòng)機(jī)器人編隊(duì)的理論[7]。集群空間控制的本質(zhì)是將n個(gè)機(jī)器人系統(tǒng)視為一個(gè)實(shí)體，或稱作一個(gè)集群，并用位置量、方位量、與編隊(duì)形狀相關(guān)的量等來(lái)表示集群屬性。在設(shè)計(jì)編隊(duì)控制器前，需先定義兩組狀態(tài)變量，分別為集群變量和單機(jī)變量。前者是基于集群屬性選擇的一組適合指定、控制、監(jiān)控編隊(duì)狀態(tài)的變量；后者是基于機(jī)器人單機(jī)屬性選擇的一組狀態(tài)變量。然后通過(guò)運(yùn)動(dòng)學(xué)轉(zhuǎn)換建立集群變量與單機(jī)變量的聯(lián)系。

1.1 集群空間變量

考慮由n個(gè)機(jī)器人組成的系統(tǒng)，假設(shè)每個(gè)機(jī)器人具有m個(gè)自由度，其中平移自由度為p，旋轉(zhuǎn)自由度為r，即m=p+r。則n-機(jī)器人系統(tǒng)中所有單機(jī)相對(duì)于慣性坐標(biāo)系的位置及旋轉(zhuǎn)角度可由nm個(gè)變量來(lái)完整表示。換言之，完整描述n-機(jī)器人系統(tǒng)的狀態(tài)至少需要nm個(gè)變量。假設(shè)n個(gè)機(jī)體坐標(biāo)系分別由obixbiybizbi，i=1,2,…,n表示，慣性坐標(biāo)系由odxdydzd表示，編隊(duì)坐標(biāo)系由ocxcyczc表示。

對(duì)于六自由度n-機(jī)器人系統(tǒng)，機(jī)體坐標(biāo)系相對(duì)慣性坐標(biāo)系的狀態(tài)可由(xi,yi,zi,φi,θi,ψi)表示，編隊(duì)坐標(biāo)系相對(duì)慣性坐標(biāo)系的狀態(tài)可由(xc,yc,zc,φc,θc,ψc)表示，其中x、y、z表示三維位置，φ、θ、ψ表示3個(gè)姿態(tài)角。通過(guò)上述分析可知，要完整表示六自由度n-機(jī)器人系統(tǒng)的編隊(duì)狀態(tài)，至少需要6n個(gè)變量，本文將描述編隊(duì)狀態(tài)的變量稱為集群變量，并用ci,i=1,2,…，6n表示，集群變量與(xi,yi,zi,φi,θi,ψi)和(xc,yc,zc,φc,θc,ψc)存在一定的函數(shù)關(guān)系，即

c1=f1(xc,yc,zc,φc,θc,ψc,x1,y1,z1,φ1,

θ1,ψ1,…,xn,yn,zn,φn,θn,ψn)

c2=f2(xc,yc,zc,φc,θc,ψc,x1,y1,z1,φ1,

θ1,ψ1,…,xn,yn,zn,φn,θn,ψn)

?

c6n=f6n(xc,yc,zc,φc,θc,ψc,x1,y1,z1,φ1,

θ1,ψ1,…,xn,yn,zn,φn,θn,ψn)

(1)

集群變量的選取根據(jù)不同任務(wù)需求會(huì)有差異，一般包括編隊(duì)位置、編隊(duì)方位、單機(jī)與編隊(duì)的相對(duì)方位、編隊(duì)形狀4個(gè)方面。編隊(duì)位置用于表示編隊(duì)相對(duì)慣性系的位置，在單機(jī)自由度均相同的前提下，編隊(duì)位置量的數(shù)量可用p表示；編隊(duì)方位用于表示編隊(duì)坐標(biāo)系相對(duì)慣性坐標(biāo)系的方位關(guān)系，編隊(duì)方位量的數(shù)量可用o表示，當(dāng)n≥p=3時(shí)，則o=3，否則，o=p-1；單機(jī)與編隊(duì)的相對(duì)方位量表示每個(gè)單機(jī)相對(duì)編隊(duì)系的方位，用nr個(gè)變量便可構(gòu)成；編隊(duì)形狀用于描述編隊(duì)的幾何關(guān)系，編隊(duì)形狀量的數(shù)量用s表示，假設(shè)共有nm個(gè)集群變量來(lái)描述編隊(duì)的狀態(tài)，則s=nm-p-o-nr=(n-1)p-o。一般從以上4個(gè)方面選取集群變量，便能夠清晰、直觀、全面地刻畫(huà)編隊(duì)的狀態(tài)。

1.2 集群運(yùn)動(dòng)學(xué)關(guān)系

因?yàn)榫庩?duì)控制的指令最終要發(fā)送給每個(gè)單機(jī)去執(zhí)行，所以需要建立集群變量與單機(jī)變量的運(yùn)動(dòng)學(xué)關(guān)系。

(1)位置運(yùn)動(dòng)學(xué)

定義集群變量構(gòu)成的向量為C，所有單機(jī)變量構(gòu)成的向量為R，并分別稱為集群位置向量和單機(jī)位置向量。兩者均為nm維列向量，兩者關(guān)系如下

(2)

(3)

稱式(2)為正向位置運(yùn)動(dòng)學(xué)關(guān)系式，式(3)為逆向位置運(yùn)動(dòng)學(xué)關(guān)系式，以此建立集群變量與單機(jī)變量的聯(lián)系。

(2)速度運(yùn)動(dòng)學(xué)

(4)

其中

(5)

式(4)實(shí)現(xiàn)了從單機(jī)速度向量到集群速度向量的映射。反過(guò)來(lái)，有

(6)

其中，J-1(C)為逆雅克比矩陣，為

中糧寧夏年產(chǎn)2萬(wàn)t葡萄酒及配套種植基地項(xiàng)目的建設(shè)內(nèi)容，符合我國(guó)釀酒行業(yè) “糧食酒向水果酒轉(zhuǎn)變”的產(chǎn)業(yè)政策要求，是國(guó)家發(fā)展和改革委員 《產(chǎn)業(yè)結(jié)構(gòu)調(diào)整指導(dǎo)目錄（2011年本）》中鼓勵(lì)的農(nóng)產(chǎn)品基地建設(shè)和生態(tài)系統(tǒng)恢復(fù)與重建工程，同時(shí)也符合寧夏發(fā)展葡萄產(chǎn)業(yè)、建設(shè)優(yōu)質(zhì)釀酒葡萄種植基地的戰(zhàn)略部署。

(7)

可見(jiàn)，隨著單機(jī)數(shù)量和自由度的增加，集群變量會(huì)相應(yīng)增加，運(yùn)動(dòng)學(xué)關(guān)系式會(huì)變得相對(duì)復(fù)雜，必然導(dǎo)致機(jī)間交互的信息量增大。故文獻(xiàn)[7]中提到，在現(xiàn)有技術(shù)程度上，若采用集中控制策略，在該控制方法下編隊(duì)機(jī)器人的數(shù)量規(guī)模大約在10個(gè)以內(nèi)。

2 四旋翼編隊(duì)控制系統(tǒng)設(shè)計(jì)

首先建立四旋翼單機(jī)模型,然后進(jìn)行編隊(duì)控制器設(shè)計(jì)。

2.1 四旋翼模型

本文采用十字形四旋翼，如圖1所示。四旋翼是一個(gè)非線性、多變量、高度耦合、欠驅(qū)動(dòng)系統(tǒng)[13]。假設(shè)四旋翼是剛體結(jié)構(gòu)，機(jī)體幾何對(duì)稱、質(zhì)量對(duì)稱，且?guī)缀沃行呐c重心重合。

圖1 十字形四旋翼Fig.1 Cross-shaped quadrotor

首先建立兩個(gè)右手坐標(biāo)系，即慣性北-東-地坐標(biāo)系及機(jī)體坐標(biāo)系，如圖2所示。其中，慣性坐標(biāo)系odxdydzd以地面任意一點(diǎn)為坐標(biāo)原點(diǎn)，odxd軸指向北，odzd軸垂直地面向下；機(jī)體坐標(biāo)系obxbybzb與四旋翼機(jī)體固連，以機(jī)體重心位置為坐標(biāo)原點(diǎn)，obxb軸指向機(jī)頭方向，obzb軸在機(jī)體對(duì)稱平面內(nèi)，垂直于obxb軸向下。

圖2 慣性坐標(biāo)系與機(jī)體坐標(biāo)系Fig.2 Inertial and body coordinate system

(8)

其中，cθ、sθ分別表示cosθ、sinθ，此規(guī)則同樣適用于ψ、φ。

假設(shè)四旋翼在飛行過(guò)程中，俯仰角和滾轉(zhuǎn)角都很小，總拉力約等于四旋翼的重力，則有下列近似：sinφ≈φ,cosφ≈1,sinθ≈θ,cosθ≈1,f≈m0g。在忽略陀螺效應(yīng)、-ω×(J·ω)項(xiàng)及空氣阻力的情況下，采用牛頓-歐拉法可以得到水平通道與高度通道相互解耦的四旋翼動(dòng)力學(xué)模型[14]為

(9)

四旋翼飛行器控制采用內(nèi)外環(huán)結(jié)構(gòu)，內(nèi)環(huán)控制姿態(tài)，外環(huán)控制位置，內(nèi)外環(huán)均使用經(jīng)典串級(jí)PID控制[14]。

2.2 編隊(duì)控制器

本文以3架四旋翼無(wú)人機(jī)為例，設(shè)計(jì)了相應(yīng)的集群控制算法，實(shí)現(xiàn)了二維平面內(nèi)的編隊(duì)控制。圖3所示為相關(guān)的參考直角坐標(biāo)系，分別為慣性坐標(biāo)系odxdyd、編隊(duì)坐標(biāo)系ocxcyc、機(jī)體坐標(biāo)系obixbiybi，i=1,2,3。后文中將機(jī)體坐標(biāo)系簡(jiǎn)記為oixiyi。受領(lǐng)航跟隨法啟發(fā)，編隊(duì)坐標(biāo)系是以無(wú)人機(jī)1的中心位置為坐標(biāo)原點(diǎn)，無(wú)人機(jī)2到無(wú)人機(jī)1中心位置連接線的延長(zhǎng)線為ocxc軸，其垂線為ocyc軸；機(jī)體坐標(biāo)系以無(wú)人機(jī)機(jī)體中心為坐標(biāo)原點(diǎn)，機(jī)頭方向?yàn)閛ixi軸，其垂線為oiyi軸。3架無(wú)人機(jī)的幾何關(guān)系如圖4所示。

圖3 平面3架無(wú)人機(jī)的參考坐標(biāo)系Fig.3 Reference frames for three UAVs on the plan

圖4 3架無(wú)人機(jī)的幾何關(guān)系Fig.4 Geometric relation of three UAVs

基于上述坐標(biāo)系，為描述編隊(duì)及單機(jī)屬性，按照1.1節(jié)所述規(guī)則，首先選擇一組集群變量C和一組單機(jī)變量R

C=(xc,yc,θc,φ1,φ2,φ3,p,q,β)T

(10)

R=(x1,y1,θ1,x2,y2,θ2,x3,y3,θ3)T

(11)

其中，(xc,yc)為編隊(duì)參考點(diǎn)在慣性系下的二維位置，由于編隊(duì)參考點(diǎn)與無(wú)人機(jī)1重合，無(wú)人機(jī)1相當(dāng)于編隊(duì)的領(lǐng)航者。(xi,yi)為無(wú)人機(jī)i在慣性系下的二維位置坐標(biāo)，定義無(wú)人機(jī)i相對(duì)慣性系的偏航角θi為odxd軸到oixi軸的夾角；定義編隊(duì)基準(zhǔn)角θc為odxd軸到ocxc軸的夾角；無(wú)人機(jī)i相對(duì)編隊(duì)坐標(biāo)系的偏航角φi為ocxc軸到oixi軸的夾角;無(wú)人機(jī)1到2的距離為p，無(wú)人機(jī)1到3的距離為q;以無(wú)人機(jī)1為三角形頂點(diǎn)的角為β。

3架無(wú)人機(jī)的通信拓?fù)鋱D如圖5所示。無(wú)人機(jī)2和3將自身狀態(tài)信息發(fā)送給無(wú)人機(jī)1，無(wú)人機(jī)1對(duì)獲取的所有無(wú)人機(jī)的信息進(jìn)行集中處理,解算得到各無(wú)人機(jī)的控制指令后，再發(fā)送給其他無(wú)人機(jī)。

圖5 3架無(wú)人機(jī)的通信拓?fù)鋱DFig.5 Communication topology of three UAVs

基于上述選擇的集群和單機(jī)狀態(tài)變量，下面給出了編隊(duì)系統(tǒng)的正向及逆向位置運(yùn)動(dòng)學(xué)關(guān)系式。正向位置運(yùn)動(dòng)學(xué)方程為C=KIN(R)，展開(kāi)為式(12)

(12)

逆向位置運(yùn)動(dòng)學(xué)方程為R=INKIN(C)，展開(kāi)為式(13)

(13)

(14)

圖6 編隊(duì)控制框圖Fig.6 Formation control architecture

出于簡(jiǎn)化的目的，同時(shí)考慮四旋翼特點(diǎn)，可設(shè)定在飛行過(guò)程中，無(wú)人機(jī)單機(jī)期望偏航角始終為恒定值，由單機(jī)控制器直接實(shí)現(xiàn)自身偏航角控制。因此，集群變量由9個(gè)簡(jiǎn)化為6個(gè)，即C=(xc,yc,θc,p,q,β)T。相應(yīng)的單機(jī)變量簡(jiǎn)化為R=(x1,y1,x2,y2,x3,y3)T。

考慮到無(wú)人機(jī)在飛行過(guò)程中存在隨機(jī)擾動(dòng)，而常規(guī)PD控制中的微分項(xiàng)易引進(jìn)高頻干擾，使控制指令出現(xiàn)突跳，對(duì)控制系統(tǒng)不利。若在控制器中加入低通濾波器，則可明顯抑制高頻干擾。因此，編隊(duì)控制器采用不完全微分PD控制器[15]，控制器結(jié)構(gòu)如圖7所示。

圖7 不完全微分PD控制器Fig.7 Incomplete differential PD controller

最終，集群變量誤差值通過(guò)編隊(duì)控制器和運(yùn)動(dòng)學(xué)轉(zhuǎn)換得到單機(jī)控制指令，進(jìn)而發(fā)送給單機(jī)控制器，作為單機(jī)x、y通道的速度指令。

3 仿真分析

利用MATLAB對(duì)3架四旋翼無(wú)人機(jī)的編隊(duì)控制算法進(jìn)行仿真，驗(yàn)證其編隊(duì)控制的性能。編隊(duì)控制器參數(shù)如表1所示。

表1 不完全微分PD控制器參數(shù)

假設(shè)3架無(wú)人機(jī)均在5m高度懸停，無(wú)人機(jī)1、2、3的初始二維位置分別為(5,5)、(3,3)、(3,7)，期望的編隊(duì)狀態(tài)為

在3架無(wú)人機(jī)的實(shí)際位置輸出端分別加入幅值為0.05、0.1、0.1，采樣時(shí)間均為0.5s的隨機(jī)干擾，仿真時(shí)間30s，仿真步長(zhǎng)0.02s。圖8(a)為編隊(duì)飛行的二維軌跡，其中在0s、9s、15s、30s時(shí)刻對(duì)無(wú)人機(jī)的位置做了標(biāo)記，無(wú)人機(jī)1、2、3分別用五角星、圓形和菱形表示?？梢?jiàn)在飛行過(guò)程中，3架無(wú)人機(jī)能夠形成穩(wěn)定的三角形編隊(duì)，且相對(duì)位置唯一確定，能夠較好地實(shí)現(xiàn)隊(duì)形放大和隊(duì)形保持任務(wù)。

由圖8(b)可見(jiàn)，領(lǐng)航者無(wú)人機(jī)1的y方向跟蹤誤差控制在1.3m以內(nèi)，x方向跟蹤誤差在1.1m以內(nèi)，此位置誤差實(shí)質(zhì)與單機(jī)控制器有關(guān)；在隊(duì)形放大過(guò)程中，距離量p、q的誤差在0.9m以內(nèi)；在隊(duì)形保持過(guò)程中，距離量p、q的誤差小于0.12m，隊(duì)形保持效果較好。圖8(c)中，角度θc的誤差小于0.015rad，β的誤差小于0.04rad。圖8(d)中，vxc和vyc表示發(fā)送給無(wú)人機(jī)的速度指令，數(shù)字1、2、3對(duì)應(yīng)無(wú)人機(jī)編號(hào)，在5s時(shí)進(jìn)行隊(duì)形放大，可見(jiàn)速度指令進(jìn)行了合理調(diào)整，在有隨機(jī)信號(hào)的干擾下，指令曲線較為平滑。

圖9所示為在常規(guī)PD編隊(duì)控制下生成的速度指令曲線，與圖8(d)對(duì)比易見(jiàn)，引入不完全微分能夠明顯抑制隨機(jī)擾動(dòng)對(duì)系統(tǒng)帶來(lái)的影響。

(a)編隊(duì)飛行軌跡

(b)位置、距離誤差曲線

(c) 角度誤差曲線

(d)3架無(wú)人機(jī)的速度指令圖8 不完全微分PD編隊(duì)控制仿真圖Fig.8 Incomplete differential PD formation control simulation diagram

圖9 常規(guī)PD編隊(duì)控制下的速度指令Fig.9 Velocity commands of conventional PD formation control

4 結(jié)束語(yǔ)

本文主要介紹了集群空間控制方法的概念，然后將該方法應(yīng)用到四旋翼編隊(duì)控制中，并以3架四旋翼為例，驗(yàn)證了所設(shè)計(jì)編隊(duì)控制算法的有效性。在考慮存在隨機(jī)擾動(dòng)的情況下，設(shè)計(jì)了不完全微分PD控制器以產(chǎn)生集群控制指令，實(shí)現(xiàn)了編隊(duì)放大與隊(duì)形保持任務(wù)。結(jié)果表明，該控制算法能夠?qū)崿F(xiàn)多架四旋翼無(wú)人機(jī)的編隊(duì)控制，清晰完整地描述指定編隊(duì)狀態(tài)，隊(duì)形保持的精度較高，與常規(guī)PD控制相比，具有較強(qiáng)的抗高頻干擾能力。