多無人機繩索懸掛協同搬運固定時間控制

劉 亞，黃攀峰，張 帆

(1.西北工業大學航天學院智能機器人研究中心，西安 710072； 2.西北工業大學航天飛行動力學技術重點實驗室，西安 710072)

0 引言

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

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

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

1 有效旋量空間分析

1.1 系統平衡方程

多無人機繩索懸掛式協同搬運系統由N個無人機、懸掛載荷(本文研究質點模型)和連接無人機與載荷的系繩構成，如圖1所示。

圖1 多無人機繩索懸掛式協同搬運系統坐標系Fig.1 Coordinate system of multiple UAVs tether suspended cooperative transportation system

根據牛頓-歐拉定理，動力學方程為

(1)

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

(2)

式中，mi為無人機i的質量；ri為無人機i在慣性系F0下的位置；fi為無人機的升力；Ri為無人機本體系Fi到慣性系F0的旋轉矩陣。

1.2 有效旋量空間

懸掛載荷是一個無主動控制的一般性物體，通過多系繩拉力提供任務空間旋量，對載荷實施被動控制，系繩拉力由無人機相對載荷的運動產生。因此，系統任務空間旋量由無人機升力空間到系繩拉力和系繩拉力到任務空間2個映射確定。無人機升力空間為

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

(3)

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

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保證系繩處于張緊狀態，Timax由系繩本身屬性和無人機共同決定。本研究中假設系繩本身的強度足夠，則系繩拉力允許的最大值Timax由無人機的運動和無人機允許的最大升力決定，根據無人機動力學可得

(5)

因此，系繩拉力與無人機推力之間的關系為

(6)

其中

(7)

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

(8)

考慮準靜態條件下，系繩i允許的最大拉力為

(9)

系繩拉力構成的系統有效旋量空間表示如下

(10)

其中

ΔTi=Timax-Timin

(11)

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

1.3 系統魯棒性分析

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

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

圖2 系統不同構型下有效旋量空間與重力旋量幾何關系Fig.2 Geometric relationship between effective wrench space and gravity wrench space under different configuration

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

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

2 系統軌跡規劃

2.1 載荷軌跡規劃

本文研究問題歸納為：通過對無人機施加主動控制，實現搬運載荷跟蹤期望軌跡的跟蹤控制問題。對于一個給定的多無人機懸掛式協同搬運系統，已知載荷需要經歷的航跡點，首先需要解決的問題是規劃出載荷的軌跡。

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

(12)

最優規劃問題即是最小化載荷軌跡rp(t)的6th導數平方的積分，規劃問題表達如下

(13)

此優化問題可以整理為二次規劃問題，形式如下

(14)

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

2.2 無人機軌跡規劃

根據2.1節中規劃的載荷軌跡，求解無人機的期望軌跡，實現通過無人機軌跡跟蹤間接控制載荷跟蹤期望軌跡的目的。假設2.1節求解的載荷期望軌跡為rpd,根據式(1)求得載荷運動期望旋量為

(15)

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

ωp=WT

(16)

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

(17)

式中，φ是避免無人機之間碰撞而規定的2根系繩之間允許的最小夾角。該優化問題利用MATLAB中的fmincon函數進行求解，得到每根系繩的拉力方向qi，再根據系統的運動學關系rid=rpd+Liqi，求得每架無人機的期望軌跡rid。

3 系統動力學與控制律設計

3.1 系統動力學

載荷和無人機的平動動力學如式(1)和式(2)，其中旋轉矩陣Ri為

(18)

其中，sφ和cφ分別是sinφ和cosφ的簡寫，φi、θi和ψi分別是無人機i的滾轉、俯仰和偏航角。假設系繩連接在無人機的質心處，無人機的轉動動力學在整個系統中處于解耦狀態。因此，ith無人機的轉動動力學方程為

(19)

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

(20)

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

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

(21)

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

(22)

(23)

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

(24)

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

(25)

(26)

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

(27)

3.2 控制器設計

外環控制器設計使得無人機跟蹤期望軌跡。實際任務中，對完成任務的時間有很高的要求，且無人機系繩懸掛式協同搬運系統本身是一種高度非線性和欠驅動的系統。因此，采用有限時間收斂的控制算法提高系統的跟蹤魯棒性具有實際意義。固定時間控制算法是一種特殊的有限時間收斂算法，除繼承了有限時間收斂的全部優點外，還避免了有限時間收斂在遠離平衡點處收斂速度慢以及收斂時間與初始狀態有關等缺點[14]。本文設計了一種新的固定時間收斂控制算法，基于積分滑模和齊次性原理，該算法不僅全局有限時間收斂，同時還避免了滑模控制的抖振問題。設計積分滑模面si為

(28)

式中，vi0為ith無人機的真實初始速度；vid為期望速度；vid0為期望速度的初始值；u0i為待設計的名義控制項。設計具有全局魯棒性的積分滑模控制項usti為

(29)

式中，μ、λ、η是正的反饋增益；p是大于1的正數。名義控制項u0i設計為

(30)

式中，k1、k2、α1、α2、β1和β2為正實數，且

(31)

綜上，無人機外環控制設計為

ui=mi(usti+u0i+ge3)

(32)

無人機內環姿態控制采用基于齊次性原理的固定時間控制方法，設計如下

(33)

(34)

(35)

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

3.3 穩定性分析

本節在證明系統閉環穩定性之前，給出了固定時間穩定的概念。

定義1[14]：如下系統

(36)

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

定理1：無人機系統在控制器(32)～(35)作用下，外環軌跡和內環姿態跟蹤都能實現有限時間收斂，且收斂時間上界與初始條件無關，僅取決于控制器的參數。

證明：每架無人機的穩定性證明類似，本文證明ith無人機的閉環穩定。首先對積分滑模面si求導得到

(37)

進一步得到

(38)

其中，di=-Tiqi/mi。類似文獻[15]中定理2的證明，當控制器參數滿足如下條件

(39)

系統的狀態量si和ζi收斂到原點，收斂時間的上界是一個與控制器參數有關的常數，即系統固定時間收斂。收斂時間Tr滿足如下關系

(40)

其中

(41)

κ>0，且當κ滿足如下關系時，收斂時間Tr的上界最小

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

(42)

當時間大于Tr時，無人機狀態到達積分滑模面，且之后一直保持在滑模面上運動，無人機的平動動力學變為

(43)

根據文獻[16]中的基于固定時間齊次性原理，系統在控制器的作用下是固定時間穩定的，收斂時間Ts滿足不等式

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

(44)

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

4 仿真驗證

為了驗證提出的軌跡規劃和跟蹤控制算法，本節分別對3架和10架無人機系繩懸掛式協同搬運系統進行了仿真驗證。

4.1 3架無人機協同搬運

3架無人機協同搬運系統參數如表1所示。給定4個載荷的期望航跡點：rp1=[0;0;1]m,rp2=[10;0;1]m,rp3=[10;10;1]m,rp4=[0;10;1]m，經過每個航跡點的時刻分別為0s, 5s, 10s, 15s。根據本文提出的載荷軌跡規劃方法，得到載荷經過4個航跡點的最優軌跡如圖4所示。控制器參數選取：μ=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架無人機的期望偏航角為ψid=0，i=1,2,3。懸掛載荷的初始位置為[-1;1;1] m，初始速度為[0;0;0] m/s。3根系繩初始時刻方向為：[0;1/sqrt(3);sqrt(2/3)], [-1/2;-1/(2sqrt(3));sqrt(2/3)], [1/2; -1/(2sqrt(3));sqrt(2/3)]。

表1 系統參數

圖4 經過4個航跡點的懸掛載荷最優軌跡Fig.4 Optimal trajectory of payload passing through four track points

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

4.2 10架無人機協同搬運

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

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

圖6 3架無人機姿態跟蹤誤差曲線Fig.6 Attitude tracking errors of three UAVs

圖7 3架無人機協同搬運載荷軌跡跟蹤誤差曲線Fig.7 Trajectory tracking errors of payload with three UAVs cooperative transportation

圖8 3架無人機系繩懸掛式協同搬運系統3D圖Fig.8 3D view of tether-suspended cooperative transportation of three UAVs

關系計算系繩長度為Li=Rsin(pi/10)/sin(φπ/360)，無人機參數與3架無人機協同搬運相同，如表1所示。懸掛載荷的軌跡規劃也同4.1 節。仿真結果如圖9和圖10所示。10架自重1kg的無人機能夠實現協同搬運5kg的載荷，在設計的內外環固定時間控制作用下，載荷能夠穩定跟蹤期望軌跡，證明了設計算法的有效性。

圖9 10架無人機協同搬運載荷軌跡跟蹤誤差曲線Fig.9 Trajectory tracking errors of payload with ten UAVs cooperative transportation

圖10 10架無人機系繩懸掛式協同搬運系統3D圖Fig.10 3D view of tether-suspended cooperative transportation of ten UAVs

5 結論

本文針對多無人機系繩懸掛式協同搬運系統跟蹤控制問題，分析了系統動態旋量空間，規劃了載荷與無人機的可行運動軌跡，提出了一種固定時間快速穩定跟蹤控制算法。仿真結果表明：

1)系統構型和運動加速度影響載荷旋量空間的大小。本文首次分析了系統動態旋量空間，研究了系統構型、運動加速度與載荷旋量之間的關系，為后續研究軌跡規劃和跟蹤控制問題奠定了基礎。

2)基于系統微分平坦性，通過優化載荷軌跡使其6th導數最小，得到最小化snap的無人機期望軌跡，使無人機運動平滑且節省能量。仿真結果驗證了軌跡規劃算法的有效性。

3)本文設計了一種無人機姿軌固定時間跟蹤控制算法，實現了無人機對期望軌跡的快速穩定跟蹤，同時間接控制懸掛載荷快速穩定跟蹤期望軌跡。

本文假設懸掛載荷為質點，忽略了載荷的幾何構型與姿態運動，后續研究將考慮一般形狀的載荷。