基于自調節VFH+的水面無人艇雷達避障控制研究

婁虎，趙澤鈺，阮文濤，吳政峰，袁明新

(江蘇科技大學 機電與動力工程學院，江蘇 張家港 215600)

0 引言

因機動性強、成本低等因素，近年來水面無人船在近海巡邏、水質監控和水產養殖等領域得到廣泛使用[1]。為了保障無人船實現高效、安全作業，自主避障控制技術是關鍵[2]。目前無人船避障主要有兩種方式[3]：一種是已知作業水域里的全局避障，但實時性低，不能靈活規避未知障礙物；另一種是基于船載傳感器，比如攝像頭、超聲波和激光雷達等，進行在線避障，具有實時性好且能實時處理未知障礙物的優點，是當前主要避障方式。相比攝像頭數據處理量大、超聲波探測精度低，激光雷達是目前主要避障傳感器，國內外學者圍繞其開展了一系列避障算法研究。

BORENSTEIN J 等[4-5]通過對虛擬勢場法[6]進行改進，在考慮機器人運動特性和機器人尺寸情形下，提出了用于避障的向量場直方圖算法(vector field histogram, VFH)，但存在閾值敏感、避障路徑曲折等不足。賈衛平等[7]提出了基于激光雷達進行SLAM建圖，并使用A*算法進行全局規劃和DWA算法進行局部規劃，但該方法只適合于室內區域規劃與避障，難以用于大范圍復雜的室外環境避障。劉杰等[8]基于激光雷達采集數據，以自適應閾值為條件對傳統矢量場直方圖進行改進，雖改善了閾值不足時機器人航向劇變的問題，但其僅介紹了最大閾值設置而未說明閾值具體調節過程。

為了解決文中水質檢測三體船在未知水域的避障問題，以激光雷達為探測平臺，針對雷達點云數據進行閾值自調節器設計，從而提出了新型自適應VFH+避障算法，仿真和水上實際測試都驗證了算法的有效性。

1 水面無人船的系統設計

1.1 無人船系統結構

圖1為文中所述無人船系統。無人船控制器采用Linux系統工控機，負責接收和處理各傳感器數據，并下達控制指令；雷達負責周圍環境信息的采集，并為無人船避障提供環境數據；定位采用GPS模塊，用于實時獲取無人船位置坐標；無人船下位機用于接收工控機指令，控制各種電機與舵機運動；無線通信模塊，用于向監控中心實時反饋無人船狀態及水質檢測數據，同時接收遠程監控系統指令。遠程監控系統，主要作為操作人員的指令下達平臺，并負責實時存儲無人船狀態信息和采集到的水質數據，供監控和分析。

圖1 無人船系統結構

1.2 無人船避障控制系統搭建

圖2給出了無人船避障控制系統電路連接圖。無人船工控機采用樹莓派，下位機采用STM32單片機。激光雷達將掃描到的障礙物信息通過USB1端口發送給樹莓派工控機，GPS實時獲取的無人船位置坐標通過USB2端口發送給樹莓派工控機；樹莓派工控機在對無人船當前坐標和周圍障礙物信息處理后，通過串口將控制信號發送給STM32F103ZET6型單片機，并實現無人船上左、右轉向舵機和左、右推進電機等的控制，進而實現無人船的航行避障控制。

圖2 避障控制系統電路連接圖

1.3 激光雷達測距原理及數據獲取

文中選用的思嵐A3型激光雷達，采用如圖3所示的斜射式激光三角法測距[9]。雷達發射器以入射角度γ射出一束激光，照射到O處障礙物，并在障礙物表面發生反射和散射。被物體反射的激光經過B處透鏡匯聚成像，然后被透鏡后方電荷耦合器件(線性CCD)采集。

圖3 斜射式激光三角法測距原理圖

圖3中：γ為入射光AO與基線AB的夾角；透鏡焦距為f；D為被測物體的反射光線在CCD上成像的極限位置；x為成像光斑E與極限位置D的距離。由三角形相似幾何原理可知：

(1)

在感光耦合器的中心作一條與基線AB平行的H軸線，通過算法可以得到成像點的像素坐標(Px，Py)，則位移x為

x=CellSize·Px+DeviationValue

(2)

式中：CellSize為光敏單元上單個像素的尺寸值，DeviationValue為通過像素點計算的投影距離與實際投影距離x的偏差量。

2 水面無人船避障控制算法設計

傳統VFH+局部避障算法采用四級數據簡化來計算機器人運動方向。避障時存在閾值敏感、避障路徑曲折等不足。為此，文中通過增加閾值自調節環節，提出了新型自適應VFH+算法。該算法將根據無人船所航行的水域環境，調節最佳障礙物強度閾值，從而幫助無人船規避局部死區，且路徑平滑地到達目標點。

2.1 VFH+算法的構建

VFH+算法是建立在以無人船為中心的極坐標上。將環境柵格化，通過計算每個柵格中障礙物強度得到機器人前進方向上的障礙物分布情況，然后利用閾值進行數據篩選得到候選方向，并對所有候選方向進行代價計算，從而得到最優避障路徑。步驟如下：

步驟1 根據激光雷達掃描得到的環境數據，即障礙物距離與角度，建立以雷達為中心的極坐標環境。為了對無人船進行避障規劃，需將此極坐標信息轉化為全局環境下的直角坐標信息，如圖4所示。

圖4 水面無人艇坐標轉換關系圖

圖4中A(x1,y1)為直角坐標系下雷達的坐標；B(x2，y2)為直角坐標系下障礙物的坐標；d為激光雷達探測到的障礙物距離；α為無人船當前航向角；β為雷達探測到的障礙物角度；θ為極坐標系到直角坐標系的變換角。則

θ=α+β

(3)

將激光雷達測到的障礙物信息變換到直角坐標系中，得到B點坐標：

(4)

步驟2 以船載雷達幾何中心為圓心，建立動態窗口Ca，其半徑即為雷達探測距離dmax。按照激光雷達分辨率f，劃分為n=360°/f個扇區，并規定以雷達為中心，以與全局直角坐標系x軸方向平行的線為起止線，逆時針對所有扇區從1到n進行編號。

步驟3 根據VFH+算法的向量化公式，計算每個區域內的障礙物強度值mn，搭建向量場直方圖。障礙物強度值mn與障礙物距離dn的關系為

(5)

步驟4 通過實驗設置合適的障礙物強度閾值M，對向量場直方圖進行二值化處理得到二值化直方圖，規則如式(6)所示。

(6)

文中思嵐A3激光雷達的探測半徑dmax為8m，且分辨率為1°，因此圓周水域劃分為n=360個扇區，編號從1到360。定義向量化公式中各常量參數：cv=10，b=2.5，閾值強度設置為M=6000時，得到如圖5所示的避障結果示意圖、向量場直方圖和二值化直方圖。

圖5 VFH+算法直方圖

步驟5 在二值化直方圖中選取大于一定扇區寬度的扇區，取其中間方向作為無人船運動候選方向ki，使用代價函數對所有候選方向ki求取代價值，選擇代價值最小的候選方向ki作為運動方向kb。kb的初值為0，代價函數如式(7)所示。

g(c)=μ1Δ(ki，kt)+μ2Δ(ki，kr)+μ3Δ(ki，kb′)

(7)

式中：μ1、μ2、μ3均為常數，且μ1>μ2+μ3；Δ(ki,kt)為候選方向與目標方向間的夾角；Δ(ki,kr)為候選方向與無人船當前航行方向的夾角；Δ(ki,kb’)為候選方向與無人船上一次航向的夾角；kr為無人船當前航向；kb為無人船上一次航向。

2.2 VFH+算法的缺陷

由上述可知，VFH+算法是通過設置合適的障礙物強度閾值M來篩選候選方向，再對候選方向進行代價計算從而確定航向，所以選擇合適的障礙物強度閾值M就尤為重要。

如圖6所示，當障礙物強度閾值設置較小時，無人船能夠及時發現航向周圍的障礙物和可通行扇區，但無人船過于“敏感”，其航行軌跡曲折，且難以到達處于半封閉處的目標點。

圖6 M=3000時避障結果示意圖

如圖7所示，當障礙物強度閾值設置過大時，無人船的可通行扇區變多，但無法及時發現下一秒前方的障礙物，不能夠有效進行躲避。

圖7 M=9000時二值化直方圖

因此，選擇合適的障礙物強度閾值是無人船能夠進行有效避障的關鍵。

2.3 閾值自調節器

對于固定障礙物強度閾值造成的缺陷，文中提出了一種閾值自調節方法。根據無人船所處環境對障礙物閾值進行實時更新，從而改善無人船避障靈活性。

a)確定閾值范圍

1) 根據無人船的運動特性確定閾值最大值

為了保障無人船航行的安全性，有最小安全距離dmin為

dmin=λ1·(S+Ru) (λ1>1)

(8)

式中：λ1為安全距離放大系數；Ru為雷達中心到無人船邊緣的距離；S為減速距離。

(9)

式中：vmax為無人船的最大航行速度；S為無人船通過減速加速度a減速到靜止狀態所需距離。

將求得的無人船最小安全距離dmin代入式(5)中，可得到障礙物強度閾值最大值Mmax：

(10)

2) 由雷達掃描特性確定閾值最小值

為了保障雷達的可靠性，取雷達探測半徑dmax為

dmax=λ2·Lmax(0<λ2<1)

(11)

式中：λ2為探測距離縮小系數；Lmax為雷達最大探測半徑。

將雷達探測半徑dmax代入式(1)中，可求得障礙物強度閾值的最小值Mmin：

(12)

3) 障礙物強度閾值范圍

通過步驟1)和步驟2)，可以得到障礙物最佳強度閾值Mth的范圍為

Mmin≤Mth≤Mmax

(13)

b)求取最佳障礙物強度閾值

閾值自調節計算規則如下：

Mthi=φi·Mmax+ηi·Mmin

(14)

式中：φi·Mmax為目標優先權重項；φi為目標優先權重系數；ηi·Mmin為安全優先權重項；ηi為安全優先權重系數；Mthi為此時所求得的最佳障礙物閾值。

目標優先權重系數φi、安全優先權重系數ηi的設置規則如下。

如圖8所示，按照無人船、目標點和障礙物間的位置關系量，即：無人船當前位置與目標點之間距離dti、無人船當前位置與所測障礙物之間的距離doi、無人船與目標點連線和無人船與障礙物間連線的夾角Δβi的關系可以劃分為圖中所標出的4個區域：

1) 當障礙物處于無人船和目標點以外區域(即①號區域)和障礙物處于無人船后方安全區域(即④號區域)時，無人船遵循目標優先原則：

φi=1，ηi=0

(15)

2) 當障礙物處于無人船和目標點間連線上(即②號線)，無人船遵循安全優先原則：

φi=0，ηi=1

(16)

3) 當障礙物處于影響區域(即③號區域)：

(17)

式中：Δβi為“無人船-目標點”連線與“無人船-障礙物”連線的夾角絕對值。

圖8 閾值自調節區域劃分圖

3 實驗結果與分析

3.1 仿真實驗結果與分析

為了驗證自調節增加后的無人船避障性能改善效果，在Matlab上搭建了無人船環境模型，將無人船分別置于開闊水域、狹長水道、連續S彎和U型死區4種典型水域環境進行避障測試，結果如圖9所示。

圖9 避障結果

仿真實驗表明，采用基于自適應VFH+算法水面無人艇在開闊水域、狹長水道和連續S彎3種典型環境下都能夠航跡平滑到達目標點；并且對于目標點處于“U”型死區時無人船也能夠順利到達目標點，改善了傳統VFH+算法存在的閾值敏感問題。

此外，在4種典型避障環境下，分別設置障礙物強度閾值M=2000、M=4000、M=6000和采用閾值自調節進行避障仿真測試，得到無人船到達目標點所需路徑點數如表1所示。

表1 無人船到達目標點所需路徑點數對比表

表1中“—”符號表示未能到達目標點。路徑點數越少表示路徑越短。由表1對比數據可以看出，無人船通過自調節環節后能夠以較短的平滑路徑到達目標點，且路徑優化效率平均達到2.08%。

3.2 實驗測試結果與分析

為了進一步驗證文中算法的有效性，基于所設計的三體無人船進行了內河過橋避障控制實驗。內河橋一共有6個橋墩，其避障過程如圖10所示。無人船在與1號橋墩相遇時，遵循安全優先原則通過算法選擇當前可航行方向，并右轉避開1號橋墩；隨后，無人船在直行過程中又與6號橋墩相遇，遵循安全優先原則計算出當前可航行方向，即左轉規避6號橋墩；最后，遵循目標優先原則，無人船轉向目標點方向直行，并最終安全到達目標點。無人船避開1號橋墩避障過程的部分雷達點云如圖11所示，而無人船完整過橋避障結果如圖12所示。

圖10 無人船規避1號橋墩

圖11 無人船規避1號橋墩的部分雷達點云圖

圖12 水上過橋避障航行軌跡圖

4 結語

為了實現水質檢測無人船在復雜環境水域執行作業任務時的實時和高效避障，提出了基于自調節VFH+的水面無人船激光雷達避障控制算法。通過仿真和實驗測試可得出如下結論：

1)使用激光雷達作為探測儀，相比超聲波和視覺等傳感器，具有掃描范圍廣、精度高和抗干擾能力強等優勢；

2)通過對水域環境進行柵格化處理，計算障礙物強度值，利用閾值進行數據篩選得到候選方向，進行代價計算，可得到無人船最優避障路徑；

3)增加閾值自調節環節，根據障礙物、無人船和目標點三者間的位置關系實時調整當前最佳閾值，改善了傳統矢量場直方圖算法中存在的閾值敏感問題。