四旋翼飛行器姿態角的自切換串級PID控制方法

丁 娃，吳功平，何文山，李謀遠

（武漢大學動力與機械學院，湖北 武漢 430072）

1 前言

四旋翼無人機在空間中沒有約束，具有六個自由度，但只有機臂末端的四個電機作為輸入量進行控制。它是典型的欠驅動系統，該系統的特點還在于強耦合、非線性和多變量[1]。因其飛行原理簡單、結構緊湊、單位體積提供扭矩大、機動性能好等特點而被應用于不同行業，在航拍、農業噴灑、軍事偵察、公安追捕、高壓電力巡檢等方面發揮著日益重要的作用[2]。在四旋翼飛行器的控制當中，姿態控制部分是整個飛行控制系統的核心部分，在國內外被廣泛研究。四旋翼飛行器可垂直起飛和降落，便于操控，機動性靈活。但是當四旋翼飛行器面對惡劣的環境時，如空氣氣流的干擾會導致飛行失穩，改進控制算法的優越性對于保護飛行器安全的飛行和管理、提升作業質量具有重要的意義。

最近十幾年來，大量的控制算法被應用于四旋翼飛行器的姿態控制。文獻[3]提出采用滑膜控制可以有效的控制系統穩定，但是其魯棒性取決于良好的模型參數，而這些參數很難精準測量。文獻[3]提出采用自適應的控制方法可以解決系統參數的不確定性，但是外部干擾的影響并未能得到充分的解決，如氣流干擾。文獻［4］提出采用基于神經網絡的控制方法，但是這種方法采用神經單元直接逼近不確定性，使得計算量更大，對硬件要求也更高。目前，主流應用的開源算法中主要采用雙環PID控制，外環作用于姿態角、內環作用于角速度，這種算法可以達到一定的效果，但控制器參數固定不變，只能將姿態控制在一定小角度范圍內，另外，其控制穩定性取決于良好的數學模型，室外作業環境多變，難以達到理想的控制效果［5］。基于此衍生出了很多控制方法，如串級自抗擾控制、基于神經網絡的PID控制，但是這些方法使得原本就欠驅動的系統更加復雜，復雜的浮點型運算及矩陣運算對處理器的計算能力要求也更高［6］。

兼顧飛機穩定性與實際運行效率，提出一種新的串級PID分控系統，外環綜合模糊PID和經典PID特點，自主切換，期望值與實際值相差較大時采用前者，不依賴精確模型、自適應性好，可快速恢復，使系統獲得良好的動態性能；期望值與實際值相差較小時采用后者，簡單有效，可消除靜態誤差，使系統獲得良好的穩定性；并對切換過程進行分析設計，使兩種控制方法平穩過渡。內環采用PD控制器，提升整個系統的響應速度。

2 建立四旋翼模型

對四旋翼模型的運動學和動力學分析，是控制仿真的基礎。為便于進行串級PID分控模擬，在構建數學模型時做出如下假設：

（1）四旋翼的質心與機體坐標的原點一致；（2）地理坐標系是慣性坐標系。即高度發生變化時，重力加速度值固定不變；（3）四旋翼為剛體，不會產生變形；（4）忽略空氣摩擦，忽略無人機平面與旋翼之間的高度差。

為建立適當的四旋翼動力學模型，還需引入合適的坐標系。目前，無人機導航中常用的坐標系包括地心慣性坐標系、地球固聯坐標系、地理坐標系、機體坐標系等。如圖1所示，綜合分析對比后選擇地理坐標系（n系）與機體坐標系（b系）相結合的方式進行解算［8］。

圖1 四旋翼機體坐標系與地理坐標系Fig.1 Airframe Coordinate System and Geographical Coordinate System of Four Rotors

經過三次繞坐標軸轉動后，地理坐標系可轉換至機體坐標系，利用數學矩陣表達式可推導出由地理坐標系向機體坐標系的轉換矩陣W［9］：

式中：θ—俯仰角；γ—橫滾角；ψ—偏航角。假設四旋翼機體完全對稱；m—無人機的重量；Ixx、Iyy、Izz—飛行器繞x軸，y軸和z軸的轉動慣量；Jr—旋翼繞轉動軸的轉動慣量；Ω =[Ω1Ω2Ω3Ω4]T—、四個旋翼的轉速矢量和；Ω=-Ω1+Ω2-Ω3+Ω4—四個旋翼轉速的總和；U1—四個旋翼生成的合力；U2、U3、U4—四旋翼繞三個坐標軸轉動的力矩。定義由無人機旋翼生成的力與力矩矢量U作為控制系統的輸入，可得：

式中：l—四旋翼幾何中心與電機中心之間的長度，單位為m；b—推力系數，單位為N·s2；d—阻力系數，單位為N·m·s2。

經過一系列的轉換計算，最終得出飛行器在地理坐標系中的動力學方程如上式[10]。

3 串級PID分控器設計

由式（2）可知，四旋翼飛行器為多輸入多輸出的非線性系統，且實際飛行時周圍環境多變，為了達到較好的控制效果，使其能迅速調整，保持一定的穩定性，如圖2所示。采用串級PID分控器。其外環控制姿態角，由模糊PID 控制和PID 控制兩部分組成，實際運行時根據輸入變量的情況在兩者之間自主切換，并設置平滑切換方式進行過渡，內環為PD 控制器，對角速度進行控制，調整速度較快。

圖2 四姿態角串級PID分控原理圖Fig.2 Four Attitude Angle Cascade PID Control Schematic Diagram

3.1 構建PID控制器

這里所使用的PID 控制器和PD 控制器均基于Parallel PID Controller[11]，在該控制器的基礎上，構建模糊PID 控制器。選取姿態角偏差e和偏差變化率ec構成輸入語言變量。

建立一種雙輸入三輸出的模糊控制器，對原有PID的結構進行修正，利用模糊控制器的輸出值對固定參數Kp，Ki，Kd進行實時修整，實現構建模糊PID控制器的目的。其結構示意圖，如圖3所示。

圖3 模糊PID結構示意圖Fig.3 Structure Schematic Diagram of Fuzzy PID

3.1.1 變量模糊化

通過實驗確定兩個輸入變量的基本論域為e=［-5，5］和ec=［-4，4］，將其劃分為7個模糊子集：｛負大，負中，負小，零，正小，正中，正大｝（即對應英文簡寫NB，NM，NS，ZO，PS，PM，PB），輸出論域為Kp為［-2，2］，Ki為［-1，1］，Kd為［-5，5］。

同樣劃分為7個模糊子集。綜合考慮系統性能后將隸屬度函數選取為三角形隸屬函數，以Kp為例，利用Simulink中的fuzzy模塊可得其隸屬度函數，如圖4所示。

圖4 Kp隸屬函數Fig.4 Membership Function Graph of Kp

3.1.2 模糊推理及去模糊

根據建立的非線性四旋翼飛行模型，設定相應的模糊控制規則，建立原則如下：

當e較大時，選取較大的Kp來提升系統反應速度，選取較小的Ki來減少超調，選取較小的Kd；

當e和ec變化中等大小時，選取較小的Kp，Kd取適中值；

當e較小時，Kp應取較大值，增大Ki值減小靜態誤差。

考慮到模糊規則對模糊控制器而言至關重要，所以在初步建立起該規則后，結合整體實驗效果和專家經驗做出了一些調整，最終建立模糊控制規則表具體，如表1～表3所示。

表1 Kp模糊控制規則表Tab.1 Basic Fuzzy Control Rule Table of Kp

表2 Ki模糊控制規則表Tab.2 Basic Fuzzy Control Rule Table of Ki

表3 Kd模糊控制規則表Tab.3 Basic Fuzzy Control Rule Table of Kd

Simulink中相應模糊規則邏輯的輸入和輸出表面，如圖5所示。

圖5 Kp Ki Kd 對應變化表面Fig.5 Corresponding Change Surface of Kp Ki Kd

去模糊化使用“重心法［12］”，最終推理的輸出結果為橫坐標與隸屬度函數曲線所圍成面積的重心，實際輸出如下：

式中：fij=ui(e)·uj(ec)；ui(e)、uj(ec)—e和ec的隸屬度；

uij根據設定的模糊規則來確定；

Ku—比例因子。

與其它的去模糊化方法相比而言，重心法的優勢在于更加平滑的輸出推理控制，即使輸入信號發生略微變化，系統輸出值也會對應的發生改變。

3.2 平滑切換方式設定

通過控制器的輸入值作為判斷依據直接進行切換會導致系統具有突變性，所以對該過程進行設計，引入平滑過渡因子w，使得兩種控制方法平穩過渡。令控制器輸出O=w*I1+(1-w)*I2；平滑過度因子越大，模糊PID控制比重越大、PID的控制作用比重越小，對w進行構造：

其中，x1和x2為設置的模糊控制切換點，輸入偏差大于x2時采用模糊PID 控制，進行快速調整；輸入偏差小于x1時，采用PID 控制，當輸入值在兩者之間時，依據w進行不同比重的綜合控制。

系數ρ對參數w會產生影響，進而導致不同的控制特性。平滑過渡曲線，如圖6所示。ρ取不同值時對w產生的影響也不同，取x1為0.01°，x2為0.1°，兩種控制模式之間的切換過渡區域為［-0.1°，-0.01°］和［0.01°，0.1°］。

圖6 平滑過渡曲線Fig.6 Smooth Transition Curve

根據曲線可以看出，ρ較小時，隨著誤差的增大，模糊PID控制的作用快速增加；ρ較大時，在過渡前期，隨著誤差增加，模糊PID 控制作用緩慢增大，PID 起主導作用，在過渡后期，模糊PID起主導作用。

3.3 構建串級PID分控系統

通過假設飛行器飛行時角度變化較小，即γ?=θ?=ψ?=0，則可將無人機動力學方程（3）簡化為下式：

通過拉普拉斯變換可得無人機狀態空間方程的系統傳遞函數為：

控制四旋翼的姿態是通過控制電機的轉速，所以還需在模型中建立電機的數學模型，這里采用經典電機模型，其傳遞函數方程近似為可得系統的傳遞函數為：

針對本文的測試四旋翼飛行器進行實驗測量計算，得出上式中的各參數對應數值，如表4所示。

表4 四旋翼無人機參數Tab.4 UAV Parameters

將參數帶入后可得三個姿態角的傳遞函數為：

以三個姿態角中的偏航角為例進行驗證分析，根據傳遞函數建立對應的串級PID分控系統，對偏航角進行模擬控制，并與串級PID控制器進行對比。為避免系統的突變性，在分控系統的外環中加入設計好的平滑切換模塊，當輸入量e的絕對值大于x1時，為模糊PID控制；小于或等于x2時，采用PID控制；介于兩者之間時，根據e的取值在兩者之間取不同比重，具體框圖，如圖7所示。

圖7 偏航角控制框圖Fig.7 Control Block Diagram of Yaw Angle

依據對三個方向上姿態角的模擬控制，結合式（3）對四旋翼飛行器進行仿真，驗證控制效果。其基本框圖，如圖8所示。左側串級PID分控器根據輸入期望姿態角和反饋值進行控制，得到飛行器的力矩矢量，右側模型根據輸入力矩，解算出電機轉速總和，進而得出四旋翼的姿態角信息和位置信息。

圖8 無人機仿真模型Fig.8 Simulation Model of UAV仿真控制結果

在串級分控系統建立后，針對實驗室的四旋翼飛行器進行Matlab/Simulink仿真控制實驗，并進行了對比驗證。以四旋翼飛行器的偏航角為例，輸入不同的信號來模擬無人機的飛行要求，并與傳統PID和串級PID兩種控制方式進行比較。采用串級控制時，內環和外環相對獨立，所以在調整內環參數時，不用考慮外環控制模型，由內而外進行調整，具體仿真結果，如圖9所示。

圖9 仿真對比驗證圖Fig.9 Figure of Simulation Comparison

為了更直觀看出串級PID分控的控制效果，對圖9（a）和圖9（c）仿真結果進行數據量化。具體數值、如表5、表6所示。

表5 偏航角控制效果對比Tab.5 Control Effect Comparison of Yaw Angle

表6 偏航角抗干擾效果對比Tab.6 Comparison of Anti-Jamming Effect of Yaw Angle

如表5 所示，在t=1s 時輸入階躍信號，三種控制器中串級PID分控系統反應快，超調量較小，僅需0.181s即可達到穩定，控制效果最佳；傳統PID控制超調量大，調節時間久；串級PID控制系統相比傳統PID控制有了較大提升，但與這里所設計的控制器相比，達到平衡需0.293s，時間久，控制精度也低一些。從圖9（b）所輸入的脈沖信號控制結果也可以看出串級PID分控系統的動態控制性能較好。為驗證所建控制器的抗干擾性能，如表6 所示。在t=2s 時，加入幅值為1 的階躍干擾信號，可以看出，串級PID分控的曲線更平緩，產生波動小，控制精度高，系統達到穩態時間最短，抗干擾能力較強，很快就可恢復平衡。如圖9（d）所示，在正弦信號中加入持續隨機干擾信號進行進一步驗證，Simulink模型模擬對比，結果表明串級PID分控系統的控制曲線更平滑，更接近實際輸入，自適應性好。實驗結果表明，串級PID分控系統與另外兩種控制器相比，可在短時間內快速精準的達到控制要求，提升無人機飛行時的穩定性。

為測試控制模型的實際效果，如圖10所示，將自組無人機的姿態控制改為串級PID分控，并進行多次現場飛行實驗。該機型重量為1.4kg，軸距0.45m，采用Pixhawk開源飛控，采用Sunny Sky電機，型號為Angel A2212-KV980，采用Flower Power 電池作為飛行器動力源，規格為3S-11.1V-5200mAh。

圖10 四旋翼驗證平臺Fig.10 UAV Verification Platform

在室外微風情況下，將四軸飛行至一定高度，切換至CIRCLE（繞圈）模式下進行效果驗證。在地面站中將旋轉半徑設置為200cm，轉向速率設置為5deg/s，采樣間隔440ms，對無人機的位置信息進行采集保存至文本，通過MATLAB 繪制四軸經緯度和海拔高度曲線，如圖11所示。可以看出，串級PID分控系統可滿足控制需求，有一定的自穩定性。

圖11 CIRCLE模式修正后飛行軌跡Fig.11 Corrected Flight Path in CIRCLE Mode

4 總結

為解決四旋翼飛行器在復雜多變環境下的穩定性問題，通過牛頓歐拉定律對無人機進行了動力學建模，提出一種新的串級PID分控系統并進行驗證。根據變量的輸入情況自主平滑切換外環控制方式，結合了PID和模糊PID控制器的優點，保障實際運行效率的情況下，兼顧動態與靜態性能；內環采用PD控制器對角速度的變化趨勢進行快速調整，提高了系統抗動態干擾的能力。無人機的飛行控制是一個極其復雜的過程，振動和高動態變化的飛行狀態是四旋翼飛行擾動的主要干擾源。

在Simulink 中模擬并驗證所建立的模型和控制器，將其移植到無人機上觀察效果，輸入不同類型的信號來驗證相應性能，結果顯示該控制系統魯棒性強、穩定性好。所建模型在面對持續干擾信號和突變干擾時，與常規控制器相比飛行效率高，反應速度快，具有更好的智能性與自主性。在野外風載狀況下飛行時，自身姿態調節快速，抵抗干擾能力強，滿足了四旋翼飛行器實用性的需求，為今后無人機實現更高要求的飛行任務奠定了堅實的基礎。