邵星靈，劉 俊，李東光

（1. 中北大學(xué)儀器與電子學(xué)院，太原030051；2. 中北大學(xué)儀器科學(xué)與動(dòng)態(tài)測試教育部重點(diǎn)實(shí)驗(yàn)室，太原030051；3. 中北大學(xué)機(jī)電工程學(xué)院，太原030051）

1 引 言

四旋翼無人機(jī)利用其搭載的傳感器對(duì)目標(biāo)進(jìn)行持續(xù)觀測，可以獲取大量目標(biāo)信息，進(jìn)而實(shí)現(xiàn)目標(biāo)識(shí)別與精確定位，因而四旋翼無人機(jī)對(duì)峙跟蹤地面目標(biāo)在軍事和民用領(lǐng)域具有很大的潛在應(yīng)用價(jià)值［1］。然而，單架四旋翼在執(zhí)行目標(biāo)跟蹤任務(wù)時(shí)由于其傳感器探測范圍有限、自身運(yùn)動(dòng)/動(dòng)力學(xué)約束往往容易造成目標(biāo)丟失、目標(biāo)狀態(tài)估計(jì)性能不佳等問題，難以確保目標(biāo)跟蹤任務(wù)的魯棒性。使用多架四旋翼從不同角度以編隊(duì)形式對(duì)目標(biāo)進(jìn)行環(huán)繞觀測，通過鄰居間的信息交互，實(shí)現(xiàn)目標(biāo)觀測性能協(xié)同超越，對(duì)目標(biāo)可能逃逸的范圍進(jìn)行超前覆蓋，可顯著提高對(duì)目標(biāo)運(yùn)動(dòng)狀態(tài)的估計(jì)精度并降低目標(biāo)丟失的概率，提高四旋翼執(zhí)行跟蹤任務(wù)的成功率［2-6］。

四旋翼對(duì)峙跟蹤目標(biāo)算法是根據(jù)四旋翼位置和目標(biāo)位置的偏差，控制四旋翼飛行，使得四旋翼能夠持續(xù)在目標(biāo)附近進(jìn)行跟蹤［7］。Kim S 等［6］利用Lyapunov 導(dǎo)航向量場提出了一種利用多架無人飛行器對(duì)移動(dòng)目標(biāo)群進(jìn)行協(xié)調(diào)對(duì)峙跟蹤的方法，并且考慮了對(duì)峙跟蹤距離改變的情況。季榮濤［8］將Lyapunov 導(dǎo)航向量場和人工勢場法結(jié)合，在考慮避障要求的情況下，完成了目標(biāo)對(duì)峙跟蹤。Agate［9］利用切向量導(dǎo)航向量場和Lyapunov 導(dǎo)航向量場提出了一種動(dòng)態(tài)路徑規(guī)劃算法，用于四旋翼跟蹤地面目標(biāo)。羅健［10］針對(duì)目標(biāo)速度的影響，提出修正Lyapunov 導(dǎo)航向量場，完成了對(duì)地面移動(dòng)目標(biāo)的對(duì)峙跟蹤。Lim S 等［11］則將李雅普諾夫向量場制導(dǎo)法應(yīng)用于多無人機(jī)協(xié)同跟蹤目標(biāo)問題中。Ratnoo A［12］提出曲率約束的Lyapunov 導(dǎo)航向量場對(duì)峙目標(biāo)跟蹤方法，將參考點(diǎn)制導(dǎo)修正為四旋翼橫向制導(dǎo)規(guī)律，利用非線性微分方程對(duì)四旋翼與目標(biāo)之間的相對(duì)距離收斂過程進(jìn)行建模。針對(duì)目標(biāo)對(duì)峙跟蹤問題，目前大部分都是在無人機(jī)質(zhì)點(diǎn)模型的基礎(chǔ)上建立的，對(duì)于動(dòng)力學(xué)約束較少考慮，這也就導(dǎo)致大多數(shù)對(duì)峙跟蹤算法只能應(yīng)用于理想的質(zhì)點(diǎn)模型因而實(shí)用性欠佳。此外，在對(duì)峙跟蹤目標(biāo)過程中由于無人機(jī)處于機(jī)動(dòng)飛行狀態(tài)，當(dāng)多架無人機(jī)協(xié)同跟蹤同一目標(biāo)時(shí)，快速盤旋的機(jī)動(dòng)飛行動(dòng)作使得無人機(jī)難以確保機(jī)間避撞的安全性。

四旋翼協(xié)同編隊(duì)飛行方式可以在對(duì)峙跟蹤過程中使四旋翼之間保持穩(wěn)定的相對(duì)距離從而避免相撞，同時(shí)具備擴(kuò)展任務(wù)能力、擴(kuò)大任務(wù)執(zhí)行范圍、增強(qiáng)高危環(huán)境作業(yè)能力、提升環(huán)境適應(yīng)性等諸多優(yōu)勢。針對(duì)多四旋翼協(xié)同編隊(duì)問題，Ghamry K等［13］研究了運(yùn)用滑?？刂扑惴▽?shí)現(xiàn)對(duì)四旋翼位置和姿態(tài)的精準(zhǔn)控制的多四旋翼協(xié)同編隊(duì)問題，運(yùn)用領(lǐng)航者-跟隨者的編隊(duì)控制算法得到了多四旋翼快速穩(wěn)定編隊(duì)的控制效果。Haibo D 等［14］研究了有限時(shí)間多四旋翼協(xié)同編隊(duì)控制策略，確保了四旋翼在較短時(shí)間內(nèi)收斂到期望隊(duì)形并跟蹤指令軌跡。由于四旋翼的空氣動(dòng)力學(xué)參數(shù)難以用測量儀器精確獲得，給模型帶來了較大參數(shù)不確定性。此外，四旋翼空間飛行過程中不可避免遭受外部未知風(fēng)擾的影響，給四旋翼控制器設(shè)計(jì)帶來了困難［15-18］。因此，需要在考慮編隊(duì)幾何構(gòu)型約束和參數(shù)不確定性以及外部干擾的情況下，構(gòu)造適用于四旋翼典型動(dòng)力學(xué)特征并具有強(qiáng)魯棒性的多四旋翼協(xié)同編隊(duì)控制方法。針對(duì)存在外部干擾的多四旋翼編隊(duì)飛行控制問題，Wesam J 等［19］提出了基于次優(yōu)H∞的四旋翼編隊(duì)抗干擾控制方法，具有良好的穩(wěn)定性和魯棒性。目前對(duì)于四旋翼編隊(duì)抗干擾控制方法的研究屈指可數(shù)，因此迫切需要設(shè)計(jì)先進(jìn)的抗干擾控制策略以適應(yīng)四旋翼編隊(duì)飛行的實(shí)際需求。

本文針對(duì)多四旋翼編隊(duì)飛行過程中對(duì)地面目標(biāo)對(duì)峙跟蹤、幾何隊(duì)形生成、穩(wěn)固保持和協(xié)同抗干擾問題，研究了一種可應(yīng)對(duì)外部環(huán)境干擾和氣動(dòng)參數(shù)不確定性的多四旋翼主從式協(xié)同對(duì)峙跟蹤目標(biāo)方法。首先，利用Lyapunov導(dǎo)航向量場使領(lǐng)航者四旋翼對(duì)峙跟蹤地面目標(biāo)；其次，在四旋翼軌跡回路中引入二階一致性理論，結(jié)合代數(shù)圖論和通信拓?fù)洌灶I(lǐng)航者位置作為動(dòng)態(tài)編隊(duì)的幾何中心，將多四旋翼期望幾何隊(duì)形生成和保持問題轉(zhuǎn)化為跟隨者位置一致性跟蹤與領(lǐng)航者的相對(duì)位置偏差問題，從而構(gòu)造多四旋翼分布式位置保持控制器；最后，對(duì)于姿態(tài)回路的參數(shù)不確定性和外部干擾，借鑒自抗擾控制思想，將參數(shù)不確定性和外部干擾視為集總擾動(dòng)，采用擴(kuò)張狀態(tài)觀測器對(duì)其進(jìn)行在線觀測與補(bǔ)償，實(shí)現(xiàn)對(duì)給定姿態(tài)的高精度跟蹤控制。所提方法可以在四旋翼編隊(duì)持續(xù)對(duì)峙跟蹤目標(biāo)的同時(shí)極大改善四旋翼編隊(duì)系統(tǒng)的抗干擾能力，提升干擾環(huán)境下多四旋翼編隊(duì)幾何構(gòu)型的穩(wěn)固性和飛行的安全性。

2 問題描述與數(shù)學(xué)模型

根據(jù)多四旋翼無人機(jī)協(xié)同編隊(duì)對(duì)地面目標(biāo)對(duì)峙跟蹤問題需求，設(shè)計(jì)如圖1所示控制器結(jié)構(gòu)框圖。針對(duì)多無人機(jī)對(duì)地目標(biāo)對(duì)峙跟蹤問題構(gòu)建了基于李雅普諾夫向量場，綜合航向和相位控制，使得無人機(jī)以固定對(duì)峙半徑飛行到以目標(biāo)為中心的上空的圓環(huán)區(qū)域內(nèi)，確保了無人機(jī)與目標(biāo)跟蹤的連續(xù)性；以多智能體二階一致性算法為理論基礎(chǔ)結(jié)合四旋翼無人機(jī)動(dòng)力學(xué)模型建立了基于虛擬領(lǐng)導(dǎo)者航跡的主從式有限時(shí)間協(xié)調(diào)編隊(duì)控制模型；考慮到四旋翼無人機(jī)具有大的不確定性和時(shí)變且有外部擾動(dòng)，引入了線性自抗擾控制器，所設(shè)計(jì)的控制器具有強(qiáng)魯棒性、抗干擾性，對(duì)未知的外界擾動(dòng)和內(nèi)部不確定性有良好的控制效果，實(shí)現(xiàn)了可適應(yīng)不同目標(biāo)運(yùn)動(dòng)軌跡的高抗擾、強(qiáng)穩(wěn)定多無人機(jī)協(xié)同編隊(duì)對(duì)峙跟蹤。

圖1 控制器結(jié)構(gòu)框圖Fig.1 Structure diagram of the control scheme

定義四旋翼編隊(duì)數(shù)量集合Γ =(1，2，…，n)，i ∈Γ，給出n 架四旋翼組成編隊(duì)中的第i 架四旋翼運(yùn)動(dòng)學(xué)/動(dòng)力學(xué)模型［20-21］：

其中，mi為第i 架四旋翼的質(zhì)量，t 為時(shí)間，Gi=[0，0，mig]T，g為重力加速度，Ji= diag(Ji，1，Ji，2，Ji，3)∈R3×3表示一個(gè)正定的對(duì)角慣性矩陣，Ji，1、Ji，2、Ji，3分別是第i架四旋翼在慣性坐標(biāo)系下沿x，y，z 軸的轉(zhuǎn)動(dòng)慣量。Θi=[φi，θi，ψi]T分別表示第i 個(gè)四旋翼在慣性坐標(biāo)系下的位置矢量和在機(jī)體坐標(biāo)系下的姿態(tài)角。Π1，i= diag(kx，i，ky，i，kz，i)，Π2，i= diag(kφ，i，kθ，i，kψ，i)是第i 個(gè)四旋翼的空氣阻尼矩陣，kr，i∈R，r = x，y，z，φ，θ，ψ 是第i 架四旋翼的空氣阻力系數(shù)?？刂戚斎雞i∈R 為第i 架四旋翼的拉力，τi=[τx，iτy，iτz，i]T為繞機(jī)體x，y，z 軸的三個(gè)控制力矩。

g1，i=[c(ψi) s(θi) c(φi)+ s(ψi) s(φi)，s(ψi) s(θi)c(φi)- c(ψi)s(φi)，c(θi)c(φi)]T表示與姿態(tài)相關(guān)的位置回路輸入矩陣，s(·)與c(·)分別表示正弦函數(shù)和余弦函數(shù)。g2，i= diag(li，li，ci)∈R3×3，其中l(wèi)i是螺旋槳到四旋翼質(zhì)心的幾何距離，ci是力矩系數(shù)。dΘ，i(t) =[dφ，i，dθ，i，dψ，i]T表示姿態(tài)回路中的有界外部干擾。

接下來，為便于后續(xù)位置控制器和姿態(tài)控制器的構(gòu)造，引入如下符號(hào)定義：

其中，F(xiàn)i=[Fi，x，F(xiàn)i，y，F(xiàn)i，z]T∈R3×1表示虛擬控制輸入量。d4，i為姿態(tài)回路的集總擾動(dòng)，包含外部擾動(dòng)和參數(shù)不確定性的綜合影響。δ1，i、δ2，i是第i架四旋翼的參數(shù)化不確定性矩陣分別是Π2、g2，i的標(biāo)稱值。

借助上述中間變量，可將四旋翼運(yùn)動(dòng)/動(dòng)力學(xué)模型式（1）改寫為如下嚴(yán)格反饋形式：

3 基于Lyapunov導(dǎo)航向量場的四旋翼對(duì)地面目標(biāo)跟蹤方法

假設(shè)四旋翼的實(shí)時(shí)位置為P0=[x0，y0]T，目標(biāo)位 置 為Pt=[xt，yt]T，設(shè)Lyapunov 函 數(shù) 為V =(r2-其 中是四旋翼和目標(biāo)之間的幾何距離，R0是對(duì)峙跟蹤半徑，xr、yr為四旋翼與地面目標(biāo)沿x 軸、沿y 軸的相對(duì)位置分量。由李雅普諾夫?qū)Ш较蛄繄隹傻玫綗o人機(jī)期望速度：

其中，gx、gy分別為地面目標(biāo)靜止時(shí)四旋翼在慣性坐標(biāo)系下沿x 軸、y 軸的期望速度分量，k = v0r(r2+為四旋翼的巡航飛行速度。α 為期望速度的修正因子，它滿足：

當(dāng)無人機(jī)的速度在有效范圍內(nèi)，α必有正數(shù)解。

在Lyapunov 導(dǎo)航向量場的作用下，四旋翼無人機(jī)將航行到以目標(biāo)上空為中心的極限環(huán)上，在對(duì)目標(biāo)持續(xù)跟蹤的同時(shí)環(huán)繞目標(biāo)飛行。

4 基于二階一致性的四旋翼無人機(jī)主從式編隊(duì)控制方法

4.1 多四旋翼通信拓?fù)浜途庩?duì)樣式設(shè)計(jì)

第i 架和第j 架從機(jī)相對(duì)于領(lǐng)航者的位置矢量為Δi=[Δi，x，Δi，y，Δi，z]T和Δj=[Δj，x，Δj，y，Δj，z]T，則第i架和第j架四旋翼從機(jī)間的相對(duì)位置偏差可由如下等式Δij= Δi- Δj=[Δi，x，Δi，y，Δi，z]T-[Δj，x，Δj，y，Δj，z]T=[Δij，x，Δij，y，Δij，z]T描述。

以三架四旋翼的幾何中心為虛擬主機(jī)，以三架四旋翼為頂點(diǎn)的幾何圖形為邊長m 的正三角形。為達(dá)到正三角形的要求，設(shè)計(jì)如下的相對(duì)坐標(biāo)分別為從機(jī)1、2、3相對(duì)于主機(jī)0的坐標(biāo)。

4.2 多四旋翼通信拓?fù)浜途庩?duì)樣式設(shè)計(jì)

領(lǐng)航者軌跡運(yùn)動(dòng)信息可由Lyapunov 導(dǎo)航向量場產(chǎn)生：

其中，xd、yd、zd分別為領(lǐng)航者在慣性坐標(biāo)系下沿x，y，z 軸的位置分量，分別為領(lǐng)航者在慣性坐標(biāo)系下沿x，y，z軸的速度分量。

由于領(lǐng)航者定高飛行，故領(lǐng)航者在z 軸的速度分量為0，并且領(lǐng)航者在z軸的位置分量為5m。

由式（1）可得第i架四旋翼的位置動(dòng)力學(xué)模型：

根據(jù)上述第i 架四旋翼的位置動(dòng)力學(xué)模型，不難選擇控制量，接下來設(shè)計(jì)位置保持控制器：

其中，F(xiàn)i，x、Fi，y、Fi，z均為虛擬控制輸入。

那么，位置動(dòng)力學(xué)模型可如下表示：

根據(jù)多四旋翼主從式編隊(duì)通訊拓?fù)浜推谕I(lǐng)航者軌跡信息，結(jié)合多智能體二階一致性原理，構(gòu)造如下虛擬控制輸入(Fi，x，F(xiàn)i，y，F(xiàn)i，z)T：

其中，k1、k2表示待設(shè)計(jì)的控制器參數(shù)，Ni?Γ 且i ?Ni，xi為第i架四旋翼在慣性坐標(biāo)系下沿x軸的位置分量，xj為第j架四旋翼沿x軸的位置分量，vi，x為第i架四旋翼在慣性坐標(biāo)系下沿x軸的速度分量，vj，x為第j架四旋翼慣性坐標(biāo)系下沿x軸的速度分量。為領(lǐng)航者在慣性坐標(biāo)系下沿x 軸，y 軸，z 軸的加速度分量。

在獲得上述虛擬控制輸入(Fi，x，F(xiàn)i，y，F(xiàn)i，z)T的基礎(chǔ)上，結(jié)合式（2）進(jìn)行逆動(dòng)力學(xué)解算，可得到如下的期望姿態(tài)角指令

其中，ui為四旋翼的期望拉力，分別為機(jī)體坐標(biāo)系下的期望滾轉(zhuǎn)角、期望俯仰角以及期望偏航角。

5 基于線性自抗擾控制的多四旋翼姿態(tài)跟蹤控制器構(gòu)造

自抗擾控制不依賴于模型精度，適用于針對(duì)存在參數(shù)不確定性和外部干擾的四旋翼模型構(gòu)造姿態(tài)跟蹤控制器。線性自抗擾控制器由PD 和線性擴(kuò)張狀態(tài)觀測器（LESO）兩部分構(gòu)成，其設(shè)計(jì)過程簡潔，且便于參數(shù)整定。

由式（1）可知四旋翼姿態(tài)動(dòng)力學(xué)模型：

設(shè)一個(gè)二階被控系統(tǒng)表達(dá)式為：

其中，f為系統(tǒng)整體擾動(dòng)，是系統(tǒng)模型參數(shù)，u是系統(tǒng)輸入，x為系統(tǒng)輸出。

線性擴(kuò)張狀態(tài)觀測器（LESO）表達(dá)式為

其中，zi是對(duì)輸出xi的估計(jì)，z2是z1的導(dǎo)數(shù)，z3是對(duì)系統(tǒng)總擾動(dòng)f 的估計(jì)，β1、β2、β3是觀測器的增益，β1=3ω0，β2=，β3=，ω0為觀測器帶寬。

線性狀態(tài)誤差反饋（LSEF）為：

當(dāng)z3對(duì)系統(tǒng)總擾動(dòng)的估計(jì)誤差忽略不計(jì)，系統(tǒng)就可以簡化為雙積分串聯(lián)型，即：

從而線性狀態(tài)誤差反饋（LSEF）控制律的表達(dá)式為：

其中，xd為期望，kp和kd是控制器增益，kp=，kd=2ξωc，ωc是閉環(huán)系統(tǒng)的自然頻率，阻尼系數(shù)ξ通常取1，且ω0與ωc成比例。

對(duì)四旋翼動(dòng)力學(xué)方程進(jìn)一步整理可得

在本文中，設(shè)控制量τi為：

其中，kp=為正的控制器比例增益，kd= 2wc為正的控制器微分增益，wc為姿態(tài)回路的控制器帶寬，為姿態(tài)回路集總干擾的估計(jì)值，可由如下僅依賴于控制輸入力矩和姿態(tài)角量測輸出的模型輔助擴(kuò)張狀態(tài)觀測器給出：

其中，z1，i是對(duì)x3，i的估計(jì)，z2，i是對(duì)x4，i的估計(jì)，z3，i是對(duì)的估計(jì)，wo為姿態(tài)回路觀測器帶寬。

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

為驗(yàn)證上述四旋翼無人機(jī)自抗擾控制算法的有效性，進(jìn)行仿真實(shí)驗(yàn)。四旋翼無人機(jī)的質(zhì)量m=2kg，重力加速度g=9.8m/s2，電機(jī)到質(zhì)心的距離l=0.4m，力矩系數(shù)c=0.05。阻尼矩陣為：

慣性矩陣為：

考慮一個(gè)以三架跟隨者四旋翼無人機(jī)和一個(gè)虛擬領(lǐng)航者組成的網(wǎng)絡(luò)系統(tǒng)，無人機(jī)間的信息交流可描述為圖2 所示的無向拓?fù)鋱D，各邊的權(quán)值為a12= a21= 1，a23= a32= 1，b1= 1。氣動(dòng)阻尼系數(shù)Ki，j= 0.01，i =1，2，3，j = 1，2，3，4，5，6。編隊(duì)控制器增益為α1= 0.8，α2= 0.9，k1= 1.7，k2= 1.7。

相對(duì)位置偏差為：

圖2 無人機(jī)三角形編隊(duì)通信拓?fù)銯ig.2 Communication topology of UAV triangle formation

各四旋翼無人機(jī)的初始條件為：

各四旋翼無人機(jī)由不同的初始條件以三角形編隊(duì)的形式對(duì)起始點(diǎn)位于(0，0)的機(jī)動(dòng)目標(biāo)展開對(duì)峙半徑為2m 的環(huán)繞跟蹤，目標(biāo)運(yùn)動(dòng)方程為：vxt=0.5，vyt= 0。

圖3為三維坐標(biāo)系下多四旋翼無人機(jī)協(xié)同編隊(duì)跟蹤地面機(jī)動(dòng)目標(biāo)的響應(yīng)曲線，三架無人機(jī)從不同的初始位置出發(fā)，經(jīng)過一段時(shí)間后，在形成穩(wěn)定三角形編隊(duì)隊(duì)形的同時(shí)對(duì)一運(yùn)動(dòng)目標(biāo)展開對(duì)峙半徑為2m的環(huán)繞跟蹤。圖4為x-y平面下多四旋翼無人機(jī)協(xié)同編隊(duì)跟蹤地面目標(biāo)的相應(yīng)曲線，從圖中可以看到無人機(jī)在跟蹤目標(biāo)的過程中始終保持著穩(wěn)定的三角形隊(duì)形。

圖5 為各四旋翼無人機(jī)x、y、z 方向的位置響應(yīng)曲線，圖6為各四旋翼無人機(jī)x、y、z方向的速度響應(yīng)曲線，圖7 為各四旋翼無人機(jī)橫滾角、俯仰角、偏航角的姿態(tài)響應(yīng)曲線，可以看到，無人機(jī)在有限時(shí)間內(nèi)形成了三角形隊(duì)形并完成對(duì)期望軌跡跟蹤，保持了良好的一致性。

圖3 三維坐標(biāo)系下多四旋翼無人機(jī)跟蹤地面目標(biāo)圖Fig.3 Tracking performance of the ground targets under 3D coordinate system

圖4 x-y平面下多四旋翼無人機(jī)跟蹤地面目標(biāo)Fig.4 Tracking performance of the ground targets under x-y coordinate system

圖5 四旋翼無人機(jī)位置響應(yīng)曲線Fig.5 Position response curve of quadrotors

圖6 四旋翼無人機(jī)速度響應(yīng)曲線Fig.6 Velocity response curve of quadrotors

圖7 四旋翼無人機(jī)姿態(tài)響應(yīng)曲線Fig.7 Attitude response curve of quadrotors

7 結(jié) 論

本文針對(duì)多源干擾影響下多無人機(jī)主從式對(duì)地目標(biāo)對(duì)峙跟蹤問題，構(gòu)建了基于Lyapunov 導(dǎo)航向量場的多無人機(jī)對(duì)地目標(biāo)對(duì)峙跟蹤策略，使得多無人機(jī)以固定對(duì)峙半徑收斂于目標(biāo)上空的極限環(huán)，確保了無人機(jī)對(duì)目標(biāo)跟蹤的連續(xù)性；以自抗擾控制和多智能體二階一致性算法為理論基礎(chǔ)，建立了基于虛擬領(lǐng)導(dǎo)者航跡的主從式有限時(shí)間協(xié)調(diào)編隊(duì)控制模型，可在外部干擾以及參數(shù)不確定性的影響下，提升多四旋翼編隊(duì)隊(duì)形的穩(wěn)固性，提高多四旋翼編隊(duì)系統(tǒng)的抗干擾能力。