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

魏新勇，黃燁笙，洪曉斌

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

0 引言

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

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

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

因此，本文針對無人艇在未知海洋環(huán)境下的局部避障問題，以激光雷達(dá)前后數(shù)據(jù)幀融合構(gòu)建的無人艇環(huán)境模型為基礎(chǔ)，采用具有前向預(yù)測功能的VFH*局部避障規(guī)劃算法，實(shí)現(xiàn)無人艇在每一瞬時(shí)的狀態(tài)感知和局部避障決策。

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

1.1 無人艇水面環(huán)境模型構(gòu)建

水面無人艇局部避障規(guī)劃的準(zhǔn)確實(shí)現(xiàn)首先依賴于其環(huán)境感知能力的好壞，激光雷達(dá)作為一種基于TOF測距原理的高精度、高分辨率的外圍傳感器，可以實(shí)時(shí)獲取水面無人艇前向區(qū)域內(nèi)障礙物的相對距離與方位，易于構(gòu)建無人艇環(huán)境模型。

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

圖1 無人艇尾端碰撞場景

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

接著通過球面正弦公式計(jì)算兩點(diǎn)之間的相對方位角：

根據(jù)相對距離和相對方位角求得平移坐標(biāo)為

1.2 障礙物向量場直方圖構(gòu)建

VFH*局部避障規(guī)劃算法采用柵格表示法將無人艇水面環(huán)境分解為二維直方圖網(wǎng)格，每個(gè)網(wǎng)格的障礙物置信度通過激光雷達(dá)建立的環(huán)境模型信息不斷更新，進(jìn)而構(gòu)建障礙物向量場直方圖來表征無人艇的可行和不可行區(qū)域。

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

(xi,yj) ——障礙物網(wǎng)格的坐標(biāo)。

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

其中：

圖2 障礙物網(wǎng)格膨化處理

主直方圖中每一個(gè)分區(qū)的障礙物密度定義為該分區(qū)最危險(xiǎn)網(wǎng)格的障礙物向量強(qiáng)度。這是因?yàn)椴捎昧思す饫走_(dá)前后幀數(shù)據(jù)融合后，實(shí)際導(dǎo)入的環(huán)境模型中在徑向上可能存在多個(gè)障礙物網(wǎng)格，采用傳統(tǒng)的加權(quán)求和方式容易造成該方向上危險(xiǎn)程度的誤判。如圖3所示的網(wǎng)格地圖，無人艇此刻向左和向右轉(zhuǎn)向避開正前方障礙物的概率是一致的，若采用加權(quán)求和的方式計(jì)算則導(dǎo)致無人艇更傾向于向左轉(zhuǎn)向。

圖3 無人艇網(wǎng)格地圖

1.3 無人艇運(yùn)動(dòng)方向選取與前向預(yù)測尋優(yōu)

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

圖4 無人艇前向預(yù)測節(jié)點(diǎn)示意圖

為了量化每一個(gè)初始候選方向的前進(jìn)代價(jià)，定義搜索樹上每一個(gè)節(jié)點(diǎn)的代價(jià)值由其代價(jià)函數(shù)和啟發(fā)式函數(shù)構(gòu)成，總的代價(jià)值為

由于無人艇在局部避障過程中需要以目標(biāo)點(diǎn)為導(dǎo)向，因此還需滿足以下條件：

2 實(shí)驗(yàn)結(jié)果與分析

2.1 仿真實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證本文所述算法的局部避障性能，通過遠(yuǎn)程API函數(shù)建立Matlab與V-REP仿真平臺(tái)之間的通信，進(jìn)行無人艇局部避障性能的半實(shí)物仿真實(shí)驗(yàn)。

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

圖5 V-REP仿真環(huán)境

圖6是未結(jié)合激光雷達(dá)前后幀數(shù)據(jù)融合的無人艇局部避障仿真實(shí)驗(yàn)。如圖6(a) 所示，在仿真初始階段，無人艇能夠順利識(shí)別到障礙物并進(jìn)行轉(zhuǎn)向避碰，然而當(dāng)障礙物處于激光雷達(dá)探測盲區(qū)時(shí)，無人艇受目標(biāo)點(diǎn)導(dǎo)向的指引，持續(xù)左轉(zhuǎn)，最終與障礙物發(fā)生碰撞，如圖6(b)所示。

圖6 未結(jié)合前后幀數(shù)據(jù)融合的仿真實(shí)驗(yàn)過程

圖7 結(jié)合前后幀數(shù)據(jù)融合的仿真實(shí)驗(yàn)過程

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

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

圖8 無人艇避障方向

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

圖9 無人艇局部避障仿真結(jié)果圖

2.2 實(shí)物實(shí)驗(yàn)結(jié)果與分析

本文采用的實(shí)驗(yàn)平臺(tái)是一艘艇長2m的全電力推進(jìn)無人艇，如圖10所示。環(huán)境感知傳感器包括激光雷達(dá)和網(wǎng)絡(luò)攝像頭，通過無線路由模塊返回岸基控制端；而位姿測量傳感器GPS和電子羅經(jīng)，則通過射頻模塊進(jìn)行傳輸。艇載激光雷達(dá)選用SICK LMS200，理論最遠(yuǎn)探測范圍為80m，最大探測角度范圍為180°。艇載網(wǎng)絡(luò)攝像頭可以實(shí)時(shí)獲取水面圖像，從而監(jiān)控?zé)o人艇行進(jìn)過程。

圖10 水面無人艇實(shí)驗(yàn)平臺(tái)

為了驗(yàn)證本文所述算法的有效性，在水池內(nèi)人為設(shè)置由泡沫箱組成的障礙物群，通過在障礙物群兩側(cè)預(yù)設(shè)無人艇起始點(diǎn)和目標(biāo)點(diǎn)，使無人艇能夠在航行過程中通過障礙物群。實(shí)艇實(shí)驗(yàn)環(huán)境設(shè)置如表1所示。無人艇啟動(dòng)后，可以在岸基控制端實(shí)時(shí)監(jiān)控?zé)o人艇航行情況及其對障礙物群的避碰行為。

表1 實(shí)船實(shí)驗(yàn)各關(guān)鍵點(diǎn)經(jīng)緯度

圖11所示為水面無人艇在一個(gè)完整避障過程的軌跡示意圖，其中綠色原點(diǎn)為無人艇起始點(diǎn)，紅色圓點(diǎn)為無人艇需要到達(dá)的目標(biāo)點(diǎn)，黃色圓點(diǎn)為水面障礙物。從實(shí)驗(yàn)過程中可以看到，無人艇在啟動(dòng)初始階段，在不存在與障礙物碰撞危險(xiǎn)的情況下，通過一系列的右轉(zhuǎn)動(dòng)作從而使無人艇的艏向能夠?qū)?zhǔn)目標(biāo)點(diǎn)；直行一段距離之后，無人艇與障礙物1相遇，如圖12所示，紅色箭頭表示當(dāng)前時(shí)刻的可行方向，算法此時(shí)輸出左轉(zhuǎn)的可行方向控制無人艇成功避開障礙物；而后，無人艇在繼續(xù)直行的過程中與障礙物2相遇，如圖13所示，算法此時(shí)輸出右轉(zhuǎn)的避障方向控制無人艇成功避開障礙物；最后無人艇逐漸逼近目標(biāo)點(diǎn)并最終成功到達(dá)。

3 結(jié)束語

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

圖12 無人艇與障礙物1會(huì)遇

圖13 無人艇與障礙物2會(huì)遇

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