楊明博，周紫陽，戴 峻，游翔茗

（1.北方工業(yè)大學(xué)機(jī)械與材料工程學(xué)院，北京 100043；2.煤炭科學(xué)技術(shù)研究有限公司，北京 100020）

0 引言

在21 世紀(jì)的今天，隨著人們對自動(dòng)控制領(lǐng)域和傳感技術(shù)領(lǐng)域的深入研究，融合這兩部分技術(shù)的無人機(jī)飛行器成為了一個(gè)專門的研究領(lǐng)域。在無人機(jī)飛行器領(lǐng)域中，四旋翼飛行器自身結(jié)構(gòu)簡單，能夠?qū)崿F(xiàn)自由懸停和垂直起降等運(yùn)動(dòng)方式，實(shí)際應(yīng)用中操作簡單，能夠克服惡劣環(huán)境到達(dá)一些特殊的區(qū)域，引起很多學(xué)者的關(guān)注［1-2］。

關(guān)于四旋翼飛行器的軌跡跟蹤控制，學(xué)者們提出了不同的控制方法。其中在實(shí)際中經(jīng)常應(yīng)用的控制算法有PID 控制［3-4］、滑?？刂疲?-7］、自適應(yīng)控制［8-9］、反步控制［10］等。在眾多控制算法當(dāng)中，PID 控制算法無論是在工業(yè)還是民用中都被廣泛使用。PID 算法能通過調(diào)節(jié)3 個(gè)環(huán)節(jié)的參數(shù)使系統(tǒng)達(dá)到穩(wěn)定，但是尋找合適的參數(shù)是非常復(fù)雜的過程，工作量非常大。文獻(xiàn)［5］中就是采用PID 控制算法進(jìn)行位置和姿態(tài)信息的軌跡跟蹤：通過對反饋信號的分析研究和基于PID 算法的控制器設(shè)計(jì)，跟蹤到四旋翼飛行器的飛行軌跡。但是調(diào)節(jié)參數(shù)的過程復(fù)雜，并且所設(shè)計(jì)的控制器在面對更高的飛行要求和較快的速度下跟蹤精度會(huì)受到影響。文獻(xiàn)［11］中提出了一種自適應(yīng)控制算法來進(jìn)行軌跡跟蹤，將四旋翼飛行器分解為位置、歐拉角和角速率3 個(gè)動(dòng)態(tài)子系統(tǒng)，并結(jié)合欠驅(qū)動(dòng)約束分別設(shè)計(jì)虛擬控制器，簡化控制器的設(shè)計(jì)。雖然文中的自適應(yīng)控制算法能夠抑制擾動(dòng)對四旋翼飛行器穩(wěn)定性的影響，但是還需要設(shè)計(jì)低通濾波器和不確定性補(bǔ)償器，使整體控制變得繁瑣。文獻(xiàn)［12］中采用反步控制方法，通過引入跟蹤誤差的積分項(xiàng)，降低飛行器進(jìn)行軌跡跟蹤時(shí)的穩(wěn)態(tài)誤差。然而并沒有考慮在強(qiáng)風(fēng)等擾動(dòng)條件下對跟蹤精度的影響，系統(tǒng)沒有顯示出很好的魯棒性。文獻(xiàn)［13］中提出了一種基于切換函數(shù)的擴(kuò)張狀態(tài)觀測器設(shè)計(jì)方法，來對系統(tǒng)中的擾動(dòng)進(jìn)行估計(jì)，并將估計(jì)值與滑模控制器的設(shè)計(jì)相結(jié)合，實(shí)現(xiàn)軌跡跟蹤控制。但是在滑?？刂浦袝?huì)產(chǎn)生抖振現(xiàn)象，影響控制精度。

相比其他方法需要設(shè)計(jì)觀測器或者是多次調(diào)整參數(shù)來抑制擾動(dòng)從而達(dá)到控制要求，本文結(jié)合了內(nèi)外環(huán)控制的思想，分別以自適應(yīng)擾動(dòng)為主體和滑模函數(shù)來設(shè)計(jì)控制器。本文中還考慮了四旋翼飛行器負(fù)重時(shí)質(zhì)量改變對系統(tǒng)穩(wěn)定性的影響，證明了滑模自適應(yīng)算法在負(fù)重的情況下穩(wěn)定性不被破壞，擁有較好的魯棒性。在對干擾的抑制和提高控制精度方面以及調(diào)整參數(shù)的過程更加簡易。

1 四旋翼飛行器動(dòng)態(tài)模型

1.1 坐標(biāo)系的建立

本文引入機(jī)體坐標(biāo)系和慣性坐標(biāo)系來描述飛行器的轉(zhuǎn)動(dòng)和位置姿態(tài)，機(jī)體坐標(biāo)系記為A（Ax，Ay，Az），慣性坐標(biāo)系并記為B（Bx，By，Bz），如圖1 所示。機(jī)體坐標(biāo)系A(chǔ) 到慣性坐標(biāo)系B 的轉(zhuǎn)換矩陣RAB。

圖1 四旋翼飛行器坐標(biāo)系建立示意圖

1.2 四旋翼飛行器動(dòng)力學(xué)模型

由于四旋翼飛行器主要運(yùn)動(dòng)形式分為兩種，一種是相對于慣性系B 的平移運(yùn)動(dòng)，另一種是相對于機(jī)體坐標(biāo)系A(chǔ) 的轉(zhuǎn)動(dòng)，一共6 個(gè)自由度。于是可根據(jù)總的自由度數(shù)確定廣義坐標(biāo)為q=（ξ，η）T，其中ξ=［x，y，z］T表示位置信息；η=［γ，θ，ψ］T表示姿態(tài)信息。飛行器的質(zhì)量為m，重力加速g，飛行器旋翼末端到飛行器重心的距離為l。四旋翼飛行器在機(jī)體坐標(biāo)系下的慣性矩陣為：

Ix，Iy，Iz分別代表在A 坐標(biāo)系下X 軸、Y 軸和Z軸的轉(zhuǎn)動(dòng)慣量，可以通過測試得出。

四旋翼飛行器的位置部分表述方程如下所示：

系數(shù)C 是科里奧利力及離心項(xiàng)［15］，具體表示為式（5）［16］。

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

本文的控制策略，總體設(shè)計(jì)方案如圖2 所示。

圖2 四旋翼飛行器控制系統(tǒng)結(jié)構(gòu)圖

2.1 位置跟蹤控制器設(shè)計(jì)

位置控制器的主要設(shè)計(jì)思路是將理想的位置軌跡和實(shí)際的位置軌跡進(jìn)行比較，最后讓兩個(gè)的誤差趨近于零。于是可將期望跟蹤的位移軌跡設(shè)為ξc，實(shí)際跟蹤得到的位移量記為ξ。將誤差設(shè)為es并且es=ξ-ξc。

于是根據(jù)式（2）可以將ξ··單獨(dú)表示出來，然后對誤差表達(dá)式兩邊求兩次導(dǎo)數(shù)，如下形式：

式（6）中，us=u1RABe3是需要進(jìn)行設(shè)計(jì)的控制輸入量，可以將us理解為一種虛擬控制律，作為一種坐標(biāo)變換的中間量來理解。

根據(jù)滑模自適應(yīng)魯棒控制方法，首先進(jìn)行滑模函數(shù)的設(shè)計(jì)?？蓪⒒：瘮?shù)設(shè)為如下形式：

設(shè)計(jì)位置子系統(tǒng)的虛擬控制律us為：

將上面的誤差es求導(dǎo)數(shù)，代入得：

本文不同于其他方法在于針對四旋翼飛行器系統(tǒng)中，由于位置運(yùn)動(dòng)部分主要是要減小干擾力矩對運(yùn)動(dòng)軌跡的影響，故設(shè)計(jì)干擾力矩作為自適應(yīng)律，讓其更好地進(jìn)行位置軌跡跟蹤。同時(shí)為了體現(xiàn)出控制方法的優(yōu)越性，考慮飛行器負(fù)載時(shí)導(dǎo)致質(zhì)量的變化，進(jìn)而影響四旋翼飛行器的升降動(dòng)力和穩(wěn)定性，故將質(zhì)量也設(shè)計(jì)自適應(yīng)律。

將自適應(yīng)律定義為如下形式：

式中，γ1和γ2均為控制系數(shù)。

定義位移部分的干擾力誤差量為：eds=ds-d^s；質(zhì)量誤差量為：em=m-m^。

定義李雅普諾夫函數(shù)并求導(dǎo)得：

當(dāng)m^大于限制范圍的最大值時(shí)，并且處于一直增加的狀態(tài)，此時(shí)m^保持不變即m^·等于0。當(dāng)m^低于限制范圍的最小值時(shí)，并且處于一直遞減的狀態(tài)，此時(shí)m^保持不變，即m^·等于0。映射自適應(yīng)算法不僅能保證m^·等于0，還能保證V˙1≤0。

在虛擬控制律us能夠被計(jì)算出來的情況下，還要將四旋翼飛行器實(shí)際的升降動(dòng)力u1和姿態(tài)子系統(tǒng)的中3 個(gè)姿態(tài)角信號量的總和ηs計(jì)算出來。于是可將虛擬控制輸入量us表示為：［uxuyuz］T，中間量ηs表示為如下形式：［γsθsψs］T。

因?yàn)閡s=u1RABe3，對us進(jìn)行矩陣運(yùn)算表示為如下形式，進(jìn)一步整理可得x，y，z 方向的動(dòng)力輸入表示。

可得俯仰角和橫滾角的表示為：

俯仰角信號量θs和橫滾角信號γs是為了跟蹤參考位置量，由外環(huán)部分虛擬控制輸入量生成，并且傳遞給內(nèi)環(huán)部分的姿態(tài)子系統(tǒng)，外環(huán)部分產(chǎn)生的誤差通過內(nèi)環(huán)部分進(jìn)行消除。偏航角ψs則是作為給定的控制信號，由信號發(fā)生器產(chǎn)生。可以在信號發(fā)生器設(shè)置其輸入值來對任意的偏航角進(jìn)行跟蹤。實(shí)際的位置控制器輸入設(shè)計(jì)為u1=uz/cosθcosγ，并且u1為實(shí)際的控制輸入量。

2.2 姿態(tài)跟蹤控制器設(shè)計(jì)

姿態(tài)控制部分為內(nèi)環(huán)子系統(tǒng)，通過內(nèi)環(huán)控制律實(shí)現(xiàn)姿態(tài)控制，同時(shí)對外環(huán)控制產(chǎn)生的俯仰角信號量θs和橫滾角信號量γs進(jìn)行跟蹤。在姿態(tài)子系統(tǒng)的控制中，主要跟蹤的量為ηs，根據(jù)上述推導(dǎo)的姿態(tài)部分方程表達(dá)式可知，姿態(tài)部分的控制器主要針對控制量輸入轉(zhuǎn)矩進(jìn)行設(shè)計(jì)。同時(shí)考慮到四旋翼飛行器模型中存在不確定性，以及旋轉(zhuǎn)運(yùn)動(dòng)中存在的外界非結(jié)構(gòu)性干擾力矩。于是可將四旋翼飛行器姿態(tài)部分的表達(dá)式寫為如下形式。

其中J=Js+JΔ，C=Cs+CΔ，進(jìn)一步可將x 方向的干擾力d1表示為與干擾力矩相關(guān)的表達(dá)式，即d1=dM-JΔη··-CΔη˙，并認(rèn)為d1是有界量。

所以姿態(tài)子系統(tǒng)的動(dòng)態(tài)模型可以表示為：

與位置控制器設(shè)計(jì)思路相同，首先定義滑模函數(shù)，滑模函數(shù)S2設(shè)為如下形式：

根據(jù)定義的滑模函數(shù)關(guān)系式，可將姿態(tài)誤差子系統(tǒng)轉(zhuǎn)變?yōu)槿缦滦问剑?/p>

于是可將控制量輸入力矩表示為如下形式：

式（34）中，ε 和c2為控制參數(shù)，并且要求ε＞dγ，c2＞0。sgn 為符號函數(shù)。

從得到的指數(shù)趨近律表達(dá)式可以看出，當(dāng)指數(shù)趨近速度按照由較大值逐漸縮減到零的規(guī)律變化時(shí)，此時(shí)趨近時(shí)間變短，并且保證了目標(biāo)點(diǎn)趨近切換平面的速度非常小。由于這是一種逐漸逼近的過程，很難保證運(yùn)動(dòng)點(diǎn)在確定的時(shí)間內(nèi)到達(dá)切換平面［19］，所以在上述基礎(chǔ)上添加一個(gè)等速趨近項(xiàng)-εsgn（S2）。

定義模型的名義值為：

進(jìn)一步進(jìn)行李雅普諾夫函數(shù)的構(gòu)造，具體設(shè)計(jì)為如下形式：

對V2求導(dǎo)函數(shù)可得：

將控制量帶入后，整理化簡可得：

由此可見李雅普諾夫?qū)Ш瘮?shù)V˙2為負(fù)數(shù)，所以李雅普諾夫函數(shù)本身特性為單調(diào)遞減。根據(jù)李雅普諾夫穩(wěn)定性定律可知，可以將姿態(tài)子系統(tǒng)部分認(rèn)為是穩(wěn)定并且誤差是收斂的。

針對整個(gè)系統(tǒng)而言，將整體的李雅普諾夫函數(shù)定義為：

將V1和V2分別代入后可得：

根據(jù)李雅普諾夫穩(wěn)定性定律可知，整個(gè)系統(tǒng)是處于漸進(jìn)穩(wěn)定的狀態(tài)。通過位置控制器得到俯仰信號θs和橫滾信號γs后，從式（34）輸入轉(zhuǎn)矩的表達(dá)式可以看出，還需要將這兩個(gè)信號量分別求一階導(dǎo)數(shù)和二階導(dǎo)數(shù)。此時(shí)是可以通過采用三階微分器的形式實(shí)現(xiàn)對θs和γs一階導(dǎo)數(shù)和二階導(dǎo)數(shù)求取。

3 仿真研究

首先確定參考位置ξs，將參考位置ξs的x 軸、y軸、z 軸的軌跡分別設(shè)計(jì)為：x=cos（t）；y=sin（t）；z=t。姿態(tài)角方面，由于俯仰角和橫滾角作為中間指令信號傳遞給內(nèi)環(huán)控制系統(tǒng)，所以此時(shí)只需要定義偏航角即可。將偏航角的常數(shù)設(shè)定為ψs=π/4。飛行器旋翼末端到飛行器重心的距離為l，取0.5 m。慣性矩陣I 中的參數(shù)設(shè)為：Ix=0.081；Iy=0.081；Iz=0.141。

位移運(yùn)動(dòng)中的干擾力ds取為：

［sin（πt） cos（πt） cos（πt）］T

旋轉(zhuǎn)運(yùn)動(dòng)中的干擾力矩dM取為：

由圖3 可以看出，軌跡跟蹤曲線和理想位置曲線是接近一致的，可以看出提出的控制算法能使系統(tǒng)到達(dá)穩(wěn)定狀態(tài)，誤差在能夠控制的范圍內(nèi)。圖4～圖6 為四旋翼飛行器x、y、z 方向上的誤差，反映位置跟蹤上的誤差。x 方向誤差在-0.03～0.07 之間變化，y 方向誤差變化范圍大致在-0.05～0.06 之間，z方向誤差大致在-0.2～0.37 之間變化，三者均是小幅震蕩的曲線。由此可以看出，滑模自適應(yīng)魯棒控制算法的優(yōu)越性。

圖3 軌跡跟蹤效果圖

圖4 x 方向位置誤差

圖5 y 方向位置誤差

圖6 z 方向位置誤差

圖7～圖9 為四旋翼飛行器橫滾角、俯仰角、偏航角的軌跡跟蹤曲線，可以看出實(shí)際的橫滾角，俯仰角和偏航角和理想的橫滾角，俯仰角和偏航角曲線基本趨于一致。圖10 為四旋翼飛行器升降動(dòng)力即輸入量u1的變化，從圖中可以看出，升力（輸入量）隨著質(zhì)量的變化而變化，并且數(shù)值上接近四旋翼飛行器的重力。圖11 為質(zhì)量自適應(yīng)估計(jì)結(jié)果，質(zhì)量估計(jì)值和質(zhì)量實(shí)際值曲線大致趨于一致，誤差屬于允許的范圍內(nèi)。圖12 為位置運(yùn)動(dòng)部分的擾動(dòng)力設(shè)計(jì)值和估計(jì)值對比圖，可以從圖中清晰地看出，x方向和y 方向的干擾力誤差基本趨近于零，達(dá)到預(yù)期目的。z 方向的誤差在1～1.5 之間，相比設(shè)計(jì)的干擾力有明顯減小，得到抑制。當(dāng)質(zhì)量改變時(shí)，能夠看出所跟蹤的量也隨之發(fā)生變化，體現(xiàn)出了自適應(yīng)的特點(diǎn)。從干擾力的前后變化也可以看出這種控制算法的有效性和魯棒性強(qiáng)的特點(diǎn)。

圖7 橫滾角γ 軌跡跟蹤

圖8 俯仰角θ 軌跡跟蹤

圖9 偏航角ψ 軌跡跟蹤

圖10 四旋翼飛行器輸入量

圖11 質(zhì)量自適應(yīng)估計(jì)

圖12 位置方向干擾力自適應(yīng)估計(jì)

通過以上仿真分析研究可以看出，在考慮四旋翼飛行器動(dòng)態(tài)特性及外部擾動(dòng)和系統(tǒng)不確定性的情況下，滑模自適應(yīng)魯棒控制方法設(shè)計(jì)的控制器能夠準(zhǔn)確穩(wěn)定地跟蹤給定參考軌跡，獲得良好的控制性能，驗(yàn)證了控制算法的有效性。

4 實(shí)驗(yàn)研究

為了更好地驗(yàn)證提出控制算法的優(yōu)越性，本文在實(shí)驗(yàn)部分加入經(jīng)典的PID 控制策略和反步控制方法與本文提出的控制策略進(jìn)行了對比實(shí)驗(yàn)，實(shí)驗(yàn)平臺如下頁圖13 所示，實(shí)驗(yàn)平臺可以實(shí)現(xiàn)四旋翼飛行器的懸停和定點(diǎn)飛行。實(shí)驗(yàn)平臺采用的硬件系統(tǒng)部分為：主控芯片的型號為STM32F427 的PIX 系列飛控；姿態(tài)獲取部件選擇的是MPU6050 傳感器；位置參數(shù)的測量選擇的是型號為M8N 的GPS 模塊和MS5611-01BA 氣壓傳感器。實(shí)驗(yàn)主要以風(fēng)作為干擾的情況下，比較四旋翼飛行器的飛行狀態(tài)，從而驗(yàn)證本文提出控制策略的優(yōu)越性。

圖13 實(shí)驗(yàn)平臺

實(shí)驗(yàn)場地為一般通用的籃球場。實(shí)驗(yàn)所設(shè)計(jì)的軌跡是以籃球場3s 區(qū)的罰球線，矩形的具體尺寸為長5.8 m 寬4.9 m，并且在此區(qū)域上建立坐標(biāo)系，如圖14 所示。四旋翼飛行器進(jìn)行軌跡跟蹤的實(shí)驗(yàn)過程中，首先從原點(diǎn)出發(fā)，先垂直起飛一定的高度，高度經(jīng)自穩(wěn)后測量為1.2 m，如圖15 所示。將四旋翼飛行器按照預(yù)先定義好的軌跡在水平方向飛行，本文的實(shí)驗(yàn)是讓飛行器先后經(jīng)過（0，0）、（5.8，0）、（5.8，4.9）、（0，4.9），最后回到（0，0）點(diǎn)。采用地面站來接收四旋翼飛行器在飛行中的姿態(tài)參數(shù)和理論參數(shù)進(jìn)行對比，得到誤差范圍，以此來證明本文所提出控制算法的優(yōu)越性。本文所提出的控制策略實(shí)驗(yàn)結(jié)果如表1 和表2 所示，采用滑模PID 控制算法的實(shí)驗(yàn)結(jié)果如表3 和表4 所示，反步控制算法結(jié)果如表5 和表6 所示。

圖14 坐標(biāo)系建立示意圖

圖15 實(shí)驗(yàn)過程

通過結(jié)合表1～表6 的實(shí)驗(yàn)數(shù)據(jù)可以看出，滑模自適應(yīng)魯棒控制算法相比滑模PID 控制算法和反步控制算法在穩(wěn)定時(shí)間上更快。雖然作為對照組的兩種算法最終也能跟蹤到四旋翼飛行器的運(yùn)動(dòng)軌跡，但是從跟蹤誤差上看，兩種算法的跟蹤誤差范圍均大于滑模自適應(yīng)魯棒控制算法?；ID 算法雖然在穩(wěn)定時(shí)間和跟蹤精度上能達(dá)到不錯(cuò)的效果，但是在調(diào)節(jié)PID 參數(shù)時(shí)非常繁瑣，參數(shù)選取不合適時(shí)超調(diào)現(xiàn)象嚴(yán)重。由此可以得出結(jié)論，在考慮擾動(dòng)的情況下，通過對比其他兩種控制算法的實(shí)驗(yàn)結(jié)果，滑模自適應(yīng)魯棒控制算法穩(wěn)定性好，精度更高，有明顯的優(yōu)越性。

表1 位置軌跡跟蹤指標(biāo)

表2 姿態(tài)軌跡跟蹤指標(biāo)

表3 PID 算法位置軌跡跟蹤指標(biāo)

表4 PID 算法姿態(tài)軌跡跟蹤指標(biāo)

表5 反步控制算法位置軌跡跟蹤指標(biāo)

表6 反步控制算法姿態(tài)軌跡跟蹤指標(biāo)

5 結(jié)論

研究結(jié)果表明本文采用的滑模自適應(yīng)魯棒控制策略，能很好地實(shí)現(xiàn)對四旋翼飛行器既定軌跡的跟蹤控制并且保證精度。在對照組的實(shí)驗(yàn)研究過程中，體現(xiàn)出了本文研究控制策略的優(yōu)越性。