考慮UAV性能約束的變速度自主避障算法研究

周硙硙，趙海濤

(海軍裝備部駐南京地區(qū)第一軍事代表室，南京 210001)

0 引言

隨著科學(xué)技術(shù)的發(fā)展，無人飛行器(Unmanned Aerial Vehicle，UAV)智能化水平得到了很大地提升，無論在民用領(lǐng)域還是軍事領(lǐng)域的應(yīng)用都越來越廣泛[1]。與此同時，UAV在執(zhí)行任務(wù)過程中的安全性問題成為人們關(guān)注的焦點，特別是隨著UAV執(zhí)行任務(wù)的范圍不斷擴大和執(zhí)行任務(wù)的環(huán)境日益復(fù)雜，其具備自主避障(Autonomous Collision Avoidance，ACA)功能變得尤為重要，且已成為智能化UAV的關(guān)鍵性能指標(biāo)。為提高UAV執(zhí)行任務(wù)的可靠性和降低執(zhí)行任務(wù)的代價，為UAV配備性能良好的自主避障系統(tǒng)成為UAV技術(shù)研究的重要方向之一，而自主避障算法是自主避障系統(tǒng)的核心部分。對UAV而言，動態(tài)障礙避碰[2]成為一個重要的、普遍存在的、具有挑戰(zhàn)性的問題。因此，如何提高UAV在復(fù)雜環(huán)境下執(zhí)行艱巨任務(wù)的生存能力，成為UAV技術(shù)研究中亟待解決的關(guān)鍵問題之一[3]。

近年來，一些學(xué)者對UAV自主避障算法進(jìn)行了深入研究，提出了基于速度障礙法的UAV自主避障算法，并取得了一些研究成果，提升了UAV自主避障系統(tǒng)的性能。學(xué)者P. Fiorini和Z. Shiller[4-5]提出了速度障礙法的理論框架；文獻(xiàn)[6]將速度障礙法應(yīng)用于多智能體的協(xié)同沖突消解；文獻(xiàn)[7]在速度障礙法的基礎(chǔ)上，提出了一種幾何優(yōu)化方法，以解決在融合空域內(nèi)面臨復(fù)雜障礙物時，UAV的飛行沖突解脫和航跡恢復(fù)問題；文獻(xiàn)[8]為解決復(fù)雜融合空域內(nèi)無人機沖突解脫過程中消耗大的問題，提出了基于速度障礙法的合作型無人機的最優(yōu)防相撞策略；也有一些學(xué)者在速度障礙法的基礎(chǔ)上，對其進(jìn)行了延伸和拓展應(yīng)用，文獻(xiàn)[9]為解決UAV與UAV之間的自主避碰問題，提出了一種選擇速度障礙法(Selective Velocity Obstacle，SVO)；文獻(xiàn)[10]在UAV的非協(xié)同自主避障系統(tǒng)中應(yīng)用了速度障礙法，并提出了一種感知與避碰速度障礙法(SA-VO)。

另外，還有一些學(xué)者將導(dǎo)引的思想引入威脅障礙物避碰的問題中，并提出了基于導(dǎo)引法的自主避障算法。文獻(xiàn)[11]基于比例導(dǎo)引律提出了UAV的最優(yōu)導(dǎo)引避障算法；文獻(xiàn)[12]在非結(jié)構(gòu)環(huán)境下，基于導(dǎo)引律方式實現(xiàn)了對威脅障礙物的避碰；H. S. Shin等[13-14]將微分幾何的思想用于解決UAV的避障問題，提出了基于非連續(xù)導(dǎo)引控制律的避障算法；A. Mujumdar等[15]基于導(dǎo)引方式，給出了兩種非線性避障導(dǎo)引方法：非線性幾何導(dǎo)引(Nonlinear Geometric Guidance，NGG)及微分幾何導(dǎo)引(Differential Geometric Guidance，DGG)，使UAV快速將向量對準(zhǔn)瞄準(zhǔn)點，并給出了相應(yīng)的導(dǎo)引方法；文獻(xiàn)[16]在動態(tài)環(huán)境下，提出了一種基于改進(jìn)比例導(dǎo)引律的機器人動態(tài)避障算法。

文獻(xiàn)[17]通過速度障礙圓弧對障礙物的威脅大小進(jìn)行量化，提出了一種基于速度障礙圓弧法的UAV自主避障算法，解決了多動態(tài)威脅障礙物避碰、考慮潛在威脅障礙物對UAV避碰的影響以及動態(tài)不確定環(huán)境下自主避障的問題，但未對考慮UAV性能約束條件和變速率自主避障進(jìn)行進(jìn)一步研究。在考慮UAV自身性能約束的情況下，UAV在復(fù)雜動態(tài)環(huán)境下對威脅障礙的避碰將受到一定的限制，本文通過研究UAV速度矢量大小與威脅障礙速度障礙圓弧之間的關(guān)系，提出了一種考慮UAV性能約束的變速度自主避障算法。與現(xiàn)有算法相比，可實現(xiàn)UAV對威脅障礙避碰偏轉(zhuǎn)更小的速度矢量角度，從而在考慮自身性能約束的條件下，為實現(xiàn)UAV對多威脅障礙的避碰提供了更大的避碰裕度，可有效地提升UAV在復(fù)雜動態(tài)環(huán)境下執(zhí)行任務(wù)的安全性和可靠性。

1 UAV的自主避障模型

UAV在動態(tài)不確定環(huán)境下執(zhí)行任務(wù)過程中，通常沿初始規(guī)劃的航路飛行，初始規(guī)劃的航路已完成對已知威脅障礙物的避碰。但UAV在跟蹤規(guī)劃航路的過程中，可能會遭遇未知的動靜態(tài)障礙物，此時需要完成以下工作：1)對傳感器感知到的動靜態(tài)障礙物進(jìn)行威脅性判斷。2)若感知到所有的障礙物均不具有威脅性，則UAV沿初始航路繼續(xù)執(zhí)行任務(wù)；若感知到的障礙物中包含威脅性障礙物，則UAV需要對初始航路進(jìn)行重規(guī)劃。3)UAV沿重規(guī)劃的航路繼續(xù)執(zhí)行任務(wù)，若再次感知到未知動靜態(tài)威脅障礙物，則重復(fù)工作1)和2)，直至UAV完成被賦予的任務(wù)。

1.1 速度障礙模型的建立

UAV在執(zhí)行任務(wù)的過程中，可通過自身攜帶的傳感器裝置獲取障礙物O和自身的位置坐標(biāo)、速度矢量等信息，即障礙物和UAV的位姿信息Pose2obs(PO,vo)、Pose2uav(PU,vu)。為簡化避碰模型的建立，依據(jù)UAV與感知到的障礙物之間的大小關(guān)系，將UAV簡化為質(zhì)點，相應(yīng)的障礙物確立為半徑R的障礙圓⊙PO；那么，UAV和障礙物之間形成的障礙錐(Collision Cone，CC)如圖1所示。其中，d0為UAV感知距離；l1和l2為障礙錐的邊界，且為過UAV位置點PU并相切于障礙圓⊙PO的切線。

圖1 障礙錐示意圖Fig.1 Sketch of collision cone

(1)

(2)

考慮UAV對多障礙物避碰及潛在威脅障礙物對避碰影響的復(fù)雜情況，將障礙錐整體平移障礙物速度矢量vo得到速度障礙錐(Velocity Obstacle，VO)，如圖2所示。在障礙錐模型上，UAV實現(xiàn)了對威脅障礙物避碰需要將與障礙物之間的相對速度vuo偏轉(zhuǎn)出障礙錐；那么，從障礙錐轉(zhuǎn)化到速度障礙錐后，可直接通過將UAV速度矢量vu偏轉(zhuǎn)出速度障礙錐，從而實現(xiàn)對障礙物的避碰，簡化了避碰模型的建立，特別是對多威脅障礙物及考慮潛在威脅障礙物影響的避碰分析。

圖2 速度障礙錐示意圖Fig.2 Sketch of velocity obstacle

1.2 速度障礙圓弧法

速度障礙圓弧法的提出主要基于轉(zhuǎn)化的思想，將障礙物對UAV的飛行安全影響轉(zhuǎn)化為UAV速度圓⊙PU位于VO內(nèi)的圓弧，稱為速度障礙圓弧，如圖3所示。那么，通過對速度障礙圓弧參數(shù)的計算，可將障礙物對UAV飛行安全的影響確定為UAV不可行的速度矢量方向范圍，從而量化了障礙物對UAV飛行安全的影響，提升了UAV自主避障能力，特別是UAV對多威脅障礙物的避碰能力，同時也可綜合考慮潛在威脅障礙物對UAV安全飛行的影響。

圖3 速度障礙圓弧示意圖Fig.3 Sketch of velocity obstacle arc

1.3 動態(tài)不確定環(huán)境下速度障礙圓弧法的應(yīng)用

PVO+∈PVOQO

圖4 動態(tài)不確定速度障礙模型建立的示意圖Fig.4 Sketch of establishment of dynamic uncertain velocity obstacle model

圖5 動態(tài)不確定速度障礙圓弧示意圖Fig.5 Sketch of dynamic uncertain velocity obstacle arc

2 考慮UAV性能約束的變速度自主避障算法

在復(fù)雜動態(tài)環(huán)境下，UAV可能遭遇多個動靜態(tài)障礙物，單純地通過UAV的速度矢量偏轉(zhuǎn)實現(xiàn)對多個動靜態(tài)障礙物的避碰較為困難，尤其在相遇的障礙物對應(yīng)較大的速度障礙圓弧時，即UAV對應(yīng)有一個較大的不可行飛行方向范圍。較文獻(xiàn)[17]在不考慮UAV機動性能的情況下，可實現(xiàn)對威脅障礙物的避碰；但復(fù)雜情況下，UAV要偏轉(zhuǎn)較大的角度才能實現(xiàn)對威脅障礙物的避碰，此時UAV會偏離初始規(guī)劃路徑較大。那么對多威脅障礙物的避碰及考慮潛在威脅障礙物影響的情況下，變速度自主避障法將更具優(yōu)勢，且充分地考慮了UAV性能約束條件。

2.1 UAV動態(tài)性能約束條件

在考慮UAV動態(tài)性能約束條件時，依據(jù)UAV自身的機動性能，設(shè)定UAV法向加速度at和縱向加速度an的范圍，則在有限自主避障時間tavo內(nèi)，相應(yīng)地UAV速度矢量vu具有一定的變化范圍。

設(shè)定at和an的取值范圍分別為

0≤at≤at_max

(3)

0≤an≤an_max

(4)

那么，在有限自主避障時間tavo內(nèi)，UAV速度矢量vu的大小vu和方向角θu邊界值可分別確定為

vu_min=vu-τ0

(5)

vu_max=vu+τ0

(6)

τ0=an_maxtavo

(7)

θu_min=θu-σ0

(8)

θu_max=θu+σ0

(9)

(10)

其中，r0為旋回半徑，由UAV自身性能條件確定。則相應(yīng)的UAV速度矢量vu的大小vu和方向角θu的范圍可分別確定為

vu∈[vu_min,vu_max]

(11)

θu∈[θu_min,θu_max]

(12)

2.2 考慮UAV性能約束的變速度自主避障算法

為實現(xiàn)UAV在復(fù)雜動態(tài)環(huán)境下的安全飛行，且充分考慮自身的性能約束條件，基于速度障礙圓弧法，量化障礙物對應(yīng)不同UAV速度矢量vu下的速度障礙圓弧Garc，再依據(jù)UAV速度矢量vu與速度障礙圓弧Garc之間的相對態(tài)勢，從而確定UAV對威脅障礙物避碰的最優(yōu)策略。以UAV對單個威脅障礙物避碰為例，考慮UAV性能約束的變速度自主避障策略態(tài)勢如圖6所示。

圖6 考慮UAV性能約束的自主避障策略態(tài)勢圖Fig.6 Sketch of autonomous obstacle avoidance strategy considering UAV’s performance constraints

根據(jù)式(5)、式(6)、式(8)和式(9)，可確定UAV速度矢量vu的大小vu和方向角θu的約束范圍分別為2τ0和2σ0。在態(tài)勢圖6中，分別以速度圓⊙PU最大半徑vu_max和最小半徑vu_min之間的差值、速度矢量方向約束范圍線之間的夾角進(jìn)行表示。相應(yīng)的速度圓⊙PU以半徑vu_min、vu和vu_max位于速度障礙內(nèi)的圓弧可分別表示為

Garc-vu_min=⊙PU(λ=vu_min)∩VO

(13)

Garc-vu=⊙PU(λ=vu)∩VO

(14)

Garc-vu_max=⊙PU(λ=vu_max)∩VO

(15)

那么，從考慮UAV性能約束的自主避障策略態(tài)勢圖中可直觀得出，UAV速度矢量方向角θu的約束范圍限定了UAV在避障過程中的偏航范圍，減少了UAV對威脅障礙物的避碰空間，尤其是UAV需要同時對多威脅障礙物進(jìn)行避碰或考慮潛在威脅障礙物對避碰影響的情形。為解決考慮UAV性能約束條件下UAV避障空間變小的問題，基于偏航的思想實現(xiàn)了UAV對威脅障礙物避碰的方法，同時采取改變UAV速度矢量大小vu的思路進(jìn)行研究。

對比態(tài)勢圖6中不同半徑vu_min、vu和vu_max的速度圓⊙PU位于速度障礙內(nèi)的速度障礙圓弧，可以確定UAV速度矢量大小vu改變了速度障礙圓弧在速度障礙上的相對位置和大小。假設(shè)UAV實現(xiàn)了對威脅障礙物避碰時速度矢量vu偏轉(zhuǎn)的角度為λ，那么依據(jù)態(tài)勢圖6可以得出結(jié)論：

1)在有限自主避障時間tavo內(nèi)，當(dāng)UAV速度矢量vu通過逆時針偏轉(zhuǎn)實現(xiàn)對威脅障礙物避碰時，UAV速度矢量大小vu變化為vu_min，對應(yīng)的速度矢量vu偏轉(zhuǎn)的角度λ最小，即

λvu_min<λvu<λvu_max

(16)

2)在有限自主避障時間tavo內(nèi)，當(dāng)UAV速度矢量vu通過順時針偏轉(zhuǎn)實現(xiàn)對威脅障礙物避碰時，UAV速度矢量大小vu變化為vu_max，對應(yīng)的速度矢量vu偏轉(zhuǎn)的角度λ最小，即

λvu_max<λvu_min<λvu

(17)

從而可通過比較λvu_min與λvu_max之間的大小，確定考慮UAV性能約束的變速度自主避障的最小速度矢量偏轉(zhuǎn)角λmin，使得UAV偏離最優(yōu)初始規(guī)劃路徑最小。因此，當(dāng)UAV基于變速度自主避障時，UAV最小速度矢量偏轉(zhuǎn)角λmin在速度矢量大小vu邊界值上取得，且λmin小于UAV基于恒定速率避障的速度矢量偏轉(zhuǎn)角度；則在偏轉(zhuǎn)速度矢量避碰的基礎(chǔ)上，輔以變速率的策略更加有助于UAV對威脅障礙物的避碰。

2.3 多威脅障礙物避碰方向分析

依據(jù)求解的速度障礙圓弧參數(shù)Garc，可將2個威脅障礙物的速度障礙圓弧表示在UAV的速度圓上，相應(yīng)的示意圖如圖7所示。

圖7 2個威脅障礙物的避碰圓弧示意圖Fig.7 Collision avoidance arcs of two threatening obstacles

依據(jù)2個威脅障礙物的速度障礙圓弧參數(shù)Garc1(r1,φr1,β1)和Garc2(r2,φr2,β2)，可分別確定速度障礙圓弧對應(yīng)的避碰不可行速度矢量方向范圍為[φr1-β1,φr1+β1]和[φr2-β2,φr2+β2]。那么，2個威脅障礙物對UAV避碰所產(chǎn)生的不可行速度矢量方向范圍為二者的并集。因此，為實現(xiàn)UAV對多威脅障礙物的避碰，需要將UAV速度矢量vu偏轉(zhuǎn)出不可行速度矢量方向范圍。為直觀地確定UAV最優(yōu)避障方向，將UAV速度矢量vu的方向角φu及2個威脅障礙圓弧所對應(yīng)的圓心角范圍表示在一維坐標(biāo)系上，示意圖如圖8所示。從而，依據(jù)UAV速度矢量vu的方向角φu在UAV避碰的不可行速度矢量方向范圍中的位置，可直觀地確定UAV對2個威脅障礙物的最優(yōu)避碰方向。

圖8 2個威脅障礙避碰角度的坐標(biāo)表示示意圖Fig.8 Coordinates representation of collision avoidance angles for two threatening obstacles

3 算法的仿真與驗證

在UAV自主避障態(tài)勢圖6中,給出了速度矢量大小vu邊界值上速度障礙圓弧的情況，同樣通過設(shè)定vu的取值范圍，也可得出UAV速度矢量大小vu在取值范圍內(nèi)相對應(yīng)的速度障礙圓弧Garc的大?。辉O(shè)定UAV法向加速度at和縱向加速度an的范圍分別為[0,0.5]和[0,20]，本文以文獻(xiàn)[17]中對威脅障礙物避碰仿真為例，作對比分析。

(1)單個威脅障礙物避碰

依據(jù)UAV性能約束條件，相應(yīng)的UAV速度矢量vu的大小vu和方向角θu的取值范圍可分別確定為[53.2,56.8]和[-10,136.8]。那么，對于任意的vu∈[53.2,56.8]，可確定一個速度障礙圓弧，即威脅障礙物對應(yīng)于一個不可行UAV的速度方向范圍；那么作出整個UAV速度矢量大小vu范圍與速度障礙圓弧對應(yīng)不可行速度方向范圍的關(guān)系圖，如圖9所示。

圖9 速度障礙圓弧對應(yīng)的圓心角邊界隨UAV速率變化關(guān)系圖Fig.9 Sketch of the relationship between the central angle boundary corresponding to velocity obstacle arc and the UAV’s speed change

在仿真圖9中，給出了UAV性能約束條件對應(yīng)的速度矢量大小vu的邊界線，以及速度障礙圓弧在不同vu情況下對應(yīng)的圓心角邊界。那么，從圖9中可以得出，威脅障礙物的速度障礙圓弧對應(yīng)的圓心角邊界隨vu變化而改變，且在vu取值范圍內(nèi)，當(dāng)vu取邊界值56.8m/s時，UAV實現(xiàn)了對威脅障礙物避碰所需偏轉(zhuǎn)的角度最小。

與文獻(xiàn)[17]進(jìn)行對比，對單個威脅障礙物避碰時UAV速度矢量所需偏轉(zhuǎn)角度如表1所示。

表1 單個威脅障礙物避碰情況下的UAV避障方向角范圍

那么，當(dāng)UAV采取變速度自主避障算法時，對單個威脅障礙物避碰所需偏轉(zhuǎn)的角度較小，較文獻(xiàn)[17]避障算法性能更優(yōu)，且更加易于實現(xiàn)。相應(yīng)的UAV對單個威脅障礙避碰的仿真如圖10所示。

圖10 UAV對單個威脅障礙避碰的仿真圖Fig.10 Simulation diagram of single threatening obstacle avoidance for UAV

(2)多威脅障礙物避碰

當(dāng)UAV需要對多個威脅障礙物進(jìn)行避碰時，與文獻(xiàn)[17]相比，UAV對多個威脅障礙物避碰的速度矢量所需偏轉(zhuǎn)角度如表2所示。

表2 多個威脅障礙物避碰情況下的UAV避障方向角范圍

那么，當(dāng)UAV采取變速度自主避障算法時，對多個威脅障礙物避碰所需偏轉(zhuǎn)的角度較小，較文獻(xiàn)[17]避障算法性能更優(yōu)，且更加易于實現(xiàn)。相應(yīng)的UAV對多個威脅障礙避碰的仿真如圖11所示。

圖11 UAV對多個威脅障礙物避碰的仿真圖Fig.11 Simulation diagram of multiple threatening obstacles avoidance for UAV

4 結(jié)論

本文針對動態(tài)不確定環(huán)境下提升UAV執(zhí)行任務(wù)的安全性和可靠性，提出了一種考慮UAV性能約束的變速度自主避障算法。算法分析和仿真結(jié)果表明：

1)考慮性能約束條件對UAV自主避障結(jié)果產(chǎn)生的影響。本文考慮UAV法向加速度約束，限定了UAV在自主避障時間內(nèi)的速度矢量變化范圍，更加貼近實際數(shù)學(xué)模型。

2)基于速度障礙圓弧法，得出障礙物對應(yīng)的速度障礙圓弧大小和方向隨UAV速度矢量大小變化，且在有限自主避障時間tavo內(nèi)，UAV對威脅障礙物避碰的最小速度矢量偏轉(zhuǎn)角λmin在速度矢量大小vu邊界值上取得。

3)本文所提出的變速度自主避障算法為UAV避障提供了更多可能性，彌補了考慮UAV性能約束條件產(chǎn)生的限制。