一種基于單位四元數(shù)的四旋翼編隊(duì)反演控制方法

李　磊，李小民，楊　森

(軍械工程學(xué)院 無(wú)人機(jī)工程系，石家莊　050000)

?

一種基于單位四元數(shù)的四旋翼編隊(duì)反演控制方法

李磊，李小民，楊森

(軍械工程學(xué)院 無(wú)人機(jī)工程系，石家莊050000)

摘要：針對(duì)四旋翼無(wú)人飛行器，研究了在理想通信條件下編隊(duì)控制問(wèn)題;四旋翼無(wú)人飛行器具有復(fù)雜的數(shù)學(xué)模型，首先用四元數(shù)描述其動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)模型，將其分解為位置和姿態(tài)兩個(gè)相互獨(dú)立的子系統(tǒng)，通過(guò)引入與期望軌跡之間的誤差建立了跟蹤誤差模型；指定編隊(duì)中一名成員為領(lǐng)航者，編隊(duì)成員通過(guò)一致性算法得到編隊(duì)的幾何中心位置，并以此作為期望軌跡;通過(guò)Backstepping方法為每一架四旋翼設(shè)計(jì)時(shí)變反饋控制律使編隊(duì)達(dá)到鎮(zhèn)定；最后通過(guò)仿真實(shí)驗(yàn)驗(yàn)證該控制方法的有效性。

關(guān)鍵詞：編隊(duì)控制； 四旋翼無(wú)人機(jī)； 四元數(shù)； 反演法； 領(lǐng)航跟隨法

0引言

近年來(lái)，小型無(wú)人機(jī)發(fā)展迅猛，四旋翼無(wú)人飛行器由于結(jié)構(gòu)簡(jiǎn)單、易于控制，受到研究者們的廣泛關(guān)注。四旋翼無(wú)人機(jī)能夠?qū)崿F(xiàn)垂直起降、自主懸停等功能，在不確定環(huán)境以及危險(xiǎn)環(huán)境中能夠高效的執(zhí)行任務(wù)。無(wú)人機(jī)編隊(duì)通過(guò)多無(wú)人機(jī)的協(xié)同飛行使系統(tǒng)的綜合效能大大提高。隨著任務(wù)日趨復(fù)雜，無(wú)人機(jī)數(shù)量逐漸增多，對(duì)編隊(duì)控制提出了更多更高的要求。無(wú)人機(jī)編隊(duì)控制是多無(wú)人機(jī)協(xié)同控制的重要研究方向。

編隊(duì)控制的研究方法主要有領(lǐng)航跟隨法、虛擬結(jié)構(gòu)法、基于行為法和圖論法[1]。目前較為主流的是將上述方法融合使用，取長(zhǎng)補(bǔ)短。早期的編隊(duì)控制主要采用集中式控制方法，其特點(diǎn)是精度高、便于控制，但是要依賴于中央控制單元的計(jì)算能力和全局通信能力，隨著編隊(duì)成員的增加，中央控制單元的計(jì)算量呈指數(shù)增加，缺乏可拓展性和靈活性。后來(lái)人們提出分布式控制方法，每架無(wú)人機(jī)只與相鄰的無(wú)人機(jī)進(jìn)行通信，利用無(wú)人機(jī)自身的計(jì)算能力，得到周邊無(wú)人機(jī)的相對(duì)位置關(guān)系，對(duì)比期望的編隊(duì)隊(duì)形，對(duì)無(wú)人機(jī)的實(shí)際位置進(jìn)行修正進(jìn)而消除隊(duì)形誤差。

近年來(lái)，國(guó)內(nèi)外諸多學(xué)者針對(duì)四旋翼飛行器編隊(duì)問(wèn)題做了大量研究[2-5]，設(shè)計(jì)出不同類型的控制器。文獻(xiàn)[2-5]通過(guò)反饋線性化、小擾動(dòng)線性化等方法對(duì)四旋翼飛行器模型進(jìn)行了不同程度的簡(jiǎn)化來(lái)設(shè)計(jì)控制器。但是四旋翼飛行器是一種典型的欠驅(qū)動(dòng)系統(tǒng)，是具有復(fù)雜約束方程的級(jí)聯(lián)非完整系統(tǒng)[7],具有很強(qiáng)的非線性，對(duì)控制系統(tǒng)提出了更高的要求。文獻(xiàn)[6]同樣采用單位四元數(shù)對(duì)四旋翼進(jìn)行建模，并且引入流形的概念，在微分幾何框架下設(shè)計(jì)了姿態(tài)控制算法，取得了不錯(cuò)的效果。文獻(xiàn)[7]利用神經(jīng)網(wǎng)絡(luò)在線辨識(shí)系統(tǒng)的非線性部分，結(jié)合領(lǐng)航跟隨法實(shí)現(xiàn)了四旋翼的編隊(duì)控制。文獻(xiàn)[8]用四元數(shù)描述四旋翼的動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)模型，引入中間控制量，通過(guò)為每一架無(wú)人機(jī)設(shè)計(jì)合適的中間控制量使得編隊(duì)達(dá)成鎮(zhèn)定。文獻(xiàn)[9]引入實(shí)際位姿與期望位姿間的誤差，建立了跟蹤誤差模型，并將四旋翼模型分解為位置和姿態(tài)兩個(gè)互相獨(dú)立的子系統(tǒng)，并分別利用Backstepping方法設(shè)計(jì)了時(shí)變反饋控制律使系統(tǒng)穩(wěn)定。Backstepping方法是一種由前向后遞推的設(shè)計(jì)方法，通過(guò)逐步迭代設(shè)計(jì)Lyapunov函數(shù)使系統(tǒng)誤差具有漸進(jìn)性，并使系統(tǒng)最終實(shí)現(xiàn)全局穩(wěn)定，降低了設(shè)計(jì)難度。本文在文獻(xiàn)[9]的基礎(chǔ)上，通過(guò)一致性算法得到編隊(duì)幾何中心位置，并將其作為期望軌跡，為每一架四旋翼設(shè)計(jì)反演控制律使整個(gè)編隊(duì)達(dá)到鎮(zhèn)定。

1單位四元數(shù)基礎(chǔ)

單位四元數(shù)是模恒為一的四元數(shù)，即有

(1)

給定一個(gè)單位四元數(shù)，其對(duì)應(yīng)的姿態(tài)矩陣與單位四元數(shù)之間有如下關(guān)系：

(2)

(3)

定義實(shí)際姿態(tài)矩陣R與期望姿態(tài)矩陣Rd間的誤差為

(4)

(5)

2四旋翼系統(tǒng)原理及模型

四旋翼無(wú)人機(jī)通常分為“X”型和“+”型，具有4個(gè)輸入6個(gè)輸出，是典型的欠驅(qū)動(dòng)系統(tǒng)。四旋翼通過(guò)控制4個(gè)獨(dú)立的電機(jī)和螺旋槳的轉(zhuǎn)速來(lái)實(shí)現(xiàn)飛機(jī)的升降、俯仰、滾轉(zhuǎn)和偏航等運(yùn)動(dòng)。四元數(shù)是描述三維空間中剛體轉(zhuǎn)動(dòng)和移動(dòng)的簡(jiǎn)潔有效的數(shù)學(xué)工具，可以有效避免奇異點(diǎn)的出現(xiàn)，并且具有計(jì)算高效等特點(diǎn)。將四旋翼無(wú)人機(jī)看做剛體結(jié)構(gòu)，假設(shè)其重心位于機(jī)體坐標(biāo)系原點(diǎn)，電機(jī)無(wú)安裝誤差角，且電機(jī)升力面與飛機(jī)質(zhì)心位于同一平面。利用四元數(shù)對(duì)四旋翼動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)建模，將四旋翼分解為位置子系統(tǒng)∑1和姿態(tài)子系統(tǒng)∑2，得到以下模型：

(6)

(7)

3控制器設(shè)計(jì)及收斂性分析

3．1問(wèn)題描述

考慮無(wú)人機(jī)編隊(duì)有N+1個(gè)成員,指定編號(hào)為1的無(wú)人機(jī)為編隊(duì)領(lǐng)航者。假設(shè)每架無(wú)人機(jī)都可以通過(guò)自身的傳感器和無(wú)線通信網(wǎng)絡(luò)獲取自身和編隊(duì)其他成員的狀態(tài)信息。根據(jù)無(wú)人機(jī)之間信息交互關(guān)系可以將無(wú)人機(jī)編隊(duì)建模成圖的形式。假設(shè)無(wú)人機(jī)通信拓?fù)鋱D為無(wú)向連通圖，如圖(1)所示，在編隊(duì)開(kāi)始時(shí)，各編隊(duì)成員通過(guò)無(wú)線通信網(wǎng)絡(luò)獲取自身和編隊(duì)其他成員狀態(tài)信息后，通過(guò)一致性算法協(xié)商得出編隊(duì)期望的位置中心和期望速度，并將其輸出到位姿控制器，驅(qū)動(dòng)執(zhí)行元件使無(wú)人機(jī)向預(yù)定編隊(duì)中心靠攏聚集，達(dá)到指定間距后，完成編隊(duì)任務(wù)。

圖1　四旋翼編隊(duì)示意圖

不考慮編隊(duì)中的四旋翼的差異性，即四旋翼為同構(gòu)系統(tǒng)，且都符合模型(6)，即

(8)

(9)

(10)

(11)

本文要做的工作是為第i架四旋翼無(wú)人機(jī)設(shè)計(jì)虛擬控制量升力Ti和控制力矩τi，使四旋翼編隊(duì)保持固定隊(duì)形和固定距離，即使得

vi→vj→vd，且ξi-ξj→δi-δj=δij

式中vd為編隊(duì)參考的期望速度，δij為第i架無(wú)人機(jī)和第j架無(wú)人機(jī)之間的期望距離，并且滿足δij=-δji。

3．2控制器設(shè)計(jì)

根據(jù)級(jí)聯(lián)系統(tǒng)分析方法將四旋翼誤差系統(tǒng)模型分解為位置誤差子系統(tǒng)和姿態(tài)誤差子系統(tǒng)[10]

(12)

(13)

下面參考文獻(xiàn)利用BackStepping方法分別設(shè)計(jì)位置子系統(tǒng)控制器和姿態(tài)子系統(tǒng)控制器.

首先，定義系統(tǒng)狀態(tài)與虛擬反饋之間的誤差

(14)

式中，α1為虛擬控制量。現(xiàn)對(duì)每一個(gè)虛擬反饋定義一個(gè)V函數(shù)，使每一個(gè)狀態(tài)分量具有適當(dāng)?shù)臐u進(jìn)性，式(14)本質(zhì)上是一個(gè)微分同胚，為了鎮(zhèn)定原系統(tǒng)，只需鎮(zhèn)定原系統(tǒng)狀態(tài)與虛擬反饋量之間的誤差即可。

第1步：對(duì)z1求時(shí)間導(dǎo)數(shù)，得

(15)

(16)

顯然，如果z2=0，由上式知z1漸進(jìn)穩(wěn)定，但一般情況下z2≠0，為此需要進(jìn)行下一步設(shè)計(jì)，使z2具有期望的漸進(jìn)特性。

V2關(guān)于時(shí)間的導(dǎo)數(shù)為:

(17)

(18)

利用同樣的方法為姿態(tài)子系統(tǒng)設(shè)計(jì)控制律使其全局指數(shù)穩(wěn)定，定義姿態(tài)子系統(tǒng)系統(tǒng)狀態(tài)與虛擬反饋之間的誤差

(19)

其中：α2為虛擬控制量。

編隊(duì)控制中主要關(guān)心飛行器的位置控制子系統(tǒng)。因此，下面直接給出四旋翼無(wú)人機(jī)i的姿態(tài)子系統(tǒng)反演控制律，具體推導(dǎo)過(guò)程參考文獻(xiàn)[10]。

(20)

(21)

(22)

這就將編隊(duì)問(wèn)題轉(zhuǎn)化成給定參考信號(hào)的軌跡跟蹤問(wèn)題，式中Ni為編隊(duì)中與第i架無(wú)人機(jī)為鄰居的飛行器的數(shù)量?？紤]編隊(duì)通信拓?fù)錇闊o(wú)向全通圖時(shí)的情形，此時(shí)除領(lǐng)航者外，編隊(duì)中任意飛行器具有同等地位，因此可以忽略下標(biāo)i，將式(21)和式(22)改寫成如下形式：

(23)

通過(guò)反演控制律可以保證編隊(duì)中第i架無(wú)人機(jī)ξi→ξd+δi，vi→vd，則可以保證ξi-ξj→δi-δj=δij，vi→vj。

4實(shí)驗(yàn)仿真

4．1參數(shù)設(shè)置

不考慮四旋翼系統(tǒng)之間存在的差異，假設(shè)系統(tǒng)質(zhì)量m=0．6 kg，轉(zhuǎn)動(dòng)慣量分別為Jx=Jy=0．2 kg·m2，Jz=0．04 kg·m2。設(shè)置編隊(duì)規(guī)模N=5，k1=10，k2=10，指定編號(hào)為5的四旋翼為領(lǐng)航者，其余四架為跟隨者，假設(shè)領(lǐng)航者向外廣播自身的狀態(tài)信息，跟隨者均能實(shí)時(shí)接收，并且跟隨者之間的通信拓?fù)錇闊o(wú)向連通圖。跟隨者初始位置矩陣P0為:

領(lǐng)航者在固定高度做勻速圓周運(yùn)動(dòng)，其運(yùn)動(dòng)軌跡為：

用來(lái)描述編隊(duì)隊(duì)形的相對(duì)位置偏差為:

仿真時(shí)間為9．5s，仿真步長(zhǎng)為0．001s。

4．2仿真結(jié)果

根據(jù)以上參數(shù)，用MATLAB中Simulink模塊搭建系統(tǒng)模型，并對(duì)控制算法進(jìn)行驗(yàn)證。圖2為編隊(duì)仿真的三維軌跡圖。圖中黑色曲線為領(lǐng)航者的運(yùn)動(dòng)軌跡，三角形“△”表示每架四旋翼無(wú)人機(jī)的最終位置。仿真開(kāi)始時(shí)，領(lǐng)航者在空中做圓周運(yùn)動(dòng)，跟隨者在地面的不同位置，起飛后接收領(lǐng)航者的狀態(tài)信息，首先編隊(duì)成員通過(guò)協(xié)商計(jì)算得到編隊(duì)中心位置，并以此作為參考跟蹤信號(hào)，算法式(18)最終使五架四旋翼收斂至預(yù)定編隊(duì)隊(duì)形。通過(guò)設(shè)置不同的相對(duì)位置誤差可以方便地設(shè)計(jì)各種隊(duì)形。

圖2　四旋翼編隊(duì)軌跡仿真曲線

圖3　X方向無(wú)人機(jī)位置與速度變化曲線

圖3~圖5分別從XYZ三個(gè)方向顯示了機(jī)群成員在編隊(duì)形成和機(jī)動(dòng)過(guò)程中位置和速度的變化規(guī)律。從圖中可以看出，跟隨者在起飛后迅速向領(lǐng)航者靠攏，在t=1s時(shí)跟隨者距離領(lǐng)航者趨于指定位置，并且與領(lǐng)航者保持相同的速度，能夠很好地跟蹤領(lǐng)航者的軌跡變化，驗(yàn)證了本文所提編隊(duì)策略的有效性。

圖4　Y方向無(wú)人機(jī)位置與速度變化曲線

圖5　Z方向無(wú)人機(jī)位置與速度變化曲線

圖6為編隊(duì)成員協(xié)商計(jì)算得出的編隊(duì)中心軌跡與領(lǐng)航者軌跡之間的誤差，可見(jiàn)，編隊(duì)中心最終收斂于領(lǐng)航者所在位置。

圖6　參考軌跡與領(lǐng)航者軌跡誤差

5結(jié)論

針對(duì)四旋翼無(wú)人飛行器編隊(duì)控制問(wèn)題，用單位四元數(shù)描述其非線性動(dòng)力學(xué)模型和運(yùn)動(dòng)學(xué)模型。采用跟蹤編隊(duì)幾何中心的方法實(shí)現(xiàn)編隊(duì)控制。編隊(duì)開(kāi)始時(shí)編隊(duì)成員通過(guò)無(wú)線網(wǎng)絡(luò)交換位置、速度等狀態(tài)信息，計(jì)算協(xié)商得出編隊(duì)幾何中心，并以此作為參考信號(hào)，通過(guò)Backstepping方法為每一架無(wú)人機(jī)設(shè)計(jì)了時(shí)變反饋控制律，這將編隊(duì)問(wèn)題轉(zhuǎn)化為給定參考信號(hào)的跟蹤問(wèn)題，通過(guò)MATLAB仿真檢驗(yàn)了該方法在大角度快速機(jī)動(dòng)時(shí)的有效性，仿真結(jié)果表明該方法計(jì)算速度快、精度高，提高了編隊(duì)系統(tǒng)收斂速度。目前方法尚未考慮朝向一致問(wèn)題和包含最大速度約束下的編隊(duì)問(wèn)題，后續(xù)研究將考慮朝向一致以及在最大速度約束等約束條件下設(shè)計(jì)編隊(duì)生成、保持及重構(gòu)方法。

參考文獻(xiàn)：

[1] 王祥科,李迅,鄭志強(qiáng). 多智能體系統(tǒng)編隊(duì)控制相關(guān)問(wèn)題研究綜述[J]. 控制與決策,2013,28(11): 1601-1613.

[2]F.Rinaldi,S.Chiesa,F.Quagliotti.LinearQuadraticControlforQuadrotorsUAVsDynamicsandFormationFlight[J].IntellRobotSyst. 2013(70):203-220.

[3] 夏梁盛.基于集群的多機(jī)器人編隊(duì)控制[J].火力與指揮控制，2013,38(1)：18-21.

[4]YuBC,DongXW,ShiZY,etal.Formationcontrolforquadrotorswarmsystems:Algorithmsandexperiments[A].Proceedingsofthe32ndChineseControlConference[C],26-28,July2013,xi’anChina.

[5]ChoiInSung,ChoiJongSuk.Leader-FollowerFormationControlUsingPIDController[A]. 5thInternationalConference[C],3-5,October,2012,Montreal,Canada.

[6] 安宏雷,李杰,馬宏緒,等.基于單位四元數(shù)的四旋翼無(wú)人直升機(jī)幾何控制[A].第24屆中國(guó)控制與決策會(huì)議論文集[C].2012.

[7]DierksT,JagannathanS.NeuralnetworkcontrolofquadrotorUAVformations[A].AmericanControlConference[C].St.Louis,USA. 2009.

[8]AbdessameudA,TayebiA.FormationcontrolofVTOL-UAVs[A].Joint48thIEEEConferenceonDecisionandControland28thChineseControlConference[C]. 2009.

[9] 李光春,王璐,王兆龍,等.基于四元數(shù)的四旋翼無(wú)人飛行器軌跡跟蹤控制[J].應(yīng)用科學(xué)學(xué)報(bào)，2012,30(4):415-422.

Formation Control of Quadrotors with Unit Quaternions Based via Backstepping Method

Li Lei, Li Xiaomin, Yang Sen

(Department of UAV Engineering，Ordnance Engineering College，Shijiazhuang050000，China)

Abstract:Formation control under ideal communication conditions is studied for quadrotor UAV. The quadrotor UAV has complex mathematical model. First of all, the quadrotor is decomposed into two independent subsystems of position and attitude by the quatenion describing its dynamics and kinematics model. The tracking deviation model is estabilished by introducing deviation of the expect track. One member in the formation is specified as leader. By consencus algorithm the fleet gets through the geometric center as the expect track. A time varying feedback control law that making formation stabilization is designed for each quadrotor with BackStepping method. Finally, the effectiveness of this algorithm is showed through the simulation results.

Keywords：formation control; quadrotor; quaternion; backstepping; follower-leader

文章編號(hào)：1671-4598(2016)02-0064-04

DOI：10.16526/j.cnki.11-4762/tp.2016.02.017

中圖分類號(hào)：TP242.6

文獻(xiàn)標(biāo)識(shí)碼：A

作者簡(jiǎn)介：李磊(1990-)，男，河北寧晉人，碩士研究生，主要從事無(wú)人機(jī)測(cè)控與飛行控制技術(shù)方向的研究。李小民(1968-)，男，河北保定人，教授，博士生導(dǎo)師，主要從事導(dǎo)航與控制及無(wú)人機(jī)運(yùn)用技術(shù)方向的研究。

基金項(xiàng)目：“十二五”國(guó)防預(yù)研項(xiàng)目(51325050101)。

收稿日期：2015-07-17；修回日期：2015-08-27。