變負(fù)載四旋翼無人機(jī)的軌跡跟蹤控制器設(shè)計(jì)

安 帥， 袁鎖中， 李華東

(南京航空航天大學(xué)自動(dòng)化學(xué)院，南京 210016)

0 引言

作為一種旋翼式無人機(jī)，四旋翼無人機(jī)具有垂直起降的能力且在軍用和民用領(lǐng)域被廣泛使用。傳感器技術(shù)和微控制器的進(jìn)一步發(fā)展使得實(shí)現(xiàn)更為先進(jìn)的控制系統(tǒng)成為可能。在先進(jìn)的控制系統(tǒng)作用下，四旋翼無人機(jī)能執(zhí)行更加復(fù)雜的任務(wù)。

許多學(xué)者已經(jīng)研究了很多關(guān)于四旋翼無人機(jī)的先進(jìn)控制方法或策略。文獻(xiàn)[1-3]深入地研究了四旋翼無人機(jī)的姿態(tài)穩(wěn)定問題，但沒有涉及路徑跟蹤;文獻(xiàn)[4]把四旋翼無人機(jī)模型劃分為全驅(qū)動(dòng)與欠驅(qū)動(dòng)子系統(tǒng)，然后采用全局快速動(dòng)態(tài)末端滑?？刂萍夹g(shù)分別為兩個(gè)子系統(tǒng)設(shè)計(jì)控制器，雖然能確保所有的系統(tǒng)狀態(tài)變量及其導(dǎo)數(shù)都能在有限時(shí)間內(nèi)收斂到零，但控制器形式十分復(fù)雜;文獻(xiàn)[5]研究了在四旋翼無人機(jī)變負(fù)載情況下的軌跡跟蹤控制，采用一種分層的非線性控制策略，設(shè)計(jì)基于自適應(yīng)魯棒反步法的內(nèi)環(huán)控制器和基于全局滑模的外環(huán)控制器，雖然在外界擾動(dòng)下能實(shí)現(xiàn)軌跡跟蹤，但沒有考慮負(fù)載減小的情況。

本文以四旋翼無人機(jī)倉庫吊運(yùn)為背景，對(duì)四旋翼無人機(jī)的軌跡跟蹤控制問題進(jìn)行研究?？紤]到在短距離運(yùn)輸中裝貨/卸貨導(dǎo)致負(fù)載突變以及外界擾動(dòng)等對(duì)飛行性能的影響，采用內(nèi)外環(huán)控制結(jié)構(gòu)，結(jié)合魯棒控制和自適應(yīng)控制以減小參數(shù)變化和外界干擾所造成的影響。由于四旋翼無人機(jī)的負(fù)載是變化的，為了軌跡跟蹤性能，運(yùn)用一種自適應(yīng)滑模控制策略設(shè)計(jì)外環(huán)控制器，采用自抗擾技術(shù)[6-8]設(shè)計(jì)內(nèi)環(huán)控制器。

1 四旋翼無人機(jī)數(shù)學(xué)建模

本文所采用的四旋翼無人機(jī)模型如圖1所示。圖1給出慣性坐標(biāo)系[I]和機(jī)體坐標(biāo)系[B]的定義。坐標(biāo)系[I]相對(duì)于地面是固定的，而坐標(biāo)系[B]的原點(diǎn)固定在空載時(shí)四旋翼無人機(jī)的質(zhì)心上。

圖1 四旋翼無人機(jī)結(jié)構(gòu)圖Fig.1 Structure of quadrotor UAV

四旋翼無人機(jī)數(shù)學(xué)模型由以下運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程[9-10]描述

(1)

(2)

(3)

(4)

式中：向量ξ=(XYZ)T表示[B]的原點(diǎn)在[I]中的位置；向量η=(φθψ)T表示四旋翼無人機(jī)的姿態(tài)，其分量為歐拉角，即滾轉(zhuǎn)角φ(-π/2<φ<π/2)，俯仰角θ(-π/2<θ<π/2)和偏航角ψ(-π<ψ<π);向量ω=(pqr)T是四旋翼無人機(jī)的角速率；矩陣W(η)的定義如下

(5)

(6)

式(4)中:I是總的慣性矩;Ga代表陀螺力矩;τext是作用在四旋翼無人機(jī)上的干擾力矩;τ代表由旋翼產(chǎn)生的控制力矩,即

(7)

式中：d是阻力因子;l是電機(jī)中心到四旋翼無人機(jī)運(yùn)動(dòng)前的質(zhì)心的距離。

在四旋翼無人機(jī)吊運(yùn)貨物過程中，裝/卸貨操作導(dǎo)致負(fù)載突變，從而使得系統(tǒng)重心在垂直方向上產(chǎn)生較大偏移，對(duì)四旋翼無人機(jī)垂直方向的運(yùn)動(dòng)影響較大。

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

2.1 控制系統(tǒng)結(jié)構(gòu)

針對(duì)四旋翼無人機(jī)的軌跡跟蹤問題，本文使用的控制系統(tǒng)結(jié)構(gòu)如圖2所示。為在外環(huán)中控制四旋翼無人機(jī)的平移運(yùn)動(dòng)，設(shè)計(jì)自適應(yīng)滑模控制器，通過李雅普諾夫穩(wěn)定性理論設(shè)計(jì)出對(duì)應(yīng)的自適應(yīng)律?；谧钥箶_控制技術(shù)設(shè)計(jì)內(nèi)環(huán)控制器，實(shí)現(xiàn)姿態(tài)穩(wěn)定與跟蹤。

圖2 四旋翼無人機(jī)控制系統(tǒng)結(jié)構(gòu)圖Fig.2 Structure of the quadrotor UAV control system

2.2 外環(huán)控制器設(shè)計(jì)

對(duì)式(1)求導(dǎo)并代入式(3)中，得

(8)

平移子系統(tǒng)的相對(duì)階為2。式中,Δ1=Fext與T之間不存在線性關(guān)系致使滑?？刂破髦械聂敯羟袚Q項(xiàng)不能確保穩(wěn)定性,需設(shè)計(jì)自適應(yīng)律來估計(jì)外界擾動(dòng)。

為實(shí)現(xiàn)軌跡跟蹤控制，定義滑模變量

(9)

式中：控制增益λ=diag(λ1,λ2,λ3),λi>0(i=1,2,3);ξd是參考軌跡。對(duì)滑模變量s求導(dǎo)，得

(10)

令U1=TRe3為所設(shè)計(jì)的虛擬控制輸入。為確保參考軌跡跟蹤性能，設(shè)計(jì)U1為

(11)

(12)

選擇自適應(yīng)律為

(13)

考慮如下李雅普諾夫函數(shù)

(14)

根據(jù)式(11)可得

(15)

將式(15)代入式(12)，得

(16)

(17)

(18)

(19)

考慮到負(fù)載的變化范圍有界，使用離散投影映射方法對(duì)質(zhì)量的自適應(yīng)律進(jìn)行修改。修正后的質(zhì)量自適應(yīng)律為

(20)

(21)

利用式(9)、式(11)、式(13)、式(20)和式(21)計(jì)算出U1。令U1=(U11U12U13)′，結(jié)合U1=TRe3，可得

(22)

在式(22)中，用ηd替代η，可得

(23)

2.3 內(nèi)環(huán)控制器設(shè)計(jì)

為確定旋轉(zhuǎn)子系統(tǒng)的相對(duì)階，對(duì)式(2)求導(dǎo)，得

(24)

把式(4)代入式(24)中，可得式(25)。旋轉(zhuǎn)子系統(tǒng)的相對(duì)階是2。

(25)

為了控制器的設(shè)計(jì)，重寫式(9)為

(26)

考慮式(25)所描述的多輸入多輸出(MIMO)的二階系統(tǒng)，利用ADRC技術(shù)為其設(shè)計(jì)控制器使?fàn)顟B(tài)變量η能夠很好地跟蹤期望信號(hào)ηd。圖3給出基于ADRC技術(shù)的解耦控制結(jié)構(gòu)。

該控制系統(tǒng)包含滾轉(zhuǎn)、俯仰和偏航通道,令虛擬控制量u=(u1u2u3)T=B2τ,在這3個(gè)通道中，基于ADRC技術(shù)的控制器設(shè)計(jì)步驟是相同的。下面以滾轉(zhuǎn)通道的控制器設(shè)計(jì)為例詳細(xì)討論自抗擾控制器1的設(shè)計(jì)過程。自抗擾控制器1由非線性跟蹤微分器(NTD)，非線性反饋控制器(NFC)和擴(kuò)張狀態(tài)觀測(cè)器(ESO)構(gòu)成[7]。圖4所示為出自抗擾控制器的結(jié)構(gòu)，其中,F21是F2的第一個(gè)分量。

圖3 基于自抗擾控制的MIMO系統(tǒng)解耦控制結(jié)構(gòu)圖Fig.3 Structure of the MIMO system decoupling control based on ADRC

圖4 自抗擾控制器1的結(jié)構(gòu)圖Fig.4 Structure of active disturbance-rejection controller 1

使用NTD跟蹤其輸入信號(hào)并估計(jì)輸入信號(hào)的微分值。本文使用二階NTD，其離散算法[11]為

(27)

ESO的形式取決于被觀測(cè)對(duì)象的形式和階次?？紤]到滾轉(zhuǎn)運(yùn)動(dòng)子系統(tǒng)是一個(gè)二階系統(tǒng)，設(shè)計(jì)如下的ESO

(28)

式中：y，u和F是ESO的輸入信號(hào),y表示被觀測(cè)對(duì)象的輸出量φ,u表示被觀測(cè)對(duì)象的輸入量u1，F(xiàn)=F21;zi(i=1,2,3)是輸出信號(hào);系數(shù)β0i(i=1,2,3)的取值使多項(xiàng)式λ(s)=s3+β01s2+β02s+β03是Hurwitz的[12]。根據(jù)文獻(xiàn)[11]，設(shè)計(jì)離散算法為

(29)

(30)

采用偏差信號(hào)與其導(dǎo)數(shù)的非線性組合設(shè)計(jì)NFC。由NTD給出x1和x2，由ESO給出z1和z2，求取其偏差值，使用fst函數(shù)構(gòu)造NFC,即

(31)

式中：ci是阻尼因子；r1為控制器增益;速度因子h1>Ts。則自抗擾控制律為

u1=u10-z3-F21。

(32)

文獻(xiàn)[13]已經(jīng)證明自抗擾控制器的穩(wěn)定性以及擴(kuò)張狀態(tài)觀測(cè)器的收斂性。對(duì)于多輸入多輸出下三角系統(tǒng)，文獻(xiàn)[14-15]已經(jīng)研究基于自抗擾控制器的閉環(huán)回路性能和穩(wěn)定性。

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

本文使用式(1)～式(4)所描述的四旋翼無人機(jī)模型驗(yàn)證所設(shè)計(jì)控制策略的性能?？蛰d下的四旋翼無人機(jī)物理參數(shù)由表1給出。在仿真開始時(shí)，給四旋翼無人機(jī)引入外界干擾，直至仿真結(jié)束。在仿真中，干擾被設(shè)置為Fext=(sin(0.1t);sin(0.1t);sin(0.1t))，單位為N，τext=(0.3sin(0.1t);0.3sin(0.1t);0.5sin(0.1t))，單位為N·m。

外環(huán)控制器的參數(shù)如下：λ=diag(0.1,0.2,2.3)，c=diag(6.3,6.1,3.9)，γ1=diag(0.7,0.7,0.4)和γ2=0.1。在滾轉(zhuǎn)和俯仰通道中，自抗擾控制器參數(shù)如下：Ts=h=0.01 s，r=1065，β01=100，β02=700，β03=4500，cφ,θ=4.5，r1=30和h1=10。在偏航通道中，除了cψ=5.5以外，剩下參數(shù)都與上述的自抗擾控制器參數(shù)一致。

表1 空載下的四旋翼無人機(jī)參數(shù)

四旋翼無人機(jī)的初始位置位于坐標(biāo)系[I]的原點(diǎn)，且初始姿態(tài)角為η(0)=(0 0 0)T,單位為(°)，所以在仿真開始時(shí)，四旋翼無人機(jī)需要執(zhí)行垂直起飛動(dòng)作。此外，期望的偏航角都為零。

圖5～圖7給出仿真實(shí)驗(yàn)結(jié)果。從圖5中的參考路徑和實(shí)際路徑可看出，四旋翼無人機(jī)首先垂直起飛，到達(dá)指定高度后開始跟蹤螺旋線軌跡。在整個(gè)過程中，由于干擾的存在和質(zhì)量的突變，四旋翼無人機(jī)的軌跡會(huì)偏離期望路徑，但其跟蹤誤差相對(duì)較小，可知所設(shè)計(jì)的控制器能夠有效地解決變負(fù)載四旋翼無人機(jī)在擾動(dòng)存在情況下的軌跡跟蹤問題。由圖6可知，在跟蹤路徑的過程中，負(fù)載的初始重量為3 kg，而且四旋翼無人機(jī)分別進(jìn)行了兩次卸貨和一次裝貨。圖7給出控制輸入信號(hào)的變化，可見負(fù)載突變對(duì)其影響較大。

圖5 路徑跟蹤Fig.5 Path tracking

圖6 質(zhì)量估計(jì)Fig.6 Estimation of mass

圖7 控制輸入Fig.7 Control inputs

圖6中的質(zhì)量估計(jì)曲線始終未能收斂到實(shí)際值曲線的原因是滑模變量不斷地來回穿越滑模面使得質(zhì)量的估計(jì)偏差在有界范圍內(nèi)變化而不是變?yōu)榱?。質(zhì)量估計(jì)偏差的有界性是滑模變量趨近于零的條件之一。從圖6中可以看出在兩次卸貨操作后，質(zhì)量估計(jì)的偏差已經(jīng)縮小許多。當(dāng)t分別為30 s，55 s和80 s時(shí)，質(zhì)量估計(jì)滿足式(19)。

4 結(jié)束語

本文提出將自抗擾控制技術(shù)和自適應(yīng)滑模控制方法相結(jié)合解決變負(fù)載四旋翼無人機(jī)的軌跡跟蹤問題。由內(nèi)外環(huán)控制結(jié)構(gòu)組成的控制策略考慮了外界擾動(dòng)。針對(duì)四旋翼無人機(jī)的平移運(yùn)動(dòng)，使用自適應(yīng)滑?？刂破鹘M成外環(huán)，確保參考路徑的跟蹤。在內(nèi)環(huán)中，設(shè)計(jì)基于自抗擾技術(shù)的控制器，穩(wěn)定與跟蹤姿態(tài)。通過仿真實(shí)驗(yàn)驗(yàn)證了所設(shè)計(jì)控制器的魯棒性和有效性。

[1] KHATOON S,SHAHID M,IBRAHEEM,et al.Dynamic mo-deling and stabilization of quadrotor using PID controller[C]//International Conference on Advances in Computing, Communications and Informatics,2014:746-750.

[2] LEE J,LEE C,KIM D E.Attitude control of quadrotor with on-board visual feature projection system[C]//IEEE/rsj International Conference on Intelligent Robots and Systems，IEEE,2013:2426-2431.

[3] LI Z,MA X,XU Z G,et al.Chattering free sliding adaptive attitude control for quadrotor[C]//IEEE Chinese Guidance,Navigation and Control Conference (CGNCC),2016:707-712.

[4] ZUO Z.Adaptive trajectory tracking control design with command filtered compensation for a quadrotor[J].Journal of Vibration and Control,2013,19(1):94-108.

[5] WANG C,SONG B F,HUANG P F,et al.Trajectory tracking control for quadrotor robot subject to payload variation and wind gust disturbance[J].Journal of Intelligent & Robotic Systems,2016,83(2):315-333.

[6] GAO Z Q.Active disturbance rejection control:a paradigm shift in feedback control system design[C]//American Control Conference,IEEE,2006:2399-2405.

[7] HAN J Q.From PID to active disturbance rejection control[J].IEEE Transactions on Industrial Electronics,2009, 56(3):900-906.

[8] HUANG Y,XUE W C.Active disturbance rejection control:methodology and theoretical analysis[J].ISA Transactions,2014,53(4):963-976.

[9] ZUO Z Y.Adaptive trajectory tracking control design with command filtered compensation for a quadrotor[J].Journal of Vibration and Control,2013,19(1):94-108.

[10] RAFFO G V,ORTEGA M G,RUBIO F R.An integral predictive/nonlinearH∞control structure for a quadrotor helicopter[J].Automatica,2010,46(1):29-39.

[11] GAO B W,SHAO J P,YANG X D.A compound control strategy combining velocity compensation with ADRC of electro-hydraulic position servo control system[J].ISA Transactions,2014,53(6):1910-1918.

[12] BALAJIWALE S,ARYA H,JOSHI A.Study of performance of ADRC for longitudinal control of MAV[J].IFAC-Papers Online,2016,49(1):585-590.

[13] ZHENG Q,DONG L L,LEE D H,et al.Active disturbance rejection control for MEMS gyroscopes[C]//American Control Conference,IEEE,2008:4425-4430.

[14] XUE W C,HUANG Y.The active disturbance rejection control for a class of MIMO block lower-triangular system[C]//The 30th Chinese Control Conference(CCC), 2011:6362-6367.

[15] XUE W C,HUANG Y.On performance analysis of ADRC for a class of MIMO lower-Triangular nonlinear uncertain systems[J].ISA Transactions,2014,53(4):955-962.