基于BSMTE的四旋翼飛行器的魯棒容錯(cuò)抗擾控制

石 川

(億航智能設(shè)備(廣州)有限公司, 廣東 廣州 510670)

0 引 言

無(wú)人機(jī)在航拍、防火防災(zāi)、空中物流運(yùn)輸、城市空中交通以及空中編隊(duì)表演等領(lǐng)域中有著十分重要的作用。作為一個(gè)新興科技產(chǎn)品,其為社會(huì)提供了更多的便利。無(wú)人機(jī)囊括了固定翼、單旋翼、多旋翼(四旋翼、六旋翼、八旋翼等)以及撲翼機(jī)等多種形態(tài),其中,四旋翼無(wú)人機(jī)憑借其自身特性,占據(jù)了較大市場(chǎng)比重。

文獻(xiàn)[1]運(yùn)用四元數(shù)結(jié)合擴(kuò)展卡爾曼濾波的方式對(duì)四旋翼飛行器的姿態(tài)進(jìn)行良好的估計(jì),并與傳統(tǒng)流行的互補(bǔ)濾波和梯度下降法進(jìn)行對(duì)比,展示出更好的魯棒性。傳統(tǒng)線性控制算法有比例積分微分(PID)[2]和線性二次型調(diào)節(jié)器(LQR)[3]等,雖然應(yīng)用在四旋翼飛行器上也有比較良好的控制效果,但四旋翼飛行器是一個(gè)非線性的多輸入多輸出強(qiáng)耦合欠驅(qū)動(dòng)的控制系統(tǒng),線性的控制算法很難解決非線性且復(fù)雜的問(wèn)題。文獻(xiàn)[4]采用積分反步法,解決了傳統(tǒng)反步控制存在的穩(wěn)態(tài)誤差問(wèn)題。文獻(xiàn)[5]融合了H2、H∞控制,解決了模型不確定和噪聲干擾導(dǎo)致控制發(fā)散的問(wèn)題,但缺少對(duì)實(shí)際飛行軌跡跟蹤控制的探討。文獻(xiàn)[6]采用自適應(yīng)積分反步控制,在文獻(xiàn)[4]的基礎(chǔ)上提高了魯棒性,降低了陣風(fēng)干擾和模型不確定帶來(lái)的墜機(jī)風(fēng)險(xiǎn)。文獻(xiàn)[7]運(yùn)用基于模型的強(qiáng)化學(xué)習(xí)方法實(shí)現(xiàn)了預(yù)先動(dòng)力學(xué)未知情況下的懸?？刂啤Ｎ墨I(xiàn)[8]采用神經(jīng)網(wǎng)絡(luò)自適應(yīng)和滑?？刂频姆椒?實(shí)現(xiàn)了四旋翼飛行器的位置姿態(tài)跟蹤和抗擾,但是由于神經(jīng)網(wǎng)絡(luò)相對(duì)依賴(lài)硬件設(shè)備的性能,故比較難應(yīng)用于低成本的真實(shí)飛機(jī)。

本文提出了一種將反步法、滑模、跟蹤微分器(TD)[9]以及擴(kuò)張狀態(tài)觀測(cè)器(ESO)[9]結(jié)合的反步滑模抗擾(BSMTE)控制方法,并開(kāi)展了數(shù)值仿真實(shí)驗(yàn)及真機(jī)驗(yàn)證實(shí)驗(yàn)。結(jié)果表明,所提方法應(yīng)用在四旋翼飛行器上,可實(shí)現(xiàn)魯棒性、容錯(cuò)性與抗擾性更好的飛行控制,以應(yīng)對(duì)更多非線性且較為復(fù)雜和困難的環(huán)境,執(zhí)行更為艱難的任務(wù)。

1 建 模

假設(shè)飛行器是剛性且對(duì)稱(chēng),螺旋槳和電機(jī)也是剛體,電機(jī)的推力和阻力正比于轉(zhuǎn)子轉(zhuǎn)速的平方,電機(jī)轉(zhuǎn)動(dòng)軸平行于軸方向,慣性矩陣是時(shí)不變的,忽略地面效應(yīng),忽略飛行器的彈性形變。

運(yùn)用文獻(xiàn)[4]提供的四旋翼飛行器的動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)公式推導(dǎo)得出的數(shù)學(xué)建模,并結(jié)合真機(jī)實(shí)驗(yàn)平臺(tái)的電機(jī)序號(hào),給出如下定義。

(1)

式中:U1為油門(mén)控制輸入;x、y、z分別為三個(gè)線性方向;φ、θ、ψ分別為三個(gè)姿態(tài)角;m、g分別為四旋翼飛行器的質(zhì)量和重力加速度;Ix、Iy、Iz為四旋翼飛行器在三個(gè)方向上的轉(zhuǎn)動(dòng)慣量;Jr為轉(zhuǎn)子慣量;ω1、ω2、ω3、ω4分別為四個(gè)電機(jī)的轉(zhuǎn)速;b、d、l分別為升力系數(shù)、阻力系數(shù)和機(jī)臂長(zhǎng)度。此處,ωd=ω1+ω2-ω4-ω3。

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

本節(jié)先對(duì)TD、ESO進(jìn)行原理描述,隨后對(duì)反步滑?？刂破鬟M(jìn)行嚴(yán)格的Lyapunov穩(wěn)定性證明,并且推導(dǎo)出了控制律。

(1)跟蹤微分器(TD)

TD作為一種合理的過(guò)渡安排過(guò)程,目的是快速跟蹤目標(biāo)輸入,并減弱由于快速響應(yīng)帶來(lái)的超調(diào),其控制器框架如圖1所示。

圖1 提出的控制器框架圖

通過(guò)函數(shù)fhan(q1,q2,r,h)來(lái)實(shí)現(xiàn)上述過(guò)程,具體為

(2)

式中:r、h、d分別為快速因子、步長(zhǎng)和濾波因子;q1、q2均為輸入狀態(tài)量。

(2)擴(kuò)張狀態(tài)觀測(cè)器(ESO)

ESO作為一種特殊的干擾狀態(tài)觀測(cè)器,輸入一維狀態(tài)v,其估計(jì)值v1和v的一階微分估計(jì)值v2,以及二階微分v3作為系統(tǒng)的總擾動(dòng)項(xiàng)。其中,v1和v2除了能逼近真實(shí)狀態(tài)v和v的一階導(dǎo)數(shù)之外,還可以濾除部分噪聲,使得曲線更平滑,從而提高系統(tǒng)的穩(wěn)定性。其功能通過(guò)式(3)的fal(c,α,δ)來(lái)實(shí)現(xiàn),其輸入輸出過(guò)程如圖1所示。

(3)

式中:c、α、δ為函數(shù)變量,無(wú)具體含義。

(3)水平位置控制器的設(shè)計(jì)

這里采用傳統(tǒng)的反步法?？紤]誤差e1,其中xd是x的期望值。

e1=xd-x

(4)

定義一個(gè)Lyapunov函數(shù)

(5)

可以看出該函數(shù)是正定的。對(duì)式(5)進(jìn)行求導(dǎo)可得

(6)

式中:f1是一個(gè)虛擬的控制量,令

(7)

代入式(6)得到

(8)

由Lyapunov穩(wěn)定性可知,該控制器是穩(wěn)定的,其中κ1為控制增益。

接著,定義一個(gè)二階跟蹤誤差

(9)

于是

(10)

繼續(xù)定義一個(gè)Lyapunov函數(shù)

(11)

可知是正定的,求導(dǎo)可得

(12)

(13)

此時(shí)控制器穩(wěn)定,其中κ2為控制增益,ux、U1分別為水平x方向位置控制器輸入和垂直位置控制器輸入,下同。由此可得

(14)

根據(jù)水平x方向的位置控制器,可以得出水平y(tǒng)方向的位置控制器,即

(15)

其中

反解模塊為

(16)

其中

(4)垂直位置控制器的設(shè)計(jì)

為了提升控制系統(tǒng)的魯棒性,這里引入滑?？刂?。首先,滑模面為

sz=-κ5e5-e6

(17)

其中

滑?？刂频腖yapunov穩(wěn)定性推導(dǎo)類(lèi)似水平位置控制器所采用的反步法,只是用滑模面來(lái)代替二階跟蹤誤差。根據(jù)滑?？刂婆c上述水平位置控制器的反步控制公式,得到如下垂直位置控制器

(18)

(5)姿態(tài)控制器

根據(jù)上述推導(dǎo),可以得出三個(gè)姿態(tài)角度控制器的控制輸入,即

(19)

(20)

(21)

其中

式中:ai(i=1,2,3,4,5)、bj(j=1,2,3)均為中間變量;sφ、sθ、sψ為三個(gè)姿態(tài)角的滑模面;ei(i=1,2,…,12)為上述控制器的誤差;其他大于0的參數(shù)均為控制增益。

3 數(shù)值仿真實(shí)驗(yàn)

本文采用文獻(xiàn)[4]中提供的四旋翼數(shù)值仿真模型參數(shù)進(jìn)行數(shù)值仿真實(shí)驗(yàn)。

(1)實(shí)驗(yàn)1:抗陣風(fēng)擾動(dòng)實(shí)驗(yàn)

軌跡跟蹤期望的三維方向?yàn)?/p>

(22)

式中:t表示仿真實(shí)驗(yàn)運(yùn)行時(shí)間。外界陣風(fēng)擾動(dòng)數(shù)值為

(23)

本實(shí)驗(yàn)在引入外界陣風(fēng)擾動(dòng)的情況下,觀察四旋翼無(wú)人機(jī)的軌跡跟蹤性能和姿態(tài)穩(wěn)定性能是否滿足響應(yīng)較快、超調(diào)量小且穩(wěn)定的控制要求,并與文獻(xiàn)[4]中提出的積分反步(IB)控制算法以及傳統(tǒng)的比例積分微分(PID)控制算法進(jìn)行了對(duì)比。

圖2和圖3展示了四旋翼飛行器的速度以及位置信息。表1為根據(jù)圖3得出的位置跟蹤控制性能對(duì)比表,其中x、y、z方向的響應(yīng)時(shí)間分別從實(shí)驗(yàn)的第9 s、第5 s和第0 s開(kāi)始計(jì)算,在第30 s～40 s引入了陣風(fēng)擾動(dòng)。由表1可見(jiàn),IB算法在x、y方向上的超調(diào)量和響應(yīng)時(shí)間均最小,但引入陣風(fēng)擾動(dòng)后,在z方向出現(xiàn)了較大的超調(diào)量。PID算法的響應(yīng)時(shí)間較小,但3個(gè)方向均產(chǎn)生了較大的超調(diào)量。而本文的BSMTE算法在滿足響應(yīng)時(shí)間較小的前提下,在3個(gè)方向均取得了較小的超調(diào)量,說(shuō)明本文算法具有良好的抗擾動(dòng)性。圖2的飛行速度信息、圖4的空間側(cè)視信息、圖5和圖6的姿態(tài)角度、角速度信息也可以佐證上述結(jié)論。

表1 實(shí)驗(yàn)1位置跟蹤控制性能對(duì)比表

圖2 四旋翼飛行器的速度信息

圖3 四旋翼飛行器的位置信息

圖4 四旋翼飛行器的三維位置空間側(cè)視圖

圖5 四旋翼飛行器的姿態(tài)角度

圖6 四旋翼飛行器的姿態(tài)角速度

(2)實(shí)驗(yàn)2:模型不確定性實(shí)驗(yàn)

本實(shí)驗(yàn)在模型不確定的情況下,使其中的單個(gè)執(zhí)行器僅提供75%的升力,另外三個(gè)執(zhí)行器提供完整的升力,來(lái)完成軌跡跟蹤任務(wù)。本實(shí)驗(yàn)僅與IB算法進(jìn)行對(duì)比,因在同等升力失效的情況下,PID算法控制已發(fā)散,無(wú)法完成目標(biāo)任務(wù)。圖7、圖8分別為單執(zhí)行器失效15%下的速度信息及位置信息。

圖7 單執(zhí)行器失效15%下的速度信息

圖8 單執(zhí)行器失效15%下的位置信息

由圖8可以計(jì)算得出實(shí)驗(yàn)2的相關(guān)控制性能數(shù)據(jù)信息,如表2所示。

表2 實(shí)驗(yàn)2位置跟蹤控制性能對(duì)比表

對(duì)比可得,IB算法和本文算法的響應(yīng)時(shí)間均符合控制要求,但本文算法的最大超調(diào)量遠(yuǎn)遠(yuǎn)小于IB算法,說(shuō)明在單個(gè)執(zhí)行器失效15%的情況下,本文算法仍能達(dá)到良好的位置控制跟蹤性能。

綜合表1可知,本文算法在模型確定無(wú)失效及模型不確定有部分失效的情況下,均可以較好地滿足位置跟蹤控制要求,說(shuō)明本文提出的算法具備較好的魯棒性和容錯(cuò)性。圖9～圖11也佐證了這一結(jié)論。

圖9 單執(zhí)行器失效15%下的三維位置信息

圖10 單執(zhí)行器失效15%下的姿態(tài)角

圖11 單執(zhí)行器失效15%下的姿態(tài)角速度

4 真機(jī)飛行實(shí)驗(yàn)

本實(shí)驗(yàn)采用億航GHOST2.0四旋翼無(wú)人機(jī)作為真機(jī)實(shí)驗(yàn)平臺(tái),如圖12所示,其整機(jī)重量為1.15 kg,其中,飛控嵌入式程序采用C/C++語(yǔ)言進(jìn)行編寫(xiě),主控采用算力較低的STM32F4單片機(jī)。

圖12 億航GHOST2.0四旋翼無(wú)人機(jī)平臺(tái)

為驗(yàn)證本文提出的BSMTE算法的容錯(cuò)性和魯棒性,在實(shí)驗(yàn)中采取對(duì)4號(hào)螺旋槳進(jìn)行適當(dāng)裁剪的方式,以確保四旋翼無(wú)人機(jī)的物理模型是不確定的。通過(guò)運(yùn)用遙控器操控四旋翼無(wú)人機(jī)解鎖并起飛到空中適當(dāng)高度,做一些橫滾俯仰的飛行動(dòng)作,而后保持飛機(jī)水平位置懸停,觀察飛機(jī)表現(xiàn),擇機(jī)降落后獲取飛行日志,進(jìn)行數(shù)據(jù)分析。當(dāng)4號(hào)螺旋槳進(jìn)行適當(dāng)裁剪后,其只能提供約75%的升力,而其他3個(gè)螺旋槳仍提供100%升力。裁剪后的4號(hào)螺旋槳和與其旋轉(zhuǎn)方向一致的正常3號(hào)螺旋槳的對(duì)比如圖13所示。

圖13 裁剪前后對(duì)比圖

本實(shí)驗(yàn)截取飛行日志中(已對(duì)4號(hào)螺旋槳進(jìn)行適當(dāng)裁剪)飛行姿態(tài)角度、角速度和位置信息數(shù)據(jù)進(jìn)行對(duì)比。通過(guò)圖14和圖15可以看出,姿態(tài)角度雖然存在小范圍的超調(diào)誤差,但是響應(yīng)速度較快,靈敏度及穩(wěn)定性較高。從俯仰角和橫滾角可以看出,飛機(jī)的真實(shí)姿態(tài)響應(yīng)緊貼目標(biāo)角度,而偏航角會(huì)存在目標(biāo)角度上下10°的波動(dòng)誤差,這是由于飛機(jī)的偏航姿態(tài)控制優(yōu)先級(jí)低于橫滾姿態(tài)和俯仰姿態(tài)控制,姿態(tài)控制會(huì)調(diào)用更多的執(zhí)行器輸出給到俯仰和橫滾姿態(tài)上,從而減弱偏航姿態(tài)的控制效果。此外,本實(shí)驗(yàn)使用的四旋翼無(wú)人機(jī)的4個(gè)執(zhí)行器電機(jī)安裝角度垂直于機(jī)臂所在機(jī)身平面,故螺旋槳的槳盤(pán)面平行于機(jī)臂所在的機(jī)身平面,從力學(xué)上分析,缺少一定的作用于偏航姿態(tài)的力和力矩,故偏航的控制效果稍差于橫滾和俯仰姿態(tài)。

圖14 姿態(tài)角度對(duì)比

圖15 姿態(tài)角速度對(duì)比

由圖16可看出,僅在小范圍存在超調(diào)誤差,整體位置基本沒(méi)有穩(wěn)態(tài)誤差,位置跟蹤控制性能穩(wěn)定。圖17反映了飛機(jī)的4個(gè)電機(jī)螺旋槳的控制輸出轉(zhuǎn)速差異,可明顯看出,4號(hào)執(zhí)行器電機(jī)螺旋槳的輸出轉(zhuǎn)速遠(yuǎn)遠(yuǎn)高于其他3個(gè),通過(guò)提升4號(hào)電機(jī)的轉(zhuǎn)速,換取更大的升力,來(lái)補(bǔ)償因螺旋槳被部分裁剪而損失的升力,進(jìn)而驗(yàn)證了本文提出的算法具有較強(qiáng)的容錯(cuò)性和魯棒性。

圖16 水平位置信息

圖17 4個(gè)電機(jī)控制轉(zhuǎn)速輸入

圖16中,對(duì)比700 s左右x和y的位置信息,可以看出飛機(jī)處于水平懸停的狀態(tài)。圖14中也可以看出,700 s前后,飛機(jī)的俯仰角和橫滾角都趨于0°且航向角誤差也是趨于0°。圖15中同一時(shí)間左右的俯仰角速度和橫滾角速度也是緊貼目標(biāo)角速度且趨近于0°/s的。這表明,使用本文提出的BSMTE算法的四旋翼無(wú)人機(jī)在懸停狀態(tài)下的位置控制和姿態(tài)控制精度較高。同一時(shí)刻,圖17中的4號(hào)電機(jī)轉(zhuǎn)速控制輸入約為1 750,3號(hào)電機(jī)轉(zhuǎn)速控制輸入近似為1 580,由于飛控的轉(zhuǎn)速映射范圍為1 100～1 900,所以差值約占總范圍的0.21,約為五分之一,而誤差是由位置偏差導(dǎo)致的姿態(tài)小角度修正所產(chǎn)生。上述結(jié)論從真實(shí)數(shù)據(jù)上驗(yàn)證了執(zhí)行器部分失效,進(jìn)而驗(yàn)證了螺旋槳提供的升力缺失,即螺旋槳有破損或人為裁剪等情況。

5 結(jié)束語(yǔ)

本文提出了BSMTE控制算法,并通過(guò)數(shù)值仿真實(shí)驗(yàn)和低算力真機(jī)飛行實(shí)驗(yàn),驗(yàn)證了本文所提算法具有良好的姿態(tài)、位置跟蹤性能,且在有外界擾動(dòng)或執(zhí)行機(jī)構(gòu)部分失效的情況下,依舊可以完成指定任務(wù),具有良好的魯棒性、容錯(cuò)性和抗擾動(dòng)性。后續(xù)將針對(duì)執(zhí)行器機(jī)構(gòu)絕大部分失效或單個(gè)執(zhí)行器完全失效的情況展開(kāi)故障檢測(cè)、分離及魯棒性更強(qiáng)的容錯(cuò)控制研究與試驗(yàn),并且增加一定的執(zhí)行器安裝傾角,以提升偏航姿態(tài)的控制效果。