基于改進(jìn)PD的LESO四旋翼控制算法

劉春玲,馮錦龍,張 瑾

(大連大學(xué)信息工程學(xué)院,遼寧 大連 116622)

1 引言

四旋翼飛行器控制系統(tǒng)是一個(gè)非線性、強(qiáng)耦合的復(fù)雜系統(tǒng),給控制設(shè)計(jì)帶來(lái)了巨大的挑戰(zhàn),針對(duì)這一復(fù)雜系統(tǒng),文獻(xiàn)[1,2]用經(jīng)典PID設(shè)計(jì)控制器,因?qū)崿F(xiàn)簡(jiǎn)單、有良好的抗干擾能力等,在四旋翼飛行器控制系統(tǒng)的設(shè)計(jì)中得到廣泛應(yīng)用,然而其處理耦合問(wèn)題的能力較差及不能自適應(yīng)調(diào)整參數(shù)。文獻(xiàn)[3,4]設(shè)計(jì)了改進(jìn)型PID控制器,利用在經(jīng)典PID控制原理基礎(chǔ)上進(jìn)行了改進(jìn),但未考慮飛行環(huán)境中的未知擾動(dòng)[3]和需要精確的數(shù)學(xué)模型[4]。文獻(xiàn)[5-7]設(shè)計(jì)了雙回路PD參數(shù)優(yōu)化控制器,該設(shè)計(jì)通用性強(qiáng)、易實(shí)現(xiàn),尋優(yōu)速度快且跟蹤效果良好,然而在四旋翼飛行器建模過(guò)程中,忽略了空氣阻力和未知擾動(dòng)等。文獻(xiàn)[8,9]設(shè)計(jì)了PID的濾波器控制器,該方法能實(shí)現(xiàn)良好的魯棒性,但對(duì)強(qiáng)耦合的非線性四旋翼控制系統(tǒng)控制不理想。文獻(xiàn)[10,11]采用模糊PID控制器,該方法有良好的動(dòng)態(tài)性能,但需要更細(xì)化的模糊規(guī)則,增加了該方法的實(shí)現(xiàn)難度。

針對(duì)四旋翼非線性、強(qiáng)耦合及易受外界干擾引起姿態(tài)不穩(wěn)定問(wèn)題,設(shè)計(jì)了基于改進(jìn)型PD的LESO(Liner Extended State Observer)飛行控制方法。首先,在反饋回路中設(shè)計(jì)了LESO,運(yùn)用LESO原理實(shí)現(xiàn)四旋翼姿態(tài)的解耦控制,同時(shí)對(duì)系統(tǒng)的內(nèi)外部擾動(dòng)進(jìn)行估計(jì)補(bǔ)償?shù)皆到y(tǒng)中,增強(qiáng)系統(tǒng)魯棒性;然后利用改進(jìn)PD控制,增強(qiáng)系統(tǒng)動(dòng)態(tài)特性,降低外界信號(hào)對(duì)飛行控制系統(tǒng)的干擾,提高系統(tǒng)的抗干擾能力。

2 四旋翼飛行器數(shù)學(xué)模型

在四旋翼建模時(shí),假設(shè)機(jī)體是質(zhì)量均勻且對(duì)稱的剛體,四個(gè)旋翼為剛性且不會(huì)發(fā)生形變,機(jī)體坐標(biāo)和質(zhì)心坐標(biāo)重合。

四旋翼飛行器的結(jié)構(gòu)如圖1所示,Mi代表各個(gè)旋翼的電機(jī),定義慣性坐標(biāo)E(XYZ)和機(jī)體坐標(biāo)B(xyz)之間的相對(duì)位置可以通過(guò)位置坐標(biāo)確定,飛行器在地面坐標(biāo)系下的姿態(tài)角θ,φ,ψ為機(jī)體坐標(biāo)系下的pitch,roll,yaw,即俯仰角、滾轉(zhuǎn)角和偏航角,令R(θ,φ,ψ)表示慣性坐標(biāo)E和機(jī)體坐標(biāo)B之間的轉(zhuǎn)換矩陣,可以表示為

圖1 四旋翼飛行器坐標(biāo)及電機(jī)示意圖

(1)

四旋翼飛行器非線性運(yùn)動(dòng)方程分為平移運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng)。由牛頓第二定律得x,y,z方向的平移運(yùn)動(dòng)表示為

(2)

其中Fi(i=1,2,3,4)為螺旋槳的升力,m為四旋翼機(jī)體的質(zhì)量。

根據(jù)角動(dòng)量守恒得四旋翼旋轉(zhuǎn)運(yùn)動(dòng)的表示為

其中Ii(i=x,y,z)為軸的轉(zhuǎn)動(dòng)慣量,l為機(jī)體軸心距,Di(i=θ,φ,ψ)為環(huán)境中不確定的干擾分解到坐標(biāo)軸的分量。

為了簡(jiǎn)化四旋翼的平移運(yùn)動(dòng)和旋轉(zhuǎn)運(yùn)動(dòng),定義虛擬控制量U1,U2,U3,U4為四個(gè)獨(dú)立的輸入,分別以升力F1,F2,F3,F4進(jìn)行變換,變換公式可表示為

(4)

結(jié)合方程式(2)、(3)和(4),四旋翼無(wú)人機(jī)飛行器系統(tǒng)的動(dòng)力學(xué)模型可表示為:

(5)

3 改進(jìn)型PD的LESO控制器設(shè)計(jì)

3.1 改進(jìn)型PD

常規(guī)PD控制器通過(guò)對(duì)比例系數(shù)和微分時(shí)間常數(shù)的適當(dāng)調(diào)整,可達(dá)到較為良好的控制效果,其結(jié)構(gòu)如圖2所示。

圖2 常規(guī)PD控制器

PD控制器的控制規(guī)律表達(dá)式可表示為

(6)

式中kP為比例系數(shù),TD為微分時(shí)間常數(shù),e(k)為系統(tǒng)誤差。

對(duì)PD控制器的微分項(xiàng)可表示為

(7)

由式(7)可以看出,微分環(huán)節(jié)的引入,改善了系統(tǒng)的動(dòng)態(tài)特性,但對(duì)高頻干擾信號(hào)特別敏感,容易引入擾動(dòng)。同時(shí)說(shuō)明在誤差擾動(dòng)突變時(shí)會(huì)顯出微分項(xiàng)的不足,而且長(zhǎng)時(shí)間系統(tǒng)會(huì)積累靜態(tài)誤差,導(dǎo)致魯棒性下降,很難達(dá)到預(yù)期的控制效果。

為克服上述問(wèn)題,在常規(guī)PD算法中加入了一階慣性環(huán)節(jié)Df=1/(1+Tf(s)),則改進(jìn)型PD控制器,結(jié)構(gòu)如圖3所示。

圖3 改進(jìn)型PD控制器

改進(jìn)型PD控制器的控制規(guī)律可表示為

u0(k)=uP(k)+uD(k)+uDf(k)

(8)

式中uP(k)為比例控制,uD(k)加上uDf(k)為微分控制,e(k)為系統(tǒng)誤差。

對(duì)于微分環(huán)節(jié)加入一階慣性后,其微分方程形式可表示為

(9)

將(9)式離散化,可表示為:

(10)

式中Tf為一階慣性常數(shù),T為采樣周期。

對(duì)(10)式整理有:

(11)

對(duì)(8)～(11)式進(jìn)行整理可得:

u0(k)=kpe(k)+uD(k)+uDf(k)=kpe(k)+kd(1-α)[e(k)-e(k-1)]+αud(k-1)

(12)

由數(shù)學(xué)模型知,四旋翼的旋轉(zhuǎn)運(yùn)動(dòng)方程是一個(gè)多輸入多輸出非線性耦合系統(tǒng),此處利用線性自抗擾控制算法中的線性擴(kuò)張狀態(tài)觀測(cè)器對(duì)系統(tǒng)耦合部分及外部擾動(dòng)進(jìn)行實(shí)時(shí)跟蹤和估計(jì),并結(jié)合系統(tǒng)控制律將估計(jì)值進(jìn)行補(bǔ)償并反饋到原系統(tǒng)中,不僅將多輸入多輸出的非線性耦合系統(tǒng)轉(zhuǎn)變成相對(duì)獨(dú)立的單輸入單輸出線性子系統(tǒng);還可以通過(guò)LESO有效的剔除系統(tǒng)內(nèi)外部擾動(dòng),減少反饋誤差,增強(qiáng)系統(tǒng)抗外部干擾能力和對(duì)自身參數(shù)不確定等因素的適應(yīng)能力,改進(jìn)型PD的LESO控制器框圖如圖4所示。

3.2 線性擴(kuò)張狀態(tài)觀測(cè)器

線性擴(kuò)張狀態(tài)觀測(cè)器[12,13]是線性自抗干擾的核心組成部分,其工作原理就是把系統(tǒng)中的各種干擾擴(kuò)張為一個(gè)新的狀態(tài)變量,通過(guò)系統(tǒng)控制律和輸出動(dòng)態(tài)的估計(jì)總擾動(dòng),并有效對(duì)總擾動(dòng)項(xiàng)進(jìn)行反饋補(bǔ)償?shù)皆到y(tǒng)中,不僅能解決四旋翼模型不確定部分引起的內(nèi)部干擾,還能增強(qiáng)控制系統(tǒng)的魯棒性。

以滾動(dòng)通道φ為例,詳細(xì)描述建立四旋翼飛行器單輸入單輸出的線性擴(kuò)張狀態(tài)觀測(cè)器的二階數(shù)學(xué)模型,有

(13)

(14)

然后對(duì)系統(tǒng)(14)的LESO進(jìn)行如下設(shè)計(jì)

(15)

由(15)式可知,LESO通過(guò)適當(dāng)?shù)挠^測(cè)器參數(shù)[β1,β2,β3]=[3w1,3w2,w3]T來(lái)估計(jì)狀態(tài)變量x1,x2和擴(kuò)展?fàn)顟B(tài)變量x3,即各個(gè)狀態(tài)X=(x1,x2,x3)對(duì)應(yīng)估計(jì)值Z=(z1,z2,z3),其中w為觀測(cè)器帶寬,利用LESO將控制系統(tǒng)中的內(nèi)外部擾動(dòng)進(jìn)行估計(jì)并補(bǔ)償?shù)皆到y(tǒng)中,不僅能減少系統(tǒng)誤差,根據(jù)文獻(xiàn)[14]可知,只需保證w的取值大于零且總和擾動(dòng)有界便可以保證LESO的穩(wěn)定性。

為有效剔除四旋翼飛行控制系統(tǒng)中的擾動(dòng),其控制率可設(shè)計(jì)為

(16)

以四旋翼動(dòng)力學(xué)數(shù)學(xué)模型中翻滾通道為例說(shuō)明,取方程式(5)做如下變化。

(17)

(18)

式中b0=l/Ix,u=u2。

根據(jù)式(17)、(18)處理后,對(duì)象的狀態(tài)方程可以寫(xiě)作如(19)所示。

(19)

式中x1=φ,u=u2,由式(19)知,原非線性系統(tǒng)(13)已轉(zhuǎn)化為線性系統(tǒng)(19),且系統(tǒng)中不確定擾動(dòng)被實(shí)時(shí)動(dòng)態(tài)線性化?；诟倪M(jìn)PD的LESO控制算法設(shè)計(jì)步驟如下:

(i) 通過(guò)歐拉方程和角動(dòng)量守恒建立四旋翼的非線性數(shù)學(xué)模型,選取對(duì)象模型的非線性、耦合及未知部分作為擾動(dòng)項(xiàng)并建立如式所示的“動(dòng)態(tài)線性化”模型。

(ii) 根據(jù)線性化模型,設(shè)計(jì)改進(jìn)型PD控制器的控制率u0。

(iii) 選取合適的β1,β2,β3,使LESO能對(duì)總擾動(dòng)項(xiàng)z3更好的估計(jì),并動(dòng)態(tài)補(bǔ)償?shù)皆到y(tǒng)中,改進(jìn)型PD控制器輸出u0,得到實(shí)時(shí)控制量u=uo-z3/b0。

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

為驗(yàn)證所提出的算法有效性,采用Matlab進(jìn)行四旋翼飛行器控制系統(tǒng)的仿真。四旋翼飛行器物理參數(shù)如表1所示,仿真參數(shù)如表2所示。

表1 四旋翼飛行器物理參數(shù)

表2 仿真參數(shù)

4.1 姿態(tài)及軌跡分析

仿真時(shí),為模擬四旋翼飛行過(guò)程中環(huán)境擾動(dòng)不確定及飛行軌跡,設(shè)定四旋翼飛行器飛行軌跡路線沿(3,2,0)到(0,0,5),在沿軌跡飛行過(guò)程中給翻滾角加入擾動(dòng),模擬四旋翼器飛行環(huán)時(shí)的內(nèi)外擾動(dòng),并將擾動(dòng)設(shè)置為0.5(sign(sin(0.5t))+cos(0.6t)+cos(0.7t)+0.5sign(sin(t))),且在3s時(shí)加入擾動(dòng),仿真如下。

由圖5和圖6可知,在3s時(shí)給翻滾角加入擾動(dòng)后,PID控制仿真圖翻滾通道位置的幅值是改進(jìn)PD的LESO控制器的幅值的1.6倍,且前者幅值跳變較為陡峭,后者較為平緩;除此之外,由PID控制和改進(jìn)PD的LESO控制器的俯仰角對(duì)比知,后者調(diào)節(jié)時(shí)間較短;由PID控制和改進(jìn)PD的LESO控制器的偏航角對(duì)比知,后者不僅調(diào)節(jié)時(shí)間較短,而且幅值調(diào)節(jié)也較為平滑。由對(duì)比可知,在四旋翼飛行器飛行中受干擾時(shí),改進(jìn)PD的LESO控制器對(duì)位置的調(diào)節(jié)能力優(yōu)于經(jīng)典的PID控制器。

圖5 PID位置調(diào)節(jié)

圖6 改進(jìn)PD的LESO位置調(diào)節(jié)

由圖7和圖8可以看出在3s時(shí)加入擾動(dòng)后,PID控制仿真圖翻滾通道的姿態(tài)調(diào)節(jié)幅值為0.057rad,受到干擾后調(diào)節(jié)趨勢(shì)較為陡峭;俯仰角調(diào)節(jié)較為平緩,但調(diào)節(jié)幅值較大;偏航角調(diào)節(jié)時(shí)間為2.5s左右。改進(jìn)PD的LESO控制器翻滾通道姿態(tài)受干擾后調(diào)節(jié)幅值約為PID控制器的一半,且受到干擾后調(diào)節(jié)趨勢(shì)較為平緩;俯仰角調(diào)節(jié)較為平緩,且調(diào)節(jié)幅值遠(yuǎn)小于PID控制的俯仰角調(diào)節(jié)幅值;偏航角調(diào)節(jié)時(shí)間為0.5s左右,響應(yīng)時(shí)間遠(yuǎn)小于PID。除此之外,PID控制翻滾角姿態(tài)的幅值為0.28rad左右,且較為陡峭;俯仰角在達(dá)到平穩(wěn)前幅值0.4rad左右。改進(jìn)PD的LESO控制器翻滾角姿態(tài)幅值為0.18rad左右,且較為平緩;俯仰角在達(dá)到平穩(wěn)前幅值為0.25rad左右?？梢?jiàn)改進(jìn)PD控制器結(jié)合LESO對(duì)擾動(dòng)進(jìn)行補(bǔ)償后,能夠明顯提高各通道的姿態(tài)角解算能力,減少幅值跳變。仿真結(jié)果表明,所設(shè)計(jì)的控制器,可以明顯提高四旋翼旋轉(zhuǎn)運(yùn)動(dòng)的抗干擾能力,增強(qiáng)系統(tǒng)的魯棒性。

圖7 PID姿態(tài)調(diào)節(jié)

圖8 改進(jìn)PD的LESO姿態(tài)調(diào)節(jié)

5 結(jié)論

針對(duì)四旋翼飛行器非線性、強(qiáng)耦合和易受外界干擾引起姿態(tài)不穩(wěn)定問(wèn)題,設(shè)計(jì)了一種改進(jìn)PD的線性擴(kuò)張狀態(tài)觀測(cè)器(LESO)控制器,并以經(jīng)典PID作為參考。仿真結(jié)果表明,通過(guò)改進(jìn)型PD控制器,利用LESO原理對(duì)擾動(dòng)估計(jì)并補(bǔ)償?shù)皆到y(tǒng)中,不僅能夠有效抑制系統(tǒng)誤差,還能增強(qiáng)控制系統(tǒng)的抗干擾能力和魯棒性,且具超調(diào)量小和控制效果更佳。該控制方法在四旋翼飛行控制領(lǐng)域有著可待的應(yīng)用前景。此外,進(jìn)行了對(duì)比仿真,表明該控制器實(shí)現(xiàn)了令人滿意的跟蹤精度和更優(yōu)越的控制性能。希望未來(lái)通過(guò)實(shí)際的四旋翼無(wú)人機(jī)飛行實(shí)驗(yàn)驗(yàn)證所提出控制器的有效性。