余志凱，黃子豪，傅瑾瑜，蔣憲鑫，辛 穎

(1. 中國人民解放軍93145部隊(duì)·上?！?01109；2. 上海航天控制技術(shù)研究所·上海·201109)

0 引 言

四旋翼無人機(jī)因?yàn)槠涞土某杀?、豐富的擴(kuò)展、靈活的機(jī)動(dòng)能力，以及典型的欠驅(qū)動(dòng)特性，一方面在工業(yè)檢測、農(nóng)業(yè)植保、商業(yè)娛樂、軍事攻防等領(lǐng)域應(yīng)用廣泛，另一方面可作為各種算法以及傳感器的理想驗(yàn)證平臺(tái)[1]。四旋翼吊掛負(fù)載的研究既有切實(shí)的運(yùn)輸、消防、救援等實(shí)際應(yīng)用，又有探索針對(duì)高耦合和強(qiáng)欠驅(qū)模型控制理論的價(jià)值。

Guerrero等提出了具有較好魯棒性的互聯(lián)和阻力分配的無源控制器，分別設(shè)計(jì)了依賴與不依賴于吊掛角度的控制律，實(shí)現(xiàn)了點(diǎn)到點(diǎn)機(jī)動(dòng)下擺動(dòng)的抑制[2-4]。范云生等人利用擴(kuò)張狀態(tài)觀測器對(duì)擾動(dòng)及不確定性等優(yōu)秀的估計(jì)能力設(shè)計(jì)非線性控制器，并通過QBall2四旋翼無人機(jī)攜帶吊掛模型的三維螺旋軌跡進(jìn)行仿真驗(yàn)證，有效實(shí)現(xiàn)了對(duì)吊掛擺動(dòng)的快速抑制與軌跡的精確跟蹤[5]。Lyu等人在四旋翼吊掛負(fù)載系統(tǒng)建模中考慮了吊掛風(fēng)擾，設(shè)計(jì)了具有指數(shù)收斂穩(wěn)定性的非線性級(jí)聯(lián)控制器[6]，之后又采用有限時(shí)間控制方法，進(jìn)一步提高了系統(tǒng)的收斂速度與魯棒性[7]。鮮斌課題組是國內(nèi)對(duì)該系統(tǒng)研究較多的團(tuán)隊(duì)，其文章主要基于能量函數(shù)開展，利用自適應(yīng)控制或神經(jīng)網(wǎng)絡(luò)等方法估計(jì)吊掛變化或外界擾動(dòng)，各控制器的有效性都經(jīng)過了理論與試驗(yàn)的雙重驗(yàn)證[8-10]。梁曉等人提出一種簡單判別系統(tǒng)微分平坦的方法，并以此為基礎(chǔ)通過動(dòng)態(tài)反饋線性化實(shí)現(xiàn)軌跡跟蹤控制[11]。焦海林等人則分析了吊掛系統(tǒng)本身的擺動(dòng)特性，借用抑制移動(dòng)容器內(nèi)液體晃動(dòng)的加速度補(bǔ)償技術(shù)，有效減少了機(jī)體移動(dòng)引起的副作用實(shí)現(xiàn)負(fù)載擺動(dòng)的抑制[12]。Omar等人根據(jù)吊掛單擺的周期，設(shè)計(jì)相關(guān)延時(shí)反饋的抑?jǐn)[控制器，只需修飾設(shè)計(jì)軌跡即可實(shí)現(xiàn)擺角抑制，便于直接移植應(yīng)用于已有成熟算法平臺(tái)(例如PX4固件)，并利用Gazebo搭建了硬件在回路仿真平臺(tái)[13-14]，之后針對(duì)延時(shí)又進(jìn)一步設(shè)計(jì)了模糊控制器，增強(qiáng)了抑?jǐn)[的有效性[15]。

上述方法主要解決了四旋翼無人機(jī)吊掛負(fù)載控制在兩個(gè)方面的問題：1)四旋翼機(jī)體抗吊掛干擾的穩(wěn)定控制；2)吊掛擺角的最終穩(wěn)定收斂。但是四旋翼無人機(jī)在大幅度位置機(jī)動(dòng)過程中吊掛擺角的抑制不夠突出，機(jī)動(dòng)過程中的最大擺角容易隨著機(jī)動(dòng)的增大而明顯增大。受文獻(xiàn)[16]的啟發(fā)，可以通過合理設(shè)置飽和區(qū)間從而抑制擺角，但因文獻(xiàn)[16]的控制效果不夠明顯，飽和控制方法有進(jìn)一步提升空間。

本文通過一種嵌套飽和控制方法，設(shè)置虛擬輸入飽和來限制實(shí)際最大過載，有效限制機(jī)動(dòng)加速度，從而減小擺動(dòng)。并考慮到嵌套飽和在抑制干擾方面沒有明顯優(yōu)勢，所以針對(duì)吊掛變化影響不敏感的垂直位置，設(shè)計(jì)基于擴(kuò)張狀態(tài)觀測器的控制器，通過結(jié)合不同算法的優(yōu)勢得到本文所提控制器。

1 四旋翼吊掛系統(tǒng)建模

四旋翼無人機(jī)本身為四輸入六自由度的典型欠驅(qū)動(dòng)系統(tǒng)，攜帶吊掛負(fù)載后(假設(shè)飛行中吊繩始終處于拉伸狀態(tài))額外增加兩個(gè)自由度，使得系統(tǒng)的欠驅(qū)動(dòng)特性更加顯著，系統(tǒng)復(fù)雜度進(jìn)一步增加，其三維示意圖如圖1所示。

圖1 四旋翼吊掛系統(tǒng)三維示意圖Fig.1 Three-dimensional schematic diagram of quadrotor suspended payload system

圖1中的四旋翼機(jī)體為X形，在四旋翼中心建立固連的機(jī)體坐標(biāo)系obxbybzb，并以地面起飛位置為原點(diǎn)建立慣性坐標(biāo)系oixiyizi，φ，?，ψ為四旋翼機(jī)體分別繞xb，yb，zb軸旋轉(zhuǎn)的姿態(tài)角，并定義慣性系到機(jī)體系的歐拉角旋轉(zhuǎn)順序?yàn)閦(ψ)→y(?)→x(φ)。吊掛擺角α，β為吊繩在平面obybzb上的投影，分別與obzb軸、吊繩的夾角。定義單機(jī)質(zhì)量為M，吊掛質(zhì)量為m，吊掛繩長為l，慣性系下旋翼升力在各軸的分力為Fx，F(xiàn)y，F(xiàn)z，機(jī)體系下沿各軸的力矩為τx，τy，τz，轉(zhuǎn)動(dòng)慣量為Jxx，Jyy，Jzz。

(1)

(2)

通過計(jì)算可得8個(gè)方程構(gòu)成的方程組作為三維數(shù)學(xué)模型，由于結(jié)果較為繁瑣冗長，且僅在仿真中使用，下文控制器設(shè)計(jì)中沒有全部使用，這里不再給出具體形式。

考慮四旋翼無人機(jī)小角度飛行時(shí)，吊掛系統(tǒng)水平運(yùn)動(dòng)在正交方向耦合較小，為了便于控制器設(shè)計(jì)，此處給出oiyizi二維平面的簡化形式

(3)

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

若控制器設(shè)計(jì)合理，則能保證擺角始終處于小角度狀態(tài)，結(jié)合式(3)的第三條方程可知，此時(shí)擺角的余弦值接近1而正弦值接近0，易得擺角變化主要取決于y向變化，與z向相關(guān)性較小。因此，本文通過y向控制同時(shí)實(shí)現(xiàn)擺角控制，z向獨(dú)立控制，其變化視為對(duì)擺角的小幅度擾動(dòng)，反之亦然，分別設(shè)計(jì)水平、垂直位置控制器。

2.1 垂直位置控制器

考慮到四旋翼垂直位置與擺角相互耦合，垂直位置控制器又不直接作用于擺角控制，為了提高z向抗擾能力，引入自抗擾控制中的擴(kuò)張狀態(tài)觀測器估計(jì)z向總擾動(dòng)，抑制后續(xù)的干擾，以提升控制性能。

為了適應(yīng)三維情況，利用式(2)計(jì)算zi軸結(jié)果，并定義zi軸的總擾動(dòng)為Δdz，經(jīng)整理得到如下表達(dá)式

(4)

其中

(5)

(6)

式中，κ1，κ2，κ3，a1，a2，a3，δ為觀測器參數(shù)，sign(·)為符號(hào)函數(shù)。

在得到擾動(dòng)估計(jì)的前提下，得到z軸垂直位置控制器

(7)

式中，zc為期望位置指令，Kp，Kd為非負(fù)的控制參數(shù)。

2.2 擺角抑制控制器

四旋翼吊掛系統(tǒng)作為高度欠驅(qū)動(dòng)系統(tǒng)，在z軸方向獨(dú)立控制前提下，本文所設(shè)計(jì)的控制器通過擺角抑制控制力uα與水平y(tǒng)軸的控制力Fy疊加的方式實(shí)現(xiàn)擺角α與y軸的完全耦合。

四旋翼機(jī)體在空中穩(wěn)定飛行(勻速直線或懸停狀態(tài))時(shí)，吊掛在水平位置的擺動(dòng)可簡化為吊繩上端的水平移動(dòng)與吊掛負(fù)載單擺運(yùn)動(dòng)的疊加，從而得到如下擺角控制器

(8)

其中kα為大于零的控制參數(shù)。

下面進(jìn)行穩(wěn)定性證明。構(gòu)造如下李雅普諾夫函數(shù)

(9)

使用“do/does/did+v.”構(gòu)式既能表達(dá)變化，也能表達(dá)移動(dòng)。變化是抽象的移動(dòng)，移動(dòng)是具體的變化。造成變化和移動(dòng)的可以是物理力，也可以是心理力，甚至是社會(huì)力。例7中，他們受到心理感受的無形的力而改變態(tài)度，例8是物理力的作用，是施事者就具體物體發(fā)出的力量，例9則是來自社會(huì)的約束力。使用目標(biāo)構(gòu)式表達(dá)變化或移動(dòng)，起到凸顯力作用的效果。

(10)

將式(8)的控制器代入上式，易得

(11)

擺角理論上漸近收斂。

2.3 水平位置控制器

水平位置控制器主體采用嵌套飽和控制器，存在如下引理[17-18]：

對(duì)于如下形式的n階多次積分系統(tǒng)

(12)

其中umax為正數(shù)，表示執(zhí)行器的最大輸出大小，令

(13)

(14)

原狀態(tài)空間經(jīng)過形式如p=Tx的狀態(tài)方程變換為Teel標(biāo)準(zhǔn)型

(15)

其中

等價(jià)變化矩陣為

(16)

當(dāng)n為偶數(shù)有

(17)

當(dāng)n為奇數(shù)有

(18)

已知ε為正數(shù)，定義飽和函數(shù)σε

(19)

若給定如下控制律，能使系統(tǒng)鎮(zhèn)定

(20)

其中

根據(jù)上述引理進(jìn)行控制器設(shè)計(jì)，對(duì)模型進(jìn)行小角度線性化，并將四旋翼無人機(jī)視為無吊掛的系統(tǒng)進(jìn)行推導(dǎo)設(shè)計(jì)?？紤]到嵌套飽和控制器無法直接控制水平分力，而需姿態(tài)角變化實(shí)現(xiàn)水平位置控制。所以本文將抑?jǐn)[控制器的輸出轉(zhuǎn)化為姿態(tài)角零位補(bǔ)償，從而引入到嵌套飽和控制器中，得到如下模型

(21)

其中φε=φ-φb，φb=-uα/(Mg)為角度零位補(bǔ)償，用于近似水平分力抑制吊掛擺動(dòng)。

對(duì)模型進(jìn)行簡單整理，易得狀態(tài)空間表達(dá)式

(22)

(23)

利用式(16)可反推變換矩陣，易得變換后的系統(tǒng)狀態(tài)變量

(24)

得到水平位置控制器

(25)

3 仿真驗(yàn)證及分析

為了驗(yàn)證本文設(shè)計(jì)控制器的控制效果，盡可能接近實(shí)物試驗(yàn)便于后續(xù)飛行驗(yàn)證，本文基于已有四旋翼無人機(jī)吊掛系統(tǒng)實(shí)物的模型參數(shù)，考慮了傳感器、電機(jī)等多種影響因素，采用連續(xù)模型和離散控制器進(jìn)行三通道仿真，計(jì)算多組典型工況并加以分析。

四旋翼吊掛模型的主要參數(shù)：M=1.5kg，m=0.1kg，l=1.0m，Jxx=0.022kg·m2。本文控制器主要參數(shù)：κ1=100，κ2=300，κ3=1000，a1=1，a2=0.5，a3=0.25，δ=0.02，Kp=2，Kd=2.8，kα=2，ε1=0.11，ε2=0.266。

首先為了驗(yàn)證抑?jǐn)[控制器的效果，對(duì)比了嵌套飽和控制有無引入抑?jǐn)[角度零位補(bǔ)償兩種情況，在z軸方向上，0s時(shí)輸入幅值為8m的階躍信號(hào)，在y軸方向上，10s時(shí)輸入幅值為8m的階躍信號(hào)，仿真結(jié)果如圖2所示。引入擴(kuò)張狀態(tài)觀測器的z軸方向位置響應(yīng)抗擾能力較強(qiáng)，幾乎不受抑?jǐn)[控制器補(bǔ)償?shù)挠绊?；y軸方向位置響應(yīng)在姿態(tài)角有無抑?jǐn)[補(bǔ)償情況下總體變化趨勢差別不大，但有抑?jǐn)[補(bǔ)償時(shí)超調(diào)有少許增加；抑?jǐn)[補(bǔ)償下姿態(tài)角變化有些許增大，用以補(bǔ)償擺動(dòng)；引入抑?jǐn)[補(bǔ)償后，吊掛擺角的全過程變化趨于平穩(wěn)，當(dāng)位置到達(dá)目標(biāo)點(diǎn)附近后，擺角會(huì)快速收斂并趨近于零，不再有明顯振蕩。

目前已有的四旋翼吊掛系統(tǒng)控制器幾乎都能保證位置機(jī)動(dòng)到位后的擺角快速收斂，本文通過引入控制效果較好的自適應(yīng)控制器進(jìn)行對(duì)比，自適應(yīng)控制器設(shè)計(jì)可見文獻(xiàn)[10]，但不包括姿態(tài)控制，仿真時(shí)使用串級(jí)PID進(jìn)行姿態(tài)控制。利用同一組控制參數(shù)，分別進(jìn)行了z軸和y軸方向上，輸入信號(hào)為幅值2m的階躍信號(hào)的小范圍位置機(jī)動(dòng)工況仿真，結(jié)果如圖3所示，以及z軸和y軸方向上，輸入信號(hào)為幅值6m的階躍信號(hào)的大范圍機(jī)動(dòng)工況仿真，結(jié)果如圖4所示。

圖3 小范圍位置機(jī)動(dòng)控制器效果對(duì)比Fig.3 Effect comparison of small range position maneuvering controllers

圖4 大范圍位置機(jī)動(dòng)控制器效果對(duì)比Fig.4 Effect comparison of large range position maneuvering controllers

首先從z軸控制器進(jìn)行對(duì)比，兩種工況下本文所提控制器與自適應(yīng)控制器均無產(chǎn)生超調(diào)，但本文控制響應(yīng)明顯收斂更快，在幅值較大的階躍指令作用下尤為明顯。不同工況下，兩種控制器在y軸方向上的位置響應(yīng)快慢各有優(yōu)勢，但本文控制器作用下響應(yīng)的上升時(shí)間、調(diào)節(jié)時(shí)間以及超調(diào)量等指標(biāo)波動(dòng)幅度更小，響應(yīng)趨勢較為一致；而自適應(yīng)控制器的參數(shù)在小范圍位置機(jī)動(dòng)工況下控制性能較好，但在階躍指令變化較大后響應(yīng)曲線變化明顯，超調(diào)量及調(diào)節(jié)時(shí)間顯著增大。y軸方向的控制間接作用于姿態(tài)控制，在幅值較大的階躍輸入信號(hào)作用下自適應(yīng)控制器更容易產(chǎn)生過大的姿態(tài)指令以提高速度，從而更容易產(chǎn)生姿態(tài)角的飽和；本文所采用的嵌套飽和控制方法，對(duì)位置、姿態(tài)分級(jí)產(chǎn)生飽和控制效果，姿態(tài)角反而不易飽和。

自適應(yīng)控制器的最大擺角在圖3中約為2.3°，圖4中約為9.0°，明顯隨指令信號(hào)的增大而增大。本文所提控制器的最大擺角在圖3中約為2.8°，圖4中約為3.8°，擺角抑制效果受指令信號(hào)影響較小。顯然在幅值較小的階躍信號(hào)作用下自適應(yīng)控制抑制效果略優(yōu)，但本文所提控制器更能顯現(xiàn)出對(duì)最大擺角的抑制，實(shí)際應(yīng)用更有優(yōu)勢。

對(duì)本文所提控制器進(jìn)行y軸方向上不同階躍輸入信號(hào)情況下的仿真，結(jié)果如圖5所示。y軸方向上的控制響應(yīng)速度近似且較快，超調(diào)量始終保持在10%以內(nèi)。y軸在1m，3m，6m，10m階躍信號(hào)下對(duì)應(yīng)的最大擺角大小分別約為1.7°，2.8°，3.8°，3.6°，即使在水平方向上做大范圍機(jī)動(dòng)，擺角大小都控制在4°之內(nèi)。除單位階躍信號(hào)外，其余信號(hào)響應(yīng)下擺角與姿態(tài)角變化趨勢近似，兩者角度響應(yīng)在縱軸正負(fù)半軸的占比接近。體現(xiàn)了嵌套飽和控制器通過較為穩(wěn)定均衡地分配位置機(jī)動(dòng)的加減速階段，減小加速度凸變引起吊掛劇烈擺動(dòng)的特點(diǎn)。

圖5 多范圍位置機(jī)動(dòng)控制器響應(yīng)對(duì)比Fig.5 Response comparison of multi-range position maneuvering controllers

4 結(jié) 論

本文研究了四旋翼無人機(jī)吊掛負(fù)載控制系統(tǒng)，針對(duì)多數(shù)控制方法忽略的大機(jī)動(dòng)過程中吊掛最大擺角抑制問題，提出了一種綜合控制方法，其借助擴(kuò)張狀態(tài)觀測器獨(dú)立控制z軸方向運(yùn)動(dòng)，以抑制吊掛干擾，減小耦合。該方法采用嵌套飽和控制方法抑制加速度劇烈變化，以及通過抑?jǐn)[控制器以姿態(tài)角零位補(bǔ)償?shù)男问揭胨娇刂破鳌Ｍㄟ^與自適應(yīng)控制器進(jìn)行仿真對(duì)比，驗(yàn)證了機(jī)動(dòng)過程抑?jǐn)[的有效性以及多工況適應(yīng)性。但本文未考慮未知吊掛質(zhì)量與繩長的情況，后續(xù)將探索飽和控制與自適應(yīng)控制相結(jié)合的方法。