周硙硙，趙海濤

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

0 引言

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

近年來(lái)，一些學(xué)者對(duì)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ù)雜障礙物時(shí)，UAV的飛行沖突解脫和航跡恢復(fù)問(wèn)題；文獻(xiàn)[8]為解決復(fù)雜融合空域內(nèi)無(wú)人機(jī)沖突解脫過(guò)程中消耗大的問(wèn)題，提出了基于速度障礙法的合作型無(wú)人機(jī)的最優(yōu)防相撞策略；也有一些學(xué)者在速度障礙法的基礎(chǔ)上，對(duì)其進(jìn)行了延伸和拓展應(yīng)用，文獻(xiàn)[9]為解決UAV與UAV之間的自主避碰問(wèn)題，提出了一種選擇速度障礙法(Selective Velocity Obstacle，SVO)；文獻(xiàn)[10]在UAV的非協(xié)同自主避障系統(tǒng)中應(yīng)用了速度障礙法，并提出了一種感知與避碰速度障礙法(SA-VO)。

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

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

1 UAV的自主避障模型

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

1.1 速度障礙模型的建立

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

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

(1)

(2)

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

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

1.2 速度障礙圓弧法

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

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

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

PVO+∈PVOQO

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

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

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

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

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

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

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

0≤at≤at_max

(3)

0≤an≤an_max

(4)

那么，在有限自主避障時(shí)間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性能約束的變速度自主避障算法

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

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

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

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

λvu_min<λvu<λvu_max

(16)

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

λvu_max<λvu_min<λvu

(17)

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

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

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

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

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

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

3 算法的仿真與驗(yàn)證

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

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

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

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

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

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

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

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

(2)多威脅障礙物避碰

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

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

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

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

4 結(jié)論

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

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

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

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