四旋翼飛行仿真器的姿態控制

高偉翔

北京信息科技大學自動化學院高動態導航技術北京市重點實驗室，北京 100192

0 前言

近年來，人們對控制方法的研究從經典控制發展到智能控制以及多種控制融合，在控制系統里的應用也從線性到非線性，從簡單到復雜[1]。針對多變量控制系統對擾動敏感等問題，經典控制略顯局限，因其僅適用于單輸入單輸出的線性定常控制系統，而采取現代控制和智能控制應用于復雜系統是現今控制領域的需求和方向[2-4]。當前研究中，對于旋翼機系統在真實作業中攜帶重物（如藥瓶等物資）而產生質量變化影響穩定性的問題有待解決，同時也需要綜合考慮旋翼機負重工作時可能遇到復雜干擾（如在狹小地帶拍照、田間作業及運送物資時易受障礙物、植物或小動物的碰撞干擾）并對之敏感的問題[5-6]，因此，所設計的控制器的性能有待優化[7-8]。

四旋翼飛行仿真器是一套多變量控制系統，用于模擬旋翼機在空中工作的姿態控制[9]。本文在該實驗平臺上添加配重以模擬四旋翼機攜帶重物，建立LQR和粒子群優化LQR姿態控制系統，分別做多組多種類的實時控制實驗，在實驗結果中取超調、響應時間以及穩態誤差做比較，從控制性能、魯棒性以及抗擾性3方面分析兩種控制系統的性能。

1 四旋翼飛行仿真器的建模和分析

1.1 系統的建模

四旋翼飛行仿真器實驗平臺的工作機制是由4個螺旋槳電機帶動扇葉轉動，由產生的反作用力提供動力，使得螺旋槳上升、下降或風力對螺旋左右擺動，使得仿真器實驗平臺在笛卡爾坐標系下進行3種姿態運動（俯仰、滾轉和偏航），如圖1所示。其中，Ff、Fl、Fr、Fb分別為某一時刻螺旋槳電機轉動所產生的風力。

通過實驗裝置的具體結構進行姿態動力學分析。

建立基于笛卡爾坐標系的3個角度姿態量的平衡態力矩模型：

（1）假設在靜態平衡的情況下，系統的重心位于偏航軸，忽略摩擦以及旋翼自身阻尼的力矩；

（2）忽略旋翼到達所需轉速需要的時間；

（3）設定逆時針轉為正方向，合外力矩方程（M=J*α）和力矩平衡公式（M=F*L）；

（4）定義繞OY軸向上運動的方向為俯仰角P正方向，機體處于水平狀態時的俯仰角度為0，此時只分析其在OY軸的轉動，則滾轉角為0。做俯仰軸的受力分析，如圖2所示。

推導得俯仰角的力矩平衡方程，如式（1）所示：其中，JP——俯仰軸轉動慣量；

Ff——前向螺旋槳扇葉轉動產生風力的反作用力；

Lf——前向螺旋槳轉動產生風力的反作用力的力臂；

Fl——左向螺旋槳扇葉轉動產生風力的反作用力；

Lc——左、右向螺旋槳轉動產生風力的反作用力的力臂；

Fr——右向螺旋槳扇葉轉動產生風力的反作用力。

做滾轉軸受力分析，如圖3所示。

推導得滾轉角的力矩平衡方程，如公式（2）所示：

其中，JR——滾轉軸轉動慣量；

La——左、右向螺旋槳轉動產生風力的反作用力的力臂。同理，推得偏航角的力矩平衡方程，如公式（3）所示：

其中，JY——偏航軸轉動慣量；

Fb——后向螺旋槳扇葉轉動產生風力的反作用力；

Lb——后向螺旋槳轉動產生風力的反作用力的力臂。

在公式（1）～（3）里，對一些參量做計算，如公式（4）～（6）所示：

其中，V——電機電壓；

Kfc——偏電壓/升力（牛頓/伏特），經過檢測，值為15.0 N/V。

Lf為0.5 m，JP、JR、JY計算后的值分別為0.9112 kg*m2、0.4050 kg*m2、1.3066 kg*m2。

將參數代入，可得儀器的近似數學模型，如式（7）～（9）所示：

其中，Vf——前向電機的電壓；

Vr——右向電機的電壓；

Vl——左向電機的電壓。

其中，Vb——后向電機的電壓。

將系統分為偏航、俯仰及滾轉3個通道去考慮。

2 控制系統的分析與設計

實際中，控制器需快速對各種變化作出反應，減小對工作狀態的影響，使系統快速回至穩定。

2.1 線性二次型最優控制系統設計

2.1.1 理論分析

LQR控制有2個特點：其狀態方程是線性的；系統的泛函數是由狀態變量和輸入變量組成的二次類型。該控制方法的最優解可以經過解析形式表達成狀態變量的線性函數[10]。

假設線性系統的空間狀態描述方程為：

其中，x(t)——系統的狀態變量；

y(t)——系統的輸出變量；

u(t)——系統的輸入變量；

A——系統狀態變量矩陣的系數矩陣；

B——系統輸入變量矩陣的系數矩陣。

由原理，需尋找狀態反饋律：u=-k*x，使指標J最小化，如公式（11）所示：

其中，J——LQR控制算法里的指標函數；

Q——指標函數J中對應狀態量的矩陣；

R——指標函數J中對應控制量的矩陣。

Q是正定或半正定對稱矩陣，R是正定對稱矩陣。Q陣所反映的是對狀態過渡過程性能的要求，R陣所反映的是對控制能量的限制。構建出Hamilton函數，如公式（12）所示：

在輸入u(t)不受約束下，求H最小值，求導并令其值為0，如公式（13）所示：

λ可由式（15）求出：

其中，P——中間變量。

2.1.2 系統搭建

LQR最優控制可由狀態反饋實現閉環最優，適用于時變系統，穩態誤差小，魯棒性較好，且對于線性、定常、具備強耦合的多變量控制系統具有好的控制效果。選用9狀態量的LQR的控制方法設計控制模塊，將控制變量與狀態變量結合成的二次型積分函數當作指標函數，最后確立最佳控制輸入，使性能指標取極小值，以設計得到恰到好處的最優的控制器。

由LQR最優控制方法求出全狀態反饋增益量K，具備較好的魯棒性。

對于LQR最優控制而言，要選取合適的兩個加權矩陣Q和R。Q矩陣為diag(q11q22q33q44q55q66q77q88q99)，由其各主對角元素影響控制器的整體性能，在調試時，要根據實際需求在輸入和輸出之間尋找一個平衡處，從而實現最優控制。

適當選擇加權矩陣Q與R，做到在控制信號與輸出性能間實現恰到好處的平衡。多次實驗后，發現取

R=diag(1 1 1 1)，Q=diag(10 0.01 0.1 10 0.01 0.5 0.1 1 1)的效果最好。

結合MATLAB的函數lqr2()，求得符合系統性能要求的K陣。

結合LQR方法的理論和上述公式，可以由MATLAB求出K，如公式（18）所示：

四旋翼飛行仿真器儀器平臺結合MATLAB/Simulink進行實時控制，結合實時工具箱搭建控制系統進行控制任務的實現。在Simulink中搭建的9狀態的LQR控制系統如圖4所示。

圖4中的Real time control模塊是實時控制子模塊，負責采集編碼器數據以及輸出控制信號。LQR controller是控制器，控制器的內部子模塊所示的4個支路分別連接實時控制模塊的4個電機，即先把求出的K值分別帶入4個子模塊，之后整個控制器將前面的9個狀態量轉換成4個電壓量傳遞給電機，同時也控制實驗儀器的4個電機帶動所連接螺旋槳進行合適的轉動，產生風力，控制3個姿態軸的姿態。

2.2 粒子群算法優化LQR控制系統設計

2.2.1 控制理論分析

群智能算法中的粒子群算法是通過無質量的粒子模擬鳥群。每個粒子都有速度（移動的快慢）與位置（移動的方向）。

該算法的優點是實現簡單且無需大量調參，在控制系統科研領域有廣泛應用[11]。涉及迭代更新，先初始化一群隨機粒子（即隨機解），通過迭代找到最優解。在每次迭代中，粒子要參考兩個極值（個體最優位置pbest(i)，全局最好位置gbest）去更新本身的兩個屬性。

粒子群算法的流程如下：

（1）初始化一個粒子群，包含群體規模、每個粒子單獨的位置xi和速度vi；

（2）計算得出每個粒子的適應值F(i)；

（3）分別將每個粒子當前的適應值F(i)和其個體極值（即其經歷的最優位置）pbest(i)的適應值F(pbest(i))作比較，如果前者小于后者，則用前者代替后者（即更新個體極值），除此之外，后者仍取當前值作為個體極值，作為粒子i當前的最優位置；

（4）分別將每個粒子更新后的個體極值的適應值和全局極值（即整個群體的最優位置適應數值）gbest的適應值F(gbest)進行比較，較小的位置更新為全局最優位；

（5）結合式（19）與（20）：

其中，rand1、rand2——[0,1]上的隨機數；

c1、c2——學習因子；

t——迭代的次數，和rand1、rand2一起更新粒子速度和位置；

Pbest(t)——當前迭代次數下的個體最優極值；

gbest(t)——當前迭代次數下的全局極值；

Xi(t)——當前迭代次數下的粒子i的位置；

Xi(t+1)——下次迭代后粒子i的位置；

Vi(t)——當前迭代次數下的粒子i的速度；

Vi(t+1)——下次迭代后粒子i的速度。

ω是非負慣性權重因子，取值大，則全局性尋優強，局部尋優弱；取值小，則全局尋優弱，局部尋優強；ω的取值由線性遞減權值取ω，即式（21）：

其中，Gk——最大迭代次數；

ωini——初始的慣性權值；

ωend——迭代到最大的代數的慣性權值。

典型的取值ωini為0.8，ωend為0.5，引入ω，性能有顯著完善。

2.2.2 系統搭建

LQR控制的核心是使目標函數值最小，根本在于Q和R矩陣的主對角線取值，用粒子群優化更具科學性，去調節Q和R矩陣的科學性強[12]，能求出使目標函數最小化的Q和R矩陣的主對角線值。

結合模型，搭建LQR控制器。經粒子群優化LQR控制離線仿真，設定30個粒子，迭代100次。因儀器啟動所需時間，則優化得出的最優解經多次調整，結果如圖5～7所示，分別為Q矩陣的主對角線的9個數值，R矩陣的主對角線的4個數值以及粒子群優化LQR算法的目標函數變化曲線。

當迭代到100次時，MATLAB得出13個最優解（即Q和R矩陣的總共13個主對角線元素值），用粒子群最優解的形式表達為：x(1)=63.430092，

x(2)=0.100000，x(3)=0.001000，x(4)=41.352183，x(5)=0.100000，x(6)=0.001000，x(7)=10.000000，x(8)=0.100000，x(9)=0.000100，x(10)=0.100000，x(11)=0.100000，x(12)=0.100000，x(13)=2.16771。目標函數值減小至1.8637。由圖7可知，在粒子群不斷更新Q和R矩陣時，目標函數值變小，證明粒子群優化LQR科學有效。由LQR求K矩陣，如式（22）：

將K矩陣帶入LQR控制器，進行后續實驗。

3 半實物仿真實驗

3.1 儀器

3.1.1 儀器選用

現實中的旋翼系統非線性、多變量、強耦合，為研究姿態控制，選用四旋翼飛行仿真器作研究對象進行半實物仿真實驗。

3.1.2 配重添加

旋翼機在現實作業中往往需要攜帶藥品（如田間撒藥）、相機（航拍）等任務所需物品。

為滿足其復雜應用性而不損害儀器且初狀態平衡，將3瓶瓶裝水分別掛在旋翼機的左、前、右3個翅膀上。設所掛配重質量分別為M前、M左、M右，M左=M右，且左右兩旋翼的所掛配重同力臂，則橫向實現力矩平衡不再做分析，只需分析縱向。據測算，左右旋翼和后向軸的夾角均為60°，則左右兩旋翼作用到后向軸的力臂L后均為左（或右）旋翼力臂L左（或）L右×cos60°。設前向軸的力臂為L前，則：L前×M前×g=L左×M左×g+L右×M右×g，令所掛瓶裝水均為500 g，力臂均為34 cm。

3.2 實驗

通過MATLAB的Simulink，結合儀器，根據LQR控制、粒子群優化LQR進行半實物仿真實驗。記錄實驗結果，以俯仰姿態為例，對比分析兩控制系統的性能。

3.2.1 控制性能檢測

分別對兩系統做階躍響應實驗。要合理檢測控制性能，則設置系統的給定姿態角[3, 3.5, 10]，觀察平臺從初狀態到給定姿態的階躍響應。兩組控制系統姿態曲線對比如圖8所示。

對系統分別采取LQR控制與粒子群優化LQR控制時，俯仰及滾轉兩個方向中，兩系統的超調控制在1°以內，調節時間均小于2 s，穩態精度高，在0.7°以內。偏航方向，LQR控制系統的超調量不超過4°，調節時間為3 s左右，穩態精度很高；粒子群優化LQR控制系統的超調量不超過0.8°，調節時間在3 s左右，穩態精度高。

3.2.2 魯棒性檢測

為驗證設計的控制系統的魯棒性，使配重水的質量可變。將瓶裝水和旋翼仿真器視為一個整體，既可以模擬魯棒性檢驗中內在因素，即實驗儀器的質量變化，又可以模擬實際作業中旋翼機所帶工作重物發生變化的工作狀態（如旋翼機在田地間播撒農藥時，農藥的質量在不斷減小）。

給定姿態角[3, 3.5, 0.4]，3瓶水的質量同時從500 g開始，以相同的速度逐漸流出，分別對兩系統做實驗，記錄結果，兩系統的姿態曲線對比如圖9所示。

俯仰及滾轉兩方向中，兩系統的超調均非常小，調節時間較短，穩態精度高，在1.5°以內。偏航方向中，LQR控制系統的超調不超過2°，調節時間一般，穩態精度高；粒子群優化LQR控制系統的超調不超過1°，調節時間一般，震蕩小，穩態精度高。

3.2.3 抗擾性檢測

實際飛控中，會受外界不可控因素干擾。既然無法從源頭遏止干擾，那就在外界因素產生的干擾來臨后，令系統迅速做出反應，飛行器快速變回穩定且最大程度接近于受擾前的狀態。

3.2.3.1 抗常規干擾性能檢測

常規干擾指有一定規律和相干性的干擾，這類干擾可恒定不變，也可規律性變化。

在Simulink里，對兩種系統的電控模塊的4個電極上分別加干擾。VF前加全程正弦波干擾，幅值：5，頻率：0.5*2*pi；VR前加全程白噪聲干擾，能量：[0.2]，sample：0.1；VL和VB前加全程階躍信號干擾，Step time：20。設給定姿態[3, 3.5,40]，記錄結果，兩系統的姿態曲線對比如圖10所示。

俯仰及滾轉兩方向中，兩系統震蕩小，穩態精度高，不超過2°。偏航方向中，LQR控制系統超調不超8°，調節時間不超過5 s，震蕩小，穩態精度高；粒子群優化LQR控制系統超調不超過5°，調節時間不超過4 s，震蕩小，穩態精度高。

3.2.3.2 抗突變類干擾性能檢測

突變干擾是指變化無規律性的干擾類型。為了能夠全面且科學地研究系統的抗擾性能，做抗軟件信號類突變擾動實驗和抗外部碰撞類突變擾動實驗。

3.2.3.2.1 抗軟件信號類突變抗擾性能

在Simulink里對兩系統的4個電極分別加干擾：VF、VR、VL、VB均加隨機噪聲，最小值和最大值分別是-5和5，Sample time：0.1。設給定姿態角[-0.9,1.4, 10]，兩系統的姿態曲線對比如圖11所示。

俯仰及滾轉兩方向中，兩系統穩態精度高，不超過2°。偏航方向中，LQR控制的超調不超過3°，調節時間不超過4 s，震蕩小，穩態精度高；粒子群優化LQR控制的超調不超過2°，調節時間不超過4 s，震蕩小，穩態精度高。

3.2.3.2.2 抗外部碰撞類突變抗擾性能

旋翼機在實際中會遇到不可控的突變式碰撞干擾，如旋翼機在田間飛行播灑農藥時被高粱稈或者鳥碰撞干擾，或在危險環境中拍攝受到樹枝碰撞干擾等。

在儀器運行的實驗過程中，用手向實驗儀器的旋翼臂施加推力。設給定姿態[-1, 1.4, 10]，觀察實驗平臺從初始狀態到給定姿態的階躍響應。兩系統分別在14 s時施加推力（使3個姿態角均變化）。兩系統的姿態曲線對比如圖12所示。

3個姿態方向：兩系統受干擾后，在1.3 s內恢復受擾前的工作狀態，穩態精度高，保持在1°以內，震蕩小。

4 結果分析

由本實驗的四旋翼飛行仿真器的建模仿真和3種實驗驗證，可做出如下分析：

（1）由動態響應實驗可知，本文設計的LQR控制器和粒子群優化LQR控制器可以達到要求，系統超調量小，調節時間短，響應速度快，穩定性高，證明控制器有效；

（2）相較于同類研究成果，本文對于實驗儀器進行合理改造，通過水量逐漸減少的實驗，既可以實現魯棒性檢驗的要求，又能模擬旋翼機在飛行中攜帶重物量不斷減少的工作情況，具備實驗的科學及全面性。由該魯棒性實驗可知，設計的兩種控制系統具備良好的魯棒性，且本文通過粒子群優化LQR控制算法，使得LQR參數調優效率更高、更準確。通過實驗豐富地研究了抗擾性；

（3）相較于同類研究成果，本文借助特定實驗儀器平臺，引入了較復雜的常規性干擾實驗和突變式干擾實驗，后者還分為軟件信號類干擾和外部碰撞類干擾，對于抗擾性的分析更具備全面性和科學性。由抗擾實驗可知，本文設計的兩種控制系統對于常規干擾和突變干擾均可有效抵抗，其中，粒子群優化LQR控制的抗擾效果較優。兩種控制系統在受到干擾后，均能快速調整旋翼姿態使其恢復期望的平衡態；

（4）本文設計的兩種控制器的參數調試很重要，無論是LQR控制的加權矩陣參數調節，還是粒子群算法進行公式參數調整再結合粒子數和迭代次數調節，最終影響Q和R矩陣，合適的參數可以提高系統的響應速度和精度。粒子群優化LQR算法適應性強，比人工試湊參數的方法效率高，科學性強，而且控制效果不弱于同類研究成果中使用粒子群優化PID控制的性能。

5 結束語

本文先由動力學分析對四旋翼飛行仿真器控制系統進行建模，設計出合理選擇加權矩陣的LQR控制器和粒子群優化LQR控制器，用MATLAB/Simulink建立四旋翼飛行仿真器系統。之后從控制性能、魯棒性以及抗擾性3個角度結合半實物實時控制仿真去檢測控制系統的多方面性能，實驗結果表明，本文設計的控制系統響應快，到達穩態值所需時間短，超調量小，在所攜帶的重物質量逐漸減小的情況下，控制系統響應結果較為穩定地保持在期望值附近，魯棒性強；在遇到內外部多種復雜的常規和突變式干擾時，系統反應迅速，能較快地恢復到受擾前的工作狀態并保持較好的穩定，抵抗干擾能力強。

綜上所述，本文設計出的合理選擇加權矩陣的LQR控制器和粒子群優化LQR控制器，作為最優控制以及與智能控制的有效結合，對像四旋翼飛行仿真器這樣的多變量控制系統具有較好的控制性、魯棒性和抗擾性。以上兩套控制方法的有效運用也能為其他類似的工程控制系統提供理論和參考價值。