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

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

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

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

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

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

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

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

2 ADRC系統(tǒng)

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

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

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

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

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

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

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

    因為ADRC中各模塊互不影響，因此，可以對各模塊的參數(shù)單獨整定，最后進行總體調(diào)整。

2.3.1 TD參數(shù)整定

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

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

2.3.2 ESO參數(shù)整定

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

2.3.3 NLSEF參數(shù)整定

    NLSEF中控制量增益b₁、b₂對控制效果的影響較大?？刂屏吭鲆娲髸?dǎo)致系統(tǒng)發(fā)散，影響系統(tǒng)的安全性；控制增益越小，則系統(tǒng)的實時性較差，故應(yīng)根據(jù)不同的控制系統(tǒng)動態(tài)調(diào)整。

3 改進EKF濾波原理及算法

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

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

4 仿真與實驗結(jié)果分析

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

4.1 改進EKF的自抗擾穩(wěn)定性控制實驗

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

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

4.2 改進EKF的自抗擾和串級PID對比實驗

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

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

4.3 高度控制對比實驗

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

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

4.4 改進EKF的ADRC、ADRC、串級PID抗干擾性對比實驗

    四旋翼實際飛行時，系統(tǒng)會受到傳感器噪聲干擾和外部突變的影響，為了模擬傳感器噪聲干擾，實驗對橫滾角的反饋變量加入了高斯白噪聲；為了模擬外部突變的影響,在200 ms時刻對橫滾角回路的角加速度加入幅值是20、脈寬為100 ms的矩形波作為突變信號來檢測控制器的抗干擾性能,仿真結(jié)果如圖10所示。

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

4.5 飛行器實際飛行橫滾角、高度曲線

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

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

5 結(jié)論

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

參考文獻 

[1] 方勇純，申輝，孫秀云，等.無人直升機航向自抗擾控制[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ū)動四旋翼無人飛行器的滑膜控制[J].哈爾濱工程大學(xué)學(xué)報，2012，33(10)：1248-1253.

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

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

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

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

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

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

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

作者信息:

姜海濤，常  青，王耀力 

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