基于VFH*的水面無人艇局部避障方法

魏新勇，黃燁笙，洪曉斌

(華南理工大學機械與汽車工程學院，廣東 廣州 510641)

0 引言

水面無人艇是具備高度非線性動力學特征，能在無人干預的情況下在各種復雜未知的水面環境下執行任務的新型載體，目前在海域巡邏、水質監控和海底測繪等領域具有巨大的應用需求。由于水面無人艇作業環境復雜，因此具備避障規劃能力不僅是無人艇安全航行的保障，也是無人艇執行各種作業的前提。無人艇避障規劃包括全局避障規劃和局部避障規劃。全局避障規劃一般采取靜態的方法實現，通過已知的海域信息，在航行前規劃出一條從航行起點到終點的無碰路徑；局部避障規劃則根據實時的艇載傳感器信息獲取無人艇位姿和探測范圍內的障礙物分布情況，從而根據局部信息實時規劃下一時刻的無碰運動。由于海域中存在眾多不可預見的未知障礙物，因此根據實時的傳感參量進行局部避障規劃具有重要的研究價值。

目前無人艇局部避障規劃算法領域有不少學者進行研究并取得了相應的成果。H Kim等[1]提出了一種基于Theta*算法的新方法來實時規劃無人艇路徑；Aaron等[2]提出了基于標準和規則的無人艇局部避障策略，該策略根據時變海域環境和目標的碰撞危險度來確定；唐平鵬等[3-4]采用分層策略將動態窗口引用到了無人艇局部危險避障中；吳博等[5]提出了一種基于速度障礙法并考慮風浪流影響下的無人艇操縱運動特性的自主避碰算法。然而上述方法多為非預測型方法，且假設障礙物的位置和運動信息完全可知，在未知復雜的海洋環境以及無人艇高速行駛下應用具有一定的局限性。

向量場直方圖法（vector field histogram，VFH）[6-8]常用于無人載體在未知環境下的局部避障規劃，通過測距傳感器信息建立向量場直方圖進行兩級環境數據壓縮，考慮無人載體前向運動代價進而得出當前最優可行方向，具有實時性高、避障可靠等優點，近年來得到了很好的應用和發展。如F.Fraundorfer等[9]結合雙目視覺信息，采用VFH+算法實現無人機在三維環境下的導航規劃；孫揚智等[10]提出了一種融合卡爾曼濾波的VFH算法，消除移動機器人在目標點附近的轉向抖動，實現了機器人在野外自主作業的動態路徑規劃；徐玉華等[11]則基于激光測距儀提出一種自適應調節閾值的VFH算法來實現移動機器人的自主避障。然而，目前VFH算法主要在移動機器人和無人機領域研究較多，而在水面無人艇研究領域涉及很少，且無人艇與機器人和無人機相比具有較大的長寬比，容易造成障礙物位于傳感器探測盲區，發生尾端碰撞等問題。

因此，本文針對無人艇在未知海洋環境下的局部避障問題，以激光雷達前后數據幀融合構建的無人艇環境模型為基礎，采用具有前向預測功能的VFH*局部避障規劃算法，實現無人艇在每一瞬時的狀態感知和局部避障決策。

1 基于VFH*的水面無人艇局部避障方法

1.1 無人艇水面環境模型構建

水面無人艇局部避障規劃的準確實現首先依賴于其環境感知能力的好壞，激光雷達作為一種基于TOF測距原理的高精度、高分辨率的外圍傳感器，可以實時獲取水面無人艇前向區域內障礙物的相對距離與方位，易于構建無人艇環境模型。

激光雷達數據獲取簡單易用，但傳感器的探測范圍存在一定的盲區且每次只能獲取一幀數據，如果僅根據當前幀的數據進行環境建模容易與已經避開的障礙物發生碰撞。如圖1所示，激光雷達安裝在無人艇前端，無人艇在行進的過程中已經探測到障礙物并且進行規避，但隨后由于障礙物位于傳感器探測的盲區且無人艇艇身未完全通過障礙物，因此在無人艇轉向的過程中容易發生尾端碰撞。針對此問題，提出結合激光雷達前后幀數據融合進行無人艇水面環境模型構建。

圖1 無人艇尾端碰撞場景

首先通過三面角余弦公式計算兩點之間的相對距離（其中R為地球半徑）：

接著通過球面正弦公式計算兩點之間的相對方位角：

根據相對距離和相對方位角求得平移坐標為

1.2 障礙物向量場直方圖構建

VFH*局部避障規劃算法采用柵格表示法將無人艇水面環境分解為二維直方圖網格，每個網格的障礙物置信度通過激光雷達建立的環境模型信息不斷更新，進而構建障礙物向量場直方圖來表征無人艇的可行和不可行區域。

式中： (x0,y0)——無人艇幾何中心的坐標；

(xi,yj) ——障礙物網格的坐標。

其中，di,j為 障礙物網格Ci,j到無人艇幾何中心的距離；ci,j為概率函數，不同于傳統的超聲波傳感器[12-13]需要對錐形區域內的障礙物網格置信度進行概率評估，激光雷達能夠準確返回某一方位的距離測量值，因此，當障礙物網格Ci,j被障礙物覆蓋時，ci,j的值設為1，反之，ci,j的值設為0。

其中：

圖2 障礙物網格膨化處理

主直方圖中每一個分區的障礙物密度定義為該分區最危險網格的障礙物向量強度。這是因為采用了激光雷達前后幀數據融合后，實際導入的環境模型中在徑向上可能存在多個障礙物網格，采用傳統的加權求和方式容易造成該方向上危險程度的誤判。如圖3所示的網格地圖，無人艇此刻向左和向右轉向避開正前方障礙物的概率是一致的，若采用加權求和的方式計算則導致無人艇更傾向于向左轉向。

圖3 無人艇網格地圖

1.3 無人艇運動方向選取與前向預測尋優

向量場直方圖的建立將無人艇周圍環境劃分可行區域和不可行區域。確定可行區域的左右邊界kl和kr，如果左右邊界的差值小于設定閾值Smax，那么此可行區域定義為窄谷區，選擇中間方向作為候選方向：

圖4 無人艇前向預測節點示意圖

為了量化每一個初始候選方向的前進代價，定義搜索樹上每一個節點的代價值由其代價函數和啟發式函數構成，總的代價值為

由于無人艇在局部避障過程中需要以目標點為導向，因此還需滿足以下條件：

2 實驗結果與分析

2.1 仿真實驗結果與分析

為了驗證本文所述算法的局部避障性能，通過遠程API函數建立Matlab與V-REP仿真平臺之間的通信，進行無人艇局部避障性能的半實物仿真實驗。

V-REP仿真平臺具有豐富的傳感器模塊和執行機構模塊，可以在Matlab窗口發送指令實時獲取V-REP平臺中無人艇載激光雷達、GPS和電子羅經等傳感器信息，經過算法解算后將下一運動方向返回V-REP仿真平臺的執行模塊，控制無人艇在V-REP仿真平臺下的運動。具體仿真環境如圖5所示，可以在無人艇周圍設置障礙物作為無人艇局部避障過程中的避碰對象，采用 SICK TiM310 Fast型號激光雷達作為障礙物測距傳感器，安裝在無人艇前端。

圖5 V-REP仿真環境

圖6是未結合激光雷達前后幀數據融合的無人艇局部避障仿真實驗。如圖6(a) 所示，在仿真初始階段，無人艇能夠順利識別到障礙物并進行轉向避碰，然而當障礙物處于激光雷達探測盲區時，無人艇受目標點導向的指引，持續左轉，最終與障礙物發生碰撞，如圖6(b)所示。

圖6 未結合前后幀數據融合的仿真實驗過程

圖7 結合前后幀數據融合的仿真實驗過程

圖7是結合激光雷達前后幀數據融合的無人艇局部避障仿真實驗，與前一仿真實驗相似，在障礙物位于激光雷達探測盲區的情況下，無人艇在左轉靠近目標點的過程中仍有可能與障礙物發生碰撞，但由于采用了激光雷達前后幀數據融合算法來建立無人艇環境模型，使得無人艇對之前探測過的障礙物仍具有一定的短期記憶功能，因此無人艇在當前時刻通過直行使整個艇體能過完全避開障礙物，然后再進行左轉靠近目標點，如圖7(b)所示。

圖8為無人艇在兩次仿真過程中輸出的避障方向。可以看出，在未結合無人艇前后幀數據融合的仿真過程中，算法在6.7 s至12.6 s的時間段內持續輸出向左的避碰方向，最終導致無人艇在轉向過程中與障礙物發生碰撞；而在結合無人艇前后幀數據融合的仿真過程中，算法在7.3 s至7.7 s的過程中輸出直行的避碰方向，使得無人艇在轉向過程中能夠短暫直行從而使艇體完全避開障礙物。

圖8 無人艇避障方向

圖9(a) 是算法未結合前向預測功能的無人艇局部避障仿真結果圖，此時預測步長算法僅僅根據當前獲取到的傳感器信息進行局部尋優，導致無人艇在第一次避障時獲得的局部最優轉向（左轉59°）并不是全局最優轉向，使得無人艇在局部避障過程中陷入局部最小值，最終獲得的避障軌跡稍顯冗長。圖9(b)是算法結合前向預測功能的無人艇局部避障仿真結果圖，此時預測步長由于考慮了無人艇沿每個初始候選方向前進的代價，無人艇在第一次避障時選擇右轉64°作為當前的避障角度，使得無人艇能夠跳出第一次避障時的局部最小值，從而能夠獲得較短的避障軌跡。

圖9 無人艇局部避障仿真結果圖

2.2 實物實驗結果與分析

本文采用的實驗平臺是一艘艇長2m的全電力推進無人艇，如圖10所示。環境感知傳感器包括激光雷達和網絡攝像頭，通過無線路由模塊返回岸基控制端；而位姿測量傳感器GPS和電子羅經，則通過射頻模塊進行傳輸。艇載激光雷達選用SICK LMS200，理論最遠探測范圍為80m，最大探測角度范圍為180°。艇載網絡攝像頭可以實時獲取水面圖像，從而監控無人艇行進過程。

圖10 水面無人艇實驗平臺

為了驗證本文所述算法的有效性，在水池內人為設置由泡沫箱組成的障礙物群，通過在障礙物群兩側預設無人艇起始點和目標點，使無人艇能夠在航行過程中通過障礙物群。實艇實驗環境設置如表1所示。無人艇啟動后，可以在岸基控制端實時監控無人艇航行情況及其對障礙物群的避碰行為。

表1 實船實驗各關鍵點經緯度

圖11所示為水面無人艇在一個完整避障過程的軌跡示意圖，其中綠色原點為無人艇起始點，紅色圓點為無人艇需要到達的目標點，黃色圓點為水面障礙物。從實驗過程中可以看到，無人艇在啟動初始階段，在不存在與障礙物碰撞危險的情況下，通過一系列的右轉動作從而使無人艇的艏向能夠對準目標點；直行一段距離之后，無人艇與障礙物1相遇，如圖12所示，紅色箭頭表示當前時刻的可行方向，算法此時輸出左轉的可行方向控制無人艇成功避開障礙物；而后，無人艇在繼續直行的過程中與障礙物2相遇，如圖13所示，算法此時輸出右轉的避障方向控制無人艇成功避開障礙物；最后無人艇逐漸逼近目標點并最終成功到達。

3 結束語

圖11 無人艇局部避障軌跡示意圖

圖12 無人艇與障礙物1會遇

圖13 無人艇與障礙物2會遇

本文提出一種基于VFH*的水面無人艇局部避障方法。通過激光雷達前后幀數據融合建立水面無人艇環境模型，使得無人艇對已探測過的障礙物具有一定的短期記憶功能，彌補傳感器探測盲區；并以此水面環境模型為基礎，構建障礙物向量場直方圖，選取候選方向以及前向預測尋優，最終獲得無人艇在當前時刻的最優避障角度。V-REP平臺仿真實驗表明，所提出的算法能結合激光雷達數據得到合理的局部避障規劃策略，避免無人艇在局部避障過程中出現尾端碰撞或陷入局部最小值的問題；通過水池實艇實驗有效驗證了算法在障礙物群中的危險規避能力和目標導向性，滿足水面無人艇在實際環境中的局部避障需求。