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

林政 吕霞付

摘 要：为了提高水面无人艇（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所示。可以看到机器人在未遇到障碍物时能快速前进且行驶轨迹平滑;当遇到障碍物时能根据障碍物分布情况自动减速并有效避开避障物;且在狭窄通道行驶时能有效克服人工势场法中易出现的摆动问题。实验结果表明具有速度反馈的模糊避障算法能有效地适应复杂环境。

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.