四旋翼飛行器是一種能夠?qū)崿F(xiàn)垂直起降、懸停、偏航等復(fù)雜動(dòng)作的多旋翼無(wú)人機(jī)，因而在軍事和民用領(lǐng)域的應(yīng)用前景十分樂觀^[1]，但其自身系統(tǒng)的復(fù)雜性以及易受外界空氣動(dòng)力和不確定因素影響的問題一直困擾著科研人員。

    針對(duì)上述問題，科研人員對(duì)四旋翼飛行器系統(tǒng)模型的控制方法和濾波算法進(jìn)行了研究。經(jīng)典PID算法是一種不依賴于系統(tǒng)模型的控制方法，該方法結(jié)構(gòu)簡(jiǎn)單，理論成熟^[2]，但參數(shù)整定比較繁瑣。文獻(xiàn)[3-4]中反步法雖然對(duì)非線性系統(tǒng)有很好的跟蹤效果且反應(yīng)速度快，但是其魯棒性差，抗干擾能力弱，且需要建立精確的系統(tǒng)模型。文獻(xiàn)[5]用滑膜控制法對(duì)四旋翼飛行器進(jìn)行控制，雖然滑膜控制相對(duì)反步法具有較好的魯棒性，但此類算法都沒有對(duì)系統(tǒng)的干擾進(jìn)行實(shí)時(shí)估計(jì)和補(bǔ)償，也沒有對(duì)各個(gè)通道進(jìn)行解耦處理，當(dāng)外界干擾較大時(shí)，其控制效果并不理想?？刂品椒ǖ倪x取雖然可以減小對(duì)四旋翼模型的依賴性，保證內(nèi)部系統(tǒng)的相對(duì)穩(wěn)定，但要想實(shí)現(xiàn)四旋翼飛行器在復(fù)雜的環(huán)境中安全飛行，濾波算法的研究也是不可或缺的。

    文獻(xiàn)[6]利用粒子濾波對(duì)飛行器姿態(tài)進(jìn)行估計(jì)，其在非線性系統(tǒng)中表現(xiàn)出較強(qiáng)的優(yōu)越性，但其計(jì)算量較大，難以滿足四旋翼飛行器實(shí)時(shí)控制的效果。文獻(xiàn)[7]利用互補(bǔ)濾波對(duì)四旋翼飛行器數(shù)據(jù)進(jìn)行融合，該算法根據(jù)陀螺儀和加速度計(jì)傳感器特性的不同，將兩者的優(yōu)點(diǎn)融合起來(lái)得到較好的數(shù)據(jù)，但其和粒子濾波一樣也存在系統(tǒng)延遲問題。文獻(xiàn)[8-9]提出基于卡爾曼濾波的姿態(tài)估計(jì)算法，其能夠有效地濾除外界噪聲，確保獲取數(shù)據(jù)的精度，但當(dāng)系統(tǒng)濾波偏差較大時(shí)會(huì)出現(xiàn)濾波發(fā)散的現(xiàn)象，從而導(dǎo)致系統(tǒng)不能正常工作。

    綜合以上問題，本文提出改進(jìn)EKF的自抗擾控制方法，利用自抗擾控制器對(duì)飛行器姿態(tài)進(jìn)行控制，將受噪聲干擾的姿態(tài)經(jīng)改進(jìn)EKF進(jìn)行濾波，使得飛行姿態(tài)更加穩(wěn)定，有效提高了系統(tǒng)的穩(wěn)定性和抗干擾性。

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

    為得到四旋翼飛行器的數(shù)學(xué)模型，首先建立兩個(gè)坐標(biāo)系：慣性坐標(biāo)系E(OXYZ)和機(jī)體坐標(biāo)系B(OX′Y′Z′)。其重心和質(zhì)心均與飛行器原點(diǎn)O(O′)重合，如圖1所示。

    則四旋翼的動(dòng)力學(xué)模型為^[10]：

    由上式可知，四旋翼飛行器是一個(gè)欠驅(qū)動(dòng)、非線性、強(qiáng)耦合系統(tǒng)。本文采用自抗擾控制器作為控制系統(tǒng)，同時(shí)利用改進(jìn)EKF對(duì)帶有噪聲的飛行器姿態(tài)進(jìn)行修正，其控制系統(tǒng)模型如圖2所示。

2 ADRC系統(tǒng)

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

    自抗擾控制是通過(guò)跟蹤微分器(TD)安排過(guò)渡過(guò)程，利用擴(kuò)張觀測(cè)器(ESO)實(shí)現(xiàn)狀態(tài)間耦合項(xiàng)的跟蹤和估計(jì)，同時(shí)對(duì)系統(tǒng)總擾動(dòng)進(jìn)行估計(jì)，并根據(jù)擾動(dòng)估計(jì)值采用非線性狀態(tài)誤差反饋(NLSEF)進(jìn)行實(shí)時(shí)補(bǔ)償。其二階的結(jié)構(gòu)如圖3所示^[11]。

2.2 姿態(tài)控制系統(tǒng)的自抗擾解耦控制 

    四旋翼姿態(tài)子系統(tǒng)方程如下:   

    式中，b₁、b₂為控制器增益。

    自抗擾控制器品質(zhì)的好壞和其內(nèi)部各模塊參數(shù)的整定有關(guān)，因此，對(duì)自抗擾控制器各模塊參數(shù)的整定是必要的。

2.3 自抗擾解耦控制參數(shù)的整定

    因?yàn)锳DRC中各模塊互不影響，因此，可以對(duì)各模塊的參數(shù)單獨(dú)整定，最后進(jìn)行總體調(diào)整。

2.3.1 TD參數(shù)整定

    跟蹤微分器中的速度因子r影響著跟蹤信號(hào)的速度，其影響效果如圖4所示。其中T=0.01，x=5是初始信號(hào)，y是跟蹤波形，分別對(duì)應(yīng)著速度因子r=1、5、14的曲線。

    由圖4可知，速度因子r越大，跟蹤速度越快，容易造成超調(diào)現(xiàn)象，失去了微分跟蹤器的意義；r越小跟蹤過(guò)程越長(zhǎng)，系統(tǒng)的實(shí)時(shí)性變差。 

2.3.2 ESO參數(shù)整定

    ESO是自抗擾控制器最重要的部分，一般選取a₁=0.5，a₂=0.25。σ是fal函數(shù)原點(diǎn)附近線性區(qū)間的寬度，若σ過(guò)大則可能無(wú)法準(zhǔn)確跟蹤一些非線性信號(hào)，大大降低了ADRC的控制效果；σ太小則會(huì)在原點(diǎn)位附近出現(xiàn)信號(hào)跳變現(xiàn)象，降低了系統(tǒng)的穩(wěn)定性。而β₁、β₂、β₃僅影響ESO的收斂速度，不因控制對(duì)象的改變而改變[12]。

2.3.3 NLSEF參數(shù)整定

    NLSEF中控制量增益b₁、b₂對(duì)控制效果的影響較大。控制量增益大會(huì)導(dǎo)致系統(tǒng)發(fā)散，影響系統(tǒng)的安全性；控制增益越小，則系統(tǒng)的實(shí)時(shí)性較差，故應(yīng)根據(jù)不同的控制系統(tǒng)動(dòng)態(tài)調(diào)整。

3 改進(jìn)EKF濾波原理及算法

    本文提出一種改進(jìn)的EKF算法,首先,計(jì)算觀測(cè)矩陣時(shí)進(jìn)行迭代處理以提高濾波精度。然后，采用兩個(gè)迭代的EKF對(duì)四旋翼飛行器的姿態(tài)數(shù)據(jù)進(jìn)行預(yù)測(cè)和更新，將它們輸出的狀態(tài)變量加權(quán)整合后輸出飛行器的最優(yōu)狀態(tài)估計(jì)，其原理圖如圖5所示。

    對(duì)于一個(gè)離散隨機(jī)系統(tǒng)：其中W_k、V_k分別為系統(tǒng)k時(shí)刻的過(guò)程噪聲和觀測(cè)噪聲且均為均值為零的高斯白噪聲，對(duì)應(yīng)的方差分別為Q_k、W_k。改進(jìn)EKF算法過(guò)程如下：

4 仿真與實(shí)驗(yàn)結(jié)果分析

    本文在前期的工作中已完成了雙環(huán)PID、改進(jìn)EKF的自抗擾飛控系統(tǒng)的仿真和實(shí)驗(yàn)，飛行器在改進(jìn)EKF的自抗擾控制器控制下的實(shí)際飛行效果如圖6所示。

4.1 改進(jìn)EKF的自抗擾穩(wěn)定性控制實(shí)驗(yàn)

    為了驗(yàn)證該方法對(duì)四旋翼飛行器穩(wěn)定性控制的效果，給定飛行器橫滾角的期望值為θ=20°，其仿真結(jié)果如圖7所示。

    由圖7可知，俯仰角可以在0.9 s左右達(dá)到期望值，且狀態(tài)調(diào)整過(guò)程中沒有出現(xiàn)任何振蕩，可以看出該控制方式對(duì)飛行器穩(wěn)定性控制效果十分理想。

4.2 改進(jìn)EKF的自抗擾和串級(jí)PID對(duì)比實(shí)驗(yàn)

    給定橫滾角的初始值均為0°，且信號(hào)是周期性變化的方波，其中橫滾通道方波增益為φ=15°。在改進(jìn)EKF的自抗擾和串級(jí)PID控制下的系統(tǒng)輸出曲線如圖8所示。

    由圖8可知，兩種控制方式對(duì)橫滾角都有很好的跟蹤效果，但串級(jí)PID控制器的姿態(tài)角響應(yīng)曲線會(huì)產(chǎn)生震蕩現(xiàn)象，而改進(jìn)的EKF的自抗擾控制則無(wú)此現(xiàn)象。 

4.3 高度控制對(duì)比實(shí)驗(yàn)

    給定四旋翼飛行器初始高度為0 m，期望高度為2 m，其在改進(jìn)EKF的自抗擾和串級(jí)PID控制下的系統(tǒng)輸出如圖9所示。

    由圖9可知，在期望值2 m條件下，串級(jí)PID控制的超調(diào)量在50%以上，穩(wěn)定時(shí)間在0.9 s左右，而改進(jìn)EKF的自抗擾控制曲線超調(diào)量在5%以下，穩(wěn)定時(shí)間大約在0.5 s左右。 

4.4 改進(jìn)EKF的ADRC、ADRC、串級(jí)PID抗干擾性對(duì)比實(shí)驗(yàn)

    四旋翼實(shí)際飛行時(shí)，系統(tǒng)會(huì)受到傳感器噪聲干擾和外部突變的影響，為了模擬傳感器噪聲干擾，實(shí)驗(yàn)對(duì)橫滾角的反饋?zhàn)兞考尤肓烁咚拱自肼暎粸榱四M外部突變的影響,在200 ms時(shí)刻對(duì)橫滾角回路的角加速度加入幅值是20、脈寬為100 ms的矩形波作為突變信號(hào)來(lái)檢測(cè)控制器的抗干擾性能,仿真結(jié)果如圖10所示。

    由圖10可知，同等條件下，改進(jìn)EKF的自抗擾控制器抗干擾效果更加的突出。

4.5 飛行器實(shí)際飛行橫滾角、高度曲線

    實(shí)際飛行時(shí)，給定橫滾角幅值為12左右的突變信號(hào)，給定高度的幅值大約是8。利用數(shù)傳把數(shù)據(jù)傳到電腦上，其在上位機(jī)上的曲線分別如圖11所示。

    由圖11可知，在改進(jìn)EKF的自抗擾控制器下的四旋翼飛行器，其可以快速實(shí)現(xiàn)姿態(tài)角的恢復(fù)及高度的控制，實(shí)現(xiàn)飛行器穩(wěn)定的飛行。

5 結(jié)論

    實(shí)驗(yàn)結(jié)果表明，該方法控制下，飛行器可以從50°橫滾角的情況下迅速恢復(fù)到平衡，且高度控制、抗干擾能力較之串級(jí)PID控制反應(yīng)更加迅速，平穩(wěn)。在接下來(lái)的工作中，會(huì)對(duì)四旋翼飛行器的避障和路徑規(guī)劃進(jìn)行研究，利用超聲波測(cè)量四旋翼與障礙物之間的距離，采用單目視覺對(duì)飛行器進(jìn)行定位，最終實(shí)現(xiàn)四旋翼飛行器自主飛行。

參考文獻(xiàn) 

[1] 方勇純，申輝，孫秀云，等.無(wú)人直升機(jī)航向自抗擾控制[J].控制理論與應(yīng)用，2014，31(2)：238-243.

[2] 劉一莎，楊晟萱，王偉.四旋翼飛行器的自抗擾飛行控制方法[J].控制理論與應(yīng)用，2015，32(10)：1351-1360.

[3] 田聰玲.基于反步法的四旋翼飛行器非線性控制[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2014.

[4] MADANI T，BENALLEGUE A.Backstepping control for a quadrotor helicopter[C].Intelligent Robots and Systems，2006 IEEE/RSJ International Conference on.2006.

[5] 王璐，李光春，王兆龍，等.欠驅(qū)動(dòng)四旋翼無(wú)人飛行器的滑膜控制[J].哈爾濱工程大學(xué)學(xué)報(bào)，2012，33(10)：1248-1253.

[6] 曲仕茹，馬志強(qiáng).改進(jìn)的粒子濾波在四旋翼姿態(tài)估計(jì)中的應(yīng)用[J].飛行力學(xué)，2013，31(5)：458-461，466.

[7] 萬(wàn)曉鳳，康利平，余運(yùn)俊，等.互補(bǔ)濾波算法在四旋翼飛行器姿態(tài)解算中的應(yīng)用[J].測(cè)控技術(shù)，2015，34(2)：8-11.

[8] 汪紹華，楊瑩.基于卡爾曼濾波的四旋翼飛行器姿態(tài)估計(jì)和控制算法研究[J].控制理論與應(yīng)用，2013，30(9)：1109-1115.

[9] 聶鵬，李佩華，李正強(qiáng)，等.基于卡爾曼濾波的小型無(wú)人機(jī)姿態(tài)估計(jì)算法研究[J].沈陽(yáng)航空航天大學(xué)學(xué)報(bào)，2013，30(6)：53-57.

[10] 張廣昱，袁昌盛.基于自抗擾理論的小型四旋翼飛行器姿態(tài)控制[J].航空工程進(jìn)展，2014，5(3)：338-342.

[11] 韓京清.自抗擾控制器及其應(yīng)用[J].控制與決策，1998(1)：19-23.

[12] 楊立本，章衛(wèi)國(guó)，黃得剛.基于ADRC姿態(tài)解耦的四旋翼飛行器魯棒軌跡跟蹤[J].北京航空航天大學(xué)學(xué)報(bào)，2015，41(6)：1026-1033.

作者信息:

姜海濤，常  青，王耀力 

(太原理工大學(xué) 信息與計(jì)算機(jī)學(xué)院，山西 太原030024)