粒子群優化模糊PID的四旋翼飛行器控制

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

（南京林業大學 機械電子工程學院，南京 210000）

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

基于上述研究成果，為更好地提高四旋翼飛行器的動態性能及控制精度，本文提出將粒子群算法與模糊控制算法相結合，設計了一種基于粒子群模糊PID 算法的四旋翼飛行器控制系統。 該系統通過粒子群優化算法對量化因子Cke、比例因子Ckp，Cki，Ckd進行迭代尋優再通過模糊控制器中的模糊化與反模糊化處理得出最優權重因子，實現了對控制器參數的智能調整，相對提高了系統的控制精度和動態性能，且該系統具有結構簡單、可操作性強等優點，有望被應用于實際的無人機控制設計工作。

1 四旋翼飛行器控制系統

1.1 非線性數學模型的建立

四旋翼是具有六自由度的強耦合模型[5]，為建立其數學模型，定義兩個坐標系：地球坐標系E（xe，ye，ze）以及機體坐標系B（xb，yb，zb）。 地球坐標系相對于原點Oe做平移運動， 機體坐標系相對于原點Ob做旋轉運動。ω1，ω2，ω3，ω4分別為四旋翼的旋轉角速度，F1，F2，F3，F4為4 個旋翼產生的力，如圖1 所示。

圖1 機體坐標系與地球坐標系Fig.1 Aircraft-body coordinate frame and Earth coordinate system

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

1.2 控制系統設計

由四旋翼非線性模型可知，系統控制部分主要可以分為位置控制與姿態控制。 根據其非線性數學模型特性，采用串級雙閉環控制系統，即內環姿態控制部分以及外環位置控制部分，具體控制結構如圖2 所示。

圖2 系統控制結構Fig.2 Control structure of system

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

2 粒子群模糊PID 控制系統的設計

2.1 模糊PID 控制系統的設計

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

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

2.2 粒子群優化算法分析及優化流程

由于模糊PID 控制存在抗干擾性較差、調節精度較低等問題[9]，在模糊PID 控制的基礎上加入粒子群優化環節改進為粒子群模糊PID 控制器。

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

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

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

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

式中：e 為系統誤差；T 為系統運行時間。

本次實驗采用粒子群算法通過不斷在線迭代優化模糊PID 控制的輸入輸出， 即優化量化因子Cke和比例因子Ckp，Cki，Ckd，再通過模糊控制算法中的模糊化與反模糊化處理，動態的調節權重因子，使系統的動態性能以及控制精度達到最佳。 控制結構如圖3 所示。

圖3 粒子群模糊PID 控制器結構Fig.3 Particle swarm fuzzy PID controller structure

在Matlab 環境中根據粒子群算法設計搭建Simulink 模型并編寫相應的程序，如圖4 所示。并由此結構驗證該算法控制下的無人機動態特性。

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

3 實驗仿真

3.1 粒子群模糊PID 階躍響應實驗

在Matlab 環境下搭建好Simulink 模型后進行實驗仿真。 選取X 軸方向的運動作為研究對象，給予系模型一階躍信號，設定目標值為5，觀察各控制狀態下的相應情況判斷四旋翼飛行器的動態性能，仿真結果如圖5 所示。

圖5 階躍響應下各控制系統仿真結果Fig.5 Simulation results of each control system under step response

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

表1 動態性能對比Tab.1 Dynamic performance comparison

3.2 粒子群模糊PID 自主跟蹤實驗

為驗證粒子群模糊PID 控制的自主跟蹤性能，給予系統一方波信號，設定目標值為5，信號周期為20，占空比為50%，采用粒子群模糊PID 控制與模糊PID 控制進行比較實驗，系統響應如圖6所示。

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

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

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

3.3 粒子群模糊PID 魯棒性和抗干擾性實驗

實際的內部信號環境具有很多干擾因素，為驗證粒子群模糊PID 控制下四旋翼飛行器的魯棒性，分別進行突發干擾實驗以及長期干擾實驗。 在階躍響應穩定后的第5 秒加入瞬時干擾信號驗證控制系統在突發干擾下的穩定性；在階躍響應穩定后的第5 秒到第10 秒的時間內加入長期的干擾信號驗證控制系統面對長期干擾下的穩定性。 選取模糊PID 控制進行對比，實驗結果如圖7 所示。

圖7 粒子群模糊PID 控制穩定性實驗Fig.7 Particle swarm fuzzy PID control stability experiment

實驗結果表明，在第5 秒加入瞬態干擾因素后，粒子群模糊PID 控制相較于模糊PID 控制展現了更加優良的動態性能，在波動后可以快速平穩的回歸到穩定值。 而在面對第5 秒到第10 秒的長期干擾時粒子群模糊PID 控制可以基本維持在誤差允許的范圍內，維持相對穩定。 因此粒子群模糊PID控制具有良好的魯棒性。

在外界環境中存在大量的外界干擾因素，如風力等。 為驗證粒子群模糊PID 控制下四旋翼飛行器的抗干擾性能，在系統達到穩定后的第5 秒引入外界干擾因素，實驗結果如圖8 所示。

圖8 粒子群模糊PID 面對外界環境干擾下的穩定性Fig.8 Stability of particle swarm fuzzy PID in the face of external environmental interference

實驗結果表明，粒子群模糊PID 在面對外界影響時具有很好的抗干擾特性，可以快速穩定的回歸到穩定值，相較于模糊PID 控制抗干擾性有了明顯的提升。

4 結語

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