基于隨機(jī)模型預(yù)測(cè)控制的四旋翼無(wú)人機(jī)研究

李?yuàn)檴?龔 薇

(四川大學(xué)電氣信息學(xué)院 四川 成都 610065)

0 引 言

近年來(lái)，由于科學(xué)技術(shù)的加速發(fā)展，無(wú)人駕駛飛行器的應(yīng)用越來(lái)越廣泛，如搜索救援任務(wù)[1-2]、火災(zāi)監(jiān)測(cè)[3]、邊境巡邏[4]、作物監(jiān)測(cè)[5]和基礎(chǔ)設(shè)施檢查[6-8]等。而四旋翼飛行器憑借其具有懸浮、高機(jī)動(dòng)性和簡(jiǎn)單設(shè)計(jì)的特性，成為最常用的飛行器之一。然而，盡管它們具有巨大的應(yīng)用潛力和操作自主性，但從技術(shù)演示到實(shí)際應(yīng)用的轉(zhuǎn)變卻是一個(gè)緩慢的過(guò)程。造成這一重大差距的原因在于無(wú)人機(jī)飛行器實(shí)際操作中通常會(huì)受到干擾的影響，為了保證飛行可行性，如何處理干擾是無(wú)人機(jī)飛行器的首要考慮之一，也是目前的重大挑戰(zhàn)之一。

在過(guò)去的幾十年中，已經(jīng)發(fā)表了許多關(guān)于四旋翼動(dòng)態(tài)建模的文章[9-11]。Mistler等[12]首次將反饋線性化方法應(yīng)用于四旋翼飛行跟蹤參考軌跡。Bouabdallah等[13]比較了兩種基于模型的直升機(jī)穩(wěn)定性控制技術(shù)的性能:一種采用簡(jiǎn)化動(dòng)力學(xué)的經(jīng)典PID方法和一種基于更完整模型的現(xiàn)代LQR技術(shù)。Castillo等[14-15]給定四旋翼的位置，并應(yīng)用嵌套飽和控制來(lái)穩(wěn)定其姿態(tài)。Xu等[16]提出了一種滑模控制器來(lái)穩(wěn)定一類(lèi)級(jí)聯(lián)欠驅(qū)動(dòng)系統(tǒng)。另一種常見(jiàn)的控制方法是逆步進(jìn)技術(shù)[17]。

然而近幾年計(jì)算機(jī)計(jì)算能力的增強(qiáng)，使得先進(jìn)的控制器成為可能。在這種情況下出現(xiàn)的一個(gè)突出的控制策略是模型預(yù)測(cè)控制(MPC)?；厮莸?0世紀(jì)70年代末，模型預(yù)測(cè)控制在最初的構(gòu)想是控制化學(xué)工業(yè)過(guò)程[18]，它通常具有緩慢的動(dòng)力學(xué)和以秒或分鐘計(jì)算的采樣時(shí)間。由于當(dāng)今的高性能處理器，MPC已經(jīng)被應(yīng)用于四旋翼飛行器中的快速系統(tǒng)。2013年Subbarao等[19]根據(jù)一個(gè)四旋翼的線性化模型建立了一個(gè)MPC模型。建立的控制器可以跟隨軌跡，同時(shí)能處理對(duì)飛機(jī)方向施加的限制(滾軸和俯仰角)。但這種方法無(wú)法處理系統(tǒng)中存在的不確定擾動(dòng)，一種常用的處理擾動(dòng)的方法為魯棒模型預(yù)測(cè)控制，但這種方法可能由于過(guò)度保守而無(wú)法解出可行解。一種自然而然的想法便是放寬約束，使約束在一定概率內(nèi)滿足，這種考慮導(dǎo)致了隨機(jī)模型預(yù)測(cè)控制(SMPC)的出現(xiàn)，該方法可以使系統(tǒng)在滿足約束與控制性能之間進(jìn)行權(quán)衡，這導(dǎo)致系統(tǒng)控制性不再那么保守。

實(shí)際上，在無(wú)人機(jī)飛行中，可以找到多個(gè)擾動(dòng)源，如位置或速度測(cè)量誤差、電機(jī)速度失調(diào)，甚至大氣阻力，以及一些未知分布的隨機(jī)擾動(dòng)。本文在只知道擾動(dòng)一階矩及二階矩的情況下，提出一種SMPC控制方案，實(shí)現(xiàn)在一定程度上滿足約束。擾動(dòng)的處理采用分布式魯棒方法求出其模糊集，再針對(duì)軟化后的單輸入約束與聯(lián)合狀態(tài)約束進(jìn)行分別處理。前者采用最壞情況下的條件風(fēng)險(xiǎn)值約束進(jìn)行等價(jià)轉(zhuǎn)換，然后運(yùn)用定理等價(jià)為可計(jì)算的半定規(guī)劃(SDP)凸優(yōu)化約束。后者采用布爾不等式將聯(lián)合約束轉(zhuǎn)換為多個(gè)單狀態(tài)約束，再運(yùn)用凱特利切比雪夫不等式將非凸約束轉(zhuǎn)換為凸約束。計(jì)算過(guò)程中采用了在線擾動(dòng)反饋，該方法可解決狀態(tài)反饋存在的非凸問(wèn)題。針對(duì)該算法，本文在末尾進(jìn)行了凸優(yōu)化可計(jì)算分析以及可行性穩(wěn)定性分析，證實(shí)了該算法的可計(jì)算性、可行性與穩(wěn)定性。

1 四旋翼無(wú)人機(jī)的模型和問(wèn)題描述

1.1 四旋翼無(wú)人機(jī)的線性模型

在本節(jié)中，我們通過(guò)轉(zhuǎn)換地面坐標(biāo)系原點(diǎn)與機(jī)體坐標(biāo)系原點(diǎn)對(duì)四旋翼無(wú)人機(jī)建立力學(xué)研究坐標(biāo)系(如圖1所示)，并建立了四旋翼無(wú)人機(jī)的六自由度動(dòng)力學(xué)模型，將其應(yīng)用于控制器的設(shè)計(jì)中。基于結(jié)構(gòu)建立的模型可以顯著提高設(shè)計(jì)和測(cè)試效率，對(duì)控制系統(tǒng)的設(shè)計(jì)和性能分析具有重要的意義。

圖1 四旋翼無(wú)人機(jī)坐標(biāo)系統(tǒng)

通常，四旋翼無(wú)人機(jī)被建模為一個(gè)具有6個(gè)自由度[x,y,z,φ,θ,φ]的剛體，其中：x、y、z分別表示笛卡爾坐標(biāo)系中的狀態(tài)；φ、θ、φ分別表示俯仰角、旋轉(zhuǎn)角與偏航角。

式中：b和d分別表示推力系數(shù)和阻力系數(shù)。則其非線性動(dòng)力學(xué)模型可以由歐拉-拉格朗日方程[13-14]推導(dǎo)得。

假設(shè)無(wú)人機(jī)在近似懸停狀態(tài)時(shí)，該動(dòng)力學(xué)模型可以由一個(gè)線性的模型來(lái)近似，在工作平衡點(diǎn)附近使用一階泰勒展開(kāi)[20]，最終轉(zhuǎn)化成線性離散狀態(tài)空間模型為:

(1)

1.2 約束條件

如上所述，四旋翼無(wú)人機(jī)的動(dòng)力學(xué)模型有6個(gè)輸出[x,y,z,φ,θ,φ]，四個(gè)獨(dú)立的輸入[Ω1,Ω2,Ω3,Ω4]。由于四旋翼的欠驅(qū)動(dòng)性質(zhì)，在[x,y,z,φ]達(dá)到理想位置時(shí)，只需[φ,θ]保持穩(wěn)定[21]。因此需要限制它們的振幅。這些約束能被表示為:

φmin≤φ≤φmax

θmin<θ≤θmax

這些約束可由線性不等式約束集合定義的凸區(qū)域Fx定義為如下形式：

(2)

在實(shí)際飛行中，馬達(dá)轉(zhuǎn)速也會(huì)受到電壓的限制，則控制輸入受到約束，可表示為:

u≤umax

同理，控制輸入可表示為如下線性不等式描述的凸可行區(qū)域Fu:

Fu={u|Hu≤h}

(3)

其中，系數(shù)H與h定義如下：

2 模型預(yù)測(cè)控制算法的重構(gòu)

2.1 模型預(yù)測(cè)控制的系統(tǒng)模型

在有限時(shí)間的最優(yōu)控制中，假設(shè)模型是從t到t+N求解優(yōu)化問(wèn)題，其中N為預(yù)測(cè)視界。定義狀態(tài)、輸入、擾動(dòng)與輸出的緊湊模式如下：

(4)

則式(1)可轉(zhuǎn)換為如下緊湊形式：

(5)

式中：C∈RNq×Nn，主對(duì)角線值為C，除主對(duì)角線外均為0；A、B、D和G可參考文獻(xiàn)[22]，文獻(xiàn)中展示了詳細(xì)推導(dǎo)過(guò)程。

同理，由定義(4)可將式(2)與式(3)轉(zhuǎn)換為如下緊湊模式:

式中：ai∈R(N+1)q，bj∈R，H∈RNu×Nm,h∈RNm。由于擾動(dòng)的分布未知且無(wú)界，不能保證約束總是能得到滿足，因此可允許一定概率違反約束。在這種情況下，通過(guò)考慮系統(tǒng)中存在不確定性的所有可能性，可在系統(tǒng)性能與約束滿足之間進(jìn)行權(quán)衡。將Fx與Fu替換為如下概率約束：

P(x∈Fx)≥1-δx

(6)

P(u∈Fu)≥1-δu

(7)

式中：δu∈(0,1)、δx∈(0，1)分別表示輸入與輸出最大的違反概率。對(duì)于線性問(wèn)題，常定義如下二次函數(shù)為目標(biāo)函數(shù)：

(8)

(A+BK)TQN(A+BK)-QN+Q+KTRK=0

故重構(gòu)后的四旋翼無(wú)人機(jī)的目標(biāo)跟蹤問(wèn)題P1如下：

minVN(xt,u,w)

(9)

s.t.x=Axt+Bu+DGw

y=Cx

P(x∈Fx)≥1-δx

P(u∈Fu)≥1-δu

2.2 擾動(dòng)反饋重構(gòu)

由于控制輸入u應(yīng)該是具有當(dāng)前和過(guò)去狀態(tài)的某種反饋控制策略，故常采用反饋結(jié)構(gòu)定義控制策略u(píng)。以往反饋形式常采用狀態(tài)反饋參數(shù)化，但預(yù)測(cè)的輸入序列和狀態(tài)序列是狀態(tài)反饋增益序列的非線性函數(shù)，故一般情況下，可行決策變量集是非凸的。另一種可計(jì)算的凸優(yōu)化反饋控制策略為放射擾動(dòng)反饋參數(shù)化[24]。

(10)

式中：Mi,j∈Rm×n，vi∈Rm。在相關(guān)文獻(xiàn)中，這種擾動(dòng)反饋控制策略被證明與狀態(tài)反饋控制策略是等價(jià)的[24]。同理，式(10)可以表示為如下緊湊形式：

u=MGw+v

式中：M∈RNm×Nn，v∈RNm，其具體形式與推導(dǎo)過(guò)程可參考文獻(xiàn)[24]。將反饋結(jié)構(gòu)加入P1，可轉(zhuǎn)換為如下P2問(wèn)題:

minVN(xt,u,w)

s.t.P(HMGw+Hv-h≤0)≥1-δu

1-δx

3 約束的重構(gòu)

3.1 擾動(dòng)的重構(gòu)

式中：E[P][·]表示在分布下P的期望，μ=1N?μ0，Σ=IN?Σ0，μ0為擾動(dòng)均值，Σ0為擾動(dòng)方差,?表示克羅內(nèi)克積，分別定義為E[ωi]=μ0，Σ[ωt]=Σ0且均已知。

在分布P未知的情況下，通常式(7)的可行集為非凸的，有時(shí)甚至是不連通的。對(duì)概率分布中不確定性處理的一種常見(jiàn)方法是采用分布魯棒方法[25]，定義分布魯棒的輸入機(jī)會(huì)約束與聯(lián)合狀態(tài)約束如下：

處理后的輸入與狀態(tài)約束再分別采用合適的方法進(jìn)行精確凸重構(gòu)。

3.2 輸入約束的凸重構(gòu)

眾所周知，魯棒的單機(jī)會(huì)約束可以被最壞情況下的條件風(fēng)險(xiǎn)值約束保守地近似，且在文獻(xiàn)[25]推理2.1中，證明了當(dāng)原硬約束為擾動(dòng)的仿射函數(shù)時(shí)，該近似是等價(jià)的。故上述單輸入約束可等價(jià)為如下最壞情況下的條件風(fēng)險(xiǎn)值約束：

P-CVaRδu(Hv-h+HMGw)=

式中：P-CVaRδu(a)為δu概率下滿足條件a的概率。并且根據(jù)文獻(xiàn)[25]，可將單輸入最壞情況下的條件風(fēng)險(xiǎn)值約束等價(jià)為如下可計(jì)算的SDP約束：

其中：trace(a)為求矩陣a的跡；N為優(yōu)化的變量。

SDP模型在計(jì)算時(shí)通常比較復(fù)雜。SOCP模型是SDP模型的特例，但變量更少，算法效率更高，故將上述SDP約束轉(zhuǎn)換為如下SOCP約束的形式，提高計(jì)算效率[26]：

(11)

3.3 聯(lián)合狀態(tài)約束的隨機(jī)模型預(yù)測(cè)控制重構(gòu)

由于聯(lián)合狀態(tài)約束需要在不確定性分布上求解多元積分，導(dǎo)致其求解過(guò)程十分棘手，且通常為非凸約束。為了得到一個(gè)近似可處理形式，常采用布爾不等式進(jìn)行轉(zhuǎn)換。首先，需要將狀態(tài)魯棒機(jī)會(huì)轉(zhuǎn)換為上確界形式。

再應(yīng)用布爾不等式得到如下轉(zhuǎn)換：

式中:δxx=δx/l為輸出最大違反概率δx的l次均值分配，這種固定風(fēng)險(xiǎn)分配可避免優(yōu)化算法的非凸性。轉(zhuǎn)換后的分布式魯棒聯(lián)合狀態(tài)約束可轉(zhuǎn)換為如下形式的保守逼近：

(12)

運(yùn)用坎特利凱特利切比雪夫不等式，可將式(12)固定風(fēng)險(xiǎn)分配后的狀態(tài)約束轉(zhuǎn)換為如下可計(jì)算的凸優(yōu)化形式，近似文獻(xiàn)[27]引理4.1。

在本文中，由P2可知l=2。單輸入約束與聯(lián)合狀態(tài)約束經(jīng)過(guò)一系列轉(zhuǎn)換，均近似成可處理的凸約束。重構(gòu)后的P2問(wèn)題可以轉(zhuǎn)換為如下的P3問(wèn)題：

minVN(xt,u,w)

(13)

4 系統(tǒng)性能分析

4.1 系統(tǒng)的可計(jì)算分析

顯然，在P4問(wèn)題中，目標(biāo)函數(shù)與單輸入約束都是凸的線性約束，但聯(lián)合輸出約束中，由于Σ[x]的存在，使得狀態(tài)約束在CVX包中不可計(jì)算，故將其進(jìn)行如下轉(zhuǎn)換：

ζi=BMGΣGTMTBT+DGΣGTDT+

BMGΣGTDT+DGΣTGTMTBT

此外，為了保證在任意狀態(tài)輸入的情況下，均存在可行解，可在目標(biāo)函數(shù)中引入了松弛變量，結(jié)合式(11)與式(12),P3問(wèn)題可以轉(zhuǎn)換為如下P4問(wèn)題：

mint

s.t.VN(xt,M,v,w)≤t

4.2 可行性與穩(wěn)定性

因?yàn)闊o(wú)界擾動(dòng)的存在，使?fàn)顟B(tài)保持在硬約束系統(tǒng)的可行域中是不切實(shí)際的。本文通過(guò)軟化約束以及加入松弛變量等方法解決了該問(wèn)題。只有當(dāng)違反約束時(shí)，松弛變量才是非零的，其余情況松弛變量則保持為零。這種精確罰函數(shù)方法可以使軟化約束的解集與原硬約束的解集等價(jià)，即針對(duì)一個(gè)初始狀態(tài)，原硬約束問(wèn)題不可行時(shí)，新構(gòu)建的軟約束問(wèn)題卻可以產(chǎn)生一個(gè)可行的終端解集，且該解集與原硬約束問(wèn)題可行時(shí)解出的解集是相同的，而這種思想保證了系統(tǒng)的可行性。

當(dāng)矩陣A為舒爾穩(wěn)定時(shí)，文獻(xiàn)[27]已證明其穩(wěn)定性，接下來(lái)我們證明當(dāng)A為李雅普諾夫穩(wěn)定時(shí)系統(tǒng)的穩(wěn)定性。

系統(tǒng)李雅普諾夫穩(wěn)定時(shí)，存在一個(gè)坐標(biāo)變化，使系統(tǒng)分解為如下形式：

式中:A1為舒爾穩(wěn)定部分，A2特征根在單位圓上，具有相同的代數(shù)和幾何多重性。假設(shè)A2為對(duì)矩陣角，且主對(duì)角線上元素要么為±1，要么為2×2的旋轉(zhuǎn)矩陣，因此A2為正交矩陣。

對(duì)于任意ε>0，考慮如下“負(fù)漂移條件”：

此外，不難得到以下關(guān)系成立：

由于控制與擾動(dòng)的一階矩均有界，故存在常數(shù)M滿足:

5 仿 真

在本節(jié)中，為測(cè)試幾種控制策略對(duì)軌跡跟蹤問(wèn)題的性能，進(jìn)行了如下數(shù)值模擬。仿真中使用的模型參數(shù)如表1所示。

表1 四旋翼無(wú)人機(jī)的參數(shù)

無(wú)人機(jī)到達(dá)目標(biāo)點(diǎn)的順序參考如下：

(0,0,0)→(0,0,5)→(5,0,5)→(5,10,5)→

(-5,10,5)→(-5,0,5)→(5,0,10)

考慮初始狀態(tài)為[x,y,z]=[0,0,0]，角度狀態(tài)為[φ,θ,φ]=[0,0,0]，采樣時(shí)間τs=50 ms,懲罰權(quán)重Q=10I12，R=0.01I4。為了加強(qiáng)飛行的穩(wěn)定性，分別對(duì)姿態(tài)和輸入進(jìn)行了約束，姿態(tài)約束為：φmin=-0.5,φmax=0.5,θmin=-0.5,θmax=0.5輸入約束為：umax=[250,250,250,250]，則仿真結(jié)果如圖2-圖4所示。

圖2 無(wú)人機(jī)在不同擾動(dòng)下的位置變化

圖3 無(wú)人機(jī)在不同擾動(dòng)下θ的穩(wěn)定情況

圖4 無(wú)人機(jī)在不同擾動(dòng)下Φ的穩(wěn)定情況

由圖2可以看出，在不同擾動(dòng)相同步數(shù)的情況下，系統(tǒng)仍然可以實(shí)時(shí)有效跟蹤，但隨著擾動(dòng)的增大，距離規(guī)定點(diǎn)的誤差越大。圖3-圖4顯示不同擾動(dòng)下俯仰角Φ與旋轉(zhuǎn)角θ均可保持穩(wěn)定。

圖5-圖8展示了MPC[21]與SMPC性能對(duì)比。圖5顯示在相同步數(shù)的情況下，SMPC不僅能克服隨機(jī)擾動(dòng)，而且在反饋的作用下反應(yīng)速度更快，比無(wú)擾動(dòng)的MPC更快到達(dá)每一步的終點(diǎn)。圖6-圖8顯示SMPC算法能夠在滿足系統(tǒng)約束的情況下克服隨機(jī)擾動(dòng)實(shí)現(xiàn)目標(biāo)跟蹤，且消耗燃料與無(wú)約束的MPC算法相差無(wú)幾。

圖5 兩種算法下的位置變化

(a) MPC

(b) SMPC圖6 兩種算法下θ的穩(wěn)定情況

(a) MPC

(b) SMPC圖7 兩種算法下Φ的穩(wěn)定情況

(a) MPC

(b) SMPC圖8 兩種算法下的輸入

圖9展示了不同預(yù)測(cè)步數(shù)對(duì)SMPC算法的影響，可以看出，當(dāng)增大SMPC的預(yù)測(cè)步數(shù)時(shí)，相應(yīng)速度也可以增加得更快，穩(wěn)定性也會(huì)更好。圖10展示了不同輸入對(duì)SMPC算法的影響，可以看出，當(dāng)初始狀態(tài)不同時(shí)，系統(tǒng)均可解出優(yōu)化解完成路徑跟蹤。

圖9 不同預(yù)測(cè)步數(shù)下的位置變化

圖10 不同初始狀態(tài)下的位置變化

6 結(jié) 語(yǔ)

針對(duì)四旋翼直升機(jī)的穩(wěn)定性和航跡跟蹤問(wèn)題，本文提出一種隨機(jī)模型預(yù)測(cè)控制器。在利用隨機(jī)模型預(yù)測(cè)技術(shù)進(jìn)行四旋翼無(wú)人機(jī)控制的相關(guān)工作中，該方案的主要優(yōu)點(diǎn)在于：1) 具有處理系統(tǒng)不確定性擾動(dòng)的能力；2) 考慮擾動(dòng)反饋下的輸入約束與聯(lián)合狀態(tài)約束；3) 運(yùn)用近似等價(jià)的方法對(duì)軟輸入約束與聯(lián)合狀態(tài)約束進(jìn)行凸重構(gòu)；4) 運(yùn)用機(jī)會(huì)約束與松弛變量保證了系統(tǒng)在不同初始狀態(tài)下均有解集，且與原硬輸入解集相同。在仿真實(shí)驗(yàn)中，我們將SMPC的性能與MPC方法進(jìn)行了比較，結(jié)果顯示本文方法能夠更好地處理擾動(dòng)且具備更優(yōu)秀的預(yù)測(cè)性能。