粒子群優(yōu)化模糊PID的四旋翼飛行器控制

劉祎瑋，唐路平，王詠婷，魏伶俐

（南京林業(yè)大學(xué) 機(jī)械電子工程學(xué)院，南京 210000）

四旋翼飛行器作為常用的無(wú)人機(jī)之一，因具有體積小、操作簡(jiǎn)單、可攜帶負(fù)重等優(yōu)點(diǎn)，被廣泛地應(yīng)用于影視、商業(yè)及軍事等領(lǐng)域。 但是由于四旋翼飛行器具有強(qiáng)耦合、非線性、多變量等特性[1]，其閉環(huán)控制穩(wěn)定的問題一直是研究熱點(diǎn)之一。 近年來(lái)，國(guó)內(nèi)外很多學(xué)者對(duì)此問題進(jìn)行了大量研究。 文獻(xiàn)[2]采用反步法來(lái)搭建四旋翼的控制系統(tǒng)，反步法具有良好的跟蹤性、無(wú)超調(diào)且調(diào)整時(shí)間快速等特點(diǎn)，但反步法設(shè)計(jì)控制器較為復(fù)雜且該方法主要針對(duì)于嚴(yán)格滿足反饋控制系統(tǒng)的結(jié)構(gòu)；文獻(xiàn)[3]采用四元數(shù)反饋控制方法研究四旋翼的姿態(tài)控制問題，利用四元數(shù)反饋控制方法搭建的控制器具有姿態(tài)解算無(wú)奇異點(diǎn)、結(jié)構(gòu)簡(jiǎn)單等特征；文獻(xiàn)[4]采用模糊PID 控制對(duì)姿態(tài)控制問題進(jìn)行了優(yōu)化，利用模糊PID 控制可以更好的提高四旋翼飛行器的動(dòng)態(tài)穩(wěn)定性，且結(jié)構(gòu)簡(jiǎn)單便于實(shí)現(xiàn)，但由于模糊控制器中模糊語(yǔ)言的特性導(dǎo)致了其過度依賴于專家經(jīng)驗(yàn)且控制精度有待提高。

基于上述研究成果，為更好地提高四旋翼飛行器的動(dòng)態(tài)性能及控制精度，本文提出將粒子群算法與模糊控制算法相結(jié)合，設(shè)計(jì)了一種基于粒子群模糊PID 算法的四旋翼飛行器控制系統(tǒng)。 該系統(tǒng)通過粒子群優(yōu)化算法對(duì)量化因子Cke、比例因子Ckp，Cki，Ckd進(jìn)行迭代尋優(yōu)再通過模糊控制器中的模糊化與反模糊化處理得出最優(yōu)權(quán)重因子，實(shí)現(xiàn)了對(duì)控制器參數(shù)的智能調(diào)整，相對(duì)提高了系統(tǒng)的控制精度和動(dòng)態(tài)性能，且該系統(tǒng)具有結(jié)構(gòu)簡(jiǎn)單、可操作性強(qiáng)等優(yōu)點(diǎn)，有望被應(yīng)用于實(shí)際的無(wú)人機(jī)控制設(shè)計(jì)工作。

1 四旋翼飛行器控制系統(tǒng)

1.1 非線性數(shù)學(xué)模型的建立

四旋翼是具有六自由度的強(qiáng)耦合模型[5]，為建立其數(shù)學(xué)模型，定義兩個(gè)坐標(biāo)系：地球坐標(biāo)系E（xe，ye，ze）以及機(jī)體坐標(biāo)系B（xb，yb，zb）。 地球坐標(biāo)系相對(duì)于原點(diǎn)Oe做平移運(yùn)動(dòng)， 機(jī)體坐標(biāo)系相對(duì)于原點(diǎn)Ob做旋轉(zhuǎn)運(yùn)動(dòng)。ω1，ω2，ω3，ω4分別為四旋翼的旋轉(zhuǎn)角速度，F(xiàn)1，F(xiàn)2，F(xiàn)3，F(xiàn)4為4 個(gè)旋翼產(chǎn)生的力，如圖1 所示。

圖1 機(jī)體坐標(biāo)系與地球坐標(biāo)系Fig.1 Aircraft-body coordinate frame and Earth coordinate system

由于實(shí)際問題中空氣阻力以及陀螺效應(yīng)產(chǎn)生的影響較小，因此可以忽略[6]。 根據(jù)牛頓-歐拉方程并經(jīng)過一系列數(shù)學(xué)運(yùn)算最終得出四旋翼飛行器的非線性模型[7]，如式（1）和式（2）所示：

1.2 控制系統(tǒng)設(shè)計(jì)

由四旋翼非線性模型可知，系統(tǒng)控制部分主要可以分為位置控制與姿態(tài)控制。 根據(jù)其非線性數(shù)學(xué)模型特性，采用串級(jí)雙閉環(huán)控制系統(tǒng)，即內(nèi)環(huán)姿態(tài)控制部分以及外環(huán)位置控制部分，具體控制結(jié)構(gòu)如圖2 所示。

圖2 系統(tǒng)控制結(jié)構(gòu)Fig.2 Control structure of system

人為給與初始狀態(tài)（xd，yd，zd，ψd）即期望的x，y，z方向上的位置以及期望的偏航角ψd，然后經(jīng)過位置控制器反解出四旋翼飛行器在機(jī)體坐標(biāo)系下的總合力U1，以及期望的俯仰角θd，期望滾轉(zhuǎn)角φd。 其中總合力U1由位置控制器產(chǎn)生的x，y，z 軸向上的分力求得。 然后姿態(tài)控制器利用求解出的θd，φd以及給定的期望偏航角ψd， 求得驅(qū)使ψ，θ，φd變化的力U2，U3，U4，經(jīng)過對(duì)電機(jī)轉(zhuǎn)速的逆變換后求得4 個(gè)旋翼的轉(zhuǎn)速ω1，ω2，ω3，ω4代入飛行器的非線性數(shù)學(xué)模型中并得出12 個(gè)狀態(tài)反饋量。 基于上述串聯(lián)雙閉環(huán)控制控制結(jié)構(gòu)搭建控制對(duì)象模型以進(jìn)行后續(xù)的控制算法研究。

2 粒子群模糊PID 控制系統(tǒng)的設(shè)計(jì)

2.1 模糊PID 控制系統(tǒng)的設(shè)計(jì)

由于傳統(tǒng)PID 控制存在人工調(diào)參較為繁瑣、動(dòng)態(tài)性能較低的問題[8]。 因此在傳統(tǒng)PID 控制器的基礎(chǔ)上加入模糊控制部分，改進(jìn)為模糊PID 控制器。模糊PID 控制器工作原理為：通過不斷檢查系統(tǒng)實(shí)際軌跡與期望軌跡的偏差以及偏差的變化率并用量化因子Cke對(duì)其進(jìn)行整理計(jì)算得出其對(duì)應(yīng)的模糊值，將之與由專家經(jīng)驗(yàn)編寫的模糊規(guī)則表進(jìn)行匹配，推理計(jì)算得出比例因子Ckp，Cki，Ckd并輸入到PID控制器當(dāng)中，不斷地對(duì)輸入量進(jìn)行調(diào)整從而達(dá)到對(duì)系統(tǒng)控制的優(yōu)化。

根據(jù)四旋翼飛行器動(dòng)力學(xué)方程的性質(zhì)，本次實(shí)驗(yàn)選擇二維模糊控制算法，即將實(shí)際軌跡與期望軌跡的誤差e 及誤差的變化率ec 作為輸入量，PID 控制器的3 個(gè)參數(shù)變量作為輸出變量。 變量的基本論域均設(shè)為［-3，3］；并使用七段式模糊語(yǔ)言值；模糊子集為｛NB，NM，NS，Z，PS，PM，PB｝對(duì)應(yīng)｛負(fù)大，負(fù)中，負(fù)小，零，正小，正中，正大｝；選取對(duì)稱式三角型隸屬函數(shù)作為隸屬度函數(shù)；采用重心法去模糊化。

2.2 粒子群優(yōu)化算法分析及優(yōu)化流程

由于模糊PID 控制存在抗干擾性較差、調(diào)節(jié)精度較低等問題[9]，在模糊PID 控制的基礎(chǔ)上加入粒子群優(yōu)化環(huán)節(jié)改進(jìn)為粒子群模糊PID 控制器。

粒子群算法于1995年由Kennedy 及Eberhart提出，該算法核心思想是將每個(gè)粒子看作成一只鳥，鳥群捕食的過程即為粒子尋求最優(yōu)解的過程[10]。 每個(gè)粒子均具有速度以及位置屬性，在D 維空間中的N 個(gè)粒子的速度與位置向量可以表示為Vi=（vi1，vi2，vi3，…，vid），Xi=（xi1，xi2，xi3，…，xid）每個(gè)粒子在尋找途中的最佳值即為最優(yōu)解。 粒子本身找到的最優(yōu)解為個(gè)體最優(yōu)即Pt，粒子群整體對(duì)比找到的最優(yōu)解為全局最優(yōu)即gt[11]。 粒子群算法通過粒子的不斷迭代尋找最優(yōu)解，進(jìn)而達(dá)到對(duì)系統(tǒng)參數(shù)的優(yōu)化。

系統(tǒng)在迭代過程中，其速度與位置的更新符合以下規(guī)律，如式（3）所示：

式中：速度的更新規(guī)律由3 部分組成，分別為社會(huì)認(rèn)知、自我認(rèn)知以及前一時(shí)刻速度的繼承部分[12]。ω 為慣性因子， 體現(xiàn)前一時(shí)刻速度對(duì)該時(shí)刻速度的影響；c1，c2為加速常數(shù)，c1影響個(gè)體最優(yōu)。當(dāng)c1過大時(shí)，粒子群整體受個(gè)體認(rèn)知影響過大，容易陷入局部最優(yōu)；當(dāng)c2過大時(shí)，社會(huì)認(rèn)知部分的影響過大，使粒子群整體過早地達(dá)到全局最優(yōu)，出現(xiàn)早熟情況[13]。對(duì)ω 的權(quán)重更新采用線性遞歸法， 更新公式如式（4）所示：

式中：t 為當(dāng)前迭代次數(shù)；tmax為最大迭代次數(shù)；ωmax為最大慣性因子；ωmin為最小慣性因子。適應(yīng)值函數(shù)采用ITAE 指標(biāo)，其可以反映出誤差與時(shí)間的關(guān)系，適應(yīng)值越小則表明控制性能越優(yōu)良[14]，如式（5）所示：

式中：e 為系統(tǒng)誤差；T 為系統(tǒng)運(yùn)行時(shí)間。

本次實(shí)驗(yàn)采用粒子群算法通過不斷在線迭代優(yōu)化模糊PID 控制的輸入輸出， 即優(yōu)化量化因子Cke和比例因子Ckp，Cki，Ckd，再通過模糊控制算法中的模糊化與反模糊化處理，動(dòng)態(tài)的調(diào)節(jié)權(quán)重因子，使系統(tǒng)的動(dòng)態(tài)性能以及控制精度達(dá)到最佳。 控制結(jié)構(gòu)如圖3 所示。

圖3 粒子群模糊PID 控制器結(jié)構(gòu)Fig.3 Particle swarm fuzzy PID controller structure

在Matlab 環(huán)境中根據(jù)粒子群算法設(shè)計(jì)搭建Simulink 模型并編寫相應(yīng)的程序，如圖4 所示。并由此結(jié)構(gòu)驗(yàn)證該算法控制下的無(wú)人機(jī)動(dòng)態(tài)特性。

圖4 基于粒子群算法的模糊PID 控制系統(tǒng)仿真圖Fig.4 Simulation diagram of fuzzy PID control system based on particle swarm optimization algorithm

3 實(shí)驗(yàn)仿真

3.1 粒子群模糊PID 階躍響應(yīng)實(shí)驗(yàn)

在Matlab 環(huán)境下搭建好Simulink 模型后進(jìn)行實(shí)驗(yàn)仿真。 選取X 軸方向的運(yùn)動(dòng)作為研究對(duì)象，給予系模型一階躍信號(hào)，設(shè)定目標(biāo)值為5，觀察各控制狀態(tài)下的相應(yīng)情況判斷四旋翼飛行器的動(dòng)態(tài)性能，仿真結(jié)果如圖5 所示。

圖5 階躍響應(yīng)下各控制系統(tǒng)仿真結(jié)果Fig.5 Simulation results of each control system under step response

由圖5（a）可看出相較于模糊PID 控制器和傳統(tǒng)PID 控制器，粒子群模糊PID 控制器的超調(diào)量更小，可以做到基本無(wú)超調(diào)。 且粒子群模糊PID 控制器的響應(yīng)速度更快，當(dāng)誤差帶取±2%時(shí)傳統(tǒng)PID 控制的調(diào)節(jié)時(shí)間為2.237 s，模糊PID 控制的調(diào)節(jié)時(shí)間為1.838 s，而粒子群模糊PID 控制器的調(diào)節(jié)時(shí)間僅為0.954 s，極大地提高了系統(tǒng)的動(dòng)態(tài)性能和控制精度，加快了收斂速度。 在三維空間中的運(yùn)動(dòng)，各控制狀態(tài)下的飛行器均可達(dá)到目標(biāo)位置，但粒子群模糊PID 控制相較于模糊PID 控制飛行路徑更優(yōu)， 運(yùn)動(dòng)更加平滑，如圖5（b）所示。 具體動(dòng)態(tài)性能指標(biāo)如表1 所示。

表1 動(dòng)態(tài)性能對(duì)比Tab.1 Dynamic performance comparison

3.2 粒子群模糊PID 自主跟蹤實(shí)驗(yàn)

為驗(yàn)證粒子群模糊PID 控制的自主跟蹤性能，給予系統(tǒng)一方波信號(hào)，設(shè)定目標(biāo)值為5，信號(hào)周期為20，占空比為50%，采用粒子群模糊PID 控制與模糊PID 控制進(jìn)行比較實(shí)驗(yàn)，系統(tǒng)響應(yīng)如圖6所示。

圖6 粒子群模糊PID 控制器的自主跟蹤實(shí)驗(yàn)Fig.6 Autonomous tracking experiment of particle swarm fuzzy PID controller

由圖6 可知粒子群模糊PID 控制可以良好地完成自主跟蹤實(shí)驗(yàn)，相較于模糊PID 控制具有更好的動(dòng)態(tài)特性。在突變發(fā)生后系統(tǒng)可以快速收斂到誤差允許的范圍內(nèi)，相較于模糊PID 其超調(diào)量大大減小，響應(yīng)速度也更快。以初始階段為例，模糊PID 的超調(diào)量為6.16%，而粒子群算法改進(jìn)后可以做到基本無(wú)超調(diào)；達(dá)到誤差帶允許誤差范圍內(nèi)的時(shí)間也由原來(lái)的2.038 s 降為1.750 s， 具體動(dòng)態(tài)性能指標(biāo)如表2 所示。

表2 自主跟蹤性能對(duì)比Tab.2 Autonomous tracking performance comparison

3.3 粒子群模糊PID 魯棒性和抗干擾性實(shí)驗(yàn)

實(shí)際的內(nèi)部信號(hào)環(huán)境具有很多干擾因素，為驗(yàn)證粒子群模糊PID 控制下四旋翼飛行器的魯棒性，分別進(jìn)行突發(fā)干擾實(shí)驗(yàn)以及長(zhǎng)期干擾實(shí)驗(yàn)。 在階躍響應(yīng)穩(wěn)定后的第5 秒加入瞬時(shí)干擾信號(hào)驗(yàn)證控制系統(tǒng)在突發(fā)干擾下的穩(wěn)定性；在階躍響應(yīng)穩(wěn)定后的第5 秒到第10 秒的時(shí)間內(nèi)加入長(zhǎng)期的干擾信號(hào)驗(yàn)證控制系統(tǒng)面對(duì)長(zhǎng)期干擾下的穩(wěn)定性。 選取模糊PID 控制進(jìn)行對(duì)比，實(shí)驗(yàn)結(jié)果如圖7 所示。

圖7 粒子群模糊PID 控制穩(wěn)定性實(shí)驗(yàn)Fig.7 Particle swarm fuzzy PID control stability experiment

實(shí)驗(yàn)結(jié)果表明，在第5 秒加入瞬態(tài)干擾因素后，粒子群模糊PID 控制相較于模糊PID 控制展現(xiàn)了更加優(yōu)良的動(dòng)態(tài)性能，在波動(dòng)后可以快速平穩(wěn)的回歸到穩(wěn)定值。 而在面對(duì)第5 秒到第10 秒的長(zhǎng)期干擾時(shí)粒子群模糊PID 控制可以基本維持在誤差允許的范圍內(nèi)，維持相對(duì)穩(wěn)定。 因此粒子群模糊PID控制具有良好的魯棒性。

在外界環(huán)境中存在大量的外界干擾因素，如風(fēng)力等。 為驗(yàn)證粒子群模糊PID 控制下四旋翼飛行器的抗干擾性能，在系統(tǒng)達(dá)到穩(wěn)定后的第5 秒引入外界干擾因素，實(shí)驗(yàn)結(jié)果如圖8 所示。

圖8 粒子群模糊PID 面對(duì)外界環(huán)境干擾下的穩(wěn)定性Fig.8 Stability of particle swarm fuzzy PID in the face of external environmental interference

實(shí)驗(yàn)結(jié)果表明，粒子群模糊PID 在面對(duì)外界影響時(shí)具有很好的抗干擾特性，可以快速穩(wěn)定的回歸到穩(wěn)定值，相較于模糊PID 控制抗干擾性有了明顯的提升。

4 結(jié)語(yǔ)

為提高四旋翼飛行器的控制精度及動(dòng)態(tài)特性，本文在Matlab 環(huán)境下設(shè)計(jì)了一種基于粒子群模糊PID 控制算法的四旋翼飛行器并同時(shí)設(shè)計(jì)了基于模糊PID 控制算法與傳統(tǒng)PID 控制算法的四旋翼飛行器進(jìn)行比較。 仿真結(jié)果表明，粒子群模糊PID 控制算法相較于其他兩者具有更好的動(dòng)態(tài)特性、抗干擾特性和控制精度。 面對(duì)自主跟蹤問題，可以良好地完成任務(wù)目標(biāo)，且超調(diào)量更小、收斂速度更快、魯棒性更好、抗干擾性更強(qiáng)、工作效率更高，可以更好地完成對(duì)系統(tǒng)的控制工作，對(duì)實(shí)際四旋翼飛行器控制設(shè)計(jì)具有參考價(jià)值。