何信弟,趙超輪,戴邵武,鄭百東

(1.中國人民解放軍第4801工廠虎門軍械修理廠, 廣東 東莞 523938; 2.海軍航空大學(xué), 山東 煙臺 264000)

0 引言

低空突防任務(wù)是無人機(jī)在戰(zhàn)場中的重要任務(wù)之一,它是一個(gè)多變量、多約束、多目標(biāo)的復(fù)雜問題,直接處理起來難度較大?？砂凑辗謱舆f階結(jié)構(gòu)的思想將這一問題分解為軌跡規(guī)劃與跟蹤控制等2個(gè)子問題,從而簡化問題的求解難度。

無人機(jī)軌跡規(guī)劃問題可以抽象為在包含微分方程、代數(shù)方程和不等式等約束下求解泛函極值的開環(huán)最優(yōu)控制問題[1]。對于最優(yōu)控制方法,按照求解策略的不同,可以分為直接法和間接法。其中直接法的應(yīng)用更為廣泛,其思路是先采用參數(shù)化方法將連續(xù)最優(yōu)控制問題轉(zhuǎn)化為非線性規(guī)劃問題(nonlinear programming problem,NLP),然后利用非線性優(yōu)化算法求解最優(yōu)軌跡[2-4]。偽譜法是直接法中的一類典型代表,因其對初值不敏感、收斂速度快、求解精度高等優(yōu)點(diǎn)已被廣泛應(yīng)用于無人機(jī)軌跡規(guī)劃、導(dǎo)彈制導(dǎo)、航天器軌道機(jī)動(dòng)等最優(yōu)控制問題。目前發(fā)展較為完善的偽譜法包括Gauss偽譜法、Legendre偽譜法、Radau偽譜法。上述方法的主要區(qū)別是配置點(diǎn)不同,以及與此對應(yīng)的估計(jì)多項(xiàng)式不同[5-8]。劉鶴鳴等[9]以雷達(dá)威脅概率最小和攻擊時(shí)間最短為目標(biāo),在考慮動(dòng)態(tài)雷達(dá)散射截面的情況下,利用可變低階自適應(yīng)偽譜法實(shí)現(xiàn)了固定翼無人機(jī)的軌跡規(guī)劃。徐建龍等[10]以軌跡最短為目標(biāo),在考慮靜態(tài)障礙的情況下,利用hp自適應(yīng)Radau偽譜法實(shí)現(xiàn)了四旋翼無人機(jī)的軌跡優(yōu)化。宋曉晨等[11]以最小油耗和最小時(shí)間為目標(biāo),利用hp自適應(yīng)偽譜法求解超音速無人機(jī)的軌跡最優(yōu)化問題。雖然偽譜法能夠較好地處理軌跡規(guī)劃問題,但是該方法一般只能離線使用,且只能應(yīng)對已知威脅。

無人機(jī)軌跡跟蹤控制方法有很多,應(yīng)用最廣泛的是PID控制,此外還有滑模控制、反步控制、模型預(yù)測控制(model predictive control,MPC)等。其中模型預(yù)測控制具有顯式處理約束、反饋校正、滾動(dòng)優(yōu)化等特點(diǎn),其應(yīng)對不確定環(huán)境的能力更強(qiáng)[12]。因此,在具有未知障礙等威脅的環(huán)境中,MPC方法具有顯著的優(yōu)勢。Yeonsik Kang等[13]在二維平面內(nèi),利用固定翼無人機(jī)的運(yùn)動(dòng)學(xué)模型設(shè)計(jì)了非線性MPC控制器,通過在目標(biāo)函數(shù)中引入避障懲罰代價(jià),實(shí)現(xiàn)了無人機(jī)與靜態(tài)障礙物間的避碰。王懌等[14]在三維空間內(nèi),針對固定翼無人機(jī)設(shè)計(jì)了非線性MPC控制器,在避免無人機(jī)與未知靜態(tài)及動(dòng)態(tài)障礙物發(fā)生碰撞的情況下,實(shí)現(xiàn)了無人機(jī)沿參考軌跡飛行。其避障懲罰代價(jià)是在文獻(xiàn)[13]基礎(chǔ)上改進(jìn)的,文中考慮了障礙物的大小,使得無人機(jī)能夠及時(shí)避開大型障礙物,且文中采用了條件觸發(fā)式避障,即當(dāng)未知障礙與無人機(jī)的最小距離小于給定閾值時(shí),才在目標(biāo)函數(shù)中加入避障懲罰代價(jià)。但是在每次求解MPC優(yōu)化問題時(shí),文中將移動(dòng)障礙物暫且視作了靜止障礙物。這種設(shè)計(jì)雖然簡便,但是不足以應(yīng)對快速移動(dòng)的障礙物。

雖然針對無人機(jī)軌跡規(guī)劃、跟蹤控制2個(gè)子問題各自的研究成果很多,但是要實(shí)現(xiàn)一個(gè)具體的任務(wù),往往需要將2個(gè)問題及對應(yīng)的方法綜合統(tǒng)籌考慮。如何將2個(gè)問題有效銜接、2種方法取長補(bǔ)短以實(shí)現(xiàn)整體效益的最大化,仍是一個(gè)較為開放且值得研究的問題?？紤]到無人機(jī)平臺任務(wù)載荷性能有限、戰(zhàn)場環(huán)境中已知威脅和未知威脅并存的實(shí)際情況,為了充分利用現(xiàn)有資源、有效保證任務(wù)實(shí)時(shí)性,采用離線與在線相結(jié)合的工作方式是較為合適的選擇。

根據(jù)以上分析,以無人機(jī)執(zhí)行低空突防任務(wù)為背景,以四旋翼為控制對象,在考慮低空雷達(dá)、動(dòng)態(tài)障礙威脅的情況下,以無人機(jī)能夠在最短時(shí)間內(nèi)穿過威脅環(huán)境并到達(dá)指定位置為控制目標(biāo),將這一問題分解為離線軌跡規(guī)劃與軌跡跟蹤控制2個(gè)子問題來統(tǒng)籌處理。本文中所作的工作主要包括:

1) 針對離線軌跡規(guī)劃問題,在已知四旋翼初始狀態(tài)、目標(biāo)狀態(tài)、雷達(dá)信息的前提下,建立簡化的四旋翼動(dòng)力學(xué)模型、雷達(dá)威脅模型,考慮四旋翼平臺性能等約束,采用hp自適應(yīng)Radau偽譜法離線規(guī)劃出一條任務(wù)時(shí)間最短、被毀傷概率最小的可飛軌跡。

2) 針對軌跡跟蹤控制問題,以離線軌跡規(guī)劃階段得到的軌跡為參考軌跡,基于模型預(yù)測控制設(shè)計(jì)跟蹤控制器,在目標(biāo)函數(shù)中引入避障懲罰代價(jià),使四旋翼在跟蹤參考軌跡的同時(shí),完成在線避障。其中避障懲罰代價(jià)采用了條件觸發(fā)式設(shè)計(jì),并且在每次求解優(yōu)化問題時(shí)考慮了移動(dòng)障礙物的預(yù)測位置。

1 離線軌跡規(guī)劃

在離線軌跡規(guī)劃階段,僅考慮已知威脅。假設(shè)敵方雷達(dá)是主要威脅,雷達(dá)中心位于地面,其探測區(qū)域近似為半球,且我方已預(yù)知雷達(dá)位置及探測半徑。這里采用hp自適應(yīng)Radau偽譜法設(shè)計(jì)軌跡規(guī)劃器,設(shè)計(jì)過程如下。

1.1 數(shù)學(xué)模型

離線軌跡規(guī)劃問題一般可抽象為有限時(shí)間內(nèi)的最優(yōu)控制問題。即求解控制輸入U(xiǎn)(t),使Bolza型性能指標(biāo)函數(shù)(式(1))最小[15],即:

J(X(t),U(t))=Φ(X(t0),t0,X(tf),tf)+

(1)

且滿足狀態(tài)方程

(2)

邊界條件

Ψ(X(t0),t0,X(tf),tf)=0

(3)

路徑約束

C[X(t),U(t),t]≤0,t∈[t0,tf]

(4)

以下是具體的建模過程。

1.1.1四旋翼模型

以“X字型”四旋翼無人機(jī)作為研究對象,如圖1所示。建立2個(gè)右手坐標(biāo)系,即慣性北東地坐標(biāo)系與機(jī)體坐標(biāo)系,如圖2所示。其中,慣性坐標(biāo)系odxdydzd以地面上任意一點(diǎn)為坐標(biāo)原點(diǎn)od,odxd軸指向地理北,odzd軸垂直地面向下;機(jī)體坐標(biāo)系obxbybzb與四旋翼機(jī)體固連,以機(jī)體重心位置為坐標(biāo)原點(diǎn)ob,obxb軸在飛機(jī)對稱平面內(nèi)指向機(jī)頭方向;obzb軸在機(jī)體對稱平面內(nèi),垂直于obxb軸向下。

圖1 X字型四旋翼Fig.1 X-shaped quadrotor

圖2 慣性坐標(biāo)系與機(jī)體坐標(biāo)系Fig.2 Inertial coordinate system and body coordinate system

有關(guān)四旋翼無人機(jī)運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)建模的詳細(xì)介紹可參考文獻(xiàn)[16]。針對四旋翼軌跡規(guī)劃問題,采用簡化的數(shù)學(xué)模型,假設(shè)無人機(jī)拉力約等于重力,滾轉(zhuǎn)角、俯仰角都非常小,可得到高度通道與水平通道解耦的模型為:

(5)

式(5)中:(x,y,z)、(vx,vy,vz)分別為無人機(jī)在慣性坐標(biāo)系下的位置和速度,單位分別為m和m/s; (φ,θ,ψ)、(ωx,ωy,ωz)分別為無人機(jī)在慣性坐標(biāo)系下的姿態(tài)和旋轉(zhuǎn)角速度,單位分別為rad和rad/s;f為拉力;(τx,τy,τz)分別為滾轉(zhuǎn)、俯仰、偏航力矩,(Jx,Jy,Jz)為3個(gè)軸的轉(zhuǎn)動(dòng)慣量;m為無人機(jī)質(zhì)量;g為重力加速度。

選取狀態(tài)量、控制輸入量為:

X=(x,y,z,vx,vy,vz,φ,θ,ψ,ωx,ωy,ωz)T

(6)

U=(f,τx,τy,τz)T

(7)

1.1.2邊界條件及路徑約束

邊界條件包括無人機(jī)狀態(tài)量的初始條件與末端條件;路徑約束包括無人機(jī)運(yùn)動(dòng)過程中需滿足的狀態(tài)約束、控制輸入約束。具體如下:

1) 邊界條件。

初始條件:X(t0)=X0,末端條件:X(tf)=Xf,其中,t0為初始時(shí)刻,tf為末端時(shí)刻。

2) 路徑約束。

(8)

其中,位置約束體現(xiàn)了對無人機(jī)運(yùn)動(dòng)區(qū)域范圍的限制,速度、姿態(tài)、角速度、拉力、力矩約束主要體現(xiàn)了平臺自身性能的局限性。

注：軌跡規(guī)劃時(shí)既需要考慮無人機(jī)自身性能約束,也需要考慮軌跡跟蹤時(shí)的實(shí)際效果。需注意軌跡規(guī)劃時(shí)所采用的速度最大值應(yīng)小于跟蹤控制時(shí)的速度最大值,否則可能會出現(xiàn)跟蹤靜差。

1.1.3目標(biāo)函數(shù)

對于無人機(jī)執(zhí)行低空突防任務(wù)而言,無人機(jī)的生存能力是遂行任務(wù)的基礎(chǔ),它決定了無人機(jī)能否安全抵達(dá)指定位置。此外,由于戰(zhàn)場局勢瞬息萬變,攻擊的時(shí)效性往往決定了任務(wù)能否順利完成。因此,軌跡規(guī)劃階段包含2個(gè)目標(biāo):一是任務(wù)時(shí)間最短,二是被毀傷的概率最小。所設(shè)計(jì)的目標(biāo)函數(shù)為:

(9)

式(9)中:w1、w2為權(quán)重系數(shù);P(X(t))表示t時(shí)刻無人機(jī)被毀傷的概率。

無人機(jī)在執(zhí)行任務(wù)過程中一般會受到地形因素、防空火力、雷達(dá)探測等威脅,其中雷達(dá)探測威脅是影響無人機(jī)生存能力的重要因素。在軌跡規(guī)劃階段,這里將無人機(jī)被雷達(dá)探測到的概率近似為其被毀傷的概率。無人機(jī)在t時(shí)刻的雷達(dá)探測概率模型可近似為[17]:

(10)

對于由Nra個(gè)雷達(dá)構(gòu)成的聯(lián)合防空系統(tǒng),無人機(jī)瞬時(shí)被探測到的概率為:

1.2 基于hp自適應(yīng)Radau偽譜法的軌跡規(guī)劃算法

hp自適應(yīng)Radau偽譜法是Radau偽譜法的hp自適應(yīng)版本。Radau偽譜法采用Legendre-Gauss-Radau(LGR)配置點(diǎn),“hp自適應(yīng)”綜合了增階(p方法)和優(yōu)化網(wǎng)格數(shù)目(h方法)2個(gè)方面的優(yōu)勢,能夠根據(jù)網(wǎng)格的曲率和約束方程的誤差自動(dòng)地調(diào)整網(wǎng)格的數(shù)量和多項(xiàng)式的階,因此其能夠以較低的計(jì)算成本來提高算法的精度[9]。

hp自適應(yīng)Radau偽譜法的一般步驟可參見文獻(xiàn)[18]。可借助GPOPSⅡ軟件求解該非線性規(guī)劃問題。

由于通過上述偽譜法所得軌跡離散點(diǎn)的時(shí)間間隔并不均勻,而軌跡跟蹤階段所需的參考軌跡一般要求是均勻的,因此需要對所得軌跡的離散點(diǎn)進(jìn)行均勻采樣、插值處理,這里采用線性插值方法。

2 軌跡跟蹤控制

在軌跡跟蹤控制階段,考慮預(yù)先未知的動(dòng)態(tài)障礙物威脅。假設(shè)障礙物為球體,當(dāng)障礙物進(jìn)入無人機(jī)的探測范圍后,無人機(jī)能夠依靠機(jī)載探測設(shè)備實(shí)時(shí)獲取障礙物的大小、位置及速度。這里采用模型預(yù)測控制設(shè)計(jì)跟蹤控制器,設(shè)計(jì)過程如下。

2.1 數(shù)學(xué)模型

模型預(yù)測控制通常只取有限時(shí)域進(jìn)行優(yōu)化。在采樣時(shí)刻k,從系統(tǒng)當(dāng)前狀態(tài)x(k|k)出發(fā)求解預(yù)測時(shí)域長度為N的優(yōu)化問題,可表示為求解最優(yōu)控制輸入序列u(k+l|k), 0≤l≤N-1使目標(biāo)函數(shù)(式(12))最小,即:

(12)

且滿足:對于l=1,…,N-1,

狀態(tài)方程約束:

x(k+l+1|k)=f(x(k+l|k),u(k+l|k))

(13)

狀態(tài)、輸入約束:

x(k+l|k)∈X,u(k+l|k)∈U

(14)

初值約束:

x(k|k)=x(k)

(15)

注：目標(biāo)函數(shù)(12)一般是系統(tǒng)狀態(tài)與輸入的函數(shù),應(yīng)根據(jù)任務(wù)需求進(jìn)行相應(yīng)的設(shè)計(jì)。本文中目標(biāo)函數(shù)的具體設(shè)計(jì)見第2.1.3節(jié)。

2.1.1四旋翼模型

假設(shè)四旋翼無人機(jī)配備自動(dòng)駕駛儀,能夠自動(dòng)實(shí)現(xiàn)速度控制,即把速度指令輸入給自動(dòng)駕駛儀后,它能夠自動(dòng)控制四旋翼跟蹤給定的速度指令。基于此,無人機(jī)的運(yùn)動(dòng)模型可以近似描述為如下連續(xù)時(shí)間線性時(shí)不變形式[19],有:

(16)

(17)

式(17)的離散化描述為:

x(k+1)=Gx(k)+Hu(k)

(18)

2.1.2約束

狀態(tài)約束:

(19)

輸入約束:

(20)

2.1.3目標(biāo)函數(shù)

跟蹤控制階段要求無人機(jī)能夠在避障的前提下,跟蹤參考軌跡。因此,目標(biāo)函數(shù)設(shè)計(jì)為:

J(k)=Jtra(k)+JΔu(k)+Jobs(k)

(21)

式(21)中:Jtra(k)表示跟蹤參考軌跡的代價(jià);JΔu(k)表示控制輸入增量代價(jià),它可保證無人機(jī)的速度不會劇烈變化;Jobs(k)為避障懲罰代價(jià)。式(21)中前2項(xiàng)的具體形式如下:

(22)

(23)

考慮到無人機(jī)計(jì)算資源有限,且障礙物的存在不一定會給無人機(jī)帶來威脅,只有當(dāng)障礙物與無人機(jī)的距離較近時(shí),才有避障的需要。因此,采用條件觸發(fā)式避障。在給出避障懲罰代價(jià)的具體形式之前,首先引入與四旋翼避障有關(guān)的幾個(gè)半徑,如圖3所示。

圖3 與避障相關(guān)的幾個(gè)半徑Fig.3 Several radii related to obstacle avoidance

假設(shè)有Nobs個(gè)障礙物,用i表示障礙物的編號,i=1,2,…,Nobs。Roi為障礙物i的半徑。以四旋翼幾何中心為球心的3個(gè)半徑的關(guān)系為Rs

基于以上分析,設(shè)計(jì)避障懲罰代價(jià)為:

(24)

式(24)中,

(25)

式(25)中,

令poi(k+l|k)=poi(k)+voi(k)·Ts·l表示k時(shí)刻預(yù)測的k+l時(shí)刻障礙物i的位置,Ts為采樣時(shí)間,poi(k)、voi(k)分別為k時(shí)刻探測得到的障礙物位置和速度。注意k時(shí)刻的實(shí)際距離Doi(k)=Doi(k|k)。λ、σ為大于0的常數(shù)。

如式(25)所示,在k時(shí)刻,若當(dāng)前時(shí)刻無人機(jī)與障礙物間的距離Doi(k)≤Ra+Roi,則Jm(k)>0,即觸發(fā)避障功能。式(26)表征當(dāng)無人機(jī)與障礙物間的預(yù)測距離Doi(k+l|k)較遠(yuǎn),即未碰撞時(shí),兩者間的預(yù)測距離越小,避障代價(jià)越大;當(dāng)兩者間的預(yù)測距離小到已發(fā)生碰撞時(shí),避障代價(jià)取一個(gè)較大的常值。

注3：區(qū)別于文獻(xiàn)[14],式(26)中Doi(k+l|k)考慮了障礙物的預(yù)測位置poi(k+l|k),即使障礙物移動(dòng)速度較快,應(yīng)用該方法仍能有較好的避障效果。這里在預(yù)測障礙物的位置變化時(shí),采用了勻速假設(shè),這一預(yù)測方法較為簡單,適用于障礙物速度不變或者變化較慢的情形。對于速度變化較快的障礙物,應(yīng)采用更加符合目標(biāo)運(yùn)動(dòng)特性的狀態(tài)模型以及狀態(tài)估計(jì)算法。

2.2 基于MPC的跟蹤控制算法

通過上述分析與建模,在采樣時(shí)刻k,無人機(jī)從x(k|k)出發(fā)求解預(yù)測時(shí)域長度為N的優(yōu)化問題,可表示為:

(27)

對于l=1,…,N-1,

s.t.x(k+l+1|k)=Gx(k+l|k)+Hu(k+l|k),

x(k+l|k)∈X,u(k+l|k)∈U,

x(k|k)=x(k)

具體算法步驟如下:

步驟1：在k時(shí)刻,定義預(yù)測狀態(tài)的初始值為當(dāng)前時(shí)刻的實(shí)際狀態(tài)x(k|k)=x(k),利用機(jī)載探測設(shè)備探測環(huán)境中的障礙物i狀態(tài),并判斷無人機(jī)與障礙物i間的距離是否滿足避障功能觸發(fā)條件Doi(k)≤Ra+Roi,根據(jù)式(25)選擇Joi(k)的形式;

步驟2：求解優(yōu)化問題(27)得到最優(yōu)控制序列u*(k+l|k), 0≤l≤N-1;

步驟3：將最優(yōu)控制序列的首項(xiàng)作為實(shí)際控制輸入應(yīng)用于無人機(jī)中,即u(k)=u*(k|k);

步驟4：在k+1時(shí)刻,基于新的狀態(tài)測量值x(k+1),循環(huán)運(yùn)行步驟1—步驟4。

根據(jù)第1、2節(jié)的描述,給出四旋翼軌跡規(guī)劃與跟蹤控制的整體框圖,如圖4所示。其中自動(dòng)駕駛儀采用常見的PID控制方法實(shí)現(xiàn)速度控制。pRadau表示利用偽譜法直接求得的最優(yōu)軌跡,pr表示經(jīng)過均勻采樣、線性插值后得到的參考軌跡。

圖4 四旋翼軌跡規(guī)劃與跟蹤控制整體框圖Fig.4 Overall block diagram of the trajectory planning and tracking control of quadrotor

3 仿真結(jié)果及分析

3.1 參數(shù)設(shè)置

3.1.1軌跡規(guī)劃階段的參數(shù)

表1為四旋翼無人機(jī)參數(shù)表;表2為雷達(dá)參數(shù)表,包括2個(gè)雷達(dá);表3為路徑約束。

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

表2 雷達(dá)參數(shù)

表3 路徑約束

四旋翼無人機(jī)的邊界條件:

X(t0)=(0,0,-100,0,0,0,0,0,0,0,0,0)T

X(tf)=(0,900,-100,0,0,0,0,0,0,0,0,0)T

即無人機(jī)初始位置為(0,0,-100)T,目標(biāo)位置為(0,900,-100)T,因?yàn)檫x用北東地坐標(biāo)系,所以z軸的值為負(fù)值時(shí)表示在地面以上。目標(biāo)函數(shù)中的權(quán)重系數(shù)分別為w1=1,w2=1。

3.1.2軌跡跟蹤控制階段的參數(shù)

設(shè)置四旋翼的控制增益lv=3,安全半徑Rs=0.8 m,避障觸發(fā)半徑Ra=20 m,目標(biāo)函數(shù)中的權(quán)重矩陣Q=diag(1,1,1),S=diag(1,1,1)。采樣時(shí)間為0.5 s,假設(shè)無人機(jī)自身實(shí)際位置更新頻率為2 Hz。

表4為無人機(jī)狀態(tài)、輸入約束,注意軌跡跟蹤控制階段的速度上限設(shè)置值應(yīng)比軌跡規(guī)劃階段的設(shè)置值略大,否則無人機(jī)可能無法跟蹤上參考軌跡。

表4 狀態(tài)、輸入約束

設(shè)置了2個(gè)勻速運(yùn)動(dòng)的動(dòng)態(tài)障礙,其中動(dòng)態(tài)障礙1的半徑為Ro1=1.5 m,初始位置為(-17,2,-108)Tm,速度為(0,8,0)Tm/s;動(dòng)態(tài)障礙2的半徑為Ro2=2 m,初始位置為(0,102,-112)Tm,速度為(-4,0,0)Tm/s。避障懲罰代價(jià)中的參數(shù)λ=500,σ=2。

3.2 結(jié)果及分析

圖5是利用hp自適應(yīng)Radau偽譜法直接求得的3個(gè)方向的位置變化曲線,無人機(jī)到達(dá)指定目標(biāo)點(diǎn)所需飛行時(shí)間約為91.47 s。從圖5中可以看出,位置曲線是較為平滑的,但是從第20～26 s的局部放大圖中可見,所規(guī)劃的軌跡點(diǎn)是不均勻的,因此需要進(jìn)行均勻化處理。圖6為經(jīng)過均勻采樣、線性插值后得到的位置變化曲線,采樣時(shí)間為0.5 s。從第20～26 s的局部放大圖中可見,此時(shí)軌跡點(diǎn)已均勻。

圖5 利用偽譜法得到的位置變化曲線Fig.5 Curves of position obtained by pseudospectral method

圖6 均勻采樣、線性插值后的位置變化曲線Fig.6 Curves of position after uniform sampling and linear interpolation

圖7中五角星表示目標(biāo)點(diǎn)位置,2個(gè)半球表示2個(gè)雷達(dá)的探測范圍。虛線表示參考軌跡,即圖6的三維軌跡形式,實(shí)線為無人機(jī)的實(shí)際跟蹤軌跡,軌跡上的圓圈為每隔5 s無人機(jī)所在的位置。由圖7中可見,所規(guī)劃的軌跡能夠避開雷達(dá)威脅,且能夠到達(dá)目標(biāo)點(diǎn)。

圖7 參考軌跡與實(shí)際軌跡Fig.7 Reference trajectory and actual trajectory

圖8為圖7的局部放大圖,展示了無人機(jī)避障的細(xì)節(jié),其中將第6 s、第11 s的無人機(jī)位置用黑色球來表示,對應(yīng)時(shí)刻的障礙物位置用灰色球來表示,虛線為無人機(jī)參考軌跡,實(shí)線為無人機(jī)實(shí)際軌跡,灰色點(diǎn)線為障礙物運(yùn)動(dòng)路線。由圖8可見,無人機(jī)能夠主動(dòng)偏離參考軌跡來避開動(dòng)態(tài)障礙物。

圖8 三維軌跡局部放大圖Fig.8 Local enlargement of the 3D trajectory

圖9為3個(gè)方向的位置跟蹤誤差曲線,e=pr-p=(ex,ey,ez)T。

圖9 位置跟蹤誤差Fig.9 Position tracking error

圖10為無人機(jī)與障礙物間的距離變化曲線,圖11為參考速度、期望速度、實(shí)際速度。由于無人機(jī)是由懸停狀態(tài)開始執(zhí)行任務(wù),在0～3 s內(nèi),雖然無人機(jī)在加速,但是其實(shí)際速度一直小于參考速度(見圖11),因此y方向的位置誤差一直在增大。在3.5～13.5 s內(nèi),無人機(jī)為避免與障礙物碰撞而暫時(shí)偏離參考軌跡(見圖10),因此這段時(shí)間的跟蹤誤差相對較大。在91～97 s內(nèi),由于在無人機(jī)到達(dá)目標(biāo)點(diǎn)后,存在超調(diào)影響(見圖11),因此跟蹤誤差表現(xiàn)為先增大后減小。在其他時(shí)間段,各方向的跟蹤誤差均小于1 m。

圖10為0～18 s內(nèi)無人機(jī)與障礙物之間的距離變化曲線,其中帶點(diǎn)的實(shí)線為無人機(jī)與障礙物1之間的距離變化曲線,圖片下方的虛線為安全臨界值(Rs+Ro1)=2.3 m,Do1>2.3 m表明無人機(jī)未與障礙物1發(fā)生碰撞,圖片上方的虛線為避障觸發(fā)臨界值(Ra+Ro1)=21.5 m,當(dāng)Do1≤21.5 m時(shí),將觸發(fā)避障功能;帶叉的實(shí)線為無人機(jī)與障礙物2之間的距離變化曲線,圖片下方的點(diǎn)線為安全臨界值(Rs+Ro2)=2.8 m,上方的點(diǎn)線為避障觸發(fā)臨界值(Ra+Ro2)=22 m。易知,無人機(jī)也未與障礙物2發(fā)生碰撞。

圖10 無人機(jī)與障礙物間的距離Fig.10 Curves of the distance between UAV and obstacles

圖11 參考速度、期望速度、實(shí)際速度Fig.11 Reference velocity,desired velocity and actual velocity

圖11中虛線為軌跡規(guī)劃生成的參考速度,點(diǎn)線為利用模型預(yù)測控制生成的期望速度,實(shí)線為無人機(jī)實(shí)際速度?？梢?各個(gè)速度均滿足所設(shè)置的約束。在t=3.5 s時(shí),由于此時(shí)無人機(jī)與障礙物1的距離小于避障觸發(fā)臨界值(見圖10),因此觸發(fā)避障功能,由MPC生成的期望速度開始明顯偏離參考速度,而在t=13.5 s后無人機(jī)與障礙物的距離大于避障觸發(fā)臨界值,因此由MPC生成的期望速度又逐漸接近參考速度。與圖8中位置跟蹤誤差的變化現(xiàn)象相符。

圖12 無人機(jī)與障礙物間的距離(對比實(shí)驗(yàn))Fig.12 Curves of the distance between UAV and obstacles (comparison experiment)

為進(jìn)一步說明本文中算法的優(yōu)越性,基于文獻(xiàn)[20]中的人工勢場(artificial potential field,APF)算法設(shè)計(jì)對比實(shí)驗(yàn)。圖13為APF算法下的位置跟蹤誤差曲線,對比圖9可見,在40～60 s內(nèi),APF算法的跟蹤誤差明顯較大,x和y方向的位置誤差值約為5 m,而本文中算法的誤差值幾乎為0。

圖13 位置跟蹤誤差(APF算法)Fig.13 Position tracking error(APF method)

圖14為APF算法下x方向的參考速度、期望速度、實(shí)際速度曲線。由于APF算法無法有效考慮無人機(jī)的輸入約束,因此其生成的期望速度值有大于13 m/s的現(xiàn)象,然而基于MPC的算法可以顯示考慮輸入約束,保證其生成的期望速度滿足輸入約束。

圖14 x方向的參考速度、期望速度、實(shí)際速度 (APF算法)Fig.14 Reference velocity,desired velocity and actual velocityin x-axis(APF method)

4 結(jié)論

1) 經(jīng)過統(tǒng)籌、協(xié)調(diào)設(shè)計(jì)的四旋翼軌跡規(guī)劃與跟蹤控制算法能夠較好地完成想定的低空突防任務(wù),即能夠控制四旋翼無人機(jī)快速穿越雷達(dá)與移動(dòng)障礙并存的威脅環(huán)境并到達(dá)指定目標(biāo)點(diǎn)。

2) 在跟蹤控制算法的避障懲罰函數(shù)中考慮障礙物的預(yù)測位置值,能夠更好地應(yīng)對移動(dòng)速度較快的障礙物。