基于加速度補(bǔ)償?shù)臒o(wú)人機(jī)吊掛飛行抗擺控制

焦海林，郭玉英，朱正為

（西南科技大學(xué)信息工程學(xué)院，四川綿陽(yáng) 621010）

（*通信作者電子郵箱2513386874@qq.com）

0 引言

在復(fù)雜、危險(xiǎn)環(huán)境中吊艙精準(zhǔn)投放等任務(wù)需求背景下，對(duì)無(wú)人機(jī)（Unmanned Aerial Vehicle，UAV）吊掛飛行控制系統(tǒng)的安全性要求越來(lái)越高。抑制載荷擺動(dòng)是吊掛飛行控制面臨的一個(gè)實(shí)際問(wèn)題，吊掛載荷顯著地改變了UAV 的動(dòng)態(tài)特性［1-2］，所以UAV吊掛飛行不僅具有挑戰(zhàn)性而且具有危險(xiǎn)性。

目前，國(guó)內(nèi)一些學(xué)者在UAV 吊掛飛行控制方面做了一些研究，如鮮斌教授團(tuán)隊(duì)建立了四旋翼UAV 吊掛系統(tǒng)模型，并通過(guò)李雅普諾夫穩(wěn)定性分析和拉塞爾不變性原理對(duì)閉環(huán)系統(tǒng)的穩(wěn)定性進(jìn)行了證明，最后通過(guò)數(shù)值仿真驗(yàn)證了所設(shè)計(jì)的非線性控制器的有效性［3-5］；文獻(xiàn)［6-8］通過(guò)相平面分析法構(gòu)造了分段式加速度軌跡，并基于反步法設(shè)計(jì)了一種非線性跟蹤控制器，仿真驗(yàn)證了該方法的可行性；文獻(xiàn)［9］利用擴(kuò)張狀態(tài)觀測(cè)器估計(jì)四旋翼UAV 吊掛飛行過(guò)程中的擾動(dòng)，并通過(guò)自抗擾控制方法抑制了載荷引起的干擾；文獻(xiàn)［10］基于滑?？刂圃O(shè)計(jì)了四旋翼UAV 吊掛飛行控制系統(tǒng)，仿真結(jié)果驗(yàn)證了滑?？刂茖?duì)負(fù)載擺動(dòng)和系統(tǒng)不確定性具有很強(qiáng)的魯棒性，實(shí)現(xiàn)了軌跡跟蹤控制目標(biāo)；文獻(xiàn)［11］所設(shè)計(jì)的軌跡跟蹤控制器能在吊掛載荷產(chǎn)生很大擺動(dòng)情況下實(shí)現(xiàn)運(yùn)動(dòng)軌跡的有效跟蹤，仿真驗(yàn)證了所研究飛行控制系統(tǒng)的有效性。上述方法均以建立較為精確的系統(tǒng)模型為前提，但由于UAV 吊掛系統(tǒng)的非線性、強(qiáng)耦合性、欠驅(qū)動(dòng)性等特點(diǎn)，以及飛行過(guò)程中的干擾，如風(fēng)擾，很難獲得系統(tǒng)精確的數(shù)學(xué)模型。

國(guó)外一些研究團(tuán)隊(duì)針對(duì)載荷抗擺控制方法也開(kāi)展了很多研究，如基于機(jī)器視覺(jué)、觀測(cè)器、角度/力傳感器等抗擺控制方法，并被廣泛應(yīng)用于先進(jìn)工業(yè)生產(chǎn)領(lǐng)域。文獻(xiàn)［12］利用時(shí)延，在控制反饋中通過(guò)測(cè)量載荷擺角以產(chǎn)生周期性繩索長(zhǎng)度來(lái)調(diào)節(jié)阻尼載荷振顫；文獻(xiàn)［13］利用鋼索角度，設(shè)計(jì)了智能起重機(jī)輔助裝置，實(shí)現(xiàn)了重型載荷水平和垂直位置的自適應(yīng)控制；文獻(xiàn)［14］中通過(guò)一種基于互聯(lián)與阻尼配置的無(wú)源控制方法減小了載荷擺角?？梢钥闯?，這些方法都需要借助傳感器測(cè)量繩索擺動(dòng)角度，明顯增加了控制系統(tǒng)硬件負(fù)擔(dān)。此外，對(duì)傳感器的高精度、觀測(cè)器的高魯棒性要求，以及機(jī)器視覺(jué)不易于工程應(yīng)用，使得這些方法不具有普適性。文獻(xiàn)［15］對(duì)并聯(lián)機(jī)床首次引入了加速度補(bǔ)償技術(shù)，利用Stewart 型并聯(lián)機(jī)床安裝的移動(dòng)機(jī)器人來(lái)證明這種方法的可行性。根據(jù)運(yùn)動(dòng)加速度，在補(bǔ)償控制方法作用下，平臺(tái)通過(guò)做傾斜運(yùn)動(dòng)以補(bǔ)償作用在被運(yùn)輸物體上的任何外力和扭矩，從而保持被運(yùn)輸物體的平穩(wěn)運(yùn)輸。文獻(xiàn)［16］利用加速度補(bǔ)償技術(shù)保證了被運(yùn)輸?shù)囊姿槲矬w的安全性；Chen在文獻(xiàn)［17］中提出了一種基于加速度補(bǔ)償?shù)姆椒▉?lái)降低物體與平臺(tái)間的摩擦力，繼而在文獻(xiàn)［18］中通過(guò)加速度補(bǔ)償技術(shù)實(shí)現(xiàn)了載荷傳輸過(guò)程中的抗擺控制，該方法無(wú)需傳感器，簡(jiǎn)單易用。

綜上分析，本文在文獻(xiàn)［18］的基礎(chǔ)上設(shè)計(jì)了一種基于加速度補(bǔ)償?shù)乃男鞺AV 吊掛飛行抗擺控制方法，用于抑制UAV 吊掛飛行過(guò)程中吊掛物體產(chǎn)生的擺動(dòng)干擾，引入載荷的廣義運(yùn)動(dòng)誤差來(lái)估計(jì)載荷擺動(dòng)帶來(lái)的干擾，接著利用UAV 的運(yùn)動(dòng)加速度來(lái)補(bǔ)償控制效果。通過(guò)構(gòu)建新的能量函數(shù)保證了本文控制器的跟蹤性能，并結(jié)合仿真驗(yàn)證了該抗擺方法的有效性。

1 四旋翼吊掛系統(tǒng)動(dòng)力學(xué)模型

四旋翼UAV 吊掛系統(tǒng)如圖1 所示，為了確定四旋翼UAV和載荷在空間中的位置和姿態(tài)，建立了如圖1所示的3種坐標(biāo)系，即慣性坐標(biāo)系OEXEYEZE、機(jī)體坐標(biāo)系OBXBYBZB和載荷坐標(biāo)系OSXSYSZS，且均滿足右手螺旋定則。擺角φL和θL描述吊掛載荷的角位移，其中φL表示吊掛繩索在pxz平面的投影與ZB軸的夾角，θL表示吊掛繩索在pyz平面的投影與ZB軸的夾角。

圖1 四旋翼吊掛系統(tǒng)示意圖Fig.1 Schematic diagram of quadrotor slung-load system

假設(shè)吊掛繩索是沒(méi)有粘性摩擦的無(wú)質(zhì)量剛性繩索，長(zhǎng)度為l，則四旋翼UAV 位置與載荷位置XL=的關(guān)系可以表示為：

利用拉格朗日方法［19］建立四旋翼UAV 吊掛系統(tǒng)非線性動(dòng)力學(xué)方程（忽略空氣阻力）為：

其中，L=T-U為拉格朗日函數(shù)，T和U分別為系統(tǒng)的動(dòng)能和勢(shì)能；q表示系統(tǒng)的廣義坐標(biāo)；Fext表示系統(tǒng)受到的廣義力，包括合外力和合外力矩。

系統(tǒng)的總動(dòng)能T由平動(dòng)動(dòng)能和轉(zhuǎn)動(dòng)動(dòng)能之和表示，即

系統(tǒng)的總勢(shì)能U表示為（以地面為零勢(shì)能面）：

系統(tǒng)的廣義坐標(biāo)和廣義力表示為：

其中：mQ為四旋翼UAV 的質(zhì)量；mL為吊掛物體的質(zhì)量；Ixx、Iyy、Izz分別為四旋翼UAV 的轉(zhuǎn)動(dòng)慣量；ωx、ωy、ωz分別為四旋翼UAV 的角速度；φ、θ、ψ分別為慣性坐標(biāo)系下的四旋翼歐拉角；Fx、Fy、Fz和τx、τy、τz分別為沿機(jī)體坐標(biāo)系三個(gè)軸的合外力和合外力矩；g為重力加速度。

將式（3）、式（4）代入式（2）可得四旋翼UAV 吊掛系統(tǒng)動(dòng)力學(xué)方程：

在UAV 飛行過(guò)程中，當(dāng)?shù)鯍煳矬w以小角度擺動(dòng)時(shí)，可作如式（13）近似：

結(jié)合式（13），對(duì)式（8）和式（9）線性化得：

把吊掛物體與繩索整體看作一個(gè)單擺系統(tǒng)，φL和θL可以在兩個(gè)擺動(dòng)平面內(nèi)完全解耦，因此，令UAV縱向加速度和橫向加速度分別為0，解耦后兩個(gè)擺角方程為：

2 控制器設(shè)計(jì)

由式（15）可知，UAV 運(yùn)動(dòng)加速度會(huì)影響擺角，因此可通過(guò)UAV 水平方向和豎直方向的加速度對(duì)擺角進(jìn)行控制，為此，采用加速度補(bǔ)償方法設(shè)計(jì)抗擺控制器以抑制載荷擺動(dòng)，即將四旋翼UAV 的位置信息和載荷的擺角信息轉(zhuǎn)化為載荷的廣義運(yùn)動(dòng)信號(hào)，把抑制載荷擺動(dòng)的控制轉(zhuǎn)化為對(duì)四旋翼UAV運(yùn)動(dòng)的控制。

2.1 能量函數(shù)構(gòu)建

為便于控制器設(shè)計(jì)，將式（3）～（7）改寫(xiě)為歐拉-龐卡萊形式的動(dòng)力學(xué)方程：

式（18）表明以F(t)作為輸入、x(t)為輸出、E(t)為儲(chǔ)能函數(shù)的吊掛系統(tǒng)是一個(gè)無(wú)源的、耗散的系統(tǒng)［20］，考慮系統(tǒng)的欠驅(qū)動(dòng)性質(zhì)，中不包含與載荷擺角φL和θL相關(guān)的信息，為了構(gòu)造抗擺控制器，需要在控制率中添加與系統(tǒng)參數(shù)相關(guān)的項(xiàng)，但這樣容易導(dǎo)致控制系統(tǒng)對(duì)模型參數(shù)不確定性比較敏感，魯棒性變差。因此，為解決上述問(wèn)題，本文構(gòu)造一種新的儲(chǔ)能函數(shù)E′：

其中，ES為新加的能量部分。以F(t)作為輸入、載荷的廣義運(yùn)動(dòng)位移xP(t)為輸出，E′(t)為新的儲(chǔ)能函數(shù)的吊掛系統(tǒng)仍是無(wú)源的、耗散的。式（19）所示無(wú)源性的主要優(yōu)點(diǎn)在于，將UAV 運(yùn)動(dòng)與載荷抗擺轉(zhuǎn)換為對(duì)載荷廣義運(yùn)動(dòng)信號(hào)的調(diào)節(jié)控制，增強(qiáng)了四旋翼UAV 運(yùn)動(dòng)與載荷擺動(dòng)間的耦合關(guān)系，為提高控制系統(tǒng)性能提供了有力保障。

由式（1）的幾何關(guān)系，令 cos(θL)sin(φL)=ω1，sin(θL)=ω2，cos(θL)cos(φL)=ω3，可假設(shè)載荷的廣義運(yùn)動(dòng)信號(hào)為xP=[xlp(t)，ylp(t)，zlp(t)]T，即

其中，f(ωi)(i=1，2，3)為待確定的標(biāo)量函數(shù)。該式表明載荷的廣義運(yùn)動(dòng)信號(hào)既包含有驅(qū)的四旋翼位移信息，又能反映無(wú)驅(qū)的載荷擺動(dòng)ωi(t)。

由式（18）～（21）得：

將式（5）～（7）代入式（22），化簡(jiǎn)可得：

由式（19）、（21）可以看出，載荷的廣義位移信號(hào)在平衡點(diǎn)處滿足xL(t)=xQ，所以取f()ω1=k1ω1，即

同理，有

令k1=k2=k3，將式（24）、（25）代入式（23）并對(duì)時(shí)間積分得：

要使ES是正定函數(shù)，需使k=-k1＞0。

所以，將式（17）、（26）代入式（19）可得新的儲(chǔ)能函數(shù)為：

定義載荷的廣義運(yùn)動(dòng)誤差為ε=[εx，εy，εz]T，四旋翼的運(yùn)動(dòng)誤差為ei=i-id(i=x，y，z)，其中

選取李雅普諾夫函數(shù)V(t)：

求導(dǎo)得

解得控制律的表達(dá)式形式為

其中，ka，kb，kc∈R+為正的控制增益。若擺角廣義運(yùn)動(dòng)誤差收斂于0，所有系統(tǒng)狀態(tài)將漸近收斂到期望參考狀態(tài)。

2.2 基于加速度補(bǔ)償?shù)目箶[控制設(shè)計(jì)

式（32）中的控制器可以使得UAV 漸近收斂到目標(biāo)位置，而擺角廣義運(yùn)動(dòng)誤差收斂于0 意味著可以把載荷的抗擺控制轉(zhuǎn)換為四旋翼UAV 運(yùn)動(dòng)信號(hào)的調(diào)節(jié)控制，具體控制方法如圖2 所示。在圖2（a）中，“虛擬工具”的桿長(zhǎng)為l，虛擬工具繞虛擬旋轉(zhuǎn)點(diǎn)以最優(yōu)傾斜角旋轉(zhuǎn)類比得到圖2（b）中四旋翼UAV新的補(bǔ)償軌跡。

圖2 加速度補(bǔ)償原理Fig.2 Principle of acceleration compensation

假設(shè)四旋翼UAV 吊掛載荷從位置Ⅰ水平運(yùn)動(dòng)到位置Ⅱ，初始值，即四旋翼UAV 吊掛系統(tǒng)的廣義速度和廣義加速度均為0，通過(guò)UAV 在水平方向和豎直方向上的加速度定義最優(yōu)傾斜角［17］為：

利用加速度補(bǔ)償原理對(duì)UAV 參考輸入軌跡進(jìn)行修正，能最大限度地抑制因UAV 快速運(yùn)動(dòng)造成吊掛物體擺動(dòng)而對(duì)UAV 系統(tǒng)產(chǎn)生的干擾，因此，在抑制載荷擺動(dòng)的同時(shí)保證了UAV按期望軌跡飛行。

2.3 加速度補(bǔ)償效果

由式（33）、（34）得經(jīng)過(guò)補(bǔ)償后的UAV運(yùn)行軌跡，如圖3所示。圖3（a）為由虛擬工具建立的原始輸入軌跡，虛擬工具做水平運(yùn)動(dòng)且保持吊掛物體軌跡為一條直線；圖3（b）為四旋翼UAV 通過(guò)改變其水平方向和垂直方向的運(yùn)動(dòng)狀態(tài)來(lái)保持載荷運(yùn)動(dòng)軌跡為一條直線，即通過(guò)補(bǔ)償U(kuò)AV 運(yùn)動(dòng)加速度來(lái)修正UAV運(yùn)動(dòng)軌跡，以實(shí)現(xiàn)抑制載荷擺動(dòng)。

圖3 加速度補(bǔ)償效果Fig.3 Effect of acceleration compensation

3 仿真結(jié)果與分析

通過(guò)Matlab/Simulink 仿真來(lái)驗(yàn)證本文所研究的抗擺飛行控制方法的有效性。采樣周期h設(shè)為0.001 s，對(duì)采用加速度補(bǔ)償前后的控制效果進(jìn)行對(duì)比分析，仿真參數(shù)如表1所示。

表1 仿真中使用的模型參數(shù)Table 1 Model parameters used in simulations

3.1 直線軌跡跟蹤

設(shè)UAV由初始位置（0，0，0）運(yùn)動(dòng)到位置（20，0，0），初始狀態(tài)，仿真時(shí)間為10 s，仿真結(jié)果如圖4～6所示。從圖4可以看出，在采用加速度補(bǔ)償后，UAV 期望軌跡發(fā)生了改變（前6 s 較明顯），載荷只在運(yùn)動(dòng)初始階段有一次大幅度擺動(dòng)，然后擺動(dòng)幅度近似為0，抗擺效果顯著，因補(bǔ)償后的UAV 期望軌跡影響了四旋翼的飛行軌跡，有效抑制了載荷擺動(dòng)。從圖5可以看出，吊掛物體擺動(dòng)幅度在逐漸減小，補(bǔ)償后的相軌跡以更小的橢圓形狀快速收斂到原點(diǎn)附近且不再發(fā)散，抗擺效果明顯。圖6是補(bǔ)償前后擺角θL的軌跡曲線，經(jīng)計(jì)算，補(bǔ)償后的殘余擺動(dòng)相對(duì)于未補(bǔ)償?shù)臍堄鄶[動(dòng)的最大幅值大約會(huì)衰減86%，同時(shí)，未補(bǔ)償?shù)那€衰減比為1.472∶1，補(bǔ)償后的曲線衰減比為6.766∶1，而且擺角在3 s后即收斂到0附近，因此，該抗擺控制方法具有較強(qiáng)的抗擺能力和足夠的穩(wěn)定裕度。

圖4 采用加速度補(bǔ)償前后的平面跟蹤軌跡Fig.4 Plane tracking trajectories before and after acceleration compensation

圖5 采用加速度補(bǔ)償前后的相平面軌跡Fig.5 Phase plane trajectories before and after acceleration compensation

圖6 采用加速度補(bǔ)償前后的擺角軌跡Fig.6 Swing angle trajectories before and after acceleration compensation

3.2 螺旋軌跡跟蹤

在高層建筑火災(zāi)撲救時(shí)，UAV 需吊掛救援工具做螺旋上升運(yùn)動(dòng)，開(kāi)展近距離探測(cè)或噴灑作業(yè)。針對(duì)此種應(yīng)用場(chǎng)景，設(shè)計(jì)了從起始位置（0，0，0）起飛的螺旋曲線軌跡跟蹤飛行任務(wù)，初始狀態(tài)，仿真時(shí)間為20 s。螺旋參考軌跡方程為：

仿真結(jié)果如圖7～9所示。從圖7、8可以看出，未補(bǔ)償?shù)妮d荷軌跡在運(yùn)動(dòng)開(kāi)始階段（前2 s）有較明顯的擺動(dòng)，且軌跡跟蹤誤差最大為1.4 m，說(shuō)明載荷擺動(dòng)對(duì)系統(tǒng)造成了嚴(yán)重干擾；而加入補(bǔ)償算法后載荷幾乎無(wú)擺動(dòng)，并且UAV 能較快地跟蹤期望運(yùn)動(dòng)軌跡，誤差最大為0.42 m，說(shuō)明抗擺控制算法減少了載荷擺動(dòng)對(duì)系統(tǒng)穩(wěn)定性的影響。

從圖9 可以看出，未進(jìn)行補(bǔ)償時(shí)，載荷擺角最大為20°，且擺角速度變化劇烈，說(shuō)明UAV 飛行不穩(wěn)定；而進(jìn)行加速度補(bǔ)償后，載荷擺角最大為10°，且擺角速度有明顯的降低，減小了載荷擺動(dòng)對(duì)四旋翼UAV飛行的干擾，從而實(shí)現(xiàn)平穩(wěn)飛行。

圖7 采用加速度補(bǔ)償前后軌跡跟蹤三維效果Fig.7 3D effect of trajectory tracking before and after acceleration compensation

圖8 采用加速度補(bǔ)償前后軌跡跟蹤誤差Fig.8 Error of trajectory tracking before and after acceleration compensation

圖9 采用加速度補(bǔ)償前后擺角變化情況Fig.9 Swing angle changes before and after acceleration compensation

4 結(jié)語(yǔ)

針對(duì)抑制吊掛載荷擺動(dòng)問(wèn)題，研究了一種基于加速度補(bǔ)償?shù)目箶[控制方法，該方法僅需要提供四旋翼無(wú)人機(jī)吊掛系統(tǒng)的參考運(yùn)動(dòng)信息和繩索長(zhǎng)度，無(wú)需額外的傳感器等硬件，計(jì)算簡(jiǎn)單，魯棒性好，易于工程實(shí)現(xiàn)。通過(guò)仿真對(duì)比分析可知，基于加速度補(bǔ)償?shù)目箶[控制方法能夠保證四旋翼無(wú)人機(jī)吊掛物體平穩(wěn)飛行，提高了吊掛飛行的安全性，并為控制系統(tǒng)提供了足夠的穩(wěn)定裕度。但無(wú)人機(jī)吊掛實(shí)際飛行過(guò)程中將面臨很多不確定性和干擾，如風(fēng)擾，所以，在有風(fēng)擾的情況下，如何抑制載荷擺動(dòng)以提高飛行控制系統(tǒng)的安全可靠性將是下一步的重點(diǎn)研究?jī)?nèi)容。