基于改進樽海鞘群算法的四旋翼飛行器姿態優化控制

丁 力 高振奇 虞 青

(江蘇理工學院機械工程學院， 常州 213001)

0 引言

四旋翼飛行器在民用領域有著廣泛的用途，如農田噴霧、城市建模、地理測繪等。但因其強耦合、高非線性、欠驅動等特點也給高精度魯棒控制器的設計帶來了巨大的難度[1]。目前，已有很多先進的控制策略用來設計四旋翼的控制器，如自抗擾控制[2]、自適應控制[3]、模型預測控制[4]、反步法控制[5]等。然而，上述方法是需要精確的模型或者依賴大量的控制器參數，這極大地限制了其在實際工程中的應用。另外，由外界擾動、未建模動態、參數漂移等組成的集總干擾也會進一步加大上述控制器設計的難度。

在控制器設計過程中引入擾動觀測器是解決上述問題一種有效途徑。擾動觀測器可估計集總干擾，提高系統的穩定性與控制性能。例如，文獻[6]將反步法與非線性擾動觀測器相結合，有效減弱了集總干擾對四旋翼控制性能的影響；為改善四旋翼容錯控制效果，文獻[7]采用高階非線性擾動觀測器去捕捉不確定項的動態特性。通常，非線性擾動觀測器存在待整定參數多、控制耗能大、易觸發執行器飽和等缺點。文獻[8]提出的線性自抗擾控制(Linear active disturbance rejection control, LADRC)結構中的線性擴張狀態觀測器(Linear extended state observer, LESO)具有結構簡單、耗能小的特點，可有效彌補非線性擾動觀測器的不足。但LADRC中的比例微分(Proportion differentiation, PD)控制結構過分依賴跟蹤誤差信號、對閉環系統的動態變化極其敏感。鑒于上述分析，可嘗試用另一種控制結構來替代PD，增強控制策略的控制性能。

滑模控制是一種能有效處理不確定問題的魯棒控制技術，但它在處理四旋翼飛行器控制時也會帶來一些問題，如大超調、長起調時間與強抖振等。為解決這些問題，文獻[9]采用積分滑模控制器來抑制四旋翼控制中的抖振問題；文獻[10]利用自適應策略來改進滑模控制，設計了四旋翼的軌跡跟蹤控制器，并在仿真環境下獲得了很好的抗干擾控制效果。近年來研究發現快速連續非奇異終端滑模控制(Fast-continuous nonsingular terminal sliding mode control, FNTSMC)既能有效改善傳統滑模控制的不足，又能保證系統狀態有限時間收斂。文獻[11-13]分析了FNTSMC及其改進策略在工程控制中的能力。目前，同時考慮LESO與FNTSMC優勢的四旋翼飛行器姿態控制的文獻鮮有報道。

另外，參數整定是獲得控制器最佳性能的一種有效方法，通常包括經驗法與智能法兩種形式。前者是通過人為試湊的方式來調整控制器參數，準確性低、工作量大、成功率低；后者是采用人工智能算法優化控制器參數，準確性高、通用性強、成功率高。因此，在學術研究中通常會選擇后者。在人工智能算法中，樽海鞘群算法(Salp swarm algorithm, SSA)是由MIRJALILI團隊于2017年提出的一種模擬樽海鞘群的協同運動與覓食行為的元啟發式人工智能算法[14]。與傳統的遺傳算法、粒子群算法、螢火蟲算法相比，SSA算法具有魯棒性高、收斂速度快、參數設置少、結構簡單等特點。但在優化具有多個局部極值的目標函數時，普通SSA算法也存在易陷入局部最優和進化停滯的缺陷。因此，研究一種改進的SSA算法來整定控制器參數意義重大。

本文結合LESO與FNTSMC的優勢，提出一種基于線性擴張狀態觀測器的快速連續非奇異終端滑模控制策略(Fast-continuous nonsingular terminal sliding mode control based on linear extended state observer, FNTSMC-LESO)設計四旋翼的姿態控制器；引入樽海鞘群算法來解決控制器的參數整定問題，并引入一維正態云模型和自適應算子加強算法優化能力；通過仿真與試驗對本文方法的有效性、實用性進行驗證。

1 動力學建模

本文研究的對象為X450型四旋翼飛行器，如圖1所示。描述其飛行動力學行為需要兩套坐標系，即地球坐標系OEXEYEZE和機體坐標系OBXBYBZB。該飛行系統具有4個旋翼，每個旋翼產生運動所需的升力fmi與力矩Ti(i=1,2,3,4)。利用牛頓-歐拉方程可推導出四旋翼的姿態動力學模型[15]

(1)

其中

Θ=(φ,θ,ψ)T
Ω=(p,q,r)T
τ=(τ1,τ2,τ3)T
d=(d1,d2,d3)T
I=diag(Ixx,Iyy,Izz)

(2)

式中Θ——地球坐標系中描述的歐拉角

φ——橫滾角θ——俯仰角

ψ——偏航角

Ω——機體坐標系中描述的角速度

τ——輸入力矩

d——外界擾動

I——轉動慣量，取Ixx=Iyy=0.094 kg·m2，Izz=0.086 kg·m2

T——地球坐標系與機體坐標系之間的轉換矩陣

圖1 四旋翼結構示意圖Fig.1 Configuration of quadrotor

四旋翼的總升力τf與其他3個控制輸入τ可通過4個槳葉的電機轉速計算獲得，即

(3)

其中

kf=ρCTAR2kτ=CMρAR2/kf

式中CT——升力系數

CM——反扭矩系數

ρ——空氣密度，取1.29 kg/m3

A——旋翼槳盤面積，取0.08 m2

R——旋翼半徑，取0.16 m

l——旋翼中心到四旋翼機體質心的距離，取0.2 m

ω1、ω2、ω3、ω4——電機轉速

通過參數辨識試驗[16]可擬合出kf與kτ的近似值為：kf≈2.5×10-5，kτ≈1.2×10-6。

2 姿態控制器設計

2.1 線性擴張狀態觀測器的設計

根據式(1)，姿態角每個通道的輸出量與輸入力矩項均呈現二階導數關系，故可以將姿態動力學模型看成是3個二階子系統來設計控制器。不失一般性，這里以一類常見的二階系統來演繹FNTSMC-LESO控制器的設計過程。選用的二階系統形式為

(4)

式中x1、x2——狀態變量

u——控制輸入y——系統輸出

b——控制器增益

f(x1,x2,d(t),t)——包含了建模不確定性與外界擾動d(t)的集總干擾

將式(4)改寫成狀態空間方程形式

(5)

x=(x1,x2)Tu=u

y=yf=f

式中x——狀態變量u——控制輸入

y——系統輸出f——集總干擾

A、B、C、E——系統矩陣

假設集總干擾是有界、可微分的，則可用一個擴張狀態量來增廣式(5)的形式，即

(6)

其中

式中x3——擴張狀態量

設計式(6)的線性擴張狀態觀測器為

(7)

(8)

L——觀測矩陣，可通過極點配置的方式計算[8]

ωo——觀測器帶寬，其值大于0

ζ1、ζ2、ζ3——特征根

特征根滿足

(9)

2.2 快速非奇異終端滑模控制律的設計

定義連續快速非奇異終端滑模面函數為

(10)

其中

式中e——跟蹤誤差

xr——參考信號

β、γ——控制器參數

這里用tanh連續函數替代滑模控制中常見的sign分段函數可有效削弱控制輸入中的抖振現象。

將式(10)的一階導數代入式(4)，可得到系統的等效控制輸入為

(11)

進一步地，選取快速終端滑模趨近律來加快控制律的收斂速度，使得系統狀態能在有限時間內收斂

(12)

式中k1、k2——控制器參數

進而，基于趨近律的控制輸入ur為

ur=b-1(k1s+k2sig(s)p)

(13)

因此，FNTSMC-LESO控制器的控制輸入可設計為

u=ue+ur

(14)

本文所設計的FNTSMC-LESO控制器結構如圖2所示。在四旋翼姿態控制中，需要為3個姿態通道分別設計1個此類控制器。

圖2 FNTSMC-LESO控制結構圖Fig.2 Structure of FNTSMC-LESO controller

3 穩定性分析

定理：對于式(4)描述的一類常見的二階系統，若集總干擾是有界且可導的，在本文所設計FNTSMC-LESO控制律作用下，可保證跟蹤誤差及其一階導數在有限時間內收斂到某個區域[17]，即

|e|≤2Δ=2min(Δ1,Δ2)

(15)

(16)

其中

Δ1=|3-f|/k1Δ2=(|3-f|/k2)1/p

證明：考慮一個Lyapunov函數為

(17)

它關于時間的一階導數為

(18)

聯立式(10)～(14)，可以推導出連續快速非奇異終端滑模面的一階導數為

(19)

則有

(20)

進而，式(18)也可被描述成其他兩種形式

(21)

(22)

引理：如果Lyapunov函數V滿足不等式

(23)

在此條件下的穩定時間為

(24)

式中V0——V的初始值

結合定理與引理，在假設成立的情況下，滑模面式(10)是有限時間穩定的，且可在有限時間內收斂到某個區域

(25)

(26)

整理式(25)、(26)，得

‖s‖≤Δ=min(Δ1,Δ2)

(27)

進一步地，為證明跟蹤誤差及其一階導數的收斂性，將滑模面式(10)改寫成

(28)

當‖s‖≤Δ時，跟蹤誤差的一階導數可在有限時間內收斂，即

(29)

結合式(28)、(29)，得

(30)

跟蹤誤差在有限時間內收斂。

證明完畢。

4 參數整定

4.1 普通的SSA算法

在本文設計的FNTSMC-LESO中，將對控制器性能影響較小的參數作為先決條件，即設置γ=1.5和p=0.5。另外，每個FNTSMC-LESO中有4個參數需要整定，即：ωo、β、k1和k2，整個姿態控制器有12個參數待整定。在SSA算法中，將樽海鞘個體分為引領者與跟隨者兩種。引領者帶領跟隨者組成樽海鞘鏈游向最優的食物源，即最優或次優的控制器參數。算法具體步驟如下：

(2)計算適應度值。定義樽海鞘鏈追逐的食物源所獲得適應度值為Fj(j=1,2,…,D)，在本優化問題中，Fj用誤差平方乘以時間(Integral of time multiplied by the squared error，ITSE)這一性能指標來描述

(31)

式中e(t)——時域內的跟蹤誤差

選擇合適的有限時間T可使目標函數，也就是適應度值趨于一個穩定值。通過式(31)可獲得初始的全局最優適應度值。

(3)更新引領者位置。引領者根據式(32)、(33)來獲得當前位置。

(32)

c3=2e-(4Q/Qmax)2

(33)

buj、blj——個體在j維位置上的上、下限

c1、c2——0～1之間的隨機數

c3——搜索平衡因子，用來權衡局部搜索和全局搜索的能力

Q——當前迭代次數

(4)更新跟隨者位置。跟隨者根據牛頓運動方程來更新位置

(34)

(35)

這樣，樽海鞘鏈的覓食機制則可通過式(32)、(35)模擬。比較當前迭代中的適應度值與全局最優適應度值，將較好的個體保留到下一代中。

(5)記錄下當前最優解。然后返回至步驟(3)循環迭代，直到Q=Qmax。

在普通SSA算法尋優過程中，全局搜索與局部搜索是個相互矛盾的過程，若此過程失衡，則算法極易陷入局部最優而導致收斂停滯。因此，本文引入一維正態云模型與自適應算子來改進引領者與跟隨者的位置更新策略，得到一種改進的樽海鞘群算法(ISSA)。

4.2 改進算子

(36)

式中Ex——期望，表示定性概念模糊度的度量

當迭代次數達到一定數量后，搜索到的適應度值會越接近最優值。為提高求解精度，控制搜索范圍，采用非線性遞減控制策略來調整xj的值

(37)

圖3 500個鄰域值的一維正態云模型Fig.3 1-dimension positive cloud model with 500 neighborhood values

在跟隨者位置更新過程中采用自適應算子[19]來幫助SSA算法跳出局部最優的束縛，使樽海鞘個體在前期有較強的全局收斂能力，在后期能夠得到相對準確的結果。自適應算子具體形式為

(38)

(39)

式中wmax、wmin——權重因子w的上、下限

rand()——0～1之間的隨機數函數

若w的值過大，則算法搜索效率低，局部挖掘能力不足，不易得到最優解；反之，若w的值過小，算法的收斂精度較高，但全局搜索能力較弱，算法易陷入局部最優。而式(39)可有效解決上述問題。

5 結果分析

5.1 仿真1

采用一個階躍信號來測試ISSA算法對FNTSMC-LESO控制器的參數整定能力。參考信號為Θr=(1.047,-1.047,1.571)Trad，歐拉角及其角速度的初始值均為0，系統中的集總干擾用隨機噪聲來模擬，仿真時間為10 s。為了更好地說明ISSA算法的性能，引入文獻[20]提出的ABC算法及普通的SSA算法與ISSA算法做對比分析。3種算法的參數設置如表1所示。各算法重復仿真5次，然后選取其中最好的結果，如圖4所示。從圖4中可以看出，ISSA算法的收斂速度要快于其他兩種算法，其獲得最優解(0.002 4)的精度分別比SSA算法(0.017 3)和ABC算法(0.020 3)高出86.13%和88.18%。另外也可看出，SSA算法及其改進算法比ABC算法具有更強的數據挖掘能力。圖5給出了基于ISSA算法的FNTSMC-LESO控制器參數整定過程。

表1 3種算法的參數Tab.1 Parameters of three algorithms

圖4 3種算法的迭代曲線Fig.4 Iteration curves of three algorithms

圖5 ISSA算法整定后的參數Fig.5 Control parameters optimized by ISSA

圖6給出了在FNTSMC-LESO控制下四旋翼飛行器姿態角的響應結果。在ISSA算法的優化下，本文所提控制器能獲得令人滿意的控制性能。仿真1的另一個作用是為其他仿真或試驗提供一組合適的控制器參數。

圖6 仿真1中姿態角的響應Fig.6 Response of attitude angles in simulation case 1

5.2 仿真2

本文設計FNTSMC-LESO控制器的想法來源于LADRC控制結構。不同點是FNTSMC-LESO引入FNTSMC來改進LADRC中PD控制律沒有充分利用誤差信號的缺陷。本算例以橫滾角為例，讓其去跟蹤一個給定參考信號，以此來比較本文所提控制算法與LADRC的性能優劣。LADRC的控制器參數也通過ISSA算法整定。仿真結果如圖7所示，可以看出FNTSMC-LESO在跟蹤精度、響應速度及抗干擾能力上均優于LADRC。

圖7 仿真2中橫滾角的響應Fig.7 Response of roll angle in simulation case 2

5.3 試驗1

圖8 X450型四旋翼飛行器試驗平臺Fig.8 X450 quadrotor experimental platform

為驗證本文所提算法的實用性，搭建如圖8所示的四旋翼飛行器試驗平臺。該平臺由地面站、Futaba遙控器、APM開源飛控、一對數傳、X450型四旋翼飛行器、變姿態機構等組成。姿態角信號通過飛控中集成的慣性測量單元(Inertial measurement unit, IMU)獲取，并由數傳實時傳送至地面站個人計算機上，采樣頻率為50 Hz。另外，需要說明的是GPS模塊在本試驗中不起作用，但為了方便今后研究位置跟蹤器的設計，故沒有拆除。

試驗前，將仿真獲得的FNTSMC-LESO控制器參數通過APM2/Simulink的協議[21]燒錄到APM飛控中。另外X450型四旋翼飛行器的安裝角為-70°，以防止飛行器在不受控時因運動慣性而撞擊變姿態機構的立柱。試驗中，X450型四旋翼飛行器先由飛控手通過Futaba遙控器操控至懸停狀態，然后切換到自由飛行模式去跟蹤3種參考軌跡。試驗中的外界擾動由一臺可調速的電風扇提供。截取試驗過程中的10 s數據用來分析，如圖9所示。隨著風速的增加，FNTSMC-LESO控制器的抗干擾能力也逐漸變弱，跟蹤誤差也隨之變大。導致這種結果的原因可能有：控制器的參數是由仿真獲得的，而非“量身定做”，故存在一定的偏差；試驗中集總干擾的邊界值是人為設定的值，無法做到像仿真那樣可以對實際擾動值進行精確估計；試驗中的機械振動、測量誤差等因素會進一步放大跟蹤誤差。但總體來講，本文所設計的控制器還是能夠幫助X450型四旋翼飛行器有效跟蹤上參考軌跡，這表明該控制策略有一定的實用性。

圖9 試驗1中姿態角的響應Fig.9 Response of attitude angles in experiment 1

5.4 試驗2

圖10 試驗2中橫滾角的響應Fig.10 Response of roll angle in experiment 2

進一步地，將LADRC也燒錄到APM開源飛控中，用來與本文方法比較在實際中應用的優劣。這里需要指出的是FNTSMC-LESO控制過程會涉及信號的一階導數與二階導數，若參考信號是非光滑曲線，則需引入微分器來求導，而微分器是物理不可實現的。因此，本試驗以橫滾角為例去跟蹤一個給定正弦信號(風速設定為2.75 m/s)，結果如圖10和圖11所示。圖10顯示了兩種控制器均能有效抑制集總干擾影響，而本文方法的跟蹤精度要明顯優于LADRC。圖11表明與LADRC相比，本文方法可獲得幅值更小的控制律，有效減弱了抖振，減小了控制信號對電機的沖擊。

圖11 兩種控制器生成的輸入信號Fig.11 Control inputs calculated by two controllers

隨后，用最大誤差(Maximum error, ME)與均方根誤差(Root mean square error, RMSE)來評價兩種控制器的跟蹤精度。FNTSMC-LESO的ME值(0.018 5)比LADRC的ME值(0.087 6)小78.88%；同樣地，前者的RMSE值(0.005 7)比后者的RMSE值(0.039 1)小85.42%。進一步說明了本文方法具有更高的控制精度。

6 結束語

提出了一種基于線性擴張狀態觀測器的快速連續非奇異終端滑模控制策略來解決集總干擾下四旋翼的姿態魯棒控制問題。該控制器利用LESO來減弱集總干擾的影響，引入FNTSMC來加快控制律的收斂速度，使系統的跟蹤誤差能在有限時間內收斂到特定區域。與LADRC相比，本文方法不管在仿真或試驗中都具有較高的控制精度、較強的抗干擾能力及較快的響應速度，也能有效削弱控制抖振，減小對電機的控制沖擊。另外，也提出基于改進樽海鞘群算法優化的控制器參數整定方法。該算法用一維正態云模型和自適應算子來增強其數據挖掘能力，避免陷入局部最優。仿真結果表明，ISSA算法能夠有效解決四旋翼姿態控制器的參數整定問題。