胡　纽，董春凯，王志强（中国船舶重工集团公司 第七〇五研究所昆明分部，云南 昆明 650118）

基于避碰声呐的远程鱼雷水下自主障碍规避算法研究

胡纽，董春凯，王志强
（中国船舶重工集团公司 第七〇五研究所昆明分部，云南 昆明 650118）

伴随着航程可达上千千米的远程鱼雷的出现，对鱼雷远程航渡过程中自主规避可能出现的障碍物提出新要求。本文以某型远程鱼雷为背景，基于模糊控制方法，提出一种基于避碰声呐的远程鱼雷自主避障算法，提出规避障碍物的规划，并通过计算模拟避碰声呐探测信息，在仿真平台上对该算法进验证，结果表明运用该算法远程鱼雷能利用自身携带的前视避碰声呐探测到障碍物，并通过障碍规避算法控制鱼雷航行规避障碍物时，本文提出的障碍规避算法可以引导鱼雷规避障碍物，到达设定的目标点。

远程鱼雷；障碍规避；算法仿真

0　引　言

远程鱼雷是一种由鱼雷发射平台在防区外发射，发射后由鱼雷自主航渡到指定作战区域，执行待机、巡航、搜索、攻击、诱骗等任务的新型水中兵器［1］。这种鱼雷的航程可达几百甚至上千千米，在水下作业时间也大大超过了传统鱼雷，在水下看不见、摸不着的环境中，也没有电子海图的情况下［2］，远程鱼雷的自主完成任务能力显得十分重要，本文以某型远程鱼雷为研究背景，提出一种基于避碰声呐信息的远程鱼雷障碍自主规避算法，通过利用自身携带的前视避碰声呐探测障碍物，并通过解算控制其绕开障碍物并回到原航线，保证自身航行安全，顺利完成预定任务。

1　系统组成及避障模拟

1.1系统组成

远程鱼雷与传统鱼雷主要区别是在传统鱼雷的雷顶自导基阵的基础上需要加装一套用于探测、发现障碍物目标的前视避碰声呐系统，与规划与决策系统、控制系统、动力系统共同引导远程鱼雷规避障碍物。系统组成结构图如图1所示。

远程鱼雷在自主避障过程中，前视避碰声呐通过发射声波并检测接收到的回波判断鱼雷航行前方是否存在障碍物［3］，并上报规划决策系统；规划决策系统根据障碍物信息和自主避障算法形成避碰方案下发控制系统和动力系统，并由控制和动力系统分别通过操舵及调整航速操纵远程鱼雷运动，避开障碍物。各系统信息传递流向及内容如图2 所示。

图1　系统组成结构图Fig. 1　System composition structure

图2　系统间信息流向及通信内容Fig. 2　Information flow and communication between systems

1.2障碍物模拟

障碍物模拟的主要功能是通过人工设定障碍物形状、位置、大小提供障碍物信息，供避碰声呐判断位置关系，产生避碰信息。

在实际应用中障碍物应该是任意形状，为简化研究本文提供了圆形和三角形 2 种基本形状障碍物的避碰模拟，对其他形状的模拟主要是通过这 2 种基本形状的组合实现，其中圆形提供圆弧段，三角形提供直线段和突出角，组合方案如图3 所示。

图3　障碍物组合示意图Fig. 3　Obstacles combination schemes

对障碍物的模拟主要在仿真平台上完成，对圆形基本障碍物由圆心 X 坐标、Y 坐标、圆半径 R 三个信息来描述；对三角形基本障碍物由顶点 A，B，C 的 X坐标、Y 坐标 6 个信息来描述。仿真平台提供 10 个圆形基本障碍物和 10 个三角形基本障碍物共 20 个障碍物的描述信息数组。

1.3避碰声呐信息模拟

采用数学模拟方法，不考虑探测精度的影响，在开角范围、波束设置、探测距离等方面采用如下设置［4］：

1）前视避碰声呐为单平面声呐，可以在远程鱼雷水平方向探测，暂不考虑远程鱼雷横滚、俯仰对前视避碰声呐的影响；

2）前视避碰声呐探测波束设置在以远程鱼雷为对称轴左右各 30 °范围内，间隔 4 °，共 16 个波束，每个波束探测距离为 300 m；

3）前视避碰声呐各波束之间无干扰，固定周期上传各波束探测信息，包括有无障碍物及障碍物距离。

2　模糊控制避障规则

2.1系统输入输出

依上文假设，避障声呐的波束开角为左右 30 °，探测距离为 300 m，以此为模糊控制方法的基本输入，过多的输入变量会造成模糊控制方法非常复杂，因此选择探测到的障碍物距离作为避障算法的核心输入变量，输出量为鱼雷航向控制角度。

2.2隶属度函数的建立

隶属度函数是对模糊概念的定量描述。隶属度函数的确定过程，本质上说应该是客观的，但每个人对于同一个模糊概念的认识理解又有差异，因此，隶属度函数的确定又带有主观性［5］。一般根据经验或统计进行确定，也可由专家给出。隶属度函数形式有多种，根据实际问题而具体确定或选用，常见的有梯形、三角形、高斯型等隶属度函数类型。

结合鱼雷航行速度，障碍物距离设定变量论域为d = ［0， 300］，变量描述为 ｛D，N，M，F｝；输出鱼雷航向转向角增量 θ 的控制量为 ［-30°，30°］。其中障碍物距离是指 16 个波束中，障碍物距离最小的那个值。变量描述 D，N，M，F 分别代表距离危险、距离较近、距离适中、距离较远，距离区间分别是［0， 50］、［50，100］、［100， 200］、［200， 300］。本文采用三角类型与梯形类型结合的的隶属度函数［6］，隶属度函数模型见公式（1），隶属度函数图形如图4 所示。

图4　隶属度函数示意图Fig. 4　Membership functions

2.3模糊控制规则

模糊控制规则是由若干个语言变量构成的模糊条件语句，表达了对被控制对象执行控制规则时的模糊思维和判断过程。模糊控制规则是对专家和理论知识与实践的经验总结，它是通过语言条件语句来模拟人类的控制行为，语句与专家的控制特性直接有关［7］。

本文针对障碍距离的不同，制定的模糊控制规则如下：

1）危险区域。在危险区域内探测到障碍物时，留给鱼雷机动规避的时间、空间有限，所以在这一区域内，一旦探测到障碍物，鱼雷先停车上报避障故障，再做处理。

2）距离较近。在这个区域内发现障碍物时，判断障碍物距离最小值对应的探测波束，然后根据该波束的安装角度，确定鱼雷的旋转方向和角度，确定原则是鱼雷转过相应的角度后，可以使该波束在鱼雷的正前方位置。如左边 10 ° 波束发现障碍物的距离最小，则鱼雷右转 10 °。

3）距离适中。这种情况下鱼雷有足够的时间和空间进行避障机动，避障控制方法有 2 种，A 方法是从外向内，B 方法是从内向外。

用 A 方法控制时，控制器首先检查左 30 ° 波束和右 30 ° 波束，控制规则如表1 所示。

表1　控制规则表ATab. 1　Control rules of table A

当鱼雷左 30 ° 波束和右 30 ° 波束都探测到障碍物时，比较哪个波束探测到的障碍物距离更近，控制器控制鱼雷向另一边转向；只有一个波束探测到障碍物时，控制器控制鱼雷向另一边转向；两边都没有探测到障碍物时，控制器首先检查左 26 ° 波束和右 26 ° 波束是否有障碍物，如有障碍物按表规则类比执行，如没有障碍物继续向内检查，直到所有波束都被检查。

用 B 方法控制时，控制器从内向外判断，只要保证一定范围内没有障碍物即认为可以安全航行，如图5所示，假设距离 200 m，安全宽度为 40 m，三角形的角度为 5.73 °，选 6 ° 波束即可满足要求。

图5　安全角度示意图Fig. 5　Security angle

表2　控制规则 B 表Tab. 2　Control rules of table B

所以需要判断第 7，8，9，10 四个波束是否有障碍物来确定鱼雷航向，共有 16 种情况，其模糊控制规则见表2（表中数字 0 代表没有探测到障碍物，数字 1代表探测到障碍物）。

当出现 7，8，9，10 号波束都有障碍的情况时，控制器向外检查，检查 10 °，即 6，11 号波束是否探测到障碍物，以此类推。

4）距离较远。这时发现障碍物只做记录，暂不规避。

3　仿真验证

3.1仿真平台介绍

依据上述算法，在 Visual Studio 2008 环境下开发了基于对话框的“新型水中兵器自主避障算法”仿真验证平台（见图6）［8］。

图6　鱼雷避碰仿真平台界面Fig. 6　Collision avoidance simulation platform

仿真平台具备鱼雷运动模拟、障碍物设定、避碰声呐信息模拟、避碰规则验证等功能。

平台界面左侧可选择障碍物的设置类型、避障规则及鱼雷基本运动设定；平台界面右侧部分可通过TeeChart 控件显示障碍物及鱼雷运动轨迹及鱼雷航行状态参数。

3.2通过功能验证

设定鱼雷从（0，0）点开始向（1 400，700）点运动，并设置一个圆形障碍物，圆心（750，450）半径200。仿真开始分别用控制规则 A 和控制规则 B 控制鱼雷避障机动，观察鱼雷是否能规避障碍物航行，到达目标点。

图7　鱼雷规避圆形障碍物Fig. 7　Avoid circular obstacle

仿真结果如图7 所示，图中圆形即为障碍物，颜色较浅的直线为鱼雷预选规划的航路，颜色较深的 2条曲线是鱼雷在按航路规划运动过程中，避碰声呐探测到障碍物后，鱼雷按 A、B 两种不同的避碰规则进行避障机动的规避轨迹。从图中可以直观地看出：2种避碰规则都可以控制鱼雷规避障碍物，到达目标点。但是避碰规则 B 中，由于优先考虑中间波束的障碍物信息，当中间没有障碍物时不考虑较外边的探测波束，这样相对于规则 A ，航程较小，可以减小一定的航程损失。

3.3穿越功能验证

设定鱼雷从（600，-600）点开始向（600，1 800）点运动，并设置 2 个圆形障碍物，障碍物一圆心（400，1 400）半径 100，障碍物二圆心（800，1 400）半径 110。仿真开始分别用控制规则 A 和控制规则 B控制鱼雷避障机动，观察鱼雷是否能规避障碍物航行，到达目标点。

仿真结果如图所示，鱼雷按 A、B 两种不同的避碰规则进行避障机动时，避碰规则 B 可以使鱼雷从障碍物的中间穿过，而避碰规则 A 只能控制鱼雷从障碍群的边缘绕过，航程损失较大（见图8）。

图8　鱼雷规避 2 个圆形障碍物Fig. 8　Avoid two circular obstacles

由于避碰算法只对当前周期内的障碍信息进行处理，即不记录曾经发现过的障碍物，更无法预料未来发现的障碍物，所以避碰规则 B 虽然可以从 2 个障碍物中穿过，但有可能陷入“U 型”陷阱中，即从障碍物中穿过后发现前方还有障碍物，走到头发现是死胡同，还需要从陷阱中绕出来，再绕过障碍群，到达目标点（见图9），这样其航程明显比避碰规则 A 所用的航程更多，并且还存在绕不出来，撞向障碍物的风险。

3.4多种障碍物通过功能验证

图10 所示分别是鱼雷通过 1 个三角形障碍物、1个复杂障碍物、多个障碍物和多个复杂障碍物的验证示意图，按照上述控制规则，鱼雷都可以绕过或者穿越障碍物，到达目标点，证明本文提出的障碍规避算法可行。

从图中可看出，虽然控制规则 A 和控制规则 B 都可以控制鱼雷绕过或者穿越障碍物，但还是存在以下问题：

1）是鱼雷在避障后期会出现航向抖动，在某一周期没有障碍物时向目标点运动；在下一周期由于鱼雷位置、航向变化又探测到障碍物，需要按规则左转或右转；转过之后没有障碍物鱼雷又改变航向向目标点运动，这种现象会对鱼雷舵机系统造成影响，如果鱼雷在避障和向目标点运动时的速度不同，又会造成鱼雷电机频繁改变转速，对电机造成影响。

2）是在控制规则 A 的控制下，由于避障声呐存在探测盲区，鱼雷在绕过三角形障碍物时，特别是绕过三角形顶点时，会离障碍物非常近，有时甚至会小于20 m，虽然在控制规则 B 中加入了在小于 50 m 时紧急停车的规则，但没有解决鱼雷在障碍规避时距离太近的问题，反而增加了鱼雷避障失败停车的概率，导致鱼雷无法正常到达目标点。

3.5控制规则改进

针对上节中提到的 2 个问题，对控制规则做了一些改进，主要方案是在鱼雷避障完成后，在某个周期没有探测到障碍物时，不是马上向目标点运动，而是在鱼雷当前航向的正前方 200 m 处增加一个路径调整点，鱼雷向路径调整点运动，到达后再向目标点运动。

由于路径调整点是在鱼雷的探测范围内，所以认为鱼雷到路径调整点无障碍，此外鱼雷向路径调整点运动的同时，依然由避碰声呐探测障碍物，若发现障碍物则继续避碰并随时调整路径调整点，这样可以保证鱼雷在向路径调整点的运动过程是安全的。

仿真验证结果如图11 所示。可以看到，鱼雷采用路径调整点对算法进行改进后，在避障机动与障碍物的距离小了很多，说明改进方法起到了一定的效果。

图9　鱼雷规避陷阱障碍物Fig. 9　Avoid trap obstacles

图10　鱼雷规避障碍物Fig. 10　Avoid different obstacles

图11　加入路径调整点后鱼雷规避障碍物Fig. 11　Avoid obstacles after joining path adjustment points

4　结　语

通过仿真验证，证明本文提出的基于避碰声呐信息的规避障碍物算法是可以引导鱼雷航行中成功规避障碍物。该算法作为鱼雷局部航路规划算法与鱼雷全局航路规划算法相结合，可以形成完整的远程鱼雷航路规划方法，满足远程鱼雷的工程化使用要求，提高远程鱼雷的自主性能力。

［1］李本昌， 梁涛. 远程鱼雷的作战样式及其技术需求［J］. 鱼雷技术， 2008， 16（4）： 54-57. LI Ben-chang， LIANG Tao. Operational concept of long-range torpedo and its technical requirements［J］. Torpedo Technology，2008， 16（4）： 54-57.

［2］胡泊， 刘锋， 陈彦勇. 基于电子海图的远程巡航鱼雷路径规划技术研究［J］. 舰船科学技术， 2015， 37（7）： 116-119. HU Bo， LIU Feng， CHEN Yan-yong. Research on route planning technology based on electronic charts for long-distance torpedo［J］. Ship Science and Technology， 2015， 37（7）： 116-119.

［3］秦政， 边信黔， 严浙平. 多波束前视声呐在潜器避障中的应用研究［J］. 电子器件， 2007， 30（5）： 1909-1913. QIN Zheng， BIAN Xin-qian， YAN Zhe-ping. Research on application of multibeam forward looking sonar in obstacle avoidance for underwater vehicle［J］. Chinese Journal of Electron Devices， 2007， 30（5）： 1909-1913.

［4］刘和祥， 边信黔， 秦政， 等. 基于前视声呐信息的AUV避碰规划研究［J］. 系统仿真学报， 2007， 19（24）： 5672-5674， 5679. LIU He-xiang， BIAN Xin-qian， QIN Zheng， et al. Research on obstacle avoidance planning for AUV based on forward looking Sonar［J］. Journal of System Simulation， 2007， 19（24）：5672-5674， 5679.

［5］曾光奇. 模糊控制理论与工程应用［M］. 武汉： 华中科技大学出版社， 2006. ZENG Guang-qi. Fuzzy control theory and engineering application［M］. Wuhan： Huazhong University of Science and Technology Press， 2006.

［6］徐昌文. 模糊数学在船舶工程中的应用［M］. 北京： 国防工业出版社， 1992. XU Chang-wen. Application of fuzzy mathematics in ship engineering［M］. Beijing： National Defence Industry Press， 1992.

［7］姜沛然. 基于模糊理论和强化学习的自主式水下机器人运动规划技术［D］. 哈尔滨： 哈尔滨工程大学， 2005. JIANG Pei-ran. Motion planning for autonomous underwater vehicle using the fuzzy theory and reinforcement learning［D］. Harbin： Harbin Engineering University， 2005.

［8］李策. 水下避碰声纳系统软件设计［D］. 哈尔滨： 哈尔滨工程大学， 2012. LI Ce. Software design of underwater obstacle avoidance sonar system［D］. Harbin： Harbin Engineering University， 2012.

Study on the algorithm of remote torpedo underwater autonomous obstacle avoiding using the collision avoidance sonar

HU Niu， DONG Chun-kai， WANG Zhi-qiang
（Kunming Branch of the 705 Research Institute of CSIC， China Shipbuilding Corporation， Kunming 650118， China）

Accompanied by the appearance of the remote torpedo whose voyage can be thousands of kilometers， we need the torpedo autonomous navigations to the designed area and autonomous avoids the obstacles in the process of the voyage. This paper is under the background of a certain type of remote torpedo， and uses the fuzzy control method， and puts forward an algorithm of remote torpedo underwater autonomous obstacle avoiding using the collision avoidance sonar and the planning of avoiding obstacles， and then through numerical simulations the information of the collision avoidance sonar，and last validation the algorithm in the simulation platform. The result shows that using this algorithm the remote torpedo can detect the obstacles and avoid the obstacles and arrive at the end of the set.

remote torpedo；obstacle avoiding；algorithm simulation

U666.7

A

1672-7619（2016）05-0116-05

10.3404/j.issn.1672-7619.2016.05.025

2015-12-14；

2016-01-06

胡纽（1989-），男，硕士研究生，从事水声工程技术研究。