基于改進PSO的無人自轉旋翼機二自由度PID飛行控制

蘇中濱 張磊磊 馬 錚 高 睿

(東北農業(yè)大學電氣與信息學院， 哈爾濱 150030)

0 引言

隨著智慧農業(yè)以及無人化技術的不斷發(fā)展，田間作業(yè)中對于具有作業(yè)時間長、載荷量大、成本低等特點的農用航空器的需求正在不斷提高。自轉旋翼機是一種依靠來流驅動旋翼自轉而產生拉力，用螺旋槳推力克服飛行阻力的旋翼類航空器[1]，在民用航空及軍用航空領域已得到了廣泛應用[2-3]。近年來，自轉旋翼機因其操作簡單、安全性高、起飛距離短等特點，在農業(yè)航空領域逐漸受到關注[4]。

王俊超等[5-6]和朱清華[7]對旋翼機空氣動力學、建模技術、飛行動力學以及技術難點等方面進行研究，為無人自轉旋翼機的自動控制奠定了基礎。陳淼[8]在自轉旋翼機的無人飛行控制研究中采用線性魯棒控制方法，解決了縱向控制問題。孫瑜[9]設計了一種BY-200型單旋翼雙矢量自轉旋翼機，通過矢量推力協(xié)助自轉旋翼機旋翼實現了直接力、氣動力混合控制方案。程曉倩[10]基于氣動參數辨識技術和已有飛行數據，分析在實際飛行中的氣動參數完成了自動飛行，但其控制方法和飛行環(huán)境都是在預設條件下，沒有針對環(huán)境影響做出相應的控制決策。林清等[11]針對常見的俯仰操作問題提出了一種升降舵輔助操縱的自轉旋翼機布局，基于神經網絡動態(tài)逆的自轉旋翼機姿態(tài)控制方法設計了動態(tài)控制分配器，能協(xié)調升降舵與縱向周期變距，抑制縱向揮舞角的高頻變化，提高了姿態(tài)控制穩(wěn)定性[12]。而農業(yè)自轉旋翼機在田間作業(yè)時，通常處于低空、低速、環(huán)境因素復雜的情況下，因此對自動飛行的安全性和穩(wěn)定性也有著更高的要求。

經典的PID控制在時變系統(tǒng)模型中控制效果較差，外界干擾大時機身會出現抖動、側邊距大、俯仰角和橫滾角誤差變大等問題，先前的研究中多采用一自由度PID控制和優(yōu)化算法相結合來解決[13-14]。但是由于旋翼機的環(huán)境復雜性和旋翼特性，控制算法不能在降噪和快速響應兩者間同時兼顧[15-18]，對此，本文在一自由度基礎上選擇二自由度PID控制器，研究基于模擬退火算法優(yōu)化粒子群算法的融合算法，以期使得控制系統(tǒng)具有良好的目標值跟蹤和抑制外擾動能力。

1 旋翼機機體建模和姿態(tài)模型

自轉旋翼機的主要結構包括無動力旋翼、全動垂尾和推力系統(tǒng)，如圖1所示。無動力旋翼是在機身向前運動時依靠迎面而來的氣流帶動旋翼旋轉而獲得升力。在設計姿態(tài)控制時主要是控制旋翼的俯仰角和橫滾角，從而實現旋翼機的控制[19]，如圖2所示。但是由于不能直接控制旋翼的轉速，所以旋翼機響應滯后。在操控旋翼過程中，自轉旋翼機可能會受到環(huán)境因素干擾，如外界風力、雨雪等，特別在農業(yè)航空作業(yè)中，超低空低速狀態(tài)下，飛行可操作性更低，要求控制響應速度更快，更精準，因此在控制過程中必須對一些外界影響進行抑制。

圖1 自轉旋翼機原型Fig.1 Prototype of gyrocopter

圖2 自轉旋翼機結構圖和受力分析圖Fig.2 Structure and force analysis diagram of gyrocopter

根據牛頓動力學和歐拉-拉格朗日方程，自轉旋翼機的控制系統(tǒng)數學模型描述為

(1)

式中θ(t)、φ(t)——t時刻俯仰角和橫滾角

ωθ(t)、ωφ(t)——t時刻俯仰和橫滾角速度

Tx(t)——旋翼垂直分力

Ty(t)——旋翼橫滾分力

Ix——俯仰角方向慣性系數

Iy——橫滾角方向慣性系數

Dx——俯仰角方向阻尼系數

Dy——橫滾角方向阻尼系數

λxx——旋翼垂直分力在縱向上的力矩系數

λxy——旋翼垂直分力在橫向上的力矩系數

λyx——旋翼橫向分力在縱向上的力矩系數

λyy——旋翼橫向分力在橫向上的力矩系數

m——自轉旋翼機質量

l——軸心至質心的距離

d1(t)、d2(t)——縱向和橫向方向外部干擾

q1(·)、q2(·)——不確定因素影響函數

由于式(1)中的參數不利于精確獲得而且存在非線性未建模和多環(huán)境因素影響的干擾，基于模型的控制方法不易于物理實現，為提升控制性能，本文研究易于物理實現的非線性自抗擾控制方法。令x1=θ*-θ(t)，x2=θ*，x3=φ*-φ(t)，x4=φ*。其中，θ*為式(1)俯仰角輸出，φ*為式(1)橫滾角輸出。則有

(2)

其中

ITx=Ix+ml2ITy=Iy+ml2

式中B——控制系統(tǒng)

可得自轉旋翼機系統(tǒng)式(1)的誤差系統(tǒng)為

(3)

式中f1(·)——俯仰角方向除控制力之外的合外力

f2(·)——橫滾角方向除控制力之外的合外力

其構成系統(tǒng)狀態(tài)內和環(huán)境因素的非線性耦合因素，將其定義為系統(tǒng)總擾動

(4)

則本文不需要太多的系統(tǒng)模型信息，只需要假定系統(tǒng)函數滿足以下先決條件：假設f1,f2∈C([0,∞],R)連續(xù)可微，同時外部擾動di(t)∈C([0,∞],R)連續(xù)有界，存在常數N>0使得

sup(|di(t)|+|di-1(t)|)≤N(i=1,2,…)

(5)

式中 sup()——上確界函數

2 二自由度PID控制器設計

為提升控制性能，本文采用非線性擴張狀態(tài)觀測器通過系統(tǒng)的輸出誤差在線估計系統(tǒng)的總擾動，并在反饋控制中實時補償，非線性擴張狀態(tài)觀測器為

(6)

式中r——增益參數，r>0

xi(t:r)、fj(t:r)——xi(t)、fj(·)的估計值，i=1,2,3,4，j=1,2

gi是一個非線性函數

(7)

k1、k2、k3是使得矩陣K為Hurwitz的常數

(8)

容易得出驗證矩陣K是Hurwitz的充要條件是ki>0(i=1,2,3),且有k1k2-k3>0。

分段光滑的fal函數式(7)是最初的用于構造擴張觀測器的非線性函數。把擾動和外界信息綜合考慮，觀測和補償的非線性輸出反饋控制——自抗擾控制模型為

(9)

其中

μ1=2μ-1μ2=(2μ-1)/μ

利用一自由度PID，系統(tǒng)的魯棒性和動態(tài)響應性能不能同時達到最優(yōu)，本文利用二自由度PID控制，克服上述缺點，根據自轉旋翼機控制具有非線性、響應滯后、空中實驗環(huán)境因素不確定性多等特點，構建圖3所示的二自由度并行結構PID控制器。

圖3 二自由度PID控制器結構框圖Fig.3 Two-degree-of-freedom PID control structure

圖3中，r(t)為輸入信號，d(t)為干擾信號，c(t)為PD控制輸出，c2(t)為PID控制器輸出，c(t)為c1(t)和c2(t)之和，y(t)為系統(tǒng)響應。

PD控制器M1(s)為

M1(s)=-Kp1(θ+δKd1s)

(10)

PID控制器M2(s)為

(11)

式中Kp1、Kp2、Ki、Kd1、Kd2——傳統(tǒng)PID控制參數

θ、δ——二自由度系數

干擾傳遞函數Myn(s)、噪聲傳遞函數Myd(s)和設定值傳遞函數Myr(s)分別為

(12)

(13)

(14)

則二自由度閉環(huán)輸出為

(15)

式中Ka、Kb——外擾抑制最佳比例增益

取Ka=Kb，則有

(16)

其中干擾傳遞函數和噪聲傳遞函數在整定二自由度PID參數之后即可被推導出，在整個系統(tǒng)中，整定PID 3個參數、二自由度系數和Ka步驟如下：

(1)整定PID 3個參數Kp、Ki、Kd。

(2)θ、δ取值范圍為[0,1]，根據式(10)采用二分法，得到控制器M1(s)響應。

(3)根據式(11)得到M2(s)控制器響應。

(4)根據式(12)～(14)得到干擾傳遞函數、噪聲傳遞函數和設定值傳遞函數。

(5)整定Ka系數，使得擾動抑制和響應控制達到最優(yōu)。

根據式(16)可以推導出，當

在該區(qū)間非線性單調遞減，得在該區(qū)間左右兩邊均為非線性單調遞增，M1(s)、M2(s)響應值受到PID參數和二自由度系數影響，Ka單調區(qū)間隨之改變。

3 基于PSO-SA的二自由度PID控制器設計

3.1 粒子群優(yōu)化算法

粒子群優(yōu)化算法(PSO)是基于種群的隨機優(yōu)化算法，是一種模仿自然界中一些群體性動物的群集行為，群體中每個個體依照某種特定的合作協(xié)調關系，通過自我學習和群體經驗來不斷改變和進化搜索模式，經迭代找到最優(yōu)值的一種算法。算法流程如圖4所示。

圖4 PSO算法流程圖Fig.4 Flow chart of PSO algorithm

在一個空間內有Nsum個粒子群落，其中第n個粒子表示為xn=(x1,x2，…，xNsum)，粒子運動速度為vn=(v1,v2，…，vNsum)，用Pbest=(P1,P2，…，PNsum)來記錄第n個粒子搜索到的最優(yōu)位置，用Gbest=(G1,G2，…，GNsum)來記錄粒子群搜索到的最優(yōu)位置，搜索最優(yōu)值的過程中用以下公式不斷更新粒子的速度和位置

vn=αvn-1+θ1Random()(Pbest,n-1-xn-1)+
θ2Random(Gbest,n-1-xn-1)

(17)

xn=xn-1+vn

(18)

Random()生成0到1的隨機數。θ1、θ2是學習因子，Pbest，n、Gbest，n分別為個體極值和群體最優(yōu)極值，α為慣性因子，為非負數，在式(17)中，每一次迭代更新的速度都受到當前粒子前一步的個體極值和全局最優(yōu)極值的影響，由此可以反映出粒子間的協(xié)同性和極值共享性。

α和算法的全局尋優(yōu)能力成正比關系，與局部尋優(yōu)能力成反比。本文采用線性遞減權值策略

αt=(αstart-αend)(Ck-g)/Ck+αend

(19)

式中αstart——初始權值

αend——最小權值

Ck——最大迭代次數

g——當前迭代次數

一般地取αstart=0.9,αend=0.4，隨著迭代遞減到0.4。

通過式(17)、(18)可以對粒子更新位置和速度，當達到最大迭代次數或者差值滿足函數要求時，終止迭代并返回參數值。PSO算法是一種不確定性算法，在優(yōu)化過程中，粒子群的規(guī)模與收斂性、收斂精度和收斂速度存在著一定的矛盾性，每次迭代粒子之間會相互競爭相互影響，因此其局部尋優(yōu)能力并不強。

為解決上述問題，本文融合模擬退火算法進行PSO局部優(yōu)化，以彌補其全局尋優(yōu)能力和局部尋優(yōu)能力相矛盾的缺點。用前饋控制器的調參，弱化學習因子對算法的不穩(wěn)定性影響，提高算法穩(wěn)定性。

3.2 模擬退火算法優(yōu)化

假設粒子前一個狀態(tài)為x(n)，根據粒子運動，下一狀態(tài)變?yōu)閤(n+1)，最靠近x軸的距離為本文需要的極值點，則相應的距離L變?yōu)長(n+1)，定義粒子群接受該極值點的概率P為

(20)

其中

(21)

式中β——退火速率，小于1正數，一般取0.8～1

T0——初始溫度

根據式(20)可知，如果L(n+1)在向最終值靠近，則該值就會被記錄為新的極值，如果不是極值，也會將其記錄下來，均勻分布在[0,1]區(qū)間，作為下一次運動的參考值。若再有重復的值則可直接采用現有結果，避免重復計算，提高算法運行速度。

帶學習因子的PSO算法不能解決局部最優(yōu)的問題、分散性差，因此使用模擬退火算法優(yōu)化PSO算法，優(yōu)化粒子群算法過程如下：

(1)初始化：設定Nsum個粒子，最大迭代次數C，設定各個參數的范圍，對于任意的Xn服從[Xmin,Xmax]均勻分布，對于任意的Vn在[Vmin,Vmax]服從均勻分布。

(2)計算每個粒子P值并保存。

(3)更新每個粒子P值，比較P(n)和P(n+1)，取最優(yōu)的值作為Pbest，若為全局最優(yōu)則作為Gbest，更新Gbest并查看是否滿足最終結果條件。

(4)模擬退火擾動，生成新的粒子群，進行迭代。

(5)如果滿足條件則退出循環(huán)，不滿足則返回步驟(2)。

該融合算法全局是PSO算法思想，每個粒子在尋找局部最優(yōu)解時利用模擬退火算法，在PSO全局尋優(yōu)能力較好時還增強PSO局部尋優(yōu)能力，能有效地找到局部最優(yōu)解而獲得全局最優(yōu)解，提高全局最優(yōu)的精度。初始種群是隨機分散的，粒子群算法有很強的群體性，算法收斂速度快、效率高。

3.3 基于PSO-SA融合算法的二自由度PID控制器

采用PSO-SA融合算法，對控制器3個參數Kp、Ki、Kd以及前饋控制器的θ、δ進行調節(jié)，M1(s)和M2(s)組合為前饋控制器的調節(jié)函數。模塊的輸入信號為自轉旋翼機的預設值和當前狀態(tài)的信號差值，使得PSO-SA算法整定下二自由度PID控制器能夠對被控對象穩(wěn)定控制。該算法能快速地找到系統(tǒng)全局最優(yōu)解，保證自轉旋翼機的穩(wěn)定飛行和抗干擾能力達到最優(yōu)，基于PSO-SA融合算法的二自由度PID控制結構如圖5所示。

圖5 基于PSO-SA算法二自由度PID控制器Fig.5 Two-degree-of-freedom PID controller based on PSO-SA algorithm

如圖5所示，PSO-SA融合算法在調節(jié)PID參數時，與一自由度PID控制器一樣，前饋控制器參數θ和δ，在調節(jié)出PID參數后，通過式(10)和二分法確定2個參數，得到M1(s)控制器響應，由式(11)得到M2(s)控制器響應，則能推出干擾傳遞函數、噪聲傳遞函數，設定值傳遞函數中的Ka參數由PSO-SA算法整定，得到最優(yōu)解。使得系統(tǒng)能夠快速響應跟蹤設定值的變化。

4 仿真結果與分析

4.1 測試函數仿真

為驗證PSO-SA算法的性能，本文采用Rastrigin函數作為測試函數，與標準粒子群算法和模擬退火算法相比較。Rastrigin函數表示為

(22)

Rastrigin函數為多峰函數，收斂于0。設置迭代次數為100，種群規(guī)模為50時，學習因子θ1=1.5，θ2=2.5。

仿真算法收斂情況見圖6，PSO、SA、PSO-SA算法的誤差分別為0.124、0.092、0.042，可以看出：

圖6 PSO、SA和PSO-SA算法收斂圖Fig.6 Convergence graph of PSO, SA and PSO-SA algorithms

(1)標準粒子群算法優(yōu)化收斂快，容易局部收斂，收斂精度低。

(2)模擬退火算法收斂精度高，有很強的局部尋優(yōu)能力，但計算效率比較低，響應時間長。

(3)PSO-SA算法收斂速度快，優(yōu)化結果精度高，模擬退火算法彌補局部尋優(yōu)能力，增強全局搜索性能，充分利用粒子群算法和模擬退火算法的優(yōu)點，使得收斂速度快，收斂精度高。

4.2 階躍信號仿真

為驗證PSO-SA算法和二自由度PID控制相結合的優(yōu)化效果，利用Matlab建立相應的控制系統(tǒng)。設置迭代次數為100，種群規(guī)模為25，學習因子θ1=1.5，θ2=2.5，Kp的取值范圍為[0,1]，Ki的取值范圍為[0,40]，Kd的取值范圍為[0,10]，采樣周期為0.01 s。對比仿真研究基于PSO-SA融合算法二自由度PID控制和PID控制對系統(tǒng)階躍信號響應，研究其對階躍信號的收斂。仿真結果如圖7、8所示。

圖7 對階躍信號的響應輸出Fig.7 Response output to step signal

圖8 前置反饋器的響應收斂Fig.8 Response of prefeedback converges

因是階躍響應，沒有外界干擾值，所以二自由度前饋控制器的輸入響應為階躍信號反信號，對該階躍抖動收斂于0，輸出信號收斂于階躍信號。由圖7、8可以看出，經典二自由度PID抖動大，響應速度慢且精度低；二自由度SA-PID控制響應速度較慢，收斂精度較高；二自由度PSO-PID控制響應較快，響應精度較低。二自由度PSO-SA-PID控制明顯優(yōu)于其他3個算法，收斂精度高，響應速度快。

4.3 模擬飛行仿真

為進一步驗證有效性，根據前文建模，設計旋翼機飛行仿真模型，旋翼控制系統(tǒng)、全動垂尾控制系統(tǒng)和油門系統(tǒng)控制，旋翼控制系統(tǒng)與本文二自由度PID控制系統(tǒng)相結合，垂尾控制系統(tǒng)主要作用為輔助轉彎和改變機頭方向，油門控制系統(tǒng)主要作用為控制空速。設置運動軌跡為爬升模式，在外環(huán)姿態(tài)角控制回路給定目標俯仰角和滾轉角情況下，分別用粒子群算法、模擬退火算法以及PSO-SA融合算法尋優(yōu)協(xié)調內外參數，確定外回路的二自由度PID參數，記錄響應時間和誤差值。待控制系統(tǒng)俯仰角穩(wěn)定在10°后加外部干擾，俯仰角收斂設定值改變?yōu)?°，研究其魯棒性和穩(wěn)定性。各個控制收斂響應值如圖9所示。

圖9 模擬飛行仿真俯仰角變化曲線Fig.9 Simulation of flight simulation of pitch angle

旋翼機的航向由橫滾角和全動垂尾共同控制，在控制過程中，全動垂尾主要起輔助作用，且舵機直接控制轉動幅度，控制較容易。本文主要研究旋翼控制滯后性和旋翼橫滾角控制，通過在模擬飛行中小幅度變化橫滾角研究其穩(wěn)定性和魯棒性，各個算法控制收斂響應值如圖10所示。

圖10 模擬飛行仿真橫滾角變化曲線Fig.10 Simulation of flight simulation of roll angle

由圖9、10和表1、2可知，傳統(tǒng)PID控制響應慢、精度低。SA算法和PSO算法都能對傳統(tǒng)PID控制進行優(yōu)化。SA算法收斂時間較PSO算法收斂時間長，但精度高約13%；PSO-SA算法收斂時間最短，較傳統(tǒng)PID、PSO、SA時間約縮短了11%。收斂精度較3種算法高，收斂誤差小。

表1 穩(wěn)定后俯仰角參數Tab.1 Pitch angle parameters after stabilization

表2 穩(wěn)定后橫滾角參數Tab.2 Roll angle parameters after stabilization

本文提出的PSO-SA算法和二自由度PID控制相結合，并設計前饋控制器抑噪的方法，能達到較好的控制效果。

5 飛行實驗

為驗證實際飛行可行性，本文采用單葉槳長為600 mm，機身長度為450 mm，機身質量為1 kg的小型自轉旋翼航模機作為實際飛行實驗機，如圖11所示。分別在無風環(huán)境和有風環(huán)境下進行飛行測試。

圖11 飛行測試航模機Fig.11 Flight test model aircraft

5.1 無風環(huán)境飛行實驗

人工手動遙控使航模機達到預定的飛行高度后，切換到自動控制模式，傳感器記錄俯仰角和橫滾角的飛行數據，如圖12、13所示，由圖12、13可知，實際飛行的橫滾角和俯仰角跟隨良好，能自動平穩(wěn)飛行。

圖12 無風環(huán)境下指定俯仰角和實際俯仰角變化曲線Fig.12 Specified and actual pitch angle in windless environment

圖13 無風環(huán)境下指定橫滾角和實際橫滾角變化曲線Fig.13 Specified and actual roll angle in windless environment

由圖12、13和表3可以看出，俯仰角在切換平飛模式時會有一定偏差，導致平均誤差比較大。飛行過程中最大誤差和平均誤差均在可控閾值范圍內，能保持平穩(wěn)飛行。橫滾角在開始切換為自動飛行模式時有一定的偏差，但響應曲線波形一致，反映了系統(tǒng)有較好的響應速度，由于旋翼機的響應滯后性，最大誤差出現在轉彎處，誤差均在可控閾值范圍內，旋翼機能保持平穩(wěn)飛行。

表3 無風時實際飛行誤差Tab.3 Actual flight error (°)

5.2 有風環(huán)境飛行實驗

在實際作業(yè)時，環(huán)境因素多變，為驗證控制系統(tǒng)的穩(wěn)定性，選定3級風力左右飛行環(huán)境。圖14、15為有風環(huán)境下的飛行記錄。

圖14 有風環(huán)境下指定俯仰角和實際俯仰角變化曲線Fig.14 Specified and actual pitch angle in wind environment

圖15 有風環(huán)境下指定橫滾角和實際橫滾角變化曲線Fig.15 Specified and actual roll angle in wind environment

由圖14、15和表4可以看出，俯仰角和橫滾角在有風環(huán)境下誤差較無風環(huán)境下大。俯仰角在切換自動平飛時誤差最大，隨后收斂，由于有風力影響，飛行過程中有抖動現象，控制參數均在閾值內，能平穩(wěn)飛行。橫滾角變化較無風環(huán)境下明顯，所以環(huán)境干擾對橫向控制影響更大，切換自動飛行時由于機體質量小，風力影響較大，機身抖動，在穩(wěn)定后能夠平穩(wěn)飛行。若機身質量大時風力影響會相對較弱，控制系統(tǒng)會更穩(wěn)定。2次飛行實驗驗證了本文提出的控制方法的可行性。

表4 有風時實際飛行誤差Tab.4 Actual flight error (°)

6 結論

(1)基于二自由度的PID控制器具有較強的快速響應能力，同時前饋控制器能有效抑制外界干擾。粒子群算法和模擬退火算法雖然可以整定PID參數，但均具有一定的局限性，而PSO-SA融合算法能有效結合2個算法的優(yōu)點，將該算法應用于自轉旋翼機的控制回路中二自由度PID控制器的參數整定，響應時間約減少了11%，精度更高，收斂誤差小，約是傳統(tǒng)PID的1/6，控制系統(tǒng)更穩(wěn)定。

(2)通過仿真和實際飛行實驗，驗證了基于PSO-SA算法的二自由度PID控制旋翼機實際飛行中的可行性，飛行過程中的俯仰角和橫滾角平均誤差在閾值范圍內，跟隨性良好。在農業(yè)自轉旋翼機田間作業(yè)時，處于大載荷、超低空、低速狀態(tài)下，基于PSO-SA算法的二自由度PID控制器響應能力和抗干擾能力較強，為農用大載荷無人自轉旋翼機的開發(fā)提供了理論基礎和可行性論證。