改進(jìn)速度障礙法的無(wú)人機(jī)局部路徑規(guī)劃算法

郭華，郭小和

南昌航空大學(xué) 飛行器工程學(xué)院，南昌 330000

隨著航空技術(shù)的發(fā)展，無(wú)人機(jī)在現(xiàn)代戰(zhàn)爭(zhēng)和民用領(lǐng)域中扮演著越來(lái)越重要的角色，應(yīng)用領(lǐng)域不斷擴(kuò)大，航跡規(guī)劃作為無(wú)人機(jī)任務(wù)規(guī)劃的核心，是無(wú)人機(jī)完成任務(wù)的關(guān)鍵步驟［1-2］。其中的關(guān)鍵為躲避動(dòng)態(tài)障礙物或威脅需進(jìn)行的局部路徑規(guī)劃算法，要求具有計(jì)算量小、實(shí)時(shí)性好的特點(diǎn)［3］。常見(jiàn)的局部路徑規(guī)劃算法主要有人工勢(shì)場(chǎng)法、導(dǎo)引法、速度障礙法、基于機(jī)器學(xué)習(xí)的避障規(guī)劃算法等［4-5］。人工勢(shì)場(chǎng)算法雖然計(jì)算量小、應(yīng)答時(shí)間短，但是存在不能準(zhǔn)確到達(dá)目標(biāo)點(diǎn)、飛行路徑波動(dòng)、路徑不可飛等問(wèn)題，許多學(xué)者對(duì)人工勢(shì)場(chǎng)法進(jìn)行了優(yōu)化設(shè)計(jì)［6-8］。導(dǎo)引法借鑒導(dǎo)彈制導(dǎo)律，將導(dǎo)引方法應(yīng)用于避障算法，但是此算法沒(méi)有很好考慮無(wú)人機(jī)避障完成后的運(yùn)動(dòng)，也不能很好解決多障礙物入侵問(wèn)題［9］。常見(jiàn)的機(jī)器學(xué)習(xí)算法有神經(jīng)網(wǎng)絡(luò)、強(qiáng)化學(xué)習(xí)與深度強(qiáng)化學(xué)習(xí)等，但此類算法復(fù)雜耗時(shí)。此外一些智能算法，比如遺傳算法、粒子群算法和模擬退火算法等，此類算法計(jì)算量偏大，多用于基于已知環(huán)境信息的無(wú)人機(jī)全局路徑規(guī)劃之中［10-12］。速度障礙法最開(kāi)始用于解決移動(dòng)機(jī)器人的自主避障問(wèn)題［13-14］，現(xiàn)在廣泛應(yīng)用于無(wú)人艇和無(wú)人機(jī)的避障與航跡規(guī)劃算法中，Durand 和Barnier［15］最早在無(wú)人機(jī)避免與其他無(wú)人機(jī)發(fā)生沖突的過(guò)程中引入速度障礙法。目前很多學(xué)者通過(guò)降維思想來(lái)處理三維空間的路徑避障問(wèn)題，文獻(xiàn)［16］通過(guò)降維處理避碰障礙錐，利用比例導(dǎo)引法完成三維避碰。文獻(xiàn)［17-18］基于平面幾何優(yōu)化處理和最少機(jī)動(dòng)的原則，建立了航空器之間的飛行沖突解脫和航跡恢復(fù)模型，并且考慮了無(wú)人機(jī)在沖突解脫后的航跡恢復(fù)問(wèn)題。文獻(xiàn)［19］提出基于時(shí)間速度障礙法，為飛行器提供了更大范圍的可選避障速度向量范圍。文獻(xiàn)［20］考慮了二維環(huán)境下的動(dòng)態(tài)障礙物運(yùn)動(dòng)的不確定性，提高了水下機(jī)器人在實(shí)際運(yùn)行中的避撞穩(wěn)定性。文獻(xiàn)［21］提出一種基于三維空間障礙球冠的動(dòng)態(tài)不確定速度障礙模型，并將三維動(dòng)態(tài)不確定無(wú)人機(jī)自主避障算法應(yīng)用于Pythagorean Hodograph（PH）曲線的在線路徑當(dāng)中。文獻(xiàn)［22-23］將速度障礙法運(yùn)用到多機(jī)同構(gòu)系統(tǒng)躲避機(jī)動(dòng)障礙物和三維空間內(nèi)單機(jī)躲避障礙物之中。

上述三維速度障礙法為保證安全避障，安全避障距離普遍偏大，降低了原規(guī)劃航跡的使用率。本文通過(guò)建立三維空間障礙物運(yùn)動(dòng)不確定性速度障礙錐，分析空間幾何模型，求解無(wú)人機(jī)在三維空間中自主避障的最小速度矢量偏轉(zhuǎn)角，并引入自適應(yīng)威脅距離，提高了原規(guī)劃航跡的使用率。

1 三維速度障礙法原理

傳統(tǒng)速度障礙法往往只考慮二維平面，根據(jù)空中交通管制規(guī)則，無(wú)人機(jī)與障礙物之間應(yīng)保持一定的安全距離［24］，若將飛機(jī)視為質(zhì)點(diǎn)，障礙物進(jìn)行相應(yīng)“膨脹”，則無(wú)人機(jī)的動(dòng)態(tài)避障過(guò)程可簡(jiǎn)化為一個(gè)質(zhì)點(diǎn)躲避膨脹圓的過(guò)程。速度障礙（Velocity Obstacle，VO）是在無(wú)人機(jī)的速度空間內(nèi)生成一個(gè)圓錐形障礙區(qū)間，只要相對(duì)速度矢量在錐形區(qū)域之外，在將來(lái)的一段時(shí)間內(nèi)無(wú)人機(jī)都不會(huì)與障礙物發(fā)生碰撞。

在三維空間中，假設(shè)無(wú)人機(jī)的輻射半徑為Ru，某動(dòng)態(tài)威脅障礙物的輻射半徑為Ro。某時(shí)刻無(wú)人機(jī)與障礙物的位置分別為Pu、Po；無(wú)人機(jī)速度矢量為Vu；障礙物速度矢量為Vo；無(wú)人機(jī)的位姿信息(Pu，Vu)和障礙物的位姿信息(Po，Vo)可由式（1）～式（4）表示。

式中：vu、vo分別為無(wú)人機(jī)與障礙物的速度矢量大?。沪萿、φu和θo、φo分別為無(wú)人機(jī)與障礙物的速度矢量方向角（θ為俯仰角，φ為偏航角）。

如圖1 所示，以Po為球心，R為半徑作球體，以Pu為頂點(diǎn)作錐體與球Po相切，球體可以較好的模擬飛機(jī)等具有機(jī)動(dòng)性較強(qiáng)的動(dòng)態(tài)障礙物。其中Vuo為二者相對(duì)矢量速度；相對(duì)速度的大小為vuo；d為無(wú)人機(jī)與障礙物之間的歐幾里得距離。若矢量Vuo末端在三維空間速度障礙錐內(nèi)，則動(dòng)態(tài)障礙物對(duì)無(wú)人機(jī)存在威脅。

圖1 三維速度障礙法原理Fig.1 Schematic of 3D velocity obstacle method

通常情況下，我們更希望直接通過(guò)無(wú)人機(jī)的速度矢量Vu來(lái)判斷障礙物與飛機(jī)是否存在飛行沖突。將Pu和Po沿向量Vo的方向平移距離‖VoΔt‖，得到Qu、Qo，如式（8）和式（9）所示；Δt為一個(gè)單位時(shí)間間隔。以Qo為球心、R為半徑作球體，建立基于無(wú)人機(jī)速度矢量Vu的三維空間速度障礙錐模型，如圖2 所示。如果Vu的矢量末端在錐體區(qū)域內(nèi)，則障礙物對(duì)無(wú)人機(jī)產(chǎn)生威脅。

圖2 三維空間速度障礙錐Fig.2 3D spatial velocity obstacle cone

障礙物對(duì)無(wú)人機(jī)是否造成威脅，還可以通過(guò)比較角度α0與α的大小來(lái)判定，其中α0為空間速度障礙錐的半頂角；α為相對(duì)速度Vuo與空間障礙錐線之間的夾角。若α>α0，障礙物不具有威脅性；若α<α0，障礙物具有威脅性。

2 速度障礙法的改進(jìn)

2.1 安全距離

無(wú)人機(jī)的局部路徑避障規(guī)劃過(guò)程是在傳感器探測(cè)到障礙物的尺寸、位置和速度等信息基礎(chǔ)上進(jìn)行的，由于傳感器精度問(wèn)題，可能導(dǎo)致探測(cè)到的障礙物信息存在偏差，并且速度障礙法在描述障礙物的信息時(shí)采用確定的值，這就可能導(dǎo)致發(fā)生碰撞。除此之外，速度障礙法的速度在周期內(nèi)被認(rèn)為是不發(fā)生變化的，直到下一刻才發(fā)生變化，所以速度障礙法要求傳感器的刷新頻率非常高。由此引入安全距離Rsafe，使膨脹半徑再次增大，可提高無(wú)人機(jī)安全性裕度，此時(shí)膨脹半徑可優(yōu)化為R1，如式（12）所示：

2.2 自適應(yīng)威脅距離

傳統(tǒng)速度障礙法存在避障保守的問(wèn)題，障礙物一旦位于無(wú)人機(jī)傳感器探測(cè)范圍內(nèi)，無(wú)人機(jī)便開(kāi)始沖突探測(cè)過(guò)程，若發(fā)現(xiàn)威脅便立刻采取避障措施。為消除無(wú)人機(jī)的這種保守避障策略，避免無(wú)人機(jī)在非必要情況下提前脫離原路徑，引入自適應(yīng)威脅距離dsafe。當(dāng)無(wú)人機(jī)與障礙物之間的相對(duì)距離大于威脅距離時(shí)，無(wú)人機(jī)仍以當(dāng)前速度飛行；相對(duì)距離小于或者等于威脅距離時(shí)，才開(kāi)始采取動(dòng)態(tài)避險(xiǎn)策略。

經(jīng)算法計(jì)算及結(jié)果分析，定義自適應(yīng)威脅距離dsafe，如式（13）所示。dsafe的數(shù)值設(shè)定應(yīng)與無(wú)人機(jī)、障礙物的速度和輻射半徑等相關(guān)，dsafe的設(shè)定不宜過(guò)小，需要保證無(wú)人機(jī)的航向調(diào)整幅度滿足無(wú)人機(jī)本身的約束條件，dsafe的設(shè)定可以提高原航跡的利用率。

假設(shè)無(wú)人機(jī)探測(cè)到危險(xiǎn)到開(kāi)始避障的時(shí)間間隔為tm，tm是無(wú)人機(jī)以當(dāng)前速度繼續(xù)飛行的最大時(shí)間，可由式（14）計(jì)算得出：

2.3 障礙物動(dòng)態(tài)不確定性處理

無(wú)人機(jī)執(zhí)行任務(wù)時(shí)，需要考慮無(wú)人機(jī)自身攜帶的障礙物感知模塊在感知?jiǎng)屿o障礙物時(shí)存在一定的測(cè)量誤差，以及感知到的動(dòng)態(tài)威脅障礙物具有一定機(jī)動(dòng)性等情況。若無(wú)人機(jī)探測(cè)到障礙物速度Vo，且該障礙物存在最大動(dòng)態(tài)不確定度ε，如圖3 所示，以Qu為球心，可以確定一個(gè)半徑為r的機(jī)動(dòng)球，結(jié)合機(jī)動(dòng)球截面圖，其中A、B兩點(diǎn)為臨界機(jī)動(dòng)點(diǎn)，設(shè)Qu點(diǎn)到AB線段的最短距離為dmin，半徑r可由式（17）表示。

圖3 機(jī)動(dòng)球Fig.3 Motive ball

在初始三維空間速度障礙的基礎(chǔ)上建立三維空間不確定速度障礙錐，如圖4 所示，母線l1、l2與機(jī)動(dòng)球相切，此時(shí)膨脹圓半徑可由R1更新為R2，由式（18）表示。

圖4 考慮障礙物機(jī)動(dòng)性的速度障礙錐Fig.4 Velocity barrier cone considering obstacle maneuverability

假設(shè)空間障礙錐軸線上有一單位向量n，圓錐體頂點(diǎn)D在軸線的延長(zhǎng)線上，D點(diǎn)位置可由式（19）～式（21）得出：

3 無(wú)人機(jī)避障模型

3.1 沖突解脫

無(wú)人機(jī)在躲避威脅障礙物時(shí)，保持速度矢量Vu的大小不變，只改變速度矢量Vu的方向角。為方便研究Vu的偏轉(zhuǎn)方向，以Pu為球心、vu為半徑作球體，該球體稱為速度球，如圖5 所示，其陰影球冠為速度球與空間障礙錐的相交部分。無(wú)人機(jī)要實(shí)現(xiàn)避障，需將Vu的末端偏轉(zhuǎn)出陰影球冠區(qū)域。在無(wú)人機(jī)求解期望速度模型圖中，表示無(wú)人機(jī)的理想期望速度；相對(duì)應(yīng)的期望相對(duì)速度分別表示為；E和F為無(wú)人機(jī)避障的臨界狀態(tài)點(diǎn)。

圖5 無(wú)人機(jī)求解期望速度模型Fig.5 Solving expected velocity model for UAVs

通過(guò)式（24）～式（26）可以求解出x、y、z的值，進(jìn)而確定單位向量m。

按照式（24）～式（30）計(jì)算方法同樣可以求得無(wú)人機(jī)避障臨界點(diǎn)E。通過(guò)求解期望矢量速度，進(jìn)而可得無(wú)人機(jī)期望速度矢量的方向角為和，如式（31）～式（33）所示：

3.2 航跡恢復(fù)

理想狀態(tài)下，無(wú)人機(jī)躲避障礙物以后，無(wú)人機(jī)要逐漸恢復(fù)到全局路徑上進(jìn)入正常飛行狀態(tài)，如圖6 所示。為了避免無(wú)人機(jī)的連續(xù)機(jī)動(dòng)而增加風(fēng)險(xiǎn)，采取無(wú)人機(jī)仍以當(dāng)前速度飛行，直至無(wú)人機(jī)與障礙物之間的距離開(kāi)始增大，再改變航向角的策略。

圖6 理想航跡恢復(fù)Fig.6 Ideal track recovery

4 仿真實(shí)驗(yàn)與分析

根據(jù)以上對(duì)改進(jìn)速度障礙法的設(shè)計(jì)和分析，基于改進(jìn)速度障礙法的無(wú)人機(jī)避障步驟如圖7 所示。無(wú)人機(jī)提取自身與障礙物信息后，建立考慮障礙物機(jī)動(dòng)性的速度障礙模型，進(jìn)行沖突探測(cè)，若障礙物具有威脅，為避免無(wú)人機(jī)保守避障，無(wú)人機(jī)需繼續(xù)以當(dāng)前速度飛行一段時(shí)間后再改變速度矢量方向角進(jìn)行避障；當(dāng)無(wú)人機(jī)開(kāi)始遠(yuǎn)離障礙物時(shí)，無(wú)人機(jī)可重新調(diào)整飛行方向，使無(wú)人機(jī)朝目標(biāo)點(diǎn)飛行。

圖7 基于改進(jìn)速度障礙法的無(wú)人機(jī)避障流程圖Fig.7 Flow chart of UAV obstacle avoidance based on improved speed obstacle method

采用MATLAB 軟件進(jìn)行仿真實(shí)驗(yàn)。首先設(shè)置一些基本初始參數(shù)：無(wú)人機(jī)的任務(wù)起點(diǎn)坐標(biāo)(0，0，0) m；終點(diǎn)坐標(biāo)(1 000，800，200) m；無(wú)人機(jī)速度大小vu=20 m/s；無(wú)人機(jī)輻射半徑Ru=25 m；障礙物輻射半徑Ro=15 m；安全距離Rsafe=8 m；傳感器探測(cè)范圍dsensor=600 m。為了驗(yàn)證算法的有效性和適用性，設(shè)定了3種場(chǎng)景，分別為無(wú)人機(jī)與障礙物對(duì)遇、追擊及交叉，依次對(duì)應(yīng)表1的3種場(chǎng)景。

表1 障礙物初始條件參數(shù)Table 1 Obstacle initial condition parameters

4.1 機(jī)動(dòng)性障礙物避障

為驗(yàn)證考慮障礙物機(jī)動(dòng)性的避障算法的有效性，假設(shè)機(jī)動(dòng)性障礙物的速度方向角的最大不確定度ε=10°，針對(duì)表1 所示的3 種場(chǎng)景，通過(guò)算法計(jì)算，不管是否考慮障礙物的機(jī)動(dòng)性，3 種情景下α始終小于α0，即障礙物具有威脅性，表2 和表3 分別記錄了考慮障礙物機(jī)動(dòng)性與未考慮障礙物機(jī)動(dòng)性的無(wú)人機(jī)避障過(guò)程的相關(guān)參數(shù)。

表2 考慮障礙物機(jī)動(dòng)性的無(wú)人機(jī)避障過(guò)程相關(guān)參數(shù)Table 2 Parameters related to UAV obstacle avoidance process considering obstacle maneuverability

表3 未考慮障礙物機(jī)動(dòng)性的無(wú)人機(jī)避障過(guò)程相關(guān)參數(shù)Table 3 Parameters related to UAV obstacle avoidance pro?cess without considering obstacle maneuverability

圖8（a）和圖8（b）展示了場(chǎng)景1 下無(wú)人機(jī)與障礙物的對(duì)遇運(yùn)動(dòng)過(guò)程（藍(lán)色圓圈代表無(wú)人機(jī)運(yùn)動(dòng)軌跡，紅色圓圈代表障礙物運(yùn)動(dòng)軌跡），可以看出無(wú)人機(jī)對(duì)障礙物都進(jìn)行了躲避，并達(dá)到任務(wù)終點(diǎn)。結(jié)合表2 和表3，在考慮障礙物機(jī)動(dòng)性的情況下，膨脹輻射半徑為50.09 m，無(wú)人機(jī)在位置（339.47，271.57，67.89）m 處開(kāi)始改變速度矢量，偏航角和俯仰角分別調(diào)整為44.03°和7.37°；在不考慮障礙物機(jī)動(dòng)性的情況下，輻射半徑為48 m，無(wú)人機(jī)在相同位置調(diào)整方向角，偏航角和俯仰角分別調(diào)整為54.71°和6.65°。圖8（c）表示無(wú)人機(jī)與障礙物之間的距離變化，在考慮障礙物機(jī)動(dòng)性二者達(dá)到的最近距離為56.262 7 m，大于對(duì)應(yīng)的膨脹半徑，二者沒(méi)有發(fā)生碰撞；而在不考慮障礙物機(jī)動(dòng)性的情況下，二者之間的最近距離為14.913 9 m，小于對(duì)應(yīng)膨脹半徑，說(shuō)明無(wú)人機(jī)與障礙物發(fā)生了碰撞，避障失敗。圖8（d）記錄無(wú)人機(jī)運(yùn)動(dòng)過(guò)程中俯仰角與偏航角的變化。

圖8 場(chǎng)景1 實(shí)驗(yàn)仿真結(jié)果Fig.8 Experimental simulation results for Scenario 1

同理分析圖9 和圖10，無(wú)人機(jī)采用考慮動(dòng)態(tài)障礙物機(jī)動(dòng)性的算法，可在場(chǎng)景2 和場(chǎng)景3 中順利完成避障過(guò)程，且無(wú)人機(jī)的偏航角度與俯仰角度都滿足無(wú)人機(jī)的自身約束條件。

圖9 場(chǎng)景2 實(shí)驗(yàn)仿真結(jié)果Fig.9 Experimental simulation results for Scenario 2

圖10 場(chǎng)景3 實(shí)驗(yàn)仿真結(jié)果Fig.10 Experimental simulation results for Scenario 3

4.2 引入自適應(yīng)威脅距離的機(jī)動(dòng)性障礙物避障

為驗(yàn)證自適應(yīng)威脅距離對(duì)無(wú)人機(jī)避障的影響，在考慮動(dòng)態(tài)障礙物機(jī)動(dòng)性的前提下，進(jìn)行前文中提到的3 種相同場(chǎng)景的仿真實(shí)驗(yàn)。

通過(guò)表4 中的相關(guān)參數(shù)可知，在避障算法中引入威脅距離，無(wú)人機(jī)同樣完成了對(duì)機(jī)動(dòng)性障礙物的躲避。通過(guò)對(duì)比表2 與表4 中的UAV 飛行狀態(tài)改變點(diǎn)的坐標(biāo)值，可以看出自適應(yīng)威脅距離的引入，無(wú)人機(jī)推遲了飛行狀態(tài)的調(diào)整，進(jìn)而提高了原飛行路徑的利用率。圖11 記錄了無(wú)人機(jī)在3 種情景下的偏航角與俯仰角的變化過(guò)程，均能滿足無(wú)人機(jī)自身的約束條件。

表4 考慮障礙物機(jī)動(dòng)性并引入威脅距離的無(wú)人機(jī)避障過(guò)程相關(guān)參數(shù)Table 4 Relevant parameters of UAV obstacle avoid?ance process considering obstacle maneuver?ability but not threat distance m

圖11 無(wú)人機(jī)方向角變化Fig.11 Change of UAV direction angle

通過(guò)分析圖8～圖11 并結(jié)合仿真計(jì)算時(shí)間，可得3 種場(chǎng)景下的算法求解耗時(shí)、飛行解脫耗時(shí)等指標(biāo)的對(duì)比仿真結(jié)果如表5～表7 所示，結(jié)果表明，雖然引入威脅距離前后的速度障礙算法在3 種場(chǎng)景下的求解耗時(shí)均在0.008 s 左右，但引入威脅距離后的優(yōu)化速度障礙法，由于無(wú)人機(jī)在保證安全距離的前提下推遲了飛行狀態(tài)的調(diào)整，在距離機(jī)動(dòng)障礙物更近的情況下才開(kāi)始規(guī)避，所以在飛行沖突解脫過(guò)程中耗時(shí)更短，提高了機(jī)動(dòng)障礙規(guī)避的時(shí)效性。

表5 場(chǎng)景1 避障算法時(shí)效性對(duì)比Table 5 Comparison of timeliness of obstacle avoid?ance algorithms in Scenario 1

表6 場(chǎng)景2 避障算法時(shí)效性對(duì)比Table 6 Comparison of timeliness of obstacle avoid?ance algorithms in Scenario 2

表7 場(chǎng)景3 避障算法時(shí)效性對(duì)比Table 7 Comparison of timeliness of obstacle avoid?ance algorithms in Scenario 3

5 結(jié)論

針對(duì)無(wú)人機(jī)基于復(fù)雜環(huán)境感知進(jìn)行局部路徑再規(guī)劃的實(shí)時(shí)與安全性問(wèn)題，提出一種基于改進(jìn)速度障礙法的局部路徑避障規(guī)劃算法，算例仿真結(jié)果表明：

1）建立動(dòng)態(tài)障礙物的三維速度障礙模型，將機(jī)動(dòng)性動(dòng)態(tài)障礙物在速度空間中的運(yùn)動(dòng)不確定轉(zhuǎn)化為位置不確定，克服了傳統(tǒng)速度障礙法僅能對(duì)勻速障礙物實(shí)現(xiàn)良好避障的缺點(diǎn)，提高了避障能力，保證了避障機(jī)體的飛行安全。

2）在避障算法中定義和引入自適應(yīng)威脅距離，保證無(wú)人機(jī)避障安全性的同時(shí)也為無(wú)人機(jī)提供了更大范圍的可選擇避障速度向量范圍，避免無(wú)人機(jī)提前脫離任務(wù)路徑，提高了原航跡的利用率，且算法計(jì)算效率滿足機(jī)動(dòng)障礙規(guī)避的時(shí)效性需求。