多無人機(jī)繩索懸掛協(xié)同搬運(yùn)固定時(shí)間控制

劉 亞，黃攀峰，張 帆

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

0 引言

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

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

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

1 有效旋量空間分析

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

多無人機(jī)繩索懸掛式協(xié)同搬運(yùn)系統(tǒng)由N個(gè)無人機(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ù)牛頓-歐拉定理，動(dòng)力學(xué)方程為

(1)

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

(2)

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

1.2 有效旋量空間

懸掛載荷是一個(gè)無主動(dòng)控制的一般性物體，通過多系繩拉力提供任務(wù)空間旋量，對(duì)載荷實(shí)施被動(dòng)控制，系繩拉力由無人機(jī)相對(duì)載荷的運(yùn)動(dòng)產(chǎn)生。因此，系統(tǒng)任務(wù)空間旋量由無人機(jī)升力空間到系繩拉力和系繩拉力到任務(wù)空間2個(gè)映射確定。無人機(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)動(dòng)和無人機(jī)允許的最大升力決定，根據(jù)無人機(jī)動(dòng)力學(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)成的有效旋量空間是一個(gè)凸包。

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

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

考慮3架無人機(jī)協(xié)同搬運(yùn)情況下，系繩不同面外角θi對(duì)有效旋量空間的影響，以及不同質(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]，針對(duì)不同的面外角C1=[θ1,θ2,θ3]，系統(tǒng)的有效旋量空間與載荷的重力旋量關(guān)系如圖2所示。圖2中，黑色六面體表示系統(tǒng)的有效旋量空間，不同顏色的小球是以對(duì)應(yīng)的載荷重力為球心，拉力容許裕度為半徑。小球在六面體內(nèi)表示系統(tǒng)在靜平衡狀態(tài)下能搬運(yùn)對(duì)應(yīng)質(zhì)量的載荷；反之，表示系統(tǒng)在靜平衡狀態(tài)下不能搬運(yùn)對(duì)應(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)允許的最大加速度減小且魯棒性降低，在外界擾動(dòng)作用下系統(tǒng)容易出現(xiàn)不穩(wěn)定現(xiàn)象。所以，對(duì)于具體的多無人機(jī)懸掛式協(xié)同搬運(yùn)系統(tǒng)，確定系統(tǒng)構(gòu)型以及搬運(yùn)載荷范圍是具有實(shí)際應(yīng)用價(jià)值的。

圖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ī)動(dòng)加速度下對(duì)有效旋量空間的影響，以及不同質(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ī)動(dòng)加速度越大，系統(tǒng)能搬運(yùn)的載荷質(zhì)量越小。同時(shí)，觀察圖3發(fā)現(xiàn)，x軸方向加速度越大，系統(tǒng)沿該方向的拉力容許裕度越小，因此系統(tǒng)運(yùn)動(dòng)魯棒性越差。

圖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ī)劃

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

根據(jù)文獻(xiàn)[5]可得，多無人機(jī)系繩懸掛式協(xié)同搬運(yùn)系統(tǒng)是一個(gè)微分平坦系統(tǒng)，通過優(yōu)化載荷軌跡使其6th導(dǎo)數(shù)最小，可以得到最小化snap的無人機(jī)期望軌跡，使無人機(jī)運(yùn)動(dòng)平滑且節(jié)省能量。載荷軌跡規(guī)劃類似于無人機(jī)航跡規(guī)劃，參考文獻(xiàn)[12]中無人機(jī)航跡規(guī)劃方法，規(guī)劃載荷經(jīng)歷約定航跡點(diǎn)的最優(yōu)軌跡。假設(shè)載荷軌跡的m+1個(gè)航跡點(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ī)劃問題是一個(gè)成熟的線性優(yōu)化問題，采用MATLAB中的Quadprog函數(shù)對(duì)上文二次規(guī)劃問題進(jìn)行求解，可得到載荷6th導(dǎo)數(shù)平方積分最小的軌跡。值得注意的是，根據(jù)1.2節(jié)分析可知，系繩施加在載荷上的旋量是有限的，規(guī)劃問題并沒有將該約束問題考慮在內(nèi)。因此，規(guī)劃得到的載荷期望加速度可能會(huì)違反旋量約束。本文通過增加2個(gè)航跡點(diǎn)之間的時(shí)間間隔，減小載荷期望軌跡的速度和加速度，直到滿足旋量約束為止，得到了載荷的期望軌跡為rpd。

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

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

(15)

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

ωp=WT

(16)

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

(17)

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

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

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

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

(18)

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

(19)

式中，li為電機(jī)中心到ith無人機(jī)本體系Fi原點(diǎn)之間的距離；u2i、u3i和u4i分別為無人機(jī)姿態(tài)控制3個(gè)方向的輸入；Ixi、Iyi和Izi分別為無人機(jī)沿本體系3個(gè)方向軸的轉(zhuǎn)動(dòng)慣量。無人機(jī)的跟蹤控制采用內(nèi)外環(huán)的雙環(huán)控制模式，外環(huán)為位置控制環(huán)，使無人機(jī)實(shí)現(xiàn)對(duì)期望軌跡的跟蹤，并且產(chǎn)生內(nèi)環(huán)的期望姿態(tài)信號(hào)；內(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]。

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

(21)

式中，rp,i=ri-rp；Li是系繩i的長度。對(duì)式(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è)計(jì)

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

(28)

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

(29)

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

(30)

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

(31)

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

ui=mi(usti+u0i+ge3)

(32)

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

(33)

(34)

(35)

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

3.3 穩(wěn)定性分析

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

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

(36)

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

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

證明：每架無人機(jī)的穩(wěn)定性證明類似，本文證明ith無人機(jī)的閉環(huán)穩(wěn)定。首先對(duì)積分滑模面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í)間的上界是一個(gè)與控制器參數(shù)有關(guān)的常數(shù)，即系統(tǒng)固定時(shí)間收斂。收斂時(shí)間Tr滿足如下關(guān)系

(40)

其中

(41)

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

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

(42)

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

(43)

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

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

(44)

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

4 仿真驗(yàn)證

為了驗(yàn)證提出的軌跡規(guī)劃和跟蹤控制算法，本節(jié)分別對(duì)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個(gè)載荷的期望航跡點(diǎn)：rp1=[0;0;1]m,rp2=[10;0;1]m,rp3=[10;10;1]m,rp4=[0;10;1]m，經(jīng)過每個(gè)航跡點(diǎn)的時(shí)刻分別為0s, 5s, 10s, 15s。根據(jù)本文提出的載荷軌跡規(guī)劃方法，得到載荷經(jīng)過4個(gè)航跡點(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根系繩初始時(shí)刻方向?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個(gè)航跡點(diǎn)的懸掛載荷最優(yōu)軌跡Fig.4 Optimal trajectory of payload passing through four track points

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

圖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é)論

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

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

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

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

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