楊秀霞,華 偉,孟啟源

(1 海軍航空工程學(xué)院控制工程系,山東煙臺 264001;2 海軍航空工程學(xué)院研究生管理大隊,山東煙臺 264001)

0 引言

無論是在軍事領(lǐng)域還是民用領(lǐng)域,UAV也都得到了廣泛應(yīng)用[1-8]。為了保證UAV的運行安全,UAV需要通過自身攜帶的機載傳感器對周圍的飛行環(huán)境和其他飛行器的運動狀態(tài)進行實時感知和判斷,并作出合理的機動進行避障。

近年來,針對UAV的自主避障問題,國內(nèi)外許多學(xué)者進行了相關(guān)研究,這些方法包括勢場法[9],導(dǎo)引法[10-11],微分幾何法,隨機搜索算法和速度障礙法[12]等等。文獻[9]通過改進的人工勢場法避免了傳統(tǒng)人工勢場法易陷入局部陷阱的問題,實現(xiàn)了多機動態(tài)航路規(guī)劃,文獻[10]通過確定UAV和障礙物之間的相對位置和相對速度關(guān)系確定最優(yōu)避障速度向量,并將相對速度導(dǎo)引至避障向量上實現(xiàn)避障,文獻[12]通過建立速度障礙錐,并將UAV速度偏轉(zhuǎn)出速度障礙錐完成實時避障,但是上述算法都沒有考慮碰撞發(fā)生時間,因此在速度障礙法基礎(chǔ)上,文中提出了一種基于有限時間速度障礙法的避障方法。

1 UAV運動學(xué)模型的建立

(1)

式中:X=[x,y,vu,σu]T,x和y、vu、σu分別為UAV在二維平面內(nèi)的坐標(biāo)、速率和方向角;u為UAV的控制輸入。在實際仿真應(yīng)用中,通常將UAV的連續(xù)運動學(xué)模型進行離散化,UAV的離散狀態(tài)方程為：

(2)

式中ΔT為仿真步長。動態(tài)障礙物的運動學(xué)模型可同理得到。由于UAV的運行環(huán)境是實時變化的,因此UAV需要通過自身攜帶的機載傳感器對周圍的飛行環(huán)境進行實時感知和判斷,UAV的實時避障工作原理圖如圖1所示。

圖1 UAV避障工作原理框圖

2 速度障礙法的原理

如圖2所示,根據(jù)UAV和障礙物之間的相對位置和相對速度關(guān)系,如果相對速度位于相對障礙錐內(nèi)(圖中陰影部分),那么經(jīng)過一段時間之后,UAV與障礙物之間就可能發(fā)生碰撞,為了進行避障,需要將相對速度偏轉(zhuǎn)出障礙錐。

如圖2所示,障礙錐可由相對障礙錐頂點坐標(biāo)pu,開口方向q,夾角大小γ三個參數(shù)進行表示。

(3)

但是通常情況下都希望通過UAV速度矢量來進行碰撞判斷,并進行相應(yīng)的機動實現(xiàn)避障,為此Fiorini和Shiller[12]在20世紀(jì)90年代提出速度障礙法(velocity obstacle,VO),作為一種一階避障方法,在進行避障時,該方法將障礙物的位置和速度同時考慮在內(nèi),比較好地解決了動態(tài)環(huán)境下UAV的避障問題。如圖3所示,將相對障礙錐移動矢量vo,即可得到絕對速度障礙錐(陰影部分),如果UAV的速度矢量vu落于速度障礙錐內(nèi),說明經(jīng)過一段時間之后,UAV與障礙物之間會發(fā)生碰撞,通過將UAV的速度矢量偏轉(zhuǎn)出速度障礙錐,即可實現(xiàn)避障。

圖2 基于相對速度和相對位置的UAV避障示意圖

圖3 速度障礙法(VO)示意圖

對于飛行器U,由障礙物O誘發(fā)形成的速度障礙錐VOU|O可定義為：

VOU|O={v?t>0,pu+(v-vO)t∈⊙(pO,rs)}

(4)

式中:pu為UAV的位置坐標(biāo);v為UAV的任一速度矢量;po、vo分別為障礙物的位置坐標(biāo)和速度矢量,⊙(po,rs)表示以po為圓心,rs為半徑的圓域。

3 速度障礙法的改進

通過前文的描述,速度障礙法將所有會導(dǎo)致碰撞的速度都排除在外,即使要經(jīng)過很長一段時間,碰撞才會發(fā)生,這就造成了UAV可選避障速度向量范圍的減小,甚至是避障失敗。為了克服這一問題,文中在此基礎(chǔ)上考慮碰撞發(fā)生時間,假設(shè)飛行器以某一速度運行,該速度位于速度障礙錐內(nèi),飛行器與障礙物之間存在碰撞危險,但是要經(jīng)過很長一段時間,碰撞才會發(fā)生,在這種情況下,為了避免不必要的機動,仍然可以將該速度作為避障速度,為此提出有限時間速度障礙錐(finite time velocity obstacle,FVO)避障方法。

圖4 有限時間速度障礙法示意圖

如上圖所示,對于飛行器U,由障礙物O誘發(fā)形成的有限時間速度障礙錐(陰影部分)可定義為:

FVOU|O={v?t>0且

t≥ts|pu-po+(v-vo)t|

(5)

式中：ts為最小允許碰撞發(fā)生時間,即飛行器以速度v運行,至少經(jīng)過ts才會發(fā)生碰撞,在此之前,飛行器可以保持當(dāng)前速度繼續(xù)運行,這樣就為飛行器提供了更大范圍的可選避障速度向量范圍。

4 基于FVO的最優(yōu)避障速度選取

通過前文的描述可知,盡管通過有限時間速度障礙法,UAV的可選避障速度向量范圍變大了,但是上述方法仍將速度范圍簡單的化為碰撞速度(collision velocity)和無碰區(qū)域(collision-free velocity),為了選擇最優(yōu)避障速度矢量,本節(jié)通過設(shè)立一系列評價指標(biāo)來對UAV的待選避障速度矢量進行評價,從而確定最優(yōu)避障速度矢量。通過選擇期望速度,UAV與障礙物之間的相對垂直距離,經(jīng)過障礙物的時間三個變量作為評價指標(biāo)來確定UAV的最優(yōu)避障速度[13]。

4.1 期望速度vd的評價函數(shù)

如圖5所示,假設(shè)UAV的目標(biāo)點位置已知,那么UAV的期望速度vd的方向為UAV當(dāng)前位置指向目標(biāo)點的方向,期望速度的評價函數(shù)可表示為：

f1=kv|vd-v|

(6)

式中v為待選的避障速度矢量。

4.2 經(jīng)過時間tp和相對垂直距離dv的評價函數(shù)

相對垂直距離dv定義為UAV與障礙物之間的相對位置向量到相對速度的垂直距離,如圖5所示。

圖5 期望速度、經(jīng)過時間和相對垂直距離示意圖

由圖5可知:

vrel=vo-vu

(7)

dv越小,意味著障礙物的危險程度越高,因此關(guān)于相對垂直距離dv的評價函數(shù)可定義為：

(8)

經(jīng)過時間tp可定義為UAV與障礙物之間距離最小的時間,如果UAV與障礙物之間會發(fā)生碰撞,那么經(jīng)過時間就變?yōu)榕鲎矔r間,tp可由下式得到：

(9)

從式(19)可以看出,tp有可能是負的,當(dāng)tp為負值時,說明UAV已經(jīng)經(jīng)過了障礙物。同樣的,tp越大,說明障礙物的危險程度越低,因此關(guān)于經(jīng)過時間tp的評價函數(shù)可定義為:

(10)

但是當(dāng)相對垂直距離足夠大時,即使經(jīng)過時間較小,對UAV來說,障礙物的危險程度也是較小的。同樣的,當(dāng)經(jīng)過時間足夠大時,即使相對垂直距離較小,對UAV來說,障礙物的危險程度也是較小的,因此為了對障礙物的危險程度進行更加客觀的評價,應(yīng)該同時考慮UAV與障礙物之間的相對垂直距離和經(jīng)過時間。修正后的評價函數(shù)可定義為：

(11)

式中k=k1k2。

最終,總的評價函數(shù)可定義為：

F(v)=f1+f2

(12)

f1和f2的權(quán)重可以通過選擇適當(dāng)?shù)南禂?shù)來進行調(diào)整。kv越大,說明UAV越傾向于趨近目標(biāo),同樣的,k越大,說明UAV越優(yōu)先進行避障。

在UAV運行過程中,可將UAV的運行狀態(tài)分為避障機動,勻速保持和執(zhí)行任務(wù)三個狀態(tài),UAV的三個運行狀態(tài)之間的轉(zhuǎn)換示意圖如圖6所示。

圖6 UAV運行狀態(tài)轉(zhuǎn)換示意圖

5 仿真驗證與分析

假設(shè)UAV在二維平面內(nèi)運行,UAV和障礙物的初始條件如表1所示。

表1 UAV與障礙物初始條件

UAV與障礙物均假設(shè)為安全半徑為r=1.5 m的圓,最大允許碰撞時間為ts=5 s,仿真步長ΔT=0.1 s,UAV的目標(biāo)點坐標(biāo)為(-10 m,-10 m)。

根據(jù)有限時間速度障礙法進行判斷可知,如果UAV保持當(dāng)前運動狀態(tài),那么經(jīng)過一段時間之后,UAV與障礙物之間有可能會發(fā)生碰撞,為此需要采取一定的機動措施進行避障。在仿真過程中,根據(jù)最優(yōu)避障速度矢量選取原則,實時確定UAV的最優(yōu)避障速度矢量方向,并在UAV的最大加速度約束范圍內(nèi)將UAV的速度矢量盡快導(dǎo)引至最優(yōu)避障速度矢量上,從而完成避障?；谟邢迺r間速度障礙法的避障過程示意圖如圖7～圖10所示。

圖7 UAV和障礙物初始狀態(tài)示意圖

圖8 UAV躲避障礙物1示意圖

圖9 UAV躲避障礙物3示意圖

圖10 UAV躲避障礙物2示意圖

圖11 UAV與各障礙物之間相對距離曲線

通過圖11可以看出,在UAV運行過程中,UAV與障礙物之間的相對距離總是大于最小安全半徑,說明了文中提出的避障方案能夠保證UAV運行過程的順利避障和運行安全。

6 結(jié)束語

針對復(fù)雜動態(tài)環(huán)境下UAV的多障礙物避障問題,文中在速度障礙法的基礎(chǔ)上,將碰撞發(fā)生時間考慮在內(nèi),提出了有限時間速度障礙法,通過設(shè)計一系列指標(biāo)函數(shù),在UAV運行過程中,實時確定UAV的最優(yōu)避障向量,仿真結(jié)果驗證了所提出的避障方法的可靠性和有效性。文中提出的避障方法具有良好的適應(yīng)性,可為UAV的現(xiàn)實避障應(yīng)用中提供一定的理論依據(jù)和借鑒意義。