基于有限時(shí)間速度障礙法的UAV避障研究*

楊秀霞,華 偉,孟啟源

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

0 引言

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

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

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

(1)

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

(2)

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

圖1 UAV避障工作原理框圖

2 速度障礙法的原理

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

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

(3)

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

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

圖4 有限時(shí)間速度障礙法示意圖

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

FVOU|O={v?t>0且

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

(5)

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

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

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

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

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

f1=kv|vd-v|

(6)

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

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

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

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

由圖5可知:

vrel=vo-vu

(7)

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

(8)

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

(9)

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

(10)

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

(11)

式中k=k1k2。

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

F(v)=f1+f2

(12)

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

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

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

5 仿真驗(yàn)證與分析

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

表1 UAV與障礙物初始條件

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

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

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

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

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

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

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

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

6 結(jié)束語

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