王宏偉，甘旭升，韋 剛，楊麗薇，李雙峰

(1.西京學(xué)院,西安 710000;2.空軍工程大學(xué),a.空管領(lǐng)航學(xué)院;b.防空反導(dǎo)學(xué)院,西安 710000)

0 引言

隨著航空技術(shù)與信息技術(shù)的不斷發(fā)展，無人機(jī)(UAV)離開隔離空域，在未知的融合空域環(huán)境內(nèi)執(zhí)行任務(wù)已成為當(dāng)前的發(fā)展趨勢。特別是隨著低空空域改革的推進(jìn)，融合空域環(huán)境更加復(fù)雜，不確定性更高，因此UAV具備對探測到的動靜態(tài)障礙物進(jìn)行避碰并進(jìn)行自主在線規(guī)劃路徑的能力對任務(wù)執(zhí)行至關(guān)重要[1]。

在UAV動態(tài)避障過程中，根據(jù)UAV與障礙物的相對運(yùn)動趨勢信息進(jìn)行沖突解脫，將位置障礙轉(zhuǎn)化為速度障礙，效果往往不錯[2-4]。文獻(xiàn)[5]擴(kuò)大沖突機(jī)與入侵機(jī)的速度適用范圍，更加符合UAV沖突實(shí)際情況，但解脫效率仍未達(dá)到最優(yōu);文獻(xiàn)[6]將合作博弈概念用在沖突解脫過程中，利用最優(yōu)化理論，將聯(lián)盟福利最優(yōu)解作為博弈雙方最優(yōu)解脫策略，實(shí)現(xiàn)支付代價最小前提下的實(shí)時沖突解脫;文獻(xiàn)[7]考慮動態(tài)障礙物的“潛在危險”，通過速度障礙圓弧法對動態(tài)障礙物的威脅程度進(jìn)行量化，但該方法只考慮轉(zhuǎn)向策略，不符合UAV高機(jī)動的特性。

現(xiàn)有的速度障礙避障算法[8-11]在UAV自主控制避障領(lǐng)域發(fā)揮重要作用，但采用單獨(dú)改變UAV航向或者單獨(dú)改變速度進(jìn)行避撞，沒有將同時改變航向與速度作為解脫策略，因而解脫效率不高。為解決現(xiàn)有的UAV避障算法效率過低的問題，在前人研究基礎(chǔ)上，本文基于最小調(diào)整量的原則，根據(jù)速度障礙法原理，建立UAV與多動態(tài)障礙物的沖突模型，并計算出最優(yōu)解脫策略，實(shí)現(xiàn)與動態(tài)障礙物的沖突解脫，提高UAV避障算法的可行性。

1 UAV沖突模型

UAV在執(zhí)行任務(wù)過程中，利用自身雷達(dá)等設(shè)備不斷探測周圍信息，判斷與障礙物之間是否存在沖突。速度障礙法將UAV與障礙物之間的相對速度作為研究對象，當(dāng)相對速度落入障礙區(qū)內(nèi)，視為存在飛行沖突，否則視為不存在。為便于分析，通常將UAV簡化為質(zhì)點(diǎn)，障礙物Oi(i=1,2,…)視為具有安全半徑di(i=1,2,…)的圓,如圖1所示。

圖1 速度障礙模型示意圖Fig.1 Schematic diagram of velocity obstacle model

UAV與障礙物分別位于A，O兩點(diǎn)，距離為Di，速度分別為vu，vOi，相對速度為vRi=vu-vOi，落在直線l上，障礙物“膨化”的安全圓半徑為di，UAV與障礙物連線AO與vRi的夾角為γi，與RCC邊界夾角為αi。

定義1相對碰撞區(qū)(Relative Collision Cone,RCC)，是能夠產(chǎn)生飛行沖突的區(qū)域集合，即

ARCC={vRi|l∩⊙O≠?}

式中，⊙O為障礙物“膨化”的安全保護(hù)區(qū)。

結(jié)論1 當(dāng)γi<αi時，UAV與障礙物Oi之間存在飛行沖突，UAV需要采取避撞策略重新進(jìn)行路徑規(guī)劃；否則，當(dāng)γi≥αi時，不存在飛行沖突。

αi和γi的數(shù)值可由式(1)、式(2)分別計算確定，即

(1)

(2)

2 UAV沖突解脫模型

如圖2所示，由相對碰撞區(qū)RCC平移vOi后得到絕對碰撞區(qū)(Absolute Collision Cone，ACC)，為便于直觀分析，UAV受到的障礙威脅可通過結(jié)論2進(jìn)行判斷。

圖2 二維平面速度障礙法示意圖Fig.2 Schematic diagram of two-dimensional plane velocity obstacle method

結(jié)論2 對于UAV與單動態(tài)障礙物O，計算出ACC區(qū)域，當(dāng)UAV的速度vu落在ACC區(qū)域內(nèi)時，UAV與障礙物O存在飛行沖突；否則,沖突不存在。

圖3顯示出UAV與多動態(tài)障礙物的態(tài)勢關(guān)系，UAV與障礙物之間是否存在飛行沖突可由結(jié)論3進(jìn)行判斷。

圖3 多動態(tài)障礙物示意圖Fig.3 Schematic diagram of multiple dynamic obstacles

3 基于速度障礙法的避撞算法

根據(jù)速度障礙法原理，UAV與障礙物Oi(i=1,2,3,…,N)存在飛行沖突的臨界條件是MACC的邊界，因此,解脫策略的求解關(guān)鍵在于UAV的速度vu落在ACC區(qū)域邊界時速度的大小與航向之間的關(guān)系。

3.1 單障礙物的避碰

對于UAV與單障礙物沖突解脫問題，根據(jù)結(jié)論2，UAV的速度vu應(yīng)落在ACC區(qū)域之外，現(xiàn)在對ACC區(qū)域邊界關(guān)鍵參數(shù)進(jìn)行求解。

圖4為單障礙物的避碰示意圖。

圖4 單障礙物的避碰示意圖Fig.4 Collision avoidance diagram of a single obstacle

圖4中，設(shè)UAV與障礙物的初始位置分別為A(x1,y1),O(x2,y2)，則ACC區(qū)域中的頂點(diǎn)與安全圓的位置分別為

(3)

(4)

ACC區(qū)域邊界l1i的斜率分別為

kl1i=tan[arctan(kAO)+αi]=tan[arctan(kA′O′)+αi]

(5)

式中，kAO為UAV與障礙物初始位置的斜率，即

kAO=kA′O′=(y2-y1)/(x2-x1)。

(6)

則ACC區(qū)域邊界l1i可分別表示為

(7)

UAV的位置A(x1,y1)到ACC區(qū)域邊界l1i的距離為

(8)

(9)

(10)

0≤|Δθ|=|θ-arctan ∠(vu)|≤Δθmax。

(11)

3.2 多障礙物的避碰

對于UAV與多動態(tài)障礙物沖突解脫問題，根據(jù)結(jié)論3，UAV的速度vu應(yīng)落在MACC區(qū)域之外。

UAV受自身速度約束，可調(diào)整的速度集合，用沖突解脫速度(Resolution Velocity，RV)表示，即

(12)

由此可得，UAV可達(dá)的速度禁區(qū)(Forbidden Resolution Velocity，F(xiàn)RV)為

AFRV=ARV∩AMACC。

(13)

UAV允許可達(dá)的解脫速度區(qū)域(Accessible Resolution Velocity，ARV)表示為

AARV=ARV∩AMACC。

(14)

圖5所示為多障礙物避碰示意圖。

圖5 多障礙物避碰示意圖Fig.5 Schematic diagram of collision avoidance with multiple obstacles

圖5(a)表示MACC區(qū)域，圖5(b)表示RV區(qū)域，圖5(c)表示FRV與ARV區(qū)域。針對多動態(tài)障礙物的避撞問題，需要將UAV速度調(diào)整至ARV區(qū)域，實(shí)現(xiàn)與障礙物的沖突解脫。

MACC區(qū)域邊界的參數(shù)計算與單個ACC區(qū)域類似，此處不再贅述。

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

在避撞過程中，UAV通過調(diào)向和調(diào)速，與動態(tài)障礙物保持一定的安全間隔。為提高沖突解脫效率，需要用目標(biāo)函數(shù)來衡量避撞的支付代價。

3.3.1 航向調(diào)整量指標(biāo)

UAV在避障過程中，單次調(diào)向程度越大，越難以保證UAV的穩(wěn)定運(yùn)行，造成安全隱患，效用函數(shù)u1(θ)表示為

u1(θ)=1+|sin Δθ|

(15)

式中，Δθ表示UAV單次調(diào)向角度。值得說明的是，當(dāng)前基于速度障礙法的常見避障策略是通過調(diào)向完成避障過程，整個沖突解脫過程假設(shè)UAV速度大小保持不變，將最小轉(zhuǎn)向作為最優(yōu)解脫策略[5]，可看作目標(biāo)函數(shù)取u1(θ)。

3.3.2 速度大小調(diào)整量指標(biāo)

運(yùn)行過程中，單次速度調(diào)整程度越大，越會在一定程度上造成UAV機(jī)械裝置的不穩(wěn)定，因此效用函數(shù)u2(vu)表示為

u2(vu)=|vu|。

(16)

3.3.3 綜合最優(yōu)策略

綜合考慮調(diào)向與調(diào)速指標(biāo)，可得到綜合效用函數(shù)u3(θ,vu)，表示為

u3(θ,vu)=k1(1+|sin Δθ|)+k2|vu|

(17)

式中，k1，k2分別為調(diào)向指標(biāo)與調(diào)速指標(biāo)的重要程度，可根據(jù)實(shí)際UAV運(yùn)行情況在[0，1]之間進(jìn)行調(diào)整。

因此融合空域UAV的障礙物避碰過程，可轉(zhuǎn)為以下優(yōu)化問題，即

minu3(θ,vu)=min[k1(1+|sin Δθ|)+k2|vu|]

(18)

3.4 針對動態(tài)障礙物的避碰流程

融合空域內(nèi)飛行的UAV對動態(tài)障礙物的避碰流程如圖6所示，具體步驟如下。

圖6 針對動態(tài)障礙物的沖突解脫流程圖Fig.6 Flow chart of conflict resolution for dynamic obstacles

1) 判斷是否沖突。UAV通過自身雷達(dá)等設(shè)備不斷探測周圍動態(tài)障礙物的速度、位置、航向等信息，基于速度障礙法，利用結(jié)論1，當(dāng)滿足γi<αi時，UAV與障礙物Oi之間存在飛行沖突，UAV需要采取避撞策略重新進(jìn)行路徑規(guī)劃；否則當(dāng)γi≥αi時，不存在飛行沖突，UAV指向目標(biāo)點(diǎn)運(yùn)行。

2) 計算最優(yōu)避碰策略。將UAV與動態(tài)障礙物的沖突解脫過程轉(zhuǎn)化為優(yōu)化問題，利用最優(yōu)化理論，計算出最優(yōu)解脫速度與航向。本文采用有約束的最優(yōu)化函數(shù)fmincon來實(shí)現(xiàn)求解過程。

3) 迭代更新位置。完成一次速度與航向更新時，同時更新UAV與動態(tài)障礙物的位置，直到完成沖突解脫過程，到達(dá)目的地。

4 UAV自主避碰算法的應(yīng)用驗(yàn)證

4.1 單障礙物避碰仿真

圖7 單障礙物避碰仿真圖Fig.7 Simulation diagram of single obstacle avoidance

圖8 UAV與單障礙物之間間隔Fig.8 Operation interval between UAV and a single obstacle

4.2 多障礙物避碰仿真

圖9 多障礙物避碰仿真圖Fig.9 Simulation diagram of collision avoidance with multiple obstacles

圖10 UAV與多障礙物之間間隔Fig.10 Operation interval between UAV and multiple obstacles

為驗(yàn)證本文所提算法的高效性，將文獻(xiàn)[7]中提到的速度圓弧法作為對照，對比兩種算法的指標(biāo)數(shù)據(jù)，結(jié)果如表1所示。

表1 算法對比Table 1 Algorithm comparison

表1中的各指標(biāo)是指仿真過程中分步目標(biāo)函數(shù)之和，無量綱。本文所提避障算法能夠通過同時調(diào)整航向與速度完成避障，靈活性強(qiáng)，且能夠保證綜合代價小，具有高效性。

5 結(jié)論

本文針對融合空域內(nèi)UAV自由飛行情況下的避障問題，提出了基于速度障礙模型的同時調(diào)向、調(diào)速策略，在規(guī)避障礙物的同時，保證支付代價最小。

分別在單動態(tài)障礙物和多動態(tài)障礙物沖突場景下，通過幾何關(guān)系精確推導(dǎo)出UAV為完成避障應(yīng)采取的調(diào)向與調(diào)速之間的關(guān)系，并能在目標(biāo)函數(shù)的約束下，通過最優(yōu)化理論求解出最優(yōu)避障策略。通過同時調(diào)整航向與速度完成避障，靈活性強(qiáng)，且能夠保證綜合代價小，具有高效性。