基于模型預測控制的四旋翼無人機雙閉環控制

楊舉 鄭建立

摘?要：針對欠驅動非線性的四旋翼無人機軌跡跟蹤問題，本文提出使用模型預測控制方法來解決小型四旋翼無人機的位置航向控制問題。在建立無人機動力學模型的基礎上，采用雙閉環路控制結構，對外環（位置控制環），MPC控制器旨在跟蹤輸入的參考軌跡；對內環（姿態控制環），為了實現對姿態的快速響應，將姿態子系統化為LPV表示，提出LPVMPC控制器使用二次規劃求解。通過仿真驗證了所提出的控制方法的有效性。

關鍵詞：四旋翼無人機；模型預測控制；軌跡跟蹤；線性變參數系統

現如今，四旋翼無人機無論是在軍用還是在民用領域，四旋翼無人機都被廣泛使用［12］。相比于大型無人機，四旋翼無人機價格低廉、操作簡單、便于維修。四旋翼無人機系統具有六個姿態狀態、四個電機，為典型的欠驅動、非線性、強耦合的系統［3］。

四旋翼無人機的飛行控制一直是無人機研究的重點［4］，內蒙古科技大學的江杰［5］使用準LPV法對四旋翼無人機系統建立了線性空間方程，設計了PID控制器；南京航空航天大學的周杰［6］設計線性擴張狀態觀測器對四旋翼外部陣風干擾進行估計，基于四旋翼無人機模型信息建立了模型預測控制器，實驗結果表明算法有效且滿足實際工程需要；武漢科技大學的程歡在他的碩士論文中，提出了外環模型預測控制（MPC）方法以實現位置跟蹤，內環（姿態環）采用PID控制器以實現姿態跟蹤，達到良好的控制效果［7］；武漢科技大學的楊艷華［8］擴展狀態卡爾曼濾波估計了風力擾動，通過反饋將姿態動力學模型線性化，設計了ESKFMPC控制器；華東交通大學的許雪松［9］針對四旋翼無人機帶負載時，負載重量、轉動慣量不確定性的問題，提出了自適應載荷補償的模糊自抗擾控制系統；華僑大學的林哲［10］在內環姿態外環位置控制中，利用模糊擴張狀態觀測器，對外部總擾動在線估計后設計非奇異快速終端滑模控制器，得到較好的控制效果。

本文提出了一種基于模型預測控制的雙閉環軌跡跟蹤控制方法。首先，建立一個四旋翼無人機的動態模型，在外環設計了基于MPC控制器對四旋翼位置的控制；其次對四旋翼無人機模型按準IPV方法變換，設計了基于MPClPV的姿態控制內環。最后，在MATLAB中進行仿真，位置和姿態軌跡跟蹤控制的仿真結果證明了所提方法的有效性。

1?四旋翼無人機動力學數學模型

本文假設飛行器為剛體且質量為常數，飛行器幾何外形對稱，且內部質量對稱。并且地面坐標系為慣性坐標系，重力加速度不隨飛行高度而變化。定義四旋翼無人機的歐拉角θ?φ?ψ，其中θ定義為俯仰角，角度范圍為-π2<θ<π2；φ定義為橫滾角，角度范圍-π2<φ<π2；ψ定義為偏航角，角度范圍為-π2<ψ<π2。

根據四旋翼無人機的機理原理，可由牛頓歐拉方程得到：

mζ¨=F

Iη=-η×Iη+τ（1）

式（1）中，m為四旋翼無人機的質量，ξ=xyzT為四旋翼無人機在大地坐標系的位置，F為四旋翼無人機所受合力；I=diagIxIyIz代表慣性張量對角矩陣，η=pqrT代表四旋翼無人機的機體角速度，τ為四旋翼無人機所受合力矩。

四旋翼無人機旋翼產生的總升力Uf1為：

Uf1=F1+F2+F3+F4（2）

其中Fi代表四旋翼無人機所受第i個旋翼所產生向上的力，大小與轉速的平方成正比。為了便于控制，飛行的狀態被分為四個：上下，左右，前后，偏航。上下控制變量為U1，當U1大于機體重力時，四旋翼無人機向上運動，反之則向下。與上下飛行狀態類似，其他三個變量定義如下：

U2=lF4-F2=λlω24-ω22

U3=lF3-F1=λlω23-ω21

U4=μω24-ω23+ω22-ω21（3）

式（3）中λ為升力系數，μ為扭力系數，l為無人機質心到旋翼中心的距離，ω2i為第i旋翼轉速的平方值。

四旋翼無人機在飛行中產生的陀螺力矩表達式為：

τg=Ipη×0?0?ωTT（4）

其中，Ip為四旋翼無人機的轉動慣量，ωT為旋翼的相對轉速。ωT可寫為：

ωT=ω4-ω3+ω2-ω1（5）

最終可以得到四旋翼無人機的合外力矩，整體表達式如下：

F=REB∑4i=1FTi-m00gT

τ=-τg+τT（6）

忽略四旋翼無人機機體和旋翼的陀螺效應，可得四旋翼無人機的一般動力學模型如下［11］：

x¨=cosψsinθcosφ+sinψsinφU1m

y¨=sinψsinθcosφ-cosψsinφU1m

z¨=cosθcosφU1m-g

φ¨=Iy-IzIxψθ-IpIxθωT+U2Ix

θ¨=Iz-IxIyψφ+IpIyφωT+U3Iy

θ¨=Ix-IyIzθφ+U4Iz（7）

2?四旋翼無人機控制器設計

本文采用雙環模型預測控制方案來進行四旋翼無人機的軌跡跟蹤控制，其中，內環為姿態控制器，外環為位置控制器。首先根據參考軌跡和位置控制器求出U1，將U1代入求解出另外兩個位置控制變量uxr和uyr；然后逆向求解歐拉角得到三個參考角ψr、φr、θr。再由姿態控制器求出U2、U3、U4；最后將U1、U2、U3、U4代入四旋翼無人機模型中，將四旋翼實際位置、姿態狀態變量，反饋到各自的控制器，從而形成雙閉環控制系統。

2.1?外環位置控制器設計

在此我們假設四旋翼無人機模型是一種線性定常離散時間的狀態空間模型，虛擬參考狀態空間模型在沒有外部干擾，并且四旋翼無人機高度穩定的情況下，得出：

U1r=m·z¨r+g，uxr=x¨r·mU1r，uyr=y¨r·mU1r（8）

fξk+1=A-k·fξk+B-k·u～ξ（k）（9）

式（9）中矩陣A-k表示與fξk相關的雅克比式矩陣，矩陣B-k表示與uξk相關的雅克比式矩陣，將其分為高度誤差模型和x，y誤差模型。

四旋翼無人機高度誤差模型為：

fξzk+1=Azk·fξzk+Bzk·u～ξz（k）（10）

式（10）中，Az=1Δt

01，Bz=0

Δtmcosθkcosφk，Δt為采樣時間。

針對高度位置控制，控制輸入U1將通過下列的二次規劃問題求得［7］：

minJz=［f^ξz-f^ξrz］TQz［f^ξz-f^ξrz］+［u～^ξz-u～^ξz］TRz［u～^ξz-u～^ξz］

stf^ξz-Pk|kf^ξzk|k-Hzk|ku～^ξz=0

u～^ξzi

公式（11）中Qz和Rz是定義的對角加權矩陣。因此，U1k可通過下式求得：

U1k=u～^ξzk|k+U1rk（12）

X、Y軸的控制將高度Z軸控制的U1當作已知量，進而轉化為分別求和ux和uy的變量函數。四旋翼無人機x，y方向的位置誤差模型采用如下式：

fξxyk+1=Axyk·fξxyk+Bxyk·u～ξxy（k）（13）

由式（13）得到x，y方向的移動誤差模型對應的矩陣Axy和Bxy分別為：

Axy=1Δt00

0100

001Δt

0001和Bxy=00

ΔtmU1k0

00

0ΔtmU1k（14）

式（14）中Δt是采樣時間。針對下x，y方向的位置控制輸入，參照高度位置控制方式可以簡單得出。

2.2?內環位置控制器設計

四旋翼無人機姿態控制內環采用LPVMPC控制器。從位置控制器中接收的參考值θrφrψr與無人機開環模型反饋的實際值θφψ，通過LPVMPC姿態控制器計算，得到U2U3U4。在應用MPC控制策略之前，有必要將無人機的非線性模型轉換為LPV格式。由于MPC控制器僅處理上述三個角度，因此將重新排列為LPV格式。如果四旋翼無人機穩定在接近懸停位置，則φ和θ角可視為零角度。

該方程組現在將用于MPC控制器設計。值得注意的是，姿態控制的內環四倍比位置—速度控制外環更快。在MPC策略中，使用的預測范圍為5個樣本，姿態MPC成本函數公式的二次規劃問題與高度控制相似［12］。

3?仿真結果

四旋翼無人機飛行參考軌跡的跟蹤仿真結果如圖3、圖4和圖5所示。圖3是一個三維軌跡跟蹤；圖4分別顯示了x、y和z方向的路徑跟蹤；圖5分別顯示了姿態角度的良好跟蹤。如圖3、圖4和圖5所示，四旋翼無人機通過使用MPC雙閉環控制方法可以在飛行中實現良好的路徑跟蹤。圖6、圖7、圖8展示了不同軌跡下的四旋翼無人機飛行參考軌跡的跟蹤仿真結果，圖6是一個三維軌跡跟蹤；圖7分別顯示了x、y和z方向的路徑跟蹤；圖8分別顯示了姿態角度的良好跟蹤。

結語

基于以上分析，級聯式雙閉環模型預測控制可以實現四旋翼無人機飛行中的路徑跟蹤，取得非常好的效果，無人機的六個自由度都以非常小的誤差進行跟蹤。本文提出將模型預測雙閉環控制方法用于四旋翼無人機軌跡規劃的控制，將姿態控制視為獨立的問題，使用模型預測雙閉環控制結構。對內環姿態控制，使用將lPV系統的MPC控制問題轉化為QP優化問題，從使用成熟的數值算法在線求解；對外環位置控制，使用MPC算法使實際位置軌跡對參考軌跡實現良好跟蹤。通過仿真實驗驗證了本方法的合理性。

參考文獻：

［1］丁承君，尹李亮，段萍.應用于植保無人機的定位數據修正方法設計［J］.傳感器與微系統，2018，37（7）：9295.

［2］Gupte?S，Mohandas?P?I?T，Conrad?J?M.A?survey?of?quadrotor?unmanned?aerial?vehicles［C］.Proceedings?of?Southeastcon.IEEE，2012：16.

［3］Zulu?A，John?S.A?review?of?control?algorithms?for?autonomous?quadrotors［J］.Open?Journal?of?Applied?Sciences，2014，04（14）：547556.

［4］Xu?M，Zhang?R，Chen?L，et?al.Key?technology?analysis?and?research?progress?of?UAV?intelligent?plant?protection［J］.Smart?Agriculture，2019，1（2）：2033.

［5］江杰，豈偉楠.四旋翼飛行器建模與PID控制器設計［J］.電子設計工程，2013，21（23）：147150.

［6］周杰，王彪，唐超穎.基于狀態空間模型預測算法的四旋翼無人機飛行控制［J］.信息與控制，2018，47（2）：149155.

［7］程歡.基于模型預測控制的四旋翼無人機路徑跟蹤控制研究［D］.武漢科技大學，2018，5.

［8］楊艷華，呂童，柴利.基于ESKFMPC的四旋翼無人機軌跡跟蹤控制［J/OL］.電子測量與儀器學報：18［20220830］.http：//kns.cnki.net/kcms/detail/11.2488.TN.20220610.1736.008.html.

［9］徐雪松，吳儀政，倪赟.變載荷四旋翼無人機的自適應模糊自抗擾控制［J］.傳感器與微系統，2022，41（07）：101105+109.

［10］林哲，李平.采用模糊擴張狀態觀測器的四旋翼無人機滑模軌跡跟蹤控制［J/OL］.華僑大學學報（自然科學版）：19［20220830］.http：//kns.cnki.net/kcms/detail/35.1079.N.20220629.1156.002.html.

［11］彭川.基于高斯過程模型預測控制的四旋翼無人機軌跡跟蹤控制［D］.武漢科技大學，2021，5.

［12］Mark?Schmidt.Deriving?the?Gradient?and?Hessian?of?Linear?and?Quadratic?Functions?in?Matrix?Notation.February?6，2019.

［13］MATLAB，“Quadraticprograming?@ONLINE，”［Online］.Available：https：//es.mathworks.com/help/optim/ug/quadprog.html？lang=en.

作者簡介：楊舉（1997—?），男，漢族，河北邯鄲人，研究生在讀，研究方向：四旋翼無人機先進控制方法、逆變器的先進控制方法。