劉 亞，黃攀峰，張 帆

(1.西北工業(yè)大學(xué)航天學(xué)院智能機(jī)器人研究中心，西安 710072； 2.西北工業(yè)大學(xué)航天飛行動力學(xué)技術(shù)重點(diǎn)實(shí)驗(yàn)室，西安 710072)

0 引言

多旋翼無人機(jī)具有機(jī)動性強(qiáng)、可垂直起降、價格低廉等優(yōu)點(diǎn)，近年來被廣泛應(yīng)用于遠(yuǎn)程監(jiān)視、影視拍攝、資源探測和民用物流等領(lǐng)域。受到軍用無人直升機(jī)繩索懸掛負(fù)載搬運(yùn)以及救援的啟發(fā)，提出了旋翼無人機(jī)繩索懸掛式搬運(yùn)系統(tǒng)。考慮到單個旋翼無人機(jī)的載荷能力有限，開展了多無人機(jī)協(xié)同搬運(yùn)系統(tǒng)的研究，主要有剛性抓取式和系繩懸掛式兩種協(xié)同搬運(yùn)連接方式[1]。相比于剛性抓取式，系繩懸掛式能夠保留無人機(jī)的高機(jī)動性以及提高載荷能力的使用效益。針對單無人機(jī)系繩懸掛式搬運(yùn)的系繩擺動問題，設(shè)計了依賴系繩或載荷實(shí)時狀態(tài)信息的擺動抑制控制器[2]，使得多無人機(jī)協(xié)同搬運(yùn)能夠有效抑制系繩擺動，因此多無人機(jī)協(xié)同搬運(yùn)受到了研究人員的廣泛關(guān)注。但是，系繩懸掛式系統(tǒng)是一個欠驅(qū)動、非線性耦合系統(tǒng)，懸掛跟蹤控制是一個具有很高挑戰(zhàn)性的問題。

文獻(xiàn)[3-4]中，基于拉格朗日建模法推導(dǎo)了任意數(shù)量無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)的動力學(xué)模型，并設(shè)計了幾何非線性控制器，實(shí)現(xiàn)了載荷的穩(wěn)定跟蹤控制。但是設(shè)計的控制器是集中式的，且需要載荷的狀態(tài)信息，這種控制方法在缺少測量系統(tǒng)的室外環(huán)境下應(yīng)用具有局限性?；跓o人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)的微分平坦性，文獻(xiàn)[5]利用平坦輸出的高階微分信息對系統(tǒng)開展了軌跡規(guī)劃和動力學(xué)控制研究。系統(tǒng)的微分平坦性對外界擾動、參數(shù)不確定以及系統(tǒng)未建模動態(tài)具有很強(qiáng)的敏感性，是一種理想條件下存在的性質(zhì)。文獻(xiàn)[6]中，建立了無人機(jī)和懸掛載荷之間的正運(yùn)動學(xué)關(guān)系，通過無人機(jī)的跟蹤控制實(shí)現(xiàn)了載荷的間接軌跡跟蹤。文中假設(shè)系繩拉力的實(shí)際值與期望值一致，忽視了無人機(jī)與載荷之間通過系繩連接的耦合動力學(xué)的影響。文獻(xiàn)[7]基于凱恩法建立了系統(tǒng)的耦合動力學(xué)模型，設(shè)計了無人機(jī)狀態(tài)反饋軌跡跟蹤控制方法。但是基于該方法，無人機(jī)協(xié)同搬運(yùn)系統(tǒng)只能完成定高和勻速運(yùn)動這種簡單的軌跡跟蹤任務(wù)?？紤]系統(tǒng)準(zhǔn)靜態(tài)條件下的運(yùn)動，文獻(xiàn)[8]對無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)的軌跡規(guī)劃和動力學(xué)控制問題展開了研究，但是忽視了載荷的動態(tài)特性。同樣地，文獻(xiàn)[9-10]研究了多無人機(jī)協(xié)同搬運(yùn)的跟蹤控制問題，但設(shè)計的控制方法只適用于系統(tǒng)的準(zhǔn)靜態(tài)運(yùn)動，忽視了懸掛載荷的動態(tài)特性。

在室外條件下，很難測量多無人機(jī)懸掛式協(xié)同搬運(yùn)系統(tǒng)的懸掛載荷狀態(tài)信息，傳統(tǒng)的集中式控制方法不利于多體系統(tǒng)提升自主性，且搬運(yùn)過程中準(zhǔn)靜態(tài)運(yùn)動不能有效覆蓋系統(tǒng)的動態(tài)運(yùn)動場景。因此，本文研究了一般動態(tài)場景下，多無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)的軌跡規(guī)劃和運(yùn)動控制問題。首先計算了系統(tǒng)的有效旋量空間，并分析了旋量空間與系統(tǒng)構(gòu)型和運(yùn)動加速度之間的關(guān)系。其次規(guī)劃了載荷與無人機(jī)的可行運(yùn)動軌跡，滿足系繩張緊狀態(tài)要求且避免了無人機(jī)之間的碰撞。最后設(shè)計了固定時間跟蹤控制器，實(shí)現(xiàn)了對期望軌跡的快速穩(wěn)定跟蹤。

1 有效旋量空間分析

1.1 系統(tǒng)平衡方程

多無人機(jī)繩索懸掛式協(xié)同搬運(yùn)系統(tǒng)由N個無人機(jī)、懸掛載荷(本文研究質(zhì)點(diǎn)模型)和連接無人機(jī)與載荷的系繩構(gòu)成，如圖1所示。

圖1 多無人機(jī)繩索懸掛式協(xié)同搬運(yùn)系統(tǒng)坐標(biāo)系Fig.1 Coordinate system of multiple UAVs tether suspended cooperative transportation system

根據(jù)牛頓-歐拉定理，動力學(xué)方程為

(1)

式中，mp為載荷的質(zhì)量；rp為載荷在慣性系F0下的位置；g為重力加速度；e3為z軸方向的單位向量；Ti為系繩i的拉力；qi為沿載荷到無人機(jī)i的系繩i的方向向量；N為無人機(jī)數(shù)量。無人機(jī)i的平動動力學(xué)方程為

(2)

式中，mi為無人機(jī)i的質(zhì)量；ri為無人機(jī)i在慣性系F0下的位置；fi為無人機(jī)的升力；Ri為無人機(jī)本體系Fi到慣性系F0的旋轉(zhuǎn)矩陣。

1.2 有效旋量空間

懸掛載荷是一個無主動控制的一般性物體，通過多系繩拉力提供任務(wù)空間旋量，對載荷實(shí)施被動控制，系繩拉力由無人機(jī)相對載荷的運(yùn)動產(chǎn)生。因此，系統(tǒng)任務(wù)空間旋量由無人機(jī)升力空間到系繩拉力和系繩拉力到任務(wù)空間2個映射確定。無人機(jī)升力空間為

H={f∈RN:fmin≤f≤fmax}

(3)

式中，f=[f1,f2,…,fN]T、fmin=[f1min,f2min,…,fNmin]T、fmax=[f1max,f2max,…,fNmax]T，fimin和fimax分別是無人機(jī)i升力允許的最小值和最大值，fimin保證無人機(jī)在空中懸停，fimax由無人機(jī)執(zhí)行機(jī)構(gòu)自身決定。拉力空間是系繩可行拉力構(gòu)成的集合，表示為

T={T∈RN:Tmin≤T≤Tmax}

(4)

式中，T=[T1,T2,…,TN]T、Tmin=[T1min,T2min,…,TNmin]T、Tmax=[T1max,T2max,…,TNmax]T，Timin和Timax分別是系繩i拉力允許的最小值和最大值，Timin保證系繩處于張緊狀態(tài)，Timax由系繩本身屬性和無人機(jī)共同決定。本研究中假設(shè)系繩本身的強(qiáng)度足夠，則系繩拉力允許的最大值Timax由無人機(jī)的運(yùn)動和無人機(jī)允許的最大升力決定，根據(jù)無人機(jī)動力學(xué)可得

(5)

因此，系繩拉力與無人機(jī)推力之間的關(guān)系為

(6)

其中

(7)

根據(jù)式(6)可知，系繩i允許的最大拉力由無人機(jī)i的最大推力決定，則最大拉力Timax為

(8)

考慮準(zhǔn)靜態(tài)條件下，系繩i允許的最大拉力為

(9)

系繩拉力構(gòu)成的系統(tǒng)有效旋量空間表示如下

(10)

其中

ΔTi=Timax-Timin

(11)

式中，w為有效旋量集中的元素；d為任務(wù)空間的維度；矩陣W=[q1,q2,…,qN]。通過式(10)得到，系繩拉力構(gòu)成的有效旋量空間是一個凸包。

1.3 系統(tǒng)魯棒性分析

根據(jù)1.2節(jié)分析可知，任務(wù)空間的有效旋量集取決于無人機(jī)的運(yùn)動加速度、最大升力以及系統(tǒng)構(gòu)型(系繩方向)。本節(jié)分析了無人機(jī)運(yùn)動加速度以及系統(tǒng)構(gòu)型對有效旋量空間的影響，以及系統(tǒng)搬運(yùn)不同載荷時的魯棒性。首先，介紹了一個衡量多無人機(jī)繩索懸掛式協(xié)同搬運(yùn)系統(tǒng)魯棒性的指標(biāo)——拉力容許裕度γ。拉力容許裕度表示載荷的重力旋量屬于有效旋量集合時，重力旋量距離有效旋量空間邊界的最小值。拉力容許裕度越小，表示對應(yīng)的系統(tǒng)容許的最大機(jī)動加速度越小，以及載荷能承受的外界擾動越小，則系統(tǒng)的魯棒性越差。

考慮3架無人機(jī)協(xié)同搬運(yùn)情況下，系繩不同面外角θi對有效旋量空間的影響，以及不同質(zhì)量的載荷重力旋量與有效旋量空間的幾何關(guān)系。系繩長度為1.5m，無人機(jī)質(zhì)量為1kg，無人機(jī)允許的最大升力為18N，保持系繩張緊需要的最小預(yù)緊力為0.1N，3架無人機(jī)均勻分布在載荷周圍，面內(nèi)角為C2=[γ1,γ2,γ3]=[0,2π/3,-2π/3]，針對不同的面外角C1=[θ1,θ2,θ3]，系統(tǒng)的有效旋量空間與載荷的重力旋量關(guān)系如圖2所示。圖2中，黑色六面體表示系統(tǒng)的有效旋量空間，不同顏色的小球是以對應(yīng)的載荷重力為球心，拉力容許裕度為半徑。小球在六面體內(nèi)表示系統(tǒng)在靜平衡狀態(tài)下能搬運(yùn)對應(yīng)質(zhì)量的載荷；反之，表示系統(tǒng)在靜平衡狀態(tài)下不能搬運(yùn)對應(yīng)質(zhì)量的載荷。通過圖2可以看出，隨著系繩面外角C1=[θ1,θ2,θ3]的減小，系統(tǒng)所能搬運(yùn)的載荷質(zhì)量上限在下降。在系統(tǒng)能夠承受的質(zhì)量范圍內(nèi)，質(zhì)量過小，容易導(dǎo)致搬運(yùn)過程中系繩出現(xiàn)松弛；質(zhì)量過大，系統(tǒng)允許的最大加速度減小且魯棒性降低，在外界擾動作用下系統(tǒng)容易出現(xiàn)不穩(wěn)定現(xiàn)象。所以，對于具體的多無人機(jī)懸掛式協(xié)同搬運(yùn)系統(tǒng)，確定系統(tǒng)構(gòu)型以及搬運(yùn)載荷范圍是具有實(shí)際應(yīng)用價值的。

圖2 系統(tǒng)不同構(gòu)型下有效旋量空間與重力旋量幾何關(guān)系Fig.2 Geometric relationship between effective wrench space and gravity wrench space under different configuration

考慮系統(tǒng)在不同機(jī)動加速度下對有效旋量空間的影響，以及不同質(zhì)量的載荷重力旋量與有效旋量空間的幾何關(guān)系。系繩長度為1.5m，無人機(jī)質(zhì)量為1kg，無人機(jī)允許的最大升力為18N，保持系繩張緊需要的最小預(yù)緊力為0.1N，3架無人機(jī)均勻分布在載荷周圍，面內(nèi)角為C2=[γ1,γ2,γ3]=[0,2π/3,-2π/3]，根據(jù)以上分析，選取面外角為C1=[θ1,θ2,θ3] =[π/3,π/3,π/3]，每架無人機(jī)加速度相等，都沿著x軸方向，在不同加速度條件下，系統(tǒng)的有效旋量空間與載荷的重力旋量關(guān)系如圖 3所示。由圖3可以看出，隨著加速度的增大，系統(tǒng)所能承受的最大載荷質(zhì)量上限在下降，主要原因是系統(tǒng)沿著加速度方向不能提供足夠的旋量穩(wěn)定載荷。因此，系統(tǒng)機(jī)動加速度越大，系統(tǒng)能搬運(yùn)的載荷質(zhì)量越小。同時，觀察圖3發(fā)現(xiàn)，x軸方向加速度越大，系統(tǒng)沿該方向的拉力容許裕度越小，因此系統(tǒng)運(yùn)動魯棒性越差。

圖3 系統(tǒng)不同加速度下有效旋量空間與重力旋量幾何關(guān)系Fig.3 Geometric relationship between effective wrench space and gravity wrench space under different acceleration

2 系統(tǒng)軌跡規(guī)劃

2.1 載荷軌跡規(guī)劃

本文研究問題歸納為：通過對無人機(jī)施加主動控制，實(shí)現(xiàn)搬運(yùn)載荷跟蹤期望軌跡的跟蹤控制問題。對于一個給定的多無人機(jī)懸掛式協(xié)同搬運(yùn)系統(tǒng)，已知載荷需要經(jīng)歷的航跡點(diǎn)，首先需要解決的問題是規(guī)劃出載荷的軌跡。

根據(jù)文獻(xiàn)[5]可得，多無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)是一個微分平坦系統(tǒng)，通過優(yōu)化載荷軌跡使其6th導(dǎo)數(shù)最小，可以得到最小化snap的無人機(jī)期望軌跡，使無人機(jī)運(yùn)動平滑且節(jié)省能量。載荷軌跡規(guī)劃類似于無人機(jī)航跡規(guī)劃，參考文獻(xiàn)[12]中無人機(jī)航跡規(guī)劃方法，規(guī)劃載荷經(jīng)歷約定航跡點(diǎn)的最優(yōu)軌跡。假設(shè)載荷軌跡的m+1個航跡點(diǎn)為rp0,rp1,…,rpm。選取n階分段多項(xiàng)式函數(shù)作為載荷的軌跡函數(shù)，具有如下形式

(12)

最優(yōu)規(guī)劃問題即是最小化載荷軌跡rp(t)的6th導(dǎo)數(shù)平方的積分，規(guī)劃問題表達(dá)如下

(13)

此優(yōu)化問題可以整理為二次規(guī)劃問題，形式如下

(14)

二次規(guī)劃問題是一個成熟的線性優(yōu)化問題，采用MATLAB中的Quadprog函數(shù)對上文二次規(guī)劃問題進(jìn)行求解，可得到載荷6th導(dǎo)數(shù)平方積分最小的軌跡。值得注意的是，根據(jù)1.2節(jié)分析可知，系繩施加在載荷上的旋量是有限的，規(guī)劃問題并沒有將該約束問題考慮在內(nèi)。因此，規(guī)劃得到的載荷期望加速度可能會違反旋量約束。本文通過增加2個航跡點(diǎn)之間的時間間隔，減小載荷期望軌跡的速度和加速度，直到滿足旋量約束為止，得到了載荷的期望軌跡為rpd。

2.2 無人機(jī)軌跡規(guī)劃

根據(jù)2.1節(jié)中規(guī)劃的載荷軌跡，求解無人機(jī)的期望軌跡，實(shí)現(xiàn)通過無人機(jī)軌跡跟蹤間接控制載荷跟蹤期望軌跡的目的。假設(shè)2.1節(jié)求解的載荷期望軌跡為rpd,根據(jù)式(1)求得載荷運(yùn)動期望旋量為

(15)

期望旋量由系繩拉力提供，表達(dá)如下

ωp=WT

(16)

針對質(zhì)點(diǎn)型載荷，當(dāng)系統(tǒng)中不共面張緊系繩不少于3根時，載荷的位置可以通過無人機(jī)位置唯一確定。本文研究的多無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)的無人機(jī)數(shù)量不少于3架。因此，式(16)是一個不定方程組。希望找到一組最優(yōu)或者次優(yōu)的可行解，使得系繩總拉力最小，以減小對無人機(jī)的干擾。拉力優(yōu)化問題描述為

(17)

式中，φ是避免無人機(jī)之間碰撞而規(guī)定的2根系繩之間允許的最小夾角。該優(yōu)化問題利用MATLAB中的fmincon函數(shù)進(jìn)行求解，得到每根系繩的拉力方向qi，再根據(jù)系統(tǒng)的運(yùn)動學(xué)關(guān)系rid=rpd+Liqi，求得每架無人機(jī)的期望軌跡rid。

3 系統(tǒng)動力學(xué)與控制律設(shè)計

3.1 系統(tǒng)動力學(xué)

載荷和無人機(jī)的平動動力學(xué)如式(1)和式(2)，其中旋轉(zhuǎn)矩陣Ri為

(18)

其中，sφ和cφ分別是sinφ和cosφ的簡寫，φi、θi和ψi分別是無人機(jī)i的滾轉(zhuǎn)、俯仰和偏航角。假設(shè)系繩連接在無人機(jī)的質(zhì)心處，無人機(jī)的轉(zhuǎn)動動力學(xué)在整個系統(tǒng)中處于解耦狀態(tài)。因此，ith無人機(jī)的轉(zhuǎn)動動力學(xué)方程為

(19)

式中，li為電機(jī)中心到ith無人機(jī)本體系Fi原點(diǎn)之間的距離；u2i、u3i和u4i分別為無人機(jī)姿態(tài)控制3個方向的輸入；Ixi、Iyi和Izi分別為無人機(jī)沿本體系3個方向軸的轉(zhuǎn)動慣量。無人機(jī)的跟蹤控制采用內(nèi)外環(huán)的雙環(huán)控制模式，外環(huán)為位置控制環(huán)，使無人機(jī)實(shí)現(xiàn)對期望軌跡的跟蹤，并且產(chǎn)生內(nèi)環(huán)的期望姿態(tài)信號；內(nèi)環(huán)為姿態(tài)控制環(huán)，實(shí)現(xiàn)外環(huán)期望的升力指向。根據(jù)式(2)和式(18)，可以得到內(nèi)環(huán)跟蹤指令

(20)

式中，fi,d、φid、θid和ψid分別為fi、φi、θi和ψi的期望值；ui=fiRie3=[uxi;uyi;uzi]。

對于一個多無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)，所有無人機(jī)通過系繩與一個共同的載荷相連。因此，式(1)和式(2)中的系繩拉力項(xiàng)Tiqi是一個與無人機(jī)和載荷運(yùn)動參數(shù)都相關(guān)的高度非線性動態(tài)函數(shù)。為了后續(xù)仿真研究，需要計算系繩拉力Tiqi。這種多體耦合系統(tǒng)產(chǎn)生的內(nèi)部系繩拉力可以利用Udwadia-Kalaba方程進(jìn)行計算[11]。首先，ith無人機(jī)和載荷之間通過系繩i連接，因此他們之間存在如下運(yùn)動學(xué)約束

(21)

式中，rp,i=ri-rp；Li是系繩i的長度。對式(21)進(jìn)行二次微分得到

(22)

(23)

因此，總的系繩拉力可以表示為

(24)

式中，T=[T1q1;…;TNqN;ΣTiqi]，M=diag[m1,…,mN,mp]，A=[A1;…;AN]，b=[b1;…;bN]，?表示直積運(yùn)算，(*)+表示Moore-Penrose偽逆。但是，由于數(shù)值積分運(yùn)算存在累積誤差，仿真中系繩實(shí)際長度存在漂移，即

(25)

(26)

式中，α和β是反饋增益。把式(26)代入式(24)，得到

(27)

3.2 控制器設(shè)計

外環(huán)控制器設(shè)計使得無人機(jī)跟蹤期望軌跡。實(shí)際任務(wù)中，對完成任務(wù)的時間有很高的要求，且無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)本身是一種高度非線性和欠驅(qū)動的系統(tǒng)。因此，采用有限時間收斂的控制算法提高系統(tǒng)的跟蹤魯棒性具有實(shí)際意義。固定時間控制算法是一種特殊的有限時間收斂算法，除繼承了有限時間收斂的全部優(yōu)點(diǎn)外，還避免了有限時間收斂在遠(yuǎn)離平衡點(diǎn)處收斂速度慢以及收斂時間與初始狀態(tài)有關(guān)等缺點(diǎn)[14]。本文設(shè)計了一種新的固定時間收斂控制算法，基于積分滑模和齊次性原理，該算法不僅全局有限時間收斂，同時還避免了滑?？刂频亩墩駟栴}。設(shè)計積分滑模面si為

(28)

式中，vi0為ith無人機(jī)的真實(shí)初始速度；vid為期望速度；vid0為期望速度的初始值；u0i為待設(shè)計的名義控制項(xiàng)。設(shè)計具有全局魯棒性的積分滑?？刂祈?xiàng)usti為

(29)

式中，μ、λ、η是正的反饋增益；p是大于1的正數(shù)。名義控制項(xiàng)u0i設(shè)計為

(30)

式中，k1、k2、α1、α2、β1和β2為正實(shí)數(shù)，且

(31)

綜上，無人機(jī)外環(huán)控制設(shè)計為

ui=mi(usti+u0i+ge3)

(32)

無人機(jī)內(nèi)環(huán)姿態(tài)控制采用基于齊次性原理的固定時間控制方法，設(shè)計如下

(33)

(34)

(35)

式中，kp和kd表示正的反饋增益。

3.3 穩(wěn)定性分析

本節(jié)在證明系統(tǒng)閉環(huán)穩(wěn)定性之前，給出了固定時間穩(wěn)定的概念。

定義1[14]：如下系統(tǒng)

(36)

如果該系統(tǒng)是全局有限時間穩(wěn)定(globally finite-time stable)且收斂時間函數(shù)T(x0)是有界的，存在Tmax>0使得T(x0)≤Tmax對任意x0都滿足。那么，系統(tǒng)的原點(diǎn)是固定時間穩(wěn)定(fixed-time stable)的。

定理1：無人機(jī)系統(tǒng)在控制器(32)～(35)作用下，外環(huán)軌跡和內(nèi)環(huán)姿態(tài)跟蹤都能實(shí)現(xiàn)有限時間收斂，且收斂時間上界與初始條件無關(guān)，僅取決于控制器的參數(shù)。

證明：每架無人機(jī)的穩(wěn)定性證明類似，本文證明ith無人機(jī)的閉環(huán)穩(wěn)定。首先對積分滑模面si求導(dǎo)得到

(37)

進(jìn)一步得到

(38)

其中，di=-Tiqi/mi。類似文獻(xiàn)[15]中定理2的證明，當(dāng)控制器參數(shù)滿足如下條件

(39)

系統(tǒng)的狀態(tài)量si和ζi收斂到原點(diǎn)，收斂時間的上界是一個與控制器參數(shù)有關(guān)的常數(shù)，即系統(tǒng)固定時間收斂。收斂時間Tr滿足如下關(guān)系

(40)

其中

(41)

κ>0，且當(dāng)κ滿足如下關(guān)系時，收斂時間Tr的上界最小

κ=(n1/4μ/λ)2/(2p+1)

(42)

當(dāng)時間大于Tr時，無人機(jī)狀態(tài)到達(dá)積分滑模面，且之后一直保持在滑模面上運(yùn)動，無人機(jī)的平動動力學(xué)變?yōu)?/p>

(43)

根據(jù)文獻(xiàn)[16]中的基于固定時間齊次性原理，系統(tǒng)在控制器的作用下是固定時間穩(wěn)定的，收斂時間Ts滿足不等式

Ts≤2/(kv(1-η1))+2/(kv(η2-1))

(44)

式中，參數(shù)kv、η1和η2的定義可在文獻(xiàn)[16]中得到。綜上所述，無人機(jī)在有限時間Tr+Ts內(nèi)跟蹤上期望軌跡，且收斂時間的上界與初始條件無關(guān)，僅取決于控制器參數(shù)。同理，基于固定時間齊次性原理，無人機(jī)內(nèi)環(huán)姿態(tài)控制是固定時間收斂的，即無人機(jī)在固定時間內(nèi)跟蹤上期望姿態(tài)。

4 仿真驗(yàn)證

為了驗(yàn)證提出的軌跡規(guī)劃和跟蹤控制算法，本節(jié)分別對3架和10架無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)進(jìn)行了仿真驗(yàn)證。

4.1 3架無人機(jī)協(xié)同搬運(yùn)

3架無人機(jī)協(xié)同搬運(yùn)系統(tǒng)參數(shù)如表1所示。給定4個載荷的期望航跡點(diǎn)：rp1=[0;0;1]m,rp2=[10;0;1]m,rp3=[10;10;1]m,rp4=[0;10;1]m，經(jīng)過每個航跡點(diǎn)的時刻分別為0s, 5s, 10s, 15s。根據(jù)本文提出的載荷軌跡規(guī)劃方法，得到載荷經(jīng)過4個航跡點(diǎn)的最優(yōu)軌跡如圖4所示。控制器參數(shù)選?。害?1，λ=1，η=2，ρ=1.5，k1=2，k2=1，α1=1/2，α2=2/3，β1=3/2，β2=6/5，kp=2，kd=1。3架無人機(jī)的期望偏航角為ψid=0，i=1,2,3。懸掛載荷的初始位置為[-1;1;1] m，初始速度為[0;0;0] m/s。3根系繩初始時刻方向?yàn)椋篬0;1/sqrt(3);sqrt(2/3)], [-1/2;-1/(2sqrt(3));sqrt(2/3)], [1/2; -1/(2sqrt(3));sqrt(2/3)]。

表1 系統(tǒng)參數(shù)

圖4 經(jīng)過4個航跡點(diǎn)的懸掛載荷最優(yōu)軌跡Fig.4 Optimal trajectory of payload passing through four track points

基于本文設(shè)計的控制器，系統(tǒng)狀態(tài)跟蹤曲線如圖5～圖7所示。圖5表示3架無人機(jī)跟蹤期望軌跡誤差曲線，仿真結(jié)果表明，設(shè)計的無人機(jī)軌跡跟蹤控制器能夠?qū)崿F(xiàn)無人機(jī)軌跡的快速高精度跟蹤。圖6表示3架無人機(jī)期望姿態(tài)與實(shí)際姿態(tài)曲線，可以看出無人機(jī)始終處于一個小角度范圍的姿態(tài)機(jī)動，避免了無人機(jī)大角度機(jī)動造成失穩(wěn)等現(xiàn)象，所以設(shè)計的算法具有可實(shí)施性?；谧藨B(tài)控制器(33)～(35)，無人機(jī)能夠?qū)崿F(xiàn)姿態(tài)的快速穩(wěn)定跟蹤。圖7表示3架無人機(jī)協(xié)同搬運(yùn)的懸掛載荷的軌跡跟蹤誤差曲線，表明在無載荷主動控制的情形下，基于本文提出的軌跡規(guī)劃和無人機(jī)姿軌控制方法，仍然能夠?qū)崿F(xiàn)懸掛載荷軌跡的穩(wěn)定跟蹤控制。對比圖5和圖7可以看出，懸掛載荷跟蹤期望軌跡過程中振動超調(diào)量大于無人機(jī)，這是因?yàn)檩d荷是通過被動控制來實(shí)現(xiàn)軌跡跟蹤的。圖8所示為無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)3D圖。

4.2 10架無人機(jī)協(xié)同搬運(yùn)

10架無人機(jī)協(xié)同搬運(yùn)系統(tǒng)懸掛載荷質(zhì)量為mp=5kg,初始時刻10架無人機(jī)均勻分布在半徑為R=2m的圓周上，相鄰系繩之間的夾角為15°，無人機(jī)之間避碰允許的系繩最小夾角為φ=10°，由幾何

圖5 3架無人機(jī)軌跡跟蹤誤差曲線Fig.5 Trajectory tracking errors of three UAVs

圖6 3架無人機(jī)姿態(tài)跟蹤誤差曲線Fig.6 Attitude tracking errors of three UAVs

圖7 3架無人機(jī)協(xié)同搬運(yùn)載荷軌跡跟蹤誤差曲線Fig.7 Trajectory tracking errors of payload with three UAVs cooperative transportation

圖8 3架無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)3D圖Fig.8 3D view of tether-suspended cooperative transportation of three UAVs

關(guān)系計算系繩長度為Li=Rsin(pi/10)/sin(φπ/360)，無人機(jī)參數(shù)與3架無人機(jī)協(xié)同搬運(yùn)相同，如表1所示。懸掛載荷的軌跡規(guī)劃也同4.1 節(jié)。仿真結(jié)果如圖9和圖10所示。10架自重1kg的無人機(jī)能夠?qū)崿F(xiàn)協(xié)同搬運(yùn)5kg的載荷，在設(shè)計的內(nèi)外環(huán)固定時間控制作用下，載荷能夠穩(wěn)定跟蹤期望軌跡，證明了設(shè)計算法的有效性。

圖9 10架無人機(jī)協(xié)同搬運(yùn)載荷軌跡跟蹤誤差曲線Fig.9 Trajectory tracking errors of payload with ten UAVs cooperative transportation

圖10 10架無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)3D圖Fig.10 3D view of tether-suspended cooperative transportation of ten UAVs

5 結(jié)論

本文針對多無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)跟蹤控制問題，分析了系統(tǒng)動態(tài)旋量空間，規(guī)劃了載荷與無人機(jī)的可行運(yùn)動軌跡，提出了一種固定時間快速穩(wěn)定跟蹤控制算法。仿真結(jié)果表明：

1)系統(tǒng)構(gòu)型和運(yùn)動加速度影響載荷旋量空間的大小。本文首次分析了系統(tǒng)動態(tài)旋量空間，研究了系統(tǒng)構(gòu)型、運(yùn)動加速度與載荷旋量之間的關(guān)系，為后續(xù)研究軌跡規(guī)劃和跟蹤控制問題奠定了基礎(chǔ)。

2)基于系統(tǒng)微分平坦性，通過優(yōu)化載荷軌跡使其6th導(dǎo)數(shù)最小，得到最小化snap的無人機(jī)期望軌跡，使無人機(jī)運(yùn)動平滑且節(jié)省能量。仿真結(jié)果驗(yàn)證了軌跡規(guī)劃算法的有效性。

3)本文設(shè)計了一種無人機(jī)姿軌固定時間跟蹤控制算法，實(shí)現(xiàn)了無人機(jī)對期望軌跡的快速穩(wěn)定跟蹤，同時間接控制懸掛載荷快速穩(wěn)定跟蹤期望軌跡。

本文假設(shè)懸掛載荷為質(zhì)點(diǎn)，忽略了載荷的幾何構(gòu)型與姿態(tài)運(yùn)動，后續(xù)研究將考慮一般形狀的載荷。