郭華，郭小和

南昌航空大學 飛行器工程學院，南昌 330000

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

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

1 三維速度障礙法原理

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

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

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

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

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

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

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

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

2 速度障礙法的改進

2.1 安全距離

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

2.2 自適應威脅距離

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

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

假設無人機探測到危險到開始避障的時間間隔為tm，tm是無人機以當前速度繼續(xù)飛行的最大時間，可由式（14）計算得出：

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

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

圖3 機動球Fig.3 Motive ball

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

圖4 考慮障礙物機動性的速度障礙錐Fig.4 Velocity barrier cone considering obstacle maneuverability

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

3 無人機避障模型

3.1 沖突解脫

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

圖5 無人機求解期望速度模型Fig.5 Solving expected velocity model for UAVs

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

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

3.2 航跡恢復

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

圖6 理想航跡恢復Fig.6 Ideal track recovery

4 仿真實驗與分析

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

圖7 基于改進速度障礙法的無人機避障流程圖Fig.7 Flow chart of UAV obstacle avoidance based on improved speed obstacle method

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

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

4.1 機動性障礙物避障

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

表2 考慮障礙物機動性的無人機避障過程相關參數(shù)Table 2 Parameters related to UAV obstacle avoidance process considering obstacle maneuverability

表3 未考慮障礙物機動性的無人機避障過程相關參數(shù)Table 3 Parameters related to UAV obstacle avoidance pro?cess without considering obstacle maneuverability

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

圖8 場景1 實驗仿真結果Fig.8 Experimental simulation results for Scenario 1

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

圖9 場景2 實驗仿真結果Fig.9 Experimental simulation results for Scenario 2

圖10 場景3 實驗仿真結果Fig.10 Experimental simulation results for Scenario 3

4.2 引入自適應威脅距離的機動性障礙物避障

為驗證自適應威脅距離對無人機避障的影響，在考慮動態(tài)障礙物機動性的前提下，進行前文中提到的3 種相同場景的仿真實驗。

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

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

圖11 無人機方向角變化Fig.11 Change of UAV direction angle

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

表5 場景1 避障算法時效性對比Table 5 Comparison of timeliness of obstacle avoid?ance algorithms in Scenario 1

表6 場景2 避障算法時效性對比Table 6 Comparison of timeliness of obstacle avoid?ance algorithms in Scenario 2

表7 場景3 避障算法時效性對比Table 7 Comparison of timeliness of obstacle avoid?ance algorithms in Scenario 3

5 結論

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

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

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