融合危險度模糊分級的UAV 動態避障方法

鄒振東，賈云飛，張佳慶

（1.南京理工大學機械工程學院，江蘇南京 210094；2.晉西工業集團有限責任公司，山西太原 030000）

隨著UAV 應用領域的日益拓寬，對UAV 自主避障的能力有了更高的要求。針對動態障礙空間的反應式避障問題，常見避障算法如人工勢場[1]、RRT 算法[2]、DWA[3]、VFH[4]等算法雖然在規劃和避障領域得到了廣泛應用，但應對運動障礙時的及時性往往欠佳。

速度障礙法[5-7](Velocity Obstacles，VO)是一種幾何類的碰撞預測方法。其本質是一種二維沖突求解方法，研究人員將其拓展到三維情況以適應UAV 的運動需求。基于速度障礙法，張毅等對UAV 的航跡優化生成進行了研究[8]。吳學禮等提出了UAV 沖突解脫方法[9]。畢可心等則對飛行沖突網絡進行了建模分析[10]。

在以上基礎上，文中提出了一種融合危險度模糊分級的UAV 動態避障方法，考慮其他靜態障礙的影響，在避免飛行沖突的過程中選擇更優的避障方向，提高了避障性能可靠性。

1 坐標變換

UAV 在實際避障飛行過程中，須知道障礙物與機身的實時位置關系，考慮到UAV 飛行姿態對位置信息的影響，需建立機體坐標系和世界坐標系的幾何關系模型。UAV 運動姿態如圖1 所示。

世界坐標系為Xw，機體坐標系為Xu，從機體坐標系到世界坐標系的轉換如下[11]：

Tu-w為機體在世界坐標系的位置，矩陣R為由UAV 俯仰、偏航、滾轉等運動所對應的三軸旋轉矩陣。在此基礎上，只以世界坐標衡量UAV 和周圍障礙狀態信息，可將UAV 進行質點化處理。

2 運動空間求解

2.1 速度障礙法原理

速度障礙法為一種幾何類的避障方法，主要基于UAV 與障礙物的相對位置、速度等信息來計算碰撞可能性。如圖2 所示，將UAV 視作質點，以UAV半徑對障礙進行膨脹處理，障礙總半徑即R=r1+r2，對碰撞區域進行圓形包含。在二維平面上，障礙兩側切線與Pu點形成的錐形區域即為障礙威脅區域，也稱碰撞錐，將碰撞錐沿著障礙速度矢量平移所得的區域即為速度障礙區域。兩者可視作同一原理的不同表征形式。當相對速度矢量位于碰撞錐內或者UAV速度矢量位于速度障礙區域內時，視作將要碰撞。在此基礎上，將預期將要碰撞的障礙稱為速度障礙。

圖2 二維速度障礙區域表征

為了充分利用三維空間的機動能力，將速度障礙擴展到三維區域，圓形障礙區需進行三維重建，采用圓球形式進行障礙區域表征，碰撞球保護區如圖3所示。

圖3 三維空間表征

已知UAV 的位置信息為Pu(xu,yu,zu)，速度矢量為vu(vux,vuy,vuz)，障礙物的位置和速度矢量分別為Pi(xi,yi,zi)、vi(vix,viy,viz)。可求得UAV 和障礙物的相對位置矢量Pui和相對速度矢量vui等信息，根據相對位置矢量和相對速度矢量的夾角α以及障礙錐的半頂角γ可以對碰撞危險進行預測計算。假定后續兩者都進行勻速直線運動，當α≥γ時，障礙物不會對當前的UAV 運動狀態產生威脅，當α＜γ時，則判定UAV 需進行避障反應。各變量計算如下：

2.2 加速度方向求解

在三維避障空間中，UAV 所需的避障速度范圍可由UAV 避障平面與速度障礙區域的相交曲線獲得，避障平面分為水平和豎直平面，如圖4 所示。

圖4 避障平面

由水平避障平面和相交曲線可計算得到UAV速度脫離障礙錐面區域所需的Δvx-、Δvx+和Δvy-、Δvy+，稱為速度偏量，如圖5 所示。

圖5 速度偏量

機動能力限制也是影響避障能力的主要因素之一[14]，設[-αm,αm]為加速度范圍限制，[-vm,vm]為速度限制。以1 代表加速度為正，-1 代表加速度為負，0代表加速度無論正負都無法在該方向上避開障礙。單向加速度方向值域為Ui=[1,-1,0]。以x軸為例，速度限制的作用規則如下：

根據速度限制確定了各方向上的非可用加速度之后，在可行值域中剔除不可用的項，即可對各Ui進行更新。

加速度大小范圍αv可由預期碰撞時間tp與所需速度偏量計算得出：

3 模糊危險度分析

3.1 模糊算法

模糊算法[12-14]是一種智能算法，它是由模糊指令組成的有序集根據執行指令的結果，是產生出指定問題近似解的一種算法，在無人設備運動領域得到了廣泛應用[15-17]。通過模糊算法可很好地求解UAV 運動時的靜止障礙威脅度，進而對避障規劃作出指導。

3.2 輸入與輸出的模糊化

分析障礙空間，一般來說，容易對UAV 飛行過程產生威脅的障礙具有以下特征：

①障礙距離較近；

②障礙尺寸較大；

③障礙相對速度方向的夾角較小。

因此，輸入量設定為障礙物的半徑R、距離DS，以及UAV 與其連線和速度矢量的夾角θ。輸入量的論域分別為[0,d]、[0,d]、[0,pi/2]。d為探測域大小，為了適應不同參數下的具體情況，將輸入論域進行歸一化處理。即將輸入量除以總的原論域區間大小，可得模糊論域的范圍大小為[0,1]。模糊語言變量規定如下：三輸入皆劃分為五個模糊論域，分別為TB（極大）、B（大）、N（一般）、S（小）、TS（極小）五個范圍。

輸出量為靜止障礙的危險度dng，設定危險度最大為1，則其模糊論域范圍為[0,1]，模糊語言變量也劃為TB、B、N、S、TS。

在劃分模糊論域之后，距離變量的模糊過程的隸屬度函數如圖6 所示。

圖6 距離輸入隸屬度

障礙半徑及速度相對夾角的隸屬度形式與圖6類似，基于三輸入的隸屬度函數便可完成輸入量的模糊化處理。

3.3 模糊規則

采用if-then 形式構建模糊規則，如表1 所示。

表1 模糊規則表

上述模糊規則主要是基于距離越小，障礙半徑越大，夾角越小則該障礙越危險的思想建立的，其中在矩陣運算時，輸出量新增的TB+，B+等符號是為了貼合規則設立的相應的中間結果量。舉例說明：TB 輸出表示為[0,0,0,0,1]，TB-為[0,0,0,0.3,0.7]，B+為[0,0,0,0.7,0.3]，B 表示為[0,0,0,1,0]，矩陣中的每個值為對應模糊論域的隸屬度大小，同理可由隸屬度（圖6）獲得其他符號的矩陣值。

在確定模糊規則后，將模糊規則語句用If DS=A，R=B，θ=C，Then dng=D 來表示，其中A、B、C、D 分別為相應模糊論域上的元素。根據Mamdani推理法[18]，即模糊蘊含關系采用最小運算規則。由表1 的13 條模糊規則可生成13 個模糊關系矩陣Ru，可用公式表示為：

其中，×為叉乘運算算子，?為合成運算算子，i為第i條規則。叉乘運算基于模糊最小蘊含過程為求交運算過程，?過程為各因素求交再求或的過程。在求出每個模糊規則后，總的模糊規則為各分規則矩陣各元素求或后得出：

任意條件下的危險度輸出為：

3.4 解模糊

由模糊規則計算得到的模糊推理結果仍然具有模糊性，需要進行解模糊化獲取精確數字量。采用重心法進行解模糊化，可得：

其中，δi為求解出的各等級危險度權重，Ei為相應數值論域中的值。

4 融合策略

由以上分析，針對環境中的預期碰撞速度障礙和靜止障礙，可分別求得加速度可用控制集和危險度。當UAV 飛行過程中遇到單個或多個速度障礙時，首先需確定UAV 可用的避障方向。由于三軸避障方向具備獨立性，以單方向運動指令為例，當該方向上加減速指令都可使UAV 脫離速度障礙時，再考慮周圍靜止障礙的影響，靜止障礙貢獻方向為相對位置的相反方向。為了綜合考慮各因素，提出了一種優化評價函數，用于計算最優指令，函數需考慮的因素如下。

1）UAV 在該方向加速，減速指令下脫離所有不同速度障礙區域的最短預期脫離時間to+和to-；

2）預期碰撞為真時的最短預期碰撞時間tpmi；

3）靜止障礙的危險度綜合度量，其值越高影響越強。

將各因素影響進行歸一化處理，用加減速方向上的脫離時間與最短預期碰撞時間作商，函數如下：

其中，U+為該方向正加速度的權重，U-為負向加速度的權重。n1為加速度貢獻為正的靜止障礙個數，n2為加速度貢獻為負的靜止障礙個數，a為調節靜止障礙影響因素大小的系數，dngi為第i個靜止障礙的危險度。后一項的值域范圍為[0,1]，函數可很好地將障礙信息綜合考慮。

接著比較正加速度和負加速度權重大小，取加速度方向為其中較小者方向。

5 仿真分析

5.1 環境測試

為驗證算法的有效性，分別在多動態障礙環境和復雜障礙環境進行了驗證分析。仿真條件設置如下:UAV 初始位置[0,50,50]，初始速度3 m/s，障礙半徑為2 m，計算步長為Δt=0.2 s，可用加速度最大1 m/s2，初始航向0 度，障礙空間為[[0,100],[0,100],[0,100]]，目標點為[100,50,50]，單位為m。

速度障礙采用隨機生成機制，避障次數為2，每次隨機生成2～4 個可能碰撞的速度障礙，在多障礙空間的避障路線如圖7 所示。

圖7 動態障礙避障軌跡

如圖7 所示，實線為飛行軌跡，虛線為動態障礙軌跡，UAV 在同時面臨多個動態障礙威脅時，通過改變速度方向，對兩個碰撞點都完成了規避，避免了碰撞的發生。

在飛行空間內添加靜態障礙團，使其分布不均勻，同樣設定了兩次預期的碰撞，通過設定較高的加速度閾值保證了多方向的避障可行性。在此條件下的避障路線如圖8 所示。

圖8 復雜障礙環境避障路線

圖8 中，UAV 在兩次預期碰撞點前方都通過改變自身運動軌跡完成了避碰，且在避碰時UAV 完成了偏向障礙較稀疏的一側的運動。

5.2 對比分析

為了驗證加入危險度模糊分級策略的有效性，通過仿真分析將該改進速度障礙法與不加入危險分級策略的速度障礙法進行了對比，分析其在1 000 次隨機環境中的性能表現。障礙空間大小、初始點、目標點及UAV 性能不變，隨機靜態障礙采用集群分布式產生，隨機初始化多個中心位置，以該位置為中心擴展障礙，形成多障礙集群的分布。每次隨機環境產生30 個靜態障礙和30 個動態障礙，測試結果如表2 所示。

表2 改進效果對比

由表2 可知，改進后的VOM 在避碰成功率上提高了6%左右，成功避障時平均路徑長度變化不大，主要由于反應避碰過程較快，偏移過程對于路徑長度的貢獻較小。平均避碰次數相比之下減小了11%左右。

6 結束語

針對三維空間中多速度障礙及復雜障礙的反應式避障問題，文中提出了一種基于危險度模糊分級的改進速度障礙法，通過求解可用加速度控制，在其基礎上采用模糊算法輸出各靜止障礙危險度分級，并提出一種優化評價函數綜合考慮障礙空間各影響來輸出避障指令，使得避障時能朝向障礙較少的方向完成避障。相比原算法，改進算法在避障成功率和平均避障次數等性能上表現出更優異的性能。