基于擴(kuò)張狀態(tài)觀測器和反步滑模法的四旋翼無人機(jī)軌跡跟蹤控制

張建揚(yáng)，于春梅,2，葉劍曉

(1.西南科技大學(xué) 信息工程學(xué)院，四川 綿陽 621000； 2.特殊環(huán)境機(jī)器人技術(shù)四川省重點(diǎn)實(shí)驗(yàn)室(西南科技大學(xué))，四川 綿陽 621000)

0 引言

近年來，由于四旋翼無人機(jī)(quadrotor Unmanned Aerial Vehicle, quadrotor UAV)具有結(jié)構(gòu)相對(duì)簡單、生產(chǎn)成本較低等優(yōu)點(diǎn)，在軍事和民用領(lǐng)域備受青睞[1]。軌跡跟蹤控制是無人機(jī)完成特定任務(wù)的基礎(chǔ)，因此，提高無人機(jī)的軌跡跟蹤精度和性能具有重要意義。

四旋翼無人機(jī)是一個(gè)4輸入、6輸出的欠驅(qū)動(dòng)系統(tǒng)[2]，系統(tǒng)變量間存在半耦合關(guān)系且模型中存在非完整性約束,這增加了控制器設(shè)計(jì)的復(fù)雜度。在實(shí)際飛行中，由于無法對(duì)空氣動(dòng)力建立精確的數(shù)學(xué)模型且模型中的無人機(jī)質(zhì)量、轉(zhuǎn)動(dòng)慣量、空氣動(dòng)力學(xué)系數(shù)等參量的測量值與實(shí)際值之間存在偏差,同時(shí)槳葉轉(zhuǎn)動(dòng)、急風(fēng)、地磁影響等外界未知干擾也會(huì)對(duì)其產(chǎn)生較大的影響[3-4]，從而進(jìn)一步增加了四旋翼無人機(jī)的控制難度。因此，設(shè)計(jì)出能夠克服系統(tǒng)內(nèi)部擾動(dòng)和外部噪聲的無人機(jī)控制系統(tǒng)是軌跡跟蹤的重中之重，即整個(gè)系統(tǒng)必須具有魯棒性。

為了克服上述困難，國內(nèi)外專家學(xué)者進(jìn)行了大量的研究，已經(jīng)有較多的解決方案被提出。如文獻(xiàn)[5-7]研究了四旋翼無人機(jī)的反步法控制，由于反步法對(duì)精確的數(shù)學(xué)模型有較高要求，因此建模誤差的大小會(huì)對(duì)系統(tǒng)的跟蹤效果產(chǎn)生影響；文獻(xiàn)[8]基于四元數(shù)法建立了無人機(jī)的數(shù)學(xué)模型，內(nèi)、外環(huán)分別采用H∞控制和積分型反步法控制，較好地補(bǔ)償了系統(tǒng)的內(nèi)外干擾；文獻(xiàn)[9-10]研究了滑?？刂?，雖然滑模變結(jié)構(gòu)對(duì)系統(tǒng)參數(shù)變化不敏感，抗干擾能力較強(qiáng)，但當(dāng)系統(tǒng)存在較強(qiáng)復(fù)合干擾時(shí)，無人機(jī)的控制精度并不理想；文獻(xiàn)[11]將反步法和滑?？刂葡嘟Y(jié)合，能夠較好地完成跟蹤任務(wù)；文獻(xiàn)[12]采用不需要模型的神經(jīng)網(wǎng)絡(luò)方法來估計(jì)系統(tǒng)的內(nèi)擾和外擾，取得了良好的跟蹤效果，但神經(jīng)網(wǎng)絡(luò)控制要求有足夠多的基函數(shù)來保證逼近精度，這嚴(yán)重增加了計(jì)算負(fù)擔(dān)；文獻(xiàn)[13]采用擴(kuò)張狀態(tài)觀測器(Extended State Observer, ESO)方法在線估計(jì)系統(tǒng)的不確定性因素，取得了良好的控制效果，但姿態(tài)角幅值變化較大。

考慮各種方法的優(yōu)缺點(diǎn)，本文在文獻(xiàn)[11]的基礎(chǔ)上設(shè)計(jì)了一種基于ESO和積分反步滑模算法的魯棒控制器，將抗干擾能力較強(qiáng)的滑?？刂迫谌氲椒床椒刂破髦?，使系統(tǒng)的魯棒性得到增強(qiáng)；接著引入積分環(huán)節(jié)以減小系統(tǒng)的穩(wěn)態(tài)誤差；最后，為了抑制內(nèi)、外干擾，利用ESO來估算系統(tǒng)的總擾動(dòng)量并在控制量中進(jìn)行補(bǔ)償。通過Lyapunov穩(wěn)定性分析驗(yàn)證了控制器的有效性，并結(jié)合仿真分析驗(yàn)證了該算法的跟蹤性能。

1 四旋翼數(shù)學(xué)模型

四旋翼無人機(jī)的空間運(yùn)動(dòng)分為質(zhì)心運(yùn)動(dòng)和繞質(zhì)心運(yùn)動(dòng)兩個(gè)部分，因此首先要建立如圖1所示的兩個(gè)坐標(biāo)系：機(jī)體坐標(biāo)系Oxyz和地面坐標(biāo)系Oexeyeze。

圖1 四旋翼無人機(jī)坐標(biāo)系

假設(shè)四旋翼無人機(jī)為結(jié)構(gòu)對(duì)稱、質(zhì)量分布均勻、質(zhì)心與機(jī)體坐標(biāo)系原點(diǎn)一致的剛體，并忽略空氣阻力和低雷諾數(shù)的影響，再考慮外界干擾n(t)和系統(tǒng)未建模Δf(t)，兩者的總和記作σ，基于文獻(xiàn)[14]的數(shù)學(xué)模型，并且為了便于反步法的研究，可得嚴(yán)反饋形式下的四旋翼無人機(jī)模型:

(1)

式中，狀態(tài)變量

即：

a1=(Jy-Jz)/Jx，a2=(Jz-Jx)/Jy，

a3=(Jx-Jy)/Jz，b1=l/Jx，

b2=l/Jy，b3=1/Jz

U2=F4-F2=b(ω42-ω22)

U3=F3-F1=b(ω32-ω12)

U4=k(ω22+ω42-ω12-ω32)

Ux=cosφsinθcosψ+sinφsinψ

Uy=cosφsinθsinψ-sinφcosψ

其中：m為四旋翼無人機(jī)的質(zhì)量；x、y、z為地面坐標(biāo)系下的位置信息，φ、θ、ψ為地面坐標(biāo)系下滾轉(zhuǎn)角、俯仰角、偏航角；l為旋翼中心到機(jī)體坐標(biāo)原點(diǎn)的距離；Ji(i=x,y,z)為轉(zhuǎn)動(dòng)慣量；U1～U4和Ux、Uy為控制輸入；ωi(i=1,2,3,4)為四個(gè)電機(jī)轉(zhuǎn)速；b為升力系數(shù)；k為阻力系數(shù)。

2 基于ESO的復(fù)合干擾估計(jì)

ESO的作用是在僅知道系統(tǒng)輸入和輸出的情況下實(shí)現(xiàn)對(duì)各個(gè)狀態(tài)變量和系統(tǒng)總擾動(dòng)量的估計(jì)，然后在控制律中實(shí)時(shí)補(bǔ)償，從而達(dá)到克服系統(tǒng)內(nèi)部擾動(dòng)和外部噪聲的目的。鑒于觀測器所估算的擾動(dòng)是內(nèi)擾與外擾的和[15],不僅系統(tǒng)的抗干擾能力得到提高，同時(shí)也讓建模誤差對(duì)跟蹤效果的影響降到最低。

為了方便ESO的設(shè)計(jì)，將數(shù)學(xué)模型式(1)改寫為：

(2)

其中：

ξ1=[x1x3x5x7x9x11]T；

ξ2=[x2x4x6x8x10x12]T；

g(t)=[a1x4x6+σφa2x2x6+σθa3x2x4+σψ-g+σzσxσy]T，它為6通道各種擾動(dòng)的和(包括未建模、建模動(dòng)態(tài)和外部擾動(dòng))，且其導(dǎo)數(shù)有界；U=[U2U3U4U1UxUy]T。

設(shè)計(jì)如式(3)所示的擴(kuò)張狀態(tài)觀測器：

(3)

通過以上擴(kuò)張狀態(tài)觀測器，可估算出6通道的總干擾g(t)，然后在控制律中實(shí)時(shí)補(bǔ)償，增強(qiáng)系統(tǒng)的魯棒性。

3 積分型反步滑?？刂破髟O(shè)計(jì)

3.1 控制結(jié)構(gòu)設(shè)計(jì)

由數(shù)學(xué)模型可知，四旋翼無人機(jī)控制量U2、U3、U4的大小影響姿態(tài)角的變化，而姿態(tài)角與控制量U1又共同決定位置信息的變化，即當(dāng)已知U2、U3、U4這3個(gè)控制量時(shí)，3個(gè)姿態(tài)角及角速度信息就能被解算出來，如果當(dāng)U1已知，無人機(jī)的位置和速度信息也能推算出來。很顯然它們之間存在半耦合關(guān)系，因此，采用雙閉環(huán)結(jié)構(gòu)，即姿態(tài)內(nèi)環(huán)、位置外環(huán)。期望姿態(tài)和期望位置：ξ1d=[x1d,x3d,x5d,x7d,x9d,x11d]T=[φd,θd,ψd,zd,xd,yd]T，其控制結(jié)構(gòu)如圖2所示。

圖2 基于ESO的四旋翼無人機(jī)控制結(jié)構(gòu)

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

仍以數(shù)學(xué)模型式(2)為對(duì)象設(shè)計(jì)控制器，步驟如下。

定義誤差變量z1=ξ1d-ξ1，并對(duì)誤差變量求導(dǎo)：

(4)

引入一階跟蹤誤差的積分：

(5)

引入虛擬控制量α，定義誤差變量:

z2=ξ2-α

(6)

取Lyapunov函數(shù)：

其中n為正常數(shù)對(duì)角矩陣。

對(duì)V1求導(dǎo)得：

(7)

(8)

由式(7)、(8)推出：

(9)

(10)

取Lyapunov函數(shù)：

求導(dǎo)得：

(11)

取滑模面S=z2，代入得：

(12)

即

(13)

規(guī)定指數(shù)趨近律：

(14)

其中，ε1、ks為正常數(shù)對(duì)角矩陣。

(15)

至此，就完成了6通道控制量U的設(shè)計(jì)。

穩(wěn)定性證明如下：

將式(8)、(14)代入式(11)得:

(16)

當(dāng)S→0時(shí)，STS<0，能夠滿足滑模面存在及可達(dá)的必要條件，即z2→0時(shí)：

(-cz1)Tz1< 0

(17)

由Lyapunov穩(wěn)定定理可知，系統(tǒng)漸進(jìn)穩(wěn)定。

3.3 非線性約束條件

由上述數(shù)學(xué)模型可知，四旋翼無人機(jī)的實(shí)際輸入是四個(gè)電機(jī)的升力F1、F2、F3、F4，而輸出則為φ、θ、ψ、x、y、z六個(gè)狀態(tài)，因此，四旋翼無人機(jī)具有欠驅(qū)動(dòng)性特性，其必須通過姿態(tài)角來間接控制無人機(jī)的水平運(yùn)動(dòng)。因此，在雙閉環(huán)控制器設(shè)計(jì)中，通過反解水平控制量Ux、Uy來獲得滾轉(zhuǎn)角和俯仰角的期望值x1d、x3d，即所謂的非線性約束條件。

(18)

通過反解式(18)可以得出要實(shí)現(xiàn)控制量Ux、Uy所需的期望姿態(tài)角指令：

(19)

4 系統(tǒng)仿真分析

為驗(yàn)證所提控制算法的有效性，采用螺旋上升法對(duì)其仿真驗(yàn)證。設(shè)置期望軌跡為：

期望偏航角ψd=0。

四旋翼無人機(jī)各參數(shù)如下：m=1.4 kg，g=9.81 m/s2，Jx=0.03 kg·m2，Jy=0.03 kg·m2，Jz=0.04 kg·m2，l=0.2 m，k=4.003×10-7N·m·s-2，b=1.923 21×10-5N·s2。

仿真實(shí)驗(yàn)中，各參數(shù)選擇如下：k1=diag(120,120,120)，k2=diag(4 400,4 400,4 400)，k3=diag(48 000,48 000,48 000)，ε1=diag(25,25,25,2.5,2.5,2.5)，ks=diag(50,50,50,6,6,6)，n=diag(3,3,3,3,3,3)，c=diag(50,50,50,30,30,30)。

無人機(jī)初始位置[xyz]T=[0 0 0]T，初始姿態(tài)角[φθψ]T=[0 0 0]T，初始角速率[pqr]T=[0 0 0]T。位置環(huán)施加的復(fù)合干擾均為2 sint，姿態(tài)環(huán)施加的復(fù)合干擾為[sin 2t0 sin 2t]T。仿真結(jié)果如圖3～7所示。

結(jié)果分析：1)從仿真圖3、4可以看出，在施加復(fù)合干擾的情況下，采用ESO和積分型反步滑模算法設(shè)計(jì)的控制器，跟蹤誤差最大僅有0.06 m，跟蹤效果顯著，有較強(qiáng)的抗干擾能力，基本滿足無人機(jī)的跟蹤要求；而采用反步滑?？刂茣r(shí)，跟蹤誤差最大為0.25 m，抗干擾能力較差。2)從仿真圖5、6可以看出，兩種算法在姿態(tài)角上都有良好的跟蹤效果，但采用ESO對(duì)復(fù)合干擾進(jìn)行估計(jì)并補(bǔ)償時(shí)，得到的姿態(tài)角振幅僅有0.05 rad，并且振蕩頻率較低，僅為0.1 Hz，飛行較為穩(wěn)定；而采用反步滑?？刂扑惴〞r(shí)，振幅最大為0.5 rad，并且姿態(tài)角變化劇烈，頻率為0.4 Hz，飛行不穩(wěn)定。3)從仿真圖7可以看出，ESO能夠精確地對(duì)復(fù)合干擾進(jìn)行估計(jì)，減小了復(fù)合干擾對(duì)系統(tǒng)跟蹤精度的影響，從而增強(qiáng)了系統(tǒng)的抗干擾能力。

圖3 軌跡跟蹤三維效果對(duì)比

圖4 軌跡跟蹤誤差對(duì)比

圖5 反步滑?？刂频淖藨B(tài)跟蹤效果

圖6 本文算法的姿態(tài)跟蹤效果

圖7 ESO復(fù)合干擾估計(jì)

5 結(jié)語

本文針對(duì)欠驅(qū)動(dòng)四旋翼無人機(jī)，從實(shí)際應(yīng)用角度出發(fā)，考慮存在外界干擾和系統(tǒng)參數(shù)攝動(dòng)的情況，設(shè)計(jì)了一種基于ESO和積分型反步滑模算法的魯棒控制器。通過該方案，只需要得知系統(tǒng)的輸入和輸出，就能夠精確估算出系統(tǒng)所受內(nèi)、外總擾動(dòng)的值，然后在控制律中實(shí)時(shí)補(bǔ)償。通過對(duì)比仿真實(shí)驗(yàn)可以看出，在系統(tǒng)受到干擾時(shí)，該算法能夠使系統(tǒng)的抗干擾能力增強(qiáng)、跟蹤精度更高，并且姿態(tài)角變化頻率低，波動(dòng)幅值較小，飛行更加穩(wěn)定。