基于改進模糊算法的水面無人艇自主避障

林政 呂霞付

摘 要：為了提高水面無人艇（USV）在未知、復雜環境下的連續避障性能，提出一種具有速度反饋的模糊避障算法。USV利用激光掃描雷達與多路超聲波傳感器感知周圍環境，通過對障礙物信息進行分組并設置權值的方式進行多傳感器數據融合，并在模糊控制的基礎上根據環境情況自動調整航速;進而提出一種考慮障礙物所有分布情況的更全面的模糊控制規則表，增強了USV對復雜環境的適應能力。實驗結果表明，所提方法能通過與環境交互調整USV航速使其成功避障并優化避障路徑，具有良好的可行性和有效性。

關鍵詞：水面無人艇;避障;數據融合;模糊控制;速度反饋

中圖分類號：TP273.4

文獻標志碼：A

Autonomous obstacle avoidance of unmanned surface vessel based on improved fuzzy algorithm

LIN Zheng*， LYU Xiafu

College of Automation， Chongqing University of Posts and Telecommunications， Chongqing 400065， China

Abstract：

In order to improve the performance of continuous obstacle avoidance ability of Unmanned Surface Vessel （USV） in unknown and complex environment， a fuzzy algorithm of obstacle avoidance with speed feedback was proposed. The USV utilized laser scanning radar and multi-channel ultrasonic sensors to perceive the surroundings and performed multi-sensor data fusion by grouping and setting the weight of the obstacle information， and the speed of USV was automatically adjusted according to the environmental situation based on fuzzy control. Then a more comprehensive fuzzy control rule table considering all the distribution of obstacles was proposed to enhance the adaptability of USV to complex environments. The experimental results show that the algorithm can make the USV successfully avoid obstacles and optimize the obstacle avoidance path by adjusting the speed through interaction with the environment， and has good feasibility and effectiveness.

Key words：

unmanned surface vessel; obstacle avoidance; data fusion; fuzzy control; speed feedback

0 引言

水面無人艇（Unmanned Surface Vessel， USV）是一類為應對各種特殊場景和不適宜有人船只執行的任務而設計的水面艦艇[1]。而路徑規劃作為USV研究領域的一個核心問題，其水平高低在一定程度上反映了USV的智能化程度[2]。根據文獻[3-15]的介紹，目前常將其分為兩類：第一類是基于環境模型的全局路徑規劃，在已知整體環境數據的情況下，USV根據相應算法規劃出自身位置到目的地的一條安全路徑;但是在環境發生變化時避障效果不太理想，具體的方法包括柵格法[3]、可視圖法[4]、A*算法[5]等。第二類是根據傳感器信息進行實時避障的局部路徑規劃，USV通過雷達等傳感裝置感知周圍的環境信息，來確定當前所在水域環境中附近的障礙物分布情況，并依此進行小范圍避障，實現實時避障功能，具體方法包括遺傳算法[6-7]、人工勢場法[8-9]、神經網絡法[10-11]、模糊邏輯法[12-15]等。在USV航行期間周圍環境不斷變化，是一個不可控的系統，難以建立精確的數學模型。模糊邏輯法借鑒人的駕駛經驗，并通過查詢知識庫等方式獲取規劃信息以實現實時避障，計算量小，可更好地解決此類問題。

本文通過對障礙物信息進行分組并設置權值的方式進行多傳感器數據融合處理，提高了環境信息的穩定性和有效性。在保持模糊控制原有特性的基礎上提出一種具有速度反饋的改進模糊控制算法，并創新性地對USV周圍障礙物分布的所有情況進行了模糊避障規則的總結。實驗結果表明，該方法在提高了USV避障性能的同時實現了對其移動速度更靈敏和更自主的控制，具有較高的可行性和有效性。

1 無人水面艇系統設計

1.1 系統結構

如圖1所示，USV的系統結構可分為三層：第一層為感知層，由多路超聲波傳感器和激光掃描雷達組成，負責感知USV周圍環境中障礙物的距離位置信息;中間為決策層，由一臺Linux系統的工控機組成，負責處理各傳感器傳輸的數據并下發運動控制指令;執行層的核心為一塊STM32F407單片機，負責執行由決策層下發的命令，最終利用差速轉向原理控制左右電機實現USV不同半徑曲道的轉彎。

1.2 測距系統

要實現USV的實時避障，首先需要利用自身傳感器獲取周圍環境信息，且該信息的準確性在一定程度上決定了最后實際避障的效果，故有必要設計一種適合本課題平臺的測距系統?？紤]到單一傳感器存在檢測精度低、控制不靈敏等缺點，本文采用激光掃描雷達和多路超聲波傳感器來采集障礙物位置信息以提高系統的準確性和可靠性[16]。

無人水面清潔船平臺寬度為1.2m，長度為1.6m，單個超聲波探頭測距角度為60°。為保證完整采集障礙物信息，將超聲波傳感器按左側、前左、前中、前右、右側、后側6個方位放置，并使用激光雷達采集前方180°范圍（前左、前中、前右各占60°）的障礙物信息輔助避障，傳感器布置如圖2所示。

1.3 數據處理

為了簡化數據處理過程并提高USV控制的實時性和準確性，有必要對傳感器進行分組。本文將傳感器分為左側、前左、前中、前右、右側和后側六組，對每組傳感器采集的數據進行預處理后分別進行融合，從而獲得6個方向上的障礙物距離信息。傳感器分組情況如表1所示。

超聲波傳感器的測量范圍為20～250cm，激光雷達的測量范圍為30～1000cm。當障礙物的距離較近時，超聲波傳感器的探測準確度要高于激光雷達，故在距離較近時，以超聲波傳感器的數據作為主要探測結果，下面給出每組傳感器的融合規則。

1）左側、右側、后側傳感器組。

左側、右側、后側情況類似，這里以左側為例：L1、L2、L3超聲波傳感器采集的信息數據中較大、居中、較小的數據在進行融合時所占權重比分別為20%、30%、50%，若障礙物距離超出超聲波傳感器的探測范圍，則忽略傳感器探測結果。

2）前左、前右傳感器組。

前左、前右情況類似，以前左為例：若障礙物距離在超聲波傳感器探測范圍內，則FL超聲波傳感器和激光雷達L方向采集的數據融合時所占權重比分別為70%、30%;若障礙物距離超出超聲波傳感器探測范圍而在激光雷達探測范圍內，則以激光雷達L方向探測數據為探測結果。若障礙物距離超出激光雷達的探測范圍，則忽略傳感器探測結果。

3）前中傳感器組。

若障礙物距離在超聲波傳感器探測范圍內，則FM1、FM2、FM3超聲波傳感器采集的信息數據中較大、居中、較小的數據和激光雷達M方向采集的信息數據在進行融合時所占權重比分別為10%、20%、40%、30%;若障礙物距離超出超聲波傳感器探測范圍而在激光雷達探測范圍內，則以激光雷達M方向探測數據為探測結果。若障礙物距離超出激光雷達的探測范圍，則忽略傳感器探測結果。

各傳感器組采集的信息數據在進行初步融合時采用加權平均法進行數據融合，下面以前中傳感器組采用加權平均法進行數據融合為例，說明通過將以上的融合規則應用于兩種傳感器，就可以建立一個有效的、有空間深度的信息融合網絡，其探測的精度將高于單個傳感器產生的探測精度。

設FM1、FM2、FM3超聲波傳感器的探測距離中較大、居中、較小數據為dmax、dmid、dmin，雷達M方向探測距離為dr，融合后前中距離為Dfl，實際障礙物距離為ds。當障礙物距離在超聲波傳感器探測范圍內時按以下公式計算：

Dfl=10%dmax+20%dmid+40%dmin+30%dr

當障礙物距離超出超聲波傳感器探測范圍而在激光雷達探測范圍內時以激光雷達數據為準。若障礙物超出激光雷達探測范圍，則不作避障處理。

如表2 所示，為實驗測得的前中傳感器組各傳感器在超聲波探測范圍內的探測距離以及融合后得到的前中距離與實際障礙物距離的比較。從表中可以看出經過加權融合得到的探測距離比單個傳感器的探測距離更加接近真實的障礙物距離。

2 模糊控制器的設計

在USV航行期間，周圍環境動態變化，使得難以建立準確的數學模型來預測障礙物的位置。模糊控制基于實時傳感器信息，借鑒人類專家控制經驗進行模糊推理，無需搭建復雜的運動模型，故可適用于解決USV的實時避障問題。

2.1 輸入輸出及模糊語言描述

傳統的模糊避障算法未考慮自身速率，僅將障礙物信息作為模糊控制器的輸入，并不完全符合人類駕駛經驗。因此，在本文的避障系統中，模糊控制器增加USV航速作為反饋信息輸入，將左右電機占空比作為輸出，如圖3所示。

本文采用雙體船結構的USV平臺，利用左右電機差速轉向實現行進的速度和方向控制。由剛體平動原理可知，USV在任何時刻繞船身瞬時中心C轉動。因此，USV近似為以單個船體中心線為軸的兩輪移動平臺，如圖4所示。

在XOY是全局坐標系的情況下，xoy是船體坐標系，C是速度瞬時中心。在本文中，GPS數據可用于推算模糊控制器輸入的反饋速度。首先將t1、t2時刻的經緯度坐標（lat1，lon1），（lat2，lon2）作為輸入，考慮到USV僅在水面航行，可以將高程設為0，故在t1、t2時刻的大地坐標分別為（lat1，lon1，0），（lat2，lon2，0），接著將其轉換為空間直角坐標系（x2，y2，z2），并以（lat1，lon1）為計算基準位置，將（x2，y2，z2）轉換為站心坐標系（de，dn，du），最后根據下式計算航速：

v（t）=（de2+dn2）1/2（t2-t1）-1

按上述融合算法處理傳感器數據，并將USV左側、前左、前中、前右、右側、后側6個方向的障礙物距離信息分別記為L、FL、FM、FR、R、B，范圍是[0m，3m]，模糊語言為{近，遠}，對應的語言變量為{N，F};USV航速V的范圍是[0m/s，1m/s]，模糊語言設為{慢，中，快}，對應的語言變量為{S，M，F};USV左右電機占空比分別記為ZL和ZR，其范圍均是[-1.0，1.0]，模糊語言設為{負大，負小，零，正大，正小}，對應語言變量為{NB，NS，ZE，PS，PB}。

2.2 輸入輸出的隸屬度函數

在模糊控制系統中，通常使用梯形隸屬度函數、三角隸屬度函數、高斯隸屬度函數等。其中：高斯隸屬度函數曲線較為平滑，使系統有較好的準確性和簡潔性，但計算較為復雜;而梯形隸屬度函數和三角隸屬度函數作為常用的分段線性隸屬度函數，其形式簡單，物理意義清晰，基本滿足系統需求。本文在參考了文獻[17-18]中同類型問題后，為簡化模糊控制器設計并提高其計算效率，采用了分段線性隸屬度函數。根據專家經驗及上述論域，取各個語言變量的隸屬度函數形狀為對稱的三角形，可以確定如下隸屬度函數。圖5（a）所示為USV 6個方向的傳感器所測距離LD、FLD、FMD、FRD、RD、BD的隸屬函數;圖5（b）所示為USV平臺航速V的隸屬度函數; 5（c）所示為左電機占空比ZL的隸屬度函數; 圖5（d）顯示了右電機占空比ZR的隸屬函數。

2.3 模糊控制規則

模糊控制的核心是建立一個合適的模糊控制規則庫[19]。在USV航行期間，障礙物的分布將影響船體的速度和航向。由人類駕駛經驗可知，當障礙物距船較近時，USV應減速慢行并根據障礙物的距離及位置信息進行轉向避障;當障礙物距船較遠時，USV應加速駛向目標點。在環境未知的情況下，若只考慮存在障礙物的方向數，則USV周圍障礙物的位置分布情況可以分為6類，如圖6所示。

目前，大多數研究雖然均考慮到了各種未知環境下障礙物的分布情況，但給出的模糊控制規則表卻只針對所有方向均有障礙物這一種分布情況[20-22]，即在模糊規則表中默認各方向均存在障礙物，只考慮了各方向障礙物的遠近情況，如只針對圖6（f）所示的障礙物分布情況來制定模糊控制規則表，這顯然是不合理的。

本文使用上述多傳感器數據融合算法，對超聲波傳感器和激光掃描雷達所采集的障礙物信息進行處理，能滿足該無人水面清潔船平臺在未知、復雜環境下的信息采集需求。接著對障礙物在USV周圍的6個方向上所有的分布情況進行了分析，并據此制定更加完善的模糊控制規則表。為方便在模糊規則表中展示出USV周圍障礙物所有的分布情況，首先將每個方向的障礙物信息分為有障礙物、無障礙物兩種情況。則在僅考慮障礙物有無且輸出為USV整體運動方向的情況下，可得到26=64條模糊控制規則，但由于部分規則沒有實際意義，故本文在考慮所有障礙物分布情況的前提下對規則進行了必要的合并簡化，最終得到一個僅有36條的基本模糊控制規則表，如表3所示。該表格依據專家經驗對USV周圍6個傳感器所能采集的障礙物信息情況均進行了相應的處理，能有效應對各種復雜環境。如規則1表示前左、前中、前右方向無障礙物，其余方向障礙物情況任意時USV前進。

其中符號Dir表示USV的運動控制方案，F、TL、TR、B、S分別表示USV前進、左轉、右轉、后退、停止，1、0分別表示有、無障礙物，X表示任意情況。

接下來將基本模糊控制規則表按照模糊變量的模糊子集展開， 及進一步考慮障礙物距離且輸出以左右電機占空比表示USV的運動控制。首先將每個方向的障礙物信息分為無障礙物、有障礙物且距離近、有障礙物且距離遠3種情況，由于采集了過多方向的障礙物信息，導致模糊變量數目過多，使

得模糊控制規則冗余，多達36×3=2187條。但由于部分規則沒有實際意義，故本文在考慮所有障礙物分布情況的前提下對規則進行了必要的合并簡化，得到改進模糊控制規則表，表4所示為部分規則表?？梢钥吹皆诳紤]了障礙物所有分布情況后，改進的模糊控制規則表將原本多達2187條的控制規則簡化到407條，在保證USV更加完善的避障性能的基礎上也未增加算法難度，有較強的可應用性。

2.4 解模糊

由上述規則庫進行模糊推理會產生一個模糊值，而模糊值必須通過解模糊操作轉換為清晰值后才能用于控制執行機構。去模糊化[23]通常使用加權平均法、中值法、面積重心法等，本文使用面積重心法來去模糊化。

ZL=∫（ZL）μL（ZL）d（ZL）/∫μL（ZL）d（ZL）

ZR=∫（ZR）μR（ZR）d（ZR）/∫μR（ZR）d（ZR）

其中：∫是輸出模糊子集中各元素的隸屬度值在連續論域上的代數積分; μL、 μR分別是左電機與右電機占空比的隸屬函數。此后便可將去模糊化得到的信號用于控制USV左右電機實現差速轉向。

3 算法驗證

3.1 仿真實驗

本文通過MobotSim仿真平臺進行模擬實驗來驗證所提出算法的有效性。首先設置了一個10m×10m的矩形地圖，采用直徑為0.5m的移動機器人并且將12路超聲波傳感器按等間距放置在機器人的四周，其中單個傳感器的輻射錐角為25°，感應范圍為[0m，3m]。圖中的深灰色直角框部分均為邊界或障礙物。

首先將人工勢場法與本文方法在凹型障礙物環境中進行了對比實驗，實驗結果如圖7所示。結果證明本文算法能成功逃脫凹型障礙物區域并抵達終點，較人工勢場法更容易克服局部最小問題。

然后將傳統的模糊算法與本文算法進行了連續避障的對比實驗，結果如圖8所示。兩種算法的運動軌跡在M點以前基本相同，而在M點之后障礙物更加密集需要連續避障，此時傳統的模糊算法由于未考慮自身速度，在速度較快的情況下會避開整個障礙物密集區域。而本文算法具有速度反饋，在相同情況下能自主調節機器人移動速度，減速通過障礙物密集區域實現連續避障。從表5可以看出本文算法較傳統模糊算法更加省時且路程更短。

最后在復雜環境下對本文算法的避障效果進行了測試，在A、B兩點設置了紅外信標為機器人提供子目標點，避免因出現迂回情況增加不必要的耗時，實驗結果如圖9所示?？梢钥吹綑C器人在未遇到障礙物時能快速前進且行駛軌跡平滑;當遇到障礙物時能根據障礙物分布情況自動減速并有效避開避障物;且在狹窄通道行駛時能有效克服人工勢場法中易出現的擺動問題。實驗結果表明具有速度反饋的模糊避障算法能有效地適應復雜環境。

3.2 實際測試

采用如圖10所示的無人水面清潔船平臺在重慶郵電大學湖泊進行測試，來驗證本文算法的有效性。湖泊形狀近似為28.8m×20.8m的矩形，且分布有0.5m高的花壇10個，USV航行過程中每隔2s返回一次航點至上位機。

本平臺通過上位機標定USV目標點如圖11所示，接收到信號后USV朝目標點航行并進行實時避障，到達目的地后根據返回的一系列航點得USV避障路徑如圖12所示。

測試過程中，USV在前方沒有障礙物時會自動加速至設定速度并按照預設軌跡前進。在有障礙物出現時，USV能根據自身速度反饋進行實時避障，根據船速不同，在障礙物前約1～3m開始避障。整體避障過程比較理想，避障軌跡平滑且對各種障礙物分布情況均有效地采取了合理安全的避障策略。

4 結語

為了提高USV在復雜動態環境中的連續避障性能，本文設計了一種帶速度反饋的模糊避障算法，提出的多傳感器數據融合算法保證了超聲波傳感器與激光掃描雷達所采集數據的穩定性和有效性。針對當前大多數模糊控制規則表僅考慮各方向均有障礙物這一種分布情況，本文提出了一種考慮障礙物所有分布情況的更全面的模糊控制規則表，并進一步簡化得到改進模糊控制規則表，在保證USV避障性能的基礎上降低了USV控制系統的復雜度。測試結果表明，本文提出的模糊避障算法計算復雜度低、魯棒性強，能夠有效適應各種復雜環境。若結合強化學習、神經網絡等理論進一步改進模糊算法，將極大地提高其自學習和自適應能力，實現更好的避障性能。

參考文獻

[1]陳華，張新宇，姜長鋒，等.水面無人艇路徑規劃研究綜述[J].世界海運，2015，38（11）：30-33.（CHEN H， ZHANG X Y， JIANG C F， et al. Review on path planning of unmanned surface vehicle[J]. World Shipping， 2015， 38（11）： 30-33.）

[2]馬天宇，楊松林，王濤濤，等.多USV協同系統研究現狀與發展概述[J].艦船科學技術，2014，36（6）：7-13.（MA T Y， YANG S L， WANG T T， et al. An outline of current status and development of the multiple USV cooperation system [J]. Ship Science and Technology， 2014， 36（6）： 7-13.）

[3]ZHANG X J， BI J， LIU M Z. The study of soccer robot path planning based on grid-based potential field method improvements [J]. Advanced Materials Research， 2010， 139（9）： 1798-1802.

[4]陳超，唐堅，靳祖光，等.一種基于可視圖法導盲機器人路徑規劃的研究[J].機械科學與技術，2014，33（4）：490-495.（CHEN C， TANG J， JIN Z G， et al. A path planning algorithm for seeing eye robots based on V-graph [J]. Mechanical Science and Technology for Aerospace Engineering， 2014， 33（4）： 490-495.）

[5]GAO X， JIA Q， SUN H， et al. Research on path planning for 7-DOF space manipulator to avoid obstacle based on A* algorithm[J]. Sensor Letters， 2011， 9（4）： 1515-1519.

[6]LEE J， KIM D W. An effective initialization method for genetic algorithm-based robot path planning using a directed acyclic graph [J]. Information Sciences—Informatics and Computer Science， Intelligent， 2016， 332（C）： 1-18.

[7]PEHLIVANOGLU Y V. A new vibrational genetic algorithm enhanced with a Voronoi diagram for path planning of autonomous UAV [J]. Aerospace Science and Technology， 2012， 16（1）： 47-55.

[8]MONTIEL O， SEPULVEDA R， OROZCO-ROSAS U. Optimal path planning generation for mobile robots using parallel evolutionary artificial potential field [J]. Journal of Intelligent and Robotic Systems， 2015， 79（2）： 237-257.

[9]HASSAN S， YOON J. Haptic assisted aircraft optimal assembly path planning scheme based on swarming and artificial potential field approach [J]. Advances in Engineering Software， 2014， 69： 18-25.

[10]VANDENBERGHE L， VANDEWALLE J. A path-following method for finding multiple equilibrium points in cellular neural networks [J]. International Journal of Circuit Theory and Applications， 1992， 20（5）： 519-531.

[11]DUAN H， HUANG L. Imperialist competitive algorithm optimized artificial neural networks for UCAV global path planning [J]. Neurocomputing， 2014， 125： 166-171.

[12]BAKDI A， HENTOUT A， BOUTAMI H， et al. Optimal path planning and execution for mobile robots using genetic algorithm and adaptive fuzzy-logic control [J]. Robotics and Autonomous Systems， 2017， 89（1）： 95-109.

[13]ELSHEIKH E A， EL-BARDINI M A， FKIRIN M A. Practical design of a path following for a non-holonomic mobile robot based on a decentralized fuzzy logic controller and multiple cameras [J]. Arabian Journal for Science and Engineering， 2016， 41（8）： 3215-3229.

[14]TRAN H-K， CHIOU J-S， PENG S-T. Design genetic algorithm optimization education software based fuzzy controller for a tricopter fly path planning [J]. EURASIA Journal of Mathematics， Science and Technology Education， 2016， 12（5）： 1303-1312.

[15]KUMAR V， PARK Y， REDDY D V， et al. A mobile robot obstacle avoidance using fuzzy logic and model predictive control[J]. Applied Mechanics and Materials， 2014， 548/549： 922-927.

[16]GRAVINA R， ALINIA P， GHASEMZADEH H， et al. Multi-sensor fusion in body sensor networks： state-of-the-art and research challenges [J]. Information Fusion， 2017， 35： 68-80.

[17]陳衛東，朱奇光.基于模糊算法的移動機器人路徑規劃[J].電子學報，2011，39（4）：971-974.（CHEN W D， ZHU Q G. Mobile robot path planning based on fuzzy algorithm [J]. Acta Electronica Sinica， 2011， 39（4）： 971-974.）

[18]李擎，張超，韓彩衛，等.動態環境下基于模糊邏輯算法的移動機器人路徑規劃[J].中南大學學報（自然科學版），2013，44（S2）：104-108.（LI Q， ZHANG C， HAN C W， et al. Path planning based on fuzzy logic algorithm for mobile robots in dynamic environments [J]. Journal of Central South University （Science and Technology）， 2013， 44（S2）： 104-108.）

[19]KOVACIC Z， BOGDAN S. Fuzzy Controller Design： Theory and Applications [M]. Boca Raton， FL： CRC Press， 2006： 32-50.

[20]彭玉青，李木，張媛媛.基于改進模糊算法的移動機器人避障[J].計算機應用，2015，35（8）：2256-2260.（PENG Y Q， LI M， ZHANG Y Y. Mobile robot obstacle avoidance based on improved fuzzy algorithm [J]. Journal of Computer Applications， 2015， 35（8）： 2256-2260.）

[21]胡靜波，陳定方，吳俊峰，等.基于改進模糊算法的移動機器人自主避障研究[J].自動化與儀表，2018，33（6）：48-52.（HU J B， CHEN D F， WU J F， et al. Research on autonomous obstacle avoidance of mobile robot based on improved fuzzy algorithm [J]. Automation and Instrumentation， 2018， 33（6）： 48-52.）

[22]陳文釗，張雨晨，梁振奇.基于改進模糊控制的機器人自主避障研究[J].工業控制計算機，2018，31（10）：22-24.（CHEN W Z， ZHANG Y C， LIANG Z Q. Research on robot autonomous obstacle avoidance based on improved fuzzy control [J]. Industrial Control Computer， 2018， 31（10）： 22-24.）

[23]張新超，李鵬峰，陳紅新，等.模糊控制在井下供電電源中的應用[J].中國科學技術大學學報，2012，42（7）：597-602.（ZHANG X C， LI P F， CHEN H X， et al. Application of fuzzy PID controlling technique in downhole power supply [J]. Journal of University of Science and Technology of China， 2012， 42（7）： 597-602.）

This work is partially supported by the National Natural Science Foundation of China （61275077）.

LIN Zheng， born in 1995， M. S. candidate. His research interests include embedded development， smart robot.

LYU Xiafu， born in 1966， Ph. D.， professor. His research interests include instruments and apparatuses， automotive electronics.