基于模型預(yù)測(cè)控制的四旋翼無(wú)人機(jī)雙閉環(huán)控制

楊舉 鄭建立

摘?要：針對(duì)欠驅(qū)動(dòng)非線性的四旋翼無(wú)人機(jī)軌跡跟蹤問(wèn)題，本文提出使用模型預(yù)測(cè)控制方法來(lái)解決小型四旋翼無(wú)人機(jī)的位置航向控制問(wèn)題。在建立無(wú)人機(jī)動(dòng)力學(xué)模型的基礎(chǔ)上，采用雙閉環(huán)路控制結(jié)構(gòu)，對(duì)外環(huán)（位置控制環(huán)），MPC控制器旨在跟蹤輸入的參考軌跡；對(duì)內(nèi)環(huán)（姿態(tài)控制環(huán)），為了實(shí)現(xiàn)對(duì)姿態(tài)的快速響應(yīng)，將姿態(tài)子系統(tǒng)化為L(zhǎng)PV表示，提出LPVMPC控制器使用二次規(guī)劃求解。通過(guò)仿真驗(yàn)證了所提出的控制方法的有效性。

關(guān)鍵詞：四旋翼無(wú)人機(jī)；模型預(yù)測(cè)控制；軌跡跟蹤；線性變參數(shù)系統(tǒng)

現(xiàn)如今，四旋翼無(wú)人機(jī)無(wú)論是在軍用還是在民用領(lǐng)域，四旋翼無(wú)人機(jī)都被廣泛使用［12］。相比于大型無(wú)人機(jī)，四旋翼無(wú)人機(jī)價(jià)格低廉、操作簡(jiǎn)單、便于維修。四旋翼無(wú)人機(jī)系統(tǒng)具有六個(gè)姿態(tài)狀態(tài)、四個(gè)電機(jī)，為典型的欠驅(qū)動(dòng)、非線性、強(qiáng)耦合的系統(tǒng)［3］。

四旋翼無(wú)人機(jī)的飛行控制一直是無(wú)人機(jī)研究的重點(diǎn)［4］，內(nèi)蒙古科技大學(xué)的江杰［5］使用準(zhǔn)LPV法對(duì)四旋翼無(wú)人機(jī)系統(tǒng)建立了線性空間方程，設(shè)計(jì)了PID控制器；南京航空航天大學(xué)的周杰［6］設(shè)計(jì)線性擴(kuò)張狀態(tài)觀測(cè)器對(duì)四旋翼外部陣風(fēng)干擾進(jìn)行估計(jì)，基于四旋翼無(wú)人機(jī)模型信息建立了模型預(yù)測(cè)控制器，實(shí)驗(yàn)結(jié)果表明算法有效且滿足實(shí)際工程需要；武漢科技大學(xué)的程歡在他的碩士論文中，提出了外環(huán)模型預(yù)測(cè)控制（MPC）方法以實(shí)現(xiàn)位置跟蹤，內(nèi)環(huán)（姿態(tài)環(huán)）采用PID控制器以實(shí)現(xiàn)姿態(tài)跟蹤，達(dá)到良好的控制效果［7］；武漢科技大學(xué)的楊艷華［8］擴(kuò)展?fàn)顟B(tài)卡爾曼濾波估計(jì)了風(fēng)力擾動(dòng)，通過(guò)反饋將姿態(tài)動(dòng)力學(xué)模型線性化，設(shè)計(jì)了ESKFMPC控制器；華東交通大學(xué)的許雪松［9］針對(duì)四旋翼無(wú)人機(jī)帶負(fù)載時(shí)，負(fù)載重量、轉(zhuǎn)動(dòng)慣量不確定性的問(wèn)題，提出了自適應(yīng)載荷補(bǔ)償?shù)哪：钥箶_控制系統(tǒng)；華僑大學(xué)的林哲［10］在內(nèi)環(huán)姿態(tài)外環(huán)位置控制中，利用模糊擴(kuò)張狀態(tài)觀測(cè)器，對(duì)外部總擾動(dòng)在線估計(jì)后設(shè)計(jì)非奇異快速終端滑?？刂破鳎玫捷^好的控制效果。

本文提出了一種基于模型預(yù)測(cè)控制的雙閉環(huán)軌跡跟蹤控制方法。首先，建立一個(gè)四旋翼無(wú)人機(jī)的動(dòng)態(tài)模型，在外環(huán)設(shè)計(jì)了基于MPC控制器對(duì)四旋翼位置的控制；其次對(duì)四旋翼無(wú)人機(jī)模型按準(zhǔn)IPV方法變換，設(shè)計(jì)了基于MPClPV的姿態(tài)控制內(nèi)環(huán)。最后，在MATLAB中進(jìn)行仿真，位置和姿態(tài)軌跡跟蹤控制的仿真結(jié)果證明了所提方法的有效性。

1?四旋翼無(wú)人機(jī)動(dòng)力學(xué)數(shù)學(xué)模型

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

根據(jù)四旋翼無(wú)人機(jī)的機(jī)理原理，可由牛頓歐拉方程得到：

mζ¨=F

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

式（1）中，m為四旋翼無(wú)人機(jī)的質(zhì)量，ξ=xyzT為四旋翼無(wú)人機(jī)在大地坐標(biāo)系的位置，F(xiàn)為四旋翼無(wú)人機(jī)所受合力；I=diagIxIyIz代表慣性張量對(duì)角矩陣，η=pqrT代表四旋翼無(wú)人機(jī)的機(jī)體角速度，τ為四旋翼無(wú)人機(jī)所受合力矩。

四旋翼無(wú)人機(jī)旋翼產(chǎn)生的總升力Uf1為：

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

其中Fi代表四旋翼無(wú)人機(jī)所受第i個(gè)旋翼所產(chǎn)生向上的力，大小與轉(zhuǎn)速的平方成正比。為了便于控制，飛行的狀態(tài)被分為四個(gè)：上下，左右，前后，偏航。上下控制變量為U1，當(dāng)U1大于機(jī)體重力時(shí)，四旋翼無(wú)人機(jī)向上運(yùn)動(dòng)，反之則向下。與上下飛行狀態(tài)類似，其他三個(gè)變量定義如下：

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

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

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

式（3）中λ為升力系數(shù)，μ為扭力系數(shù)，l為無(wú)人機(jī)質(zhì)心到旋翼中心的距離，ω2i為第i旋翼轉(zhuǎn)速的平方值。

四旋翼無(wú)人機(jī)在飛行中產(chǎn)生的陀螺力矩表達(dá)式為：

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

其中，Ip為四旋翼無(wú)人機(jī)的轉(zhuǎn)動(dòng)慣量，ωT為旋翼的相對(duì)轉(zhuǎn)速。ωT可寫為：

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

最終可以得到四旋翼無(wú)人機(jī)的合外力矩，整體表達(dá)式如下：

F=REB∑4i=1FTi-m00gT

τ=-τg+τT（6）

忽略四旋翼無(wú)人機(jī)機(jī)體和旋翼的陀螺效應(yīng)，可得四旋翼無(wú)人機(jī)的一般動(dòng)力學(xué)模型如下［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?四旋翼無(wú)人機(jī)控制器設(shè)計(jì)

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

2.1?外環(huán)位置控制器設(shè)計(jì)

在此我們假設(shè)四旋翼無(wú)人機(jī)模型是一種線性定常離散時(shí)間的狀態(tài)空間模型，虛擬參考狀態(tài)空間模型在沒(méi)有外部干擾，并且四旋翼無(wú)人機(jī)高度穩(wěn)定的情況下，得出：

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相關(guān)的雅克比式矩陣，矩陣B-k表示與uξk相關(guān)的雅克比式矩陣，將其分為高度誤差模型和x，y誤差模型。

四旋翼無(wú)人機(jī)高度誤差模型為：

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

式（10）中，Az=1Δt

01，Bz=0

Δtmcosθkcosφk，Δt為采樣時(shí)間。

針對(duì)高度位置控制，控制輸入U(xiǎn)1將通過(guò)下列的二次規(guī)劃問(wèn)題求得［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是定義的對(duì)角加權(quán)矩陣。因此，U1k可通過(guò)下式求得：

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

X、Y軸的控制將高度Z軸控制的U1當(dāng)作已知量，進(jìn)而轉(zhuǎn)化為分別求和ux和uy的變量函數(shù)。四旋翼無(wú)人機(jī)x，y方向的位置誤差模型采用如下式：

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

由式（13）得到x，y方向的移動(dòng)誤差模型對(duì)應(yīng)的矩陣Axy和Bxy分別為：

Axy=1Δt00

0100

001Δt

0001和Bxy=00

ΔtmU1k0

00

0ΔtmU1k（14）

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

2.2?內(nèi)環(huán)位置控制器設(shè)計(jì)

四旋翼無(wú)人機(jī)姿態(tài)控制內(nèi)環(huán)采用LPVMPC控制器。從位置控制器中接收的參考值θrφrψr與無(wú)人機(jī)開(kāi)環(huán)模型反饋的實(shí)際值θφψ，通過(guò)LPVMPC姿態(tài)控制器計(jì)算，得到U2U3U4。在應(yīng)用MPC控制策略之前，有必要將無(wú)人機(jī)的非線性模型轉(zhuǎn)換為L(zhǎng)PV格式。由于MPC控制器僅處理上述三個(gè)角度，因此將重新排列為L(zhǎng)PV格式。如果四旋翼無(wú)人機(jī)穩(wěn)定在接近懸停位置，則φ和θ角可視為零角度。

該方程組現(xiàn)在將用于MPC控制器設(shè)計(jì)。值得注意的是，姿態(tài)控制的內(nèi)環(huán)四倍比位置—速度控制外環(huán)更快。在MPC策略中，使用的預(yù)測(cè)范圍為5個(gè)樣本，姿態(tài)MPC成本函數(shù)公式的二次規(guī)劃問(wèn)題與高度控制相似［12］。

3?仿真結(jié)果

四旋翼無(wú)人機(jī)飛行參考軌跡的跟蹤仿真結(jié)果如圖3、圖4和圖5所示。圖3是一個(gè)三維軌跡跟蹤；圖4分別顯示了x、y和z方向的路徑跟蹤；圖5分別顯示了姿態(tài)角度的良好跟蹤。如圖3、圖4和圖5所示，四旋翼無(wú)人機(jī)通過(guò)使用MPC雙閉環(huán)控制方法可以在飛行中實(shí)現(xiàn)良好的路徑跟蹤。圖6、圖7、圖8展示了不同軌跡下的四旋翼無(wú)人機(jī)飛行參考軌跡的跟蹤仿真結(jié)果，圖6是一個(gè)三維軌跡跟蹤；圖7分別顯示了x、y和z方向的路徑跟蹤；圖8分別顯示了姿態(tài)角度的良好跟蹤。

結(jié)語(yǔ)

基于以上分析，級(jí)聯(lián)式雙閉環(huán)模型預(yù)測(cè)控制可以實(shí)現(xiàn)四旋翼無(wú)人機(jī)飛行中的路徑跟蹤，取得非常好的效果，無(wú)人機(jī)的六個(gè)自由度都以非常小的誤差進(jìn)行跟蹤。本文提出將模型預(yù)測(cè)雙閉環(huán)控制方法用于四旋翼無(wú)人機(jī)軌跡規(guī)劃的控制，將姿態(tài)控制視為獨(dú)立的問(wèn)題，使用模型預(yù)測(cè)雙閉環(huán)控制結(jié)構(gòu)。對(duì)內(nèi)環(huán)姿態(tài)控制，使用將lPV系統(tǒng)的MPC控制問(wèn)題轉(zhuǎn)化為QP優(yōu)化問(wèn)題，從使用成熟的數(shù)值算法在線求解；對(duì)外環(huán)位置控制，使用MPC算法使實(shí)際位置軌跡對(duì)參考軌跡實(shí)現(xiàn)良好跟蹤。通過(guò)仿真實(shí)驗(yàn)驗(yàn)證了本方法的合理性。

參考文獻(xiàn)：

［1］丁承君，尹李亮，段萍.應(yīng)用于植保無(wú)人機(jī)的定位數(shù)據(jù)修正方法設(shè)計(jì)［J］.傳感器與微系統(tǒng)，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控制器設(shè)計(jì)［J］.電子設(shè)計(jì)工程，2013，21（23）：147150.

［6］周杰，王彪，唐超穎.基于狀態(tài)空間模型預(yù)測(cè)算法的四旋翼無(wú)人機(jī)飛行控制［J］.信息與控制，2018，47（2）：149155.

［7］程歡.基于模型預(yù)測(cè)控制的四旋翼無(wú)人機(jī)路徑跟蹤控制研究［D］.武漢科技大學(xué)，2018，5.

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

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

［10］林哲，李平.采用模糊擴(kuò)張狀態(tài)觀測(cè)器的四旋翼無(wú)人機(jī)滑模軌跡跟蹤控制［J/OL］.華僑大學(xué)學(xué)報(bào)（自然科學(xué)版）：19［20220830］.http：//kns.cnki.net/kcms/detail/35.1079.N.20220629.1156.002.html.

［11］彭川.基于高斯過(guò)程模型預(yù)測(cè)控制的四旋翼無(wú)人機(jī)軌跡跟蹤控制［D］.武漢科技大學(xué)，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.

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