四旋翼飛行仿真器的姿態(tài)控制

高偉翔

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

0 前言

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

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

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

1.1 系統(tǒng)的建模

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

通過實驗裝置的具體結(jié)構進行姿態(tài)動力學分析。

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

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

（2）忽略旋翼到達所需轉(zhuǎn)速需要的時間；

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

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

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

Ff——前向螺旋槳扇葉轉(zhuǎn)動產(chǎn)生風力的反作用力；

Lf——前向螺旋槳轉(zhuǎn)動產(chǎn)生風力的反作用力的力臂；

Fl——左向螺旋槳扇葉轉(zhuǎn)動產(chǎn)生風力的反作用力；

Lc——左、右向螺旋槳轉(zhuǎn)動產(chǎn)生風力的反作用力的力臂；

Fr——右向螺旋槳扇葉轉(zhuǎn)動產(chǎn)生風力的反作用力。

做滾轉(zhuǎn)軸受力分析，如圖3所示。

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

其中，JR——滾轉(zhuǎn)軸轉(zhuǎn)動慣量；

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

其中，JY——偏航軸轉(zhuǎn)動慣量；

Fb——后向螺旋槳扇葉轉(zhuǎn)動產(chǎn)生風力的反作用力；

Lb——后向螺旋槳轉(zhuǎn)動產(chǎn)生風力的反作用力的力臂。

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

其中，V——電機電壓；

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

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

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

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

Vr——右向電機的電壓；

Vl——左向電機的電壓。

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

將系統(tǒng)分為偏航、俯仰及滾轉(zhuǎn)3個通道去考慮。

2 控制系統(tǒng)的分析與設計

實際中，控制器需快速對各種變化作出反應，減小對工作狀態(tài)的影響，使系統(tǒng)快速回至穩(wěn)定。

2.1 線性二次型最優(yōu)控制系統(tǒng)設計

2.1.1 理論分析

LQR控制有2個特點：其狀態(tài)方程是線性的；系統(tǒng)的泛函數(shù)是由狀態(tài)變量和輸入變量組成的二次類型。該控制方法的最優(yōu)解可以經(jīng)過解析形式表達成狀態(tài)變量的線性函數(shù)[10]。

假設線性系統(tǒng)的空間狀態(tài)描述方程為：

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

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

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

A——系統(tǒng)狀態(tài)變量矩陣的系數(shù)矩陣；

B——系統(tǒng)輸入變量矩陣的系數(shù)矩陣。

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

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

Q——指標函數(shù)J中對應狀態(tài)量的矩陣；

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

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

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

λ可由式（15）求出：

其中，P——中間變量。

2.1.2 系統(tǒng)搭建

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

由LQR最優(yōu)控制方法求出全狀態(tài)反饋增益量K，具備較好的魯棒性。

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

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

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

結(jié)合MATLAB的函數(shù)lqr2()，求得符合系統(tǒng)性能要求的K陣。

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

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

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

2.2 粒子群算法優(yōu)化LQR控制系統(tǒng)設計

2.2.1 控制理論分析

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

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

粒子群算法的流程如下：

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

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

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

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

（5）結(jié)合式（19）與（20）：

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

c1、c2——學習因子；

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

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

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

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

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

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

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

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

其中，Gk——最大迭代次數(shù)；

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

ωend——迭代到最大的代數(shù)的慣性權值。

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

2.2.2 系統(tǒng)搭建

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

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

當?shù)?00次時，MATLAB得出13個最優(yōu)解（即Q和R矩陣的總共13個主對角線元素值），用粒子群最優(yōu)解的形式表達為：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。目標函數(shù)值減小至1.8637。由圖7可知，在粒子群不斷更新Q和R矩陣時，目標函數(shù)值變小，證明粒子群優(yōu)化LQR科學有效。由LQR求K矩陣，如式（22）：

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

3 半實物仿真實驗

3.1 儀器

3.1.1 儀器選用

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

3.1.2 配重添加

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

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

3.2 實驗

通過MATLAB的Simulink，結(jié)合儀器，根據(jù)LQR控制、粒子群優(yōu)化LQR進行半實物仿真實驗。記錄實驗結(jié)果，以俯仰姿態(tài)為例，對比分析兩控制系統(tǒng)的性能。

3.2.1 控制性能檢測

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

對系統(tǒng)分別采取LQR控制與粒子群優(yōu)化LQR控制時，俯仰及滾轉(zhuǎn)兩個方向中，兩系統(tǒng)的超調(diào)控制在1°以內(nèi)，調(diào)節(jié)時間均小于2 s，穩(wěn)態(tài)精度高，在0.7°以內(nèi)。偏航方向，LQR控制系統(tǒng)的超調(diào)量不超過4°，調(diào)節(jié)時間為3 s左右，穩(wěn)態(tài)精度很高；粒子群優(yōu)化LQR控制系統(tǒng)的超調(diào)量不超過0.8°，調(diào)節(jié)時間在3 s左右，穩(wěn)態(tài)精度高。

3.2.2 魯棒性檢測

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

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

俯仰及滾轉(zhuǎn)兩方向中，兩系統(tǒng)的超調(diào)均非常小，調(diào)節(jié)時間較短，穩(wěn)態(tài)精度高，在1.5°以內(nèi)。偏航方向中，LQR控制系統(tǒng)的超調(diào)不超過2°，調(diào)節(jié)時間一般，穩(wěn)態(tài)精度高；粒子群優(yōu)化LQR控制系統(tǒng)的超調(diào)不超過1°，調(diào)節(jié)時間一般，震蕩小，穩(wěn)態(tài)精度高。

3.2.3 抗擾性檢測

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

3.2.3.1 抗常規(guī)干擾性能檢測

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

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

俯仰及滾轉(zhuǎn)兩方向中，兩系統(tǒng)震蕩小，穩(wěn)態(tài)精度高，不超過2°。偏航方向中，LQR控制系統(tǒng)超調(diào)不超8°，調(diào)節(jié)時間不超過5 s，震蕩小，穩(wěn)態(tài)精度高；粒子群優(yōu)化LQR控制系統(tǒng)超調(diào)不超過5°，調(diào)節(jié)時間不超過4 s，震蕩小，穩(wěn)態(tài)精度高。

3.2.3.2 抗突變類干擾性能檢測

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

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

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

俯仰及滾轉(zhuǎn)兩方向中，兩系統(tǒng)穩(wěn)態(tài)精度高，不超過2°。偏航方向中，LQR控制的超調(diào)不超過3°，調(diào)節(jié)時間不超過4 s，震蕩小，穩(wěn)態(tài)精度高；粒子群優(yōu)化LQR控制的超調(diào)不超過2°，調(diào)節(jié)時間不超過4 s，震蕩小，穩(wěn)態(tài)精度高。

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

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

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

3個姿態(tài)方向：兩系統(tǒng)受干擾后，在1.3 s內(nèi)恢復受擾前的工作狀態(tài)，穩(wěn)態(tài)精度高，保持在1°以內(nèi)，震蕩小。

4 結(jié)果分析

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

（1）由動態(tài)響應實驗可知，本文設計的LQR控制器和粒子群優(yōu)化LQR控制器可以達到要求，系統(tǒng)超調(diào)量小，調(diào)節(jié)時間短，響應速度快，穩(wěn)定性高，證明控制器有效；

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

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

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

5 結(jié)束語

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

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