竇立謙，陳 濤，毛 奇

(天津大學(xué)電氣自動(dòng)化與信息工程學(xué)院，天津 300072)

近年來，具有機(jī)載智能能力的無人機(jī)(UAV)已廣泛應(yīng)用于軍事領(lǐng)域和民用工業(yè)領(lǐng)域．這些系統(tǒng)在機(jī)器人領(lǐng)域開辟新的應(yīng)用，包括監(jiān)視、災(zāi)害(環(huán)境、工業(yè)和城市)修復(fù)、搜索、救援等[1]．四旋翼無人機(jī)因其結(jié)構(gòu)簡(jiǎn)單且具有垂直起降、定點(diǎn)懸停等優(yōu)點(diǎn)，獲得了廣泛關(guān)注．四旋翼無人機(jī)是一個(gè)具有 6自由度和 4個(gè)控制輸入的欠驅(qū)動(dòng)系統(tǒng)[2]，具有非線性、強(qiáng)耦合、多變量等特點(diǎn)．許多控制方法，諸如 PID、反饋線性化、自適應(yīng)、滑模等方法已經(jīng)被運(yùn)用到四旋翼飛行控制中．然而單架四旋翼無人機(jī)在完成復(fù)雜任務(wù)時(shí)，由于自身載荷和傳感器性能的不足，應(yīng)用日益受到限制，取而代之的是越來越多的多無人機(jī)協(xié)同工作完成任務(wù)．因而多無人機(jī)協(xié)作越來越引起國(guó)內(nèi)外諸多研究人員的研究興趣．

無人機(jī)編隊(duì)控制方法主要有領(lǐng)航跟隨法[3]、虛擬結(jié)構(gòu)法[4]、基于行為法[5]和圖論法[6]等．盡管在大多數(shù)情況中，假定長(zhǎng)機(jī)的軌跡可以傳遞到每個(gè)智能體，但在實(shí)際中，假設(shè)只有部分個(gè)體可以知曉期望的軌跡是更有意義的．近來，線性時(shí)不變?nèi)后w系統(tǒng)或多智能體系統(tǒng)一致性問題受到越來越廣泛的關(guān)注．一致性意味著所有智能體就某些相關(guān)的協(xié)作變量上達(dá)成一致．文獻(xiàn)[7]采用一致性策略設(shè)計(jì)分布式編隊(duì)控制器，實(shí)現(xiàn)了多四旋翼系統(tǒng)時(shí)變隊(duì)形的協(xié)同控制．文獻(xiàn)[8]研究了小型四旋翼機(jī)群自主編隊(duì)，在串級(jí)控制系統(tǒng)框架下采用平均一致性算法和有領(lǐng)導(dǎo)一致性算法產(chǎn)生各四旋翼飛行器的期望軌跡，并給出一種基于有向Hamilton 環(huán)的通信拓?fù)湓O(shè)計(jì)方案．但文中將四旋翼模型簡(jiǎn)化為一階系統(tǒng)，模型較為簡(jiǎn)單，控制誤差較大．文獻(xiàn)[9]將四旋翼飛行器描述為二階系統(tǒng)，利用二階一致性算法，實(shí)現(xiàn)多四旋翼編隊(duì)集結(jié)和編隊(duì)機(jī)動(dòng)等行為．但該控制器中沒有充分考慮四旋翼模型的特點(diǎn)，也會(huì)有一定的控制偏差．文獻(xiàn)[10]充分考慮了四旋翼的模型特點(diǎn)，為編隊(duì)系統(tǒng)設(shè)計(jì)了分布式控制算法，實(shí)現(xiàn)編隊(duì)的協(xié)作控制．以上文獻(xiàn)都沒有考慮系統(tǒng)參數(shù)攝動(dòng)和外界環(huán)境干擾，這些因素都會(huì)對(duì)系統(tǒng)穩(wěn)定性和控制效果產(chǎn)生較大影響．

本文針對(duì)存在外界環(huán)境干擾和系統(tǒng)參數(shù)攝動(dòng)等不確定性的多四旋翼編隊(duì)系統(tǒng)，設(shè)計(jì)了一種基于干擾觀測(cè)器和終端滑模的分布式編隊(duì)跟蹤控制算法，而且期望的位置信息只有部分個(gè)體可以獲得．利用快速終端滑模觀測(cè)器實(shí)現(xiàn)對(duì)復(fù)合干擾的實(shí)時(shí)估計(jì)并補(bǔ)償，利用積分濾波器避免對(duì)虛擬控制輸入求導(dǎo)時(shí)產(chǎn)生的微分爆炸問題．最后通過穩(wěn)定性分析來驗(yàn)證分布式控制器的有效性，并通過仿真來驗(yàn)證該算法的性能．

1 四旋翼飛行器數(shù)學(xué)模型

圖1給出了四旋翼飛行器的基本模型，并且建立兩個(gè)坐標(biāo)系：機(jī)體坐標(biāo)系Oxyz和地面坐標(biāo)系

圖1 四旋翼的結(jié)構(gòu)和坐標(biāo)系Fig.1 Structure of the quadrotor and coordinate system

為了簡(jiǎn)化四旋翼的建模和控制器設(shè)計(jì)過程，文中做出如下假設(shè)．

假設(shè)1四旋翼飛行器為剛體，忽略其彈性形變；飛行器質(zhì)心與坐標(biāo)原點(diǎn)重合，且結(jié)構(gòu)關(guān)于平面Oxz和Oyz對(duì)稱．

假設(shè) 2由于四旋翼飛行器的飛行速度較低，氣動(dòng)阻力對(duì)飛行器影響較小，飛行器受到的氣動(dòng)阻力可以忽略不計(jì)[11]．

根據(jù) Newton-Euler公式，四旋翼飛行器模型的動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)方程[11]為

式中：P和V分別為飛行器在地面坐標(biāo)系中的位置向量和速度向量；Θ為歐拉角向量T；Ω為飛行器在機(jī)體坐標(biāo)系中的角速度；m為飛行器的質(zhì)量；g為重力加速度；e3為在地面坐標(biāo)系中的單位向量，J為飛行器的慣性矩陣，Ga為電機(jī)轉(zhuǎn)動(dòng)時(shí)產(chǎn)生的附加力矩，，Jr為電機(jī)的轉(zhuǎn)動(dòng)慣量為各旋翼轉(zhuǎn)速；f為控制力，f=，uT為旋翼產(chǎn)生的升力；τ為控制力矩．

Oxyz到的轉(zhuǎn)換矩陣R( Θ)為

轉(zhuǎn)換矩陣Π ( Θ)為

假設(shè) 3由于四旋翼飛行器的飛行速度較慢，飛行高度較低，升力系數(shù)c和阻力系數(shù)k可以視為常值．

式中l(wèi)、c、k分別為旋翼中心到飛行器質(zhì)心的距離、升力系數(shù)和阻力系數(shù)．

考慮模型中存在的外界干擾和不確定性，式(1)改寫為

式中為模型中的復(fù)合干擾，包括系統(tǒng)參數(shù)攝動(dòng)不確定以及外界環(huán)境干擾．

假設(shè) 4 當(dāng)t≥0時(shí)恒成立，則式(3)中Π( Θ )是非奇異的．

2 圖論基礎(chǔ)知識(shí)

文中設(shè)計(jì)了一組具有n架四旋翼的編隊(duì)系統(tǒng)．在控制這些四旋翼運(yùn)動(dòng)時(shí)，假設(shè)每架四旋翼通過傳感器和通信通道來獲取自己的狀態(tài)信息和部分相鄰個(gè)體的位置信息．因此，本文通過有向圖來模擬四旋翼之間的信息交換．通常將有向圖G表示為(N,)ε，其中N代表非空節(jié)點(diǎn)集合，ε是邊的集合．有向圖中的邊(i, j)表示個(gè)體i可以從個(gè)體 j獲取信息，并把 j定義為父節(jié)點(diǎn)，將i定義為子節(jié)點(diǎn)．若對(duì)于圖G中的每一對(duì)節(jié)點(diǎn)，都存在一條路徑連接兩點(diǎn)，則稱該圖為連通圖．在有向圖中，如果存在一個(gè)邊的子集，能夠連接圖中所有節(jié)點(diǎn)，則稱這個(gè)邊的子集為一個(gè)生成樹，同時(shí)稱該圖包含一個(gè)有向生成樹．

鄰接矩陣用代數(shù)矩陣的方式來描述節(jié)點(diǎn)之間的位置關(guān)系，其中

拉普拉斯矩陣定義為，其中

同時(shí)定義一個(gè)矩陣表示個(gè)體i和長(zhǎng)機(jī)之間的連接情況，當(dāng)個(gè)體i與長(zhǎng)機(jī)連通，，否則

如圖2所示，本文的目標(biāo)是設(shè)計(jì)一個(gè)分布式編隊(duì)控制器，使多架四旋翼飛行器在長(zhǎng)機(jī)的帶領(lǐng)下達(dá)到并保持期望的隊(duì)形．

圖2 編隊(duì)控制結(jié)構(gòu)Fig.2 Formation control architecture

定義為第i個(gè)四旋翼相對(duì)長(zhǎng)機(jī)的期望偏差，i=1,???,n ，則本文的控制目標(biāo)為

式中和Lψ分別代表虛擬長(zhǎng)機(jī)的位置和偏航角．

3 分布式編隊(duì)控制方法設(shè)計(jì)

圖 3給出了第i架四旋翼的控制結(jié)構(gòu)，每架四旋翼可以獲取自身的狀態(tài)信息和部分鄰居個(gè)體的狀態(tài)信息，并根據(jù)獲取的信息計(jì)算出自身控制量，使多架四旋翼形成期望的隊(duì)形．

圖3 第i架四旋翼控制結(jié)構(gòu)Fig.3 Controller structure for the i-th quadrotor

3.1 干擾觀測(cè)器設(shè)計(jì)

本節(jié)針對(duì)實(shí)際系統(tǒng)中存在的外界環(huán)境干擾和系統(tǒng)參數(shù)攝動(dòng)等不確定性的影響，設(shè)計(jì)一種基于快速終端滑模的干擾觀測(cè)器(TSMDO)，對(duì)干擾進(jìn)行估計(jì)和補(bǔ)償，從而減少外加干擾和系統(tǒng)參數(shù)攝動(dòng)等對(duì)控制效果的影響．

假設(shè) 5假設(shè)式(6)和式(7)中復(fù)合干擾是有界的，即存在正實(shí)常數(shù)ρVi和 ρΩi，使得和成立．

對(duì)于系統(tǒng)(6)，設(shè)計(jì)干擾觀測(cè)器使得

式中是對(duì)系統(tǒng)狀態(tài)的估計(jì)；d?Vi是對(duì)復(fù)合干擾 dVi的估計(jì)．

定義滑模變量為

對(duì)sVi求導(dǎo)得

設(shè)計(jì)基于快速終端滑模的干擾觀測(cè)器為

式中：為正常數(shù)，且滿足

引理1[12]針對(duì)非線性系統(tǒng)

式中是定義域u到n維空間滿足局部 Lipschitz連續(xù)性的連續(xù)函數(shù)．假設(shè)存在連續(xù)正定 函 數(shù) W(x):u→R ，α1＞0和使 得成立，則該非線性系統(tǒng)是有限時(shí)間穩(wěn)定的，且到達(dá)時(shí)間為

式中W(x(0))為W(x)的初始值．

定理 1 在假設(shè) 5的條件下，針對(duì)位置子系統(tǒng)動(dòng)力學(xué)模型(式(6))，選取合適的參數(shù)，則設(shè)計(jì)的干擾觀測(cè)器(式(15))可以使干擾的估計(jì)值快速收斂到

證明 為證明可以快速收斂到dVi，將式(15)代入到式(14)中，得到

選取Lyapunov函數(shù)

對(duì)時(shí)間求導(dǎo)得

根據(jù)引理 1可得輔助變量sVi可以在有限時(shí)間內(nèi)收斂到 0，從而保證干擾估計(jì)能夠快速收斂到同理，可以設(shè)計(jì)相同的干擾觀測(cè)器使得干擾估計(jì)快速收斂到dΩi．

3.2 位置控制器設(shè)計(jì)

首先，定義第i架四旋翼的位置跟蹤誤差

式中為長(zhǎng)機(jī)的位置．對(duì)其求導(dǎo)得

選取Lyapunov函數(shù)

對(duì)時(shí)間求導(dǎo)后得

為保證，設(shè)計(jì)虛擬控制Vd,i為

式中；K1i為待設(shè)計(jì)的正常數(shù)．定義速度跟蹤誤差，則有

對(duì)Z2i求導(dǎo)并將式(6)代入得

式中．定義Ui為

Uequ,i和Usmc,i分別表示等效控制和轉(zhuǎn)換控制．其中Uequ,i設(shè)計(jì)為

式中是對(duì)復(fù)合干擾dVi的估計(jì)．

將式(28)和式(29)代入式(27)中，得

式中

設(shè)計(jì)基于快速終端滑模的控制方法為

式中1λi、2λi、1γi、2γi為正常數(shù)，且滿足

定理 2 針對(duì)位置子系統(tǒng)(6)，若干擾觀測(cè)器設(shè)計(jì)為式(15)，且參數(shù)選取合適，則在控制率式(28)、式(29)和式(31)的作用下，速度跟蹤誤差Z2i可以在有限時(shí)間收斂到 0，從而使得位置跟蹤誤差式(21)快速收斂到0．

證明 為證明Z2i可以快速收斂到 0，將式(31)代入到式(30)中得

選取Lyapunov函數(shù)

求導(dǎo)得

由第3.1節(jié)可知，可以有限時(shí)間收斂到dVi，表明干擾估計(jì)誤差有界．參考式(20)證明過程可得

根據(jù)引理 1可知，在參數(shù)選取合適的條件下，速度跟蹤誤差Z2i可以在有限時(shí)間收斂到 0，此時(shí)式(26)更新為

故以上所設(shè)計(jì)的位置控制器能夠保證該四旋翼的位置跟蹤誤差快速收斂到0．

由可得旋翼提供的升力uT,i為

并通過簡(jiǎn)單運(yùn)算可得

對(duì)式(36)～式(38)進(jìn)行代數(shù)運(yùn)算，可得輸入到姿態(tài)回路的給定滾轉(zhuǎn)角φd,i和俯仰角θd,i分別為

式中ψi為第i架四旋翼的偏航角．

3.3 姿態(tài)控制器設(shè)計(jì)

針對(duì)姿態(tài)子系統(tǒng)數(shù)學(xué)模型式(7)以及式(41)、式(42)和Lψ組成期望的姿態(tài)角．由于直接對(duì)該虛擬控制量求導(dǎo)會(huì)產(chǎn)生微分爆炸問題，而且其抖振也會(huì)傳遞到其導(dǎo)數(shù)中，因此本文采用積分濾波器來避免這些問題．

參考文獻(xiàn)[13]設(shè)計(jì)積分濾波器為

式中：ε1i、ε2i、ε3i是濾波常數(shù)；μ1i、μ2i、μ3i、Δ1i、Δ2i、Δ3i是設(shè)計(jì)參數(shù)．由文獻(xiàn)[13]可知，ξ1i收斂到Θd,i，且 ξ2i和 ξ3i分別是和的估計(jì)值．定義濾波誤差為

定義姿態(tài)角跟蹤誤差為

對(duì)時(shí)間求導(dǎo)后得

選取Lyapunov函數(shù)為

求導(dǎo)后得

為保證3＜0，設(shè)計(jì)虛擬控制Ωd,i為

式中：ξ2i是的估計(jì)值；K3i為待設(shè)計(jì)的正常數(shù)．定義速度跟蹤誤差，則

仿照位置控制器設(shè)計(jì)姿態(tài)控制器為

式中：3iλ、λ4i、γ3i、γ4i為正常數(shù)，且滿足；d?Ωi是對(duì)復(fù)合干擾dΩi的估計(jì)，定義估計(jì)誤差，則

由位置控制器的證明過程可知，式(52)、式(53)和式(54)設(shè)計(jì)的姿態(tài)控制器能夠保證姿態(tài)角跟蹤誤差快速收斂到0．

3.4 編隊(duì)穩(wěn)定性分析

定義，則式(21)改寫為

定義為編隊(duì)系統(tǒng)的拉普拉斯矩陣，則式(55)可以寫成向量形式為

式中，?表示 Kronecker積為單位矩陣．

由第 3.2節(jié)可知Z1可以快速收斂到 0，且，可得

定義，式(57)可以寫為

根據(jù)文獻(xiàn)[14]中引理 3可知，若編隊(duì)系統(tǒng)通信拓?fù)鋱DG是連通的，則H為非奇異矩陣，表明，即，從而實(shí)現(xiàn)編隊(duì)的穩(wěn)定．

4 系統(tǒng)仿真分析

為驗(yàn)證所提控制策略的正確性和有效性，下面進(jìn)行仿真分析．圖4給出了編隊(duì)通信拓?fù)?，其?代表虛擬長(zhǎng)機(jī)，1～4分別代表4架四旋翼從機(jī)．

圖4 編隊(duì)通信拓?fù)銯ig.4 Formation communication topology

虛擬長(zhǎng)機(jī)的軌跡為

期望偏航角為

仿真所用四旋翼飛行器參數(shù)如下：

仿真實(shí)驗(yàn)中，控制器各參數(shù)選擇如下：

四旋翼飛行器初始狀態(tài)如下：

期望偏差為

四旋翼飛行過程中受到的復(fù)合干擾選取為

結(jié)果分析如下．

(1) 從圖 5可以看到，終端滑模干擾觀測(cè)器(TSMDO)的三維估計(jì)誤差可以快速收斂到 0，表明文中提出的干擾觀測(cè)器滿足系統(tǒng)要求．

圖5 第1架四旋翼干擾估計(jì)誤差Fig.5 Estimated error of disturbances for quadrotor 1

(2) 從圖6和圖7中可以看到，虛擬長(zhǎng)機(jī)的軌跡是半徑為15,m的圓，其余4架四旋翼從機(jī)由初始狀態(tài)快速形成圍繞長(zhǎng)機(jī)的期望的正方形，并且保持該隊(duì)形．為了便于觀察隊(duì)形的形成過程，圖 7給出了 x-y平面的編隊(duì)軌跡．可以看到，在t=3s時(shí)，編隊(duì)的隊(duì)形基本形成，并且一直保持該隊(duì)形直到仿真結(jié)束，表明所設(shè)計(jì)的編隊(duì)控制器可以保證編隊(duì)系統(tǒng)的穩(wěn)定性．

(3) 從圖 8(a)和(b)中可以看到，俯仰角和滾轉(zhuǎn)角跟蹤誤差可以快速收斂到0，從圖8(c)可以看到偏航角可以快速收斂到長(zhǎng)機(jī)的偏航角，表明所設(shè)計(jì)的姿態(tài)控制器可以使四旋翼的姿態(tài)角快速收斂到期望值，從而使整個(gè)編隊(duì)系統(tǒng)實(shí)現(xiàn)穩(wěn)定．

圖6 編隊(duì)軌跡三維效果Fig.6 Formation trajectory in three dimensions

圖7 x-y平面編隊(duì)軌跡Fig.7 Formation trajectory in x-y plane

圖8 姿態(tài)角跟蹤效果Fig.8 Attitude tracking effect

5 結(jié) 語

本文針對(duì)多四旋翼無人機(jī)編隊(duì)，提出了一種基于干擾觀測(cè)器和終端滑模的分布式編隊(duì)跟蹤控制算法而且虛擬長(zhǎng)機(jī)的位置信息只有部分從機(jī)可以獲得．利用快速終端滑模觀測(cè)器來快速估計(jì)模型中的復(fù)合干擾，并在控制律中實(shí)時(shí)補(bǔ)償，增強(qiáng)了系統(tǒng)的抗干擾能力．對(duì)于姿態(tài)控制的虛擬參考輸入量，采用濾波器進(jìn)行積分濾波，不僅避免了微分爆炸，同時(shí)也提高了系統(tǒng)穩(wěn)定性．最后，仿真結(jié)果驗(yàn)證了綜合控制策略的有效性．

參考文獻(xiàn)：

［1］Kendoul F，Lara D，F(xiàn)antoni I，et al. Nonlinear control for systems with bounded inputs：Real-time embedded control applied to UAVs[C]// IEEE Conference on Decision and Control. San Diego，CA，USA，2006：5888-5893.

［2］Lee D B，Nataraj C，Burg T C，et al. Adaptive tracking control of an underactuated aerial vehicle[C]//American Control Conference(ACC). San Francisco，California，USA，2011：2326-2331.

［3］Rold?o V，Cunha R，Cabecinhas D，et al. A leaderfollowing trajectory generator with application to quadrotor formation flight[J]. Robotics & Autonomous Systems，2014，62(10)：1597-1609.

［4］Wang J，Xin M. Integrated optimal formation control of multiple unmanned aerial vehicles[J]. IEEE Transactions on Control Systems Technology，2013，21(5)：1731-1744.

［5］Turpin M，Michael N，Kumar V. Decentralized formation control with variable shapes for aerial robots[C]//IEEE International Conference on Robotics and Automation. Saint Paul，Mi nnesotas，USA，2012：23-30.

［6］Ramazani S，Selmic R R，de Queiroz M. Stabilization of non-planar multi-agent layered formations with double integrator model[C]// Control Applications IEEE. Sydney，Australia，2015：1386-1391.

［7］Dong X，Yu B，Shi Z，et al. Time-varying formation control for unmanned aerial vehicles：Theories and applications[J]. IEEE Transactions on Control Systems Technology，2015，23(1)：340-348.

［8］邢關(guān)生，杜春燕，宗 群，等. 基于一致性的小型四旋翼機(jī)群自主編隊(duì)分布式運(yùn)動(dòng)規(guī)劃[J]. 控制與決策，2014，29(11)：2081-2084.Xing Guansheng，Du Chunyan，Zong Qun，et al. Consensus-based distributed motion planning for autonomous formation of miniature quadrotor groups[J]. Control and Decision，2014，29(11)：2081-2084(in Chinese).

［9］李 磊，李小民，鄭忠貴，等. 基于一致性理論的四旋翼無人機(jī)分布式編隊(duì)控制方法[J]. 電光與控制，2015，22(10)：19-23.Li Lei，Li Xiaomin，Zheng Zhonggui，et al. Consensus based distributed formation control of quadrotor UAVs[J]. Electronics Optics and Control， 2015，22(10)：19-23(in Chinese).

［10］Ghommam J，Luque-Vega L F，Castillo-Toledo B，et al. Three-dimensional distributed tracking control for multiple quadrotor helicopters[J]. Journal of the Franklin Institute，2016，353(10)：2344-2372.

［11］Zuo Z. Trajectory tracking control design with commandfiltered compensation for a quadrotor[J]. Control Theory and Applications，IET，2010，4(11)：2343-2355.

［12］Moulay E，Perruquetti W. Finite time stability and stabilization of a class of continuous systems[J]. Journal of Mathematical Analysis & Applications，2006，323(2)：1430-1443.

［13］Li C Y，Jing W X，Gao C S. Adaptive backsteppingbased flight control system using integral filters[J].Aerospace Science & Technology，2009，13(2/3)：105-113.

［14］Hong Y，Hu J，Gao L. Tracking control for multi-agent consensus with an active leader and variable topology[J]. Automatica，2007，42(7)：1177-1182.