CN112835349B - 一种基于模糊逻辑的自主水下机器人路径规划方法 - Google Patents

Abstract

本发明涉及一种基于模糊逻辑的自主水下机器人路径规划方法，属于机器人路径规划领域。一种基于模糊逻辑的自主水下机器人路径规划方法，包括以下步骤：图像的获取及图像的网格划分与障碍判断，用于将声纳获取到的图像进行网格划分以及判断网格内是否存在障碍，若存在，切换为避障状态；障碍网格距离最小值模糊求解，用于在避障状态下确定航向角变化值；无障碍判断并返回原航向角。本发明通过对声纳图像的划分确定障碍位置，使障碍确定更为快速；利用网格计数器保证了障碍判读中出现的假值；基于模糊逻辑的避障策略运算速度快进而满足水下机器人水下安全快速避障的要求。

Description

一种基于模糊逻辑的自主水下机器人路径规划方法

技术领域

本发明涉及一种基于模糊逻辑的自主水下机器人路径规划方法，属于机器人路径规划领域。

背景技术

随着水下机器人受到国内各研究机构的重视以及水下机器人市场的蓬勃发展，水下机器人设备的研制日趋简单，水下机器人的研究重点将会转移到与人工智能结合进而使水下机器人智能化。避障策略作为在探测作业过程中的安全保障和关键技术，将是与人工智能结合极为紧密的环节。目前水下机器人避障策略主要包括模糊逻辑算法、神经网络算法、人工势场法、粒子群算法，各个算法均存在其优缺点，例如人工势场法会产生目标不可达和局部极小值的问题；神经网络算法运算复杂造成响应较慢等问题。相较而言模糊逻辑算法更为简单且运算快速，在实际应用中更为广泛，但与此同时也需要依托良好的模糊控制规则。

模糊逻辑算法主要包括三个步骤，首先对输入变量模糊化，并确定其隶属度；然后根据模糊控制规则确定模糊输出值；根据模糊输入隶属度对模糊输出值解模糊从而确定输出确切值。

发明内容

为了满足水下机器人智能化需求，本发明为实现上述目的所采用的一种基于模糊逻辑的自主水下机器人路径规划方法。

本发明为实现上述目的所采用的技术方案是：

一种基于模糊逻辑的自主水下机器人路径规划方法，包括以下步骤：

a)获取图像及划分图像的网格以及障碍判断，用于将声纳获取到的图像进行网格划分以及判断网格内是否存在障碍，若存在，切换为避障状态；

b)模糊求解存在障碍点的网格中障碍点与潜器的距离最小值，用于在避障状态下确定航向角变化值；

c)判断是否无障碍并返回原航向角。

步骤a)包括：

1)将声纳获取到的图像进行网格划分，每个网格代表不同距离和角度下的潜器周围环境；

2)每个网格设定一个障碍计数器，另外单独设置一个无障碍计数器，初始化所有障碍计数器和无障碍计数器,设定其数值均为0；

3)统计每个网格内的像素点数以及障碍点数，并赋值；

4)判断网格内是否存在障碍。

所述障碍点数为根据声纳图像采集到的障碍点的数量。

步骤4)内容如下：

设网格图像总像素点数为pixel_sum，障碍点数为pixel_obs，若任一网格内pixel_obs≥pixel_sum/3，则该网格的障碍计数器值增加1，无障碍计数器值减1；

若各网格均满足pixel_obs＜pixel_sum/3，则无障碍计数器值增加1，所有网格障碍计数器值减1；

当无障碍计数器值为30时，重置无障碍计数器值为0，切换至无障碍航行状态；

当任一网格障碍计数器值为5时，重置该网格障碍计数器值为0，并切换至避障状态。

步骤b)包括：

1)保存避障状态前的航向角；

2)统计存在障碍点的网格中障碍点与潜器的距离最小值；

3)根据模糊定义将障碍点距离最小值模糊化，并确定隶属度；

4)根据模糊规则确定模糊输出和隶属度，进而确定航向角变化值。

所述模糊定义为：

输入语言变量为障碍点距离，障碍点距离论域划分为{NEAR、MID、FAR、TOO FAR}，其中，NEAR为近距离，MID为中距离，FAR为远距离，TOO FAR为无障碍，则距离隶属度函数表示为：

μ(d)＝1，d＝-1

其中，d为存在障碍点的网格中障碍点与潜器的最小距离；

输出语言变量为航向角变化值ΔH，ΔH论域划分为{NB、NM、NS、PS、PM、PB}，其中NB为负大角度，NM为负适中角度，NS为负小角度，PB为正大角度，PM为正适中角度，PS为正小角度，

则航向角变化值隶属度函数表示为：

μ(ΔH)＝{-2-6×ΔH/π，-π/2≤ΔH≤-π/3

所述模糊规则描述为：

步骤c)包括：

1)无障碍计数器数值大于设定阈值，切换为无障碍航行状态；

2)航向角减小为当前航向与避障前航向角差值的1/3逐步回归至原航向，继续执行航行任务。

本发明具有以下有益效果及优点：

本发明通过对声纳图像的划分确定障碍位置，使障碍确定更为快速；利用网格计数器保证了障碍判读中出现的假值；基于模糊逻辑的避障策略运算速度快进而满足水下机器人水下安全快速避障的要求。

附图说明

图1是本发明的声纳图像划分图；

图2是本发明实施方法流程图。

具体实施方式

本发明涉及一种基于模糊逻辑的自主水下机器人路径规划方法，该方法的主要内容包括：将水下机器人在水下航行划分为两种状态：无障碍航行状态和避障状态。水下机器人通过前视声纳判断当前所处周围环境，若前方无任何障碍信息情况下，水下机器人处于无障碍航行状态；若在一定周期内始终存在障碍信息，则进入避障状态，避障策略采取航速一定改变航向角的方式实现避障，设定水下机器人避障时航行速度为1kn。航向角的变化值由如下方法确定，首先将声纳划分为5×3网格，确定障碍在声纳图像的网格位置，确定该障碍网格内的障碍点距离最小值，对距离最小值进行模糊化及确定隶属度，根据制定的模糊规则确定模糊输出以及解模糊，最终确定输出航向角变化值。避障结束定义为在一定周期内周围环境内无任何障碍，进而切换为无障碍航行状态，水下机器人设定为避障前航向角而继续航行。

下面结合附图及实例对本发明做进一步的详细说明，其中该实例程序采用C++完成编写和调试，采用Unity3D设计的视景软件实现避障的仿真验证。

本发明具体如下：

1、声纳图像的获取及图像划分与障碍判定

各计数器初始化；避障单元接受来自视景软件发送的声纳探测信息，对获取的图像按照角度与距离划分为5×3个部分；若障碍点数大于像素点总数的1/3，相应计数器值增加一，计数器值大于设定阈值，则判定前方存在障碍。

2、障碍网格距离最小值进行模糊求解

统计障碍网格距离最小值，对各个方向网格障碍最小值模糊化，根据模糊规则表确定模糊输出值，根据输入变量对应的隶属度确定航向角变化值。

3、无障碍判断并返回原航向角

若无障碍计数器数值大于设定阈值，则切换至无障碍航行状态，返回至避障前原航向角，继续执行航行任务。

水下机器人避障单元作为水下机器人航行过程中的重要组成部分，为载体安全可靠航行提供保证，模糊逻辑算法运算速度快的优点确保了水下机器人的避障处理的快速性。

本发明采用如下技术方案：一种基于模糊逻辑的避障策略，包括将声纳图像进行划分及判断；障碍网格距离最小值进行模糊求解；无障碍判断并返回原航向角，包括以下步骤：

声纳图像进行划分及判断，用于判断有无障碍，若存在障碍，切换为避障状态；

障碍网格距离最小值进行模糊求解，用于确定水下机器人航向角变化值；

无障碍判断及返回原航向角，用于切换至无障碍航行状态。

如图1所示，声纳图像进行划分及判断执行以下步骤：

将前视声纳获取到的声纳图像划分为5×3的网格，每个网格代表一定距离和角度下的潜器周围环境。

每个网格设定一个计数器；另设无障碍计数器；初始化这些计数器值为0；

统计网格内像素点数及障碍点数并赋值；

设总像素点数为pixel_sum，障碍点数为pixel_obs，若pixel_obs≥pixel_sum/3，则相应网格的计数器值增加1，无障碍计数器减1；

若pixel_obs＜pixel_sum/3，则无障碍计数器增加1，各网格计数器值减1；

当无障碍计数器值为30时，重置无障碍计数器值为0，切换至无障碍航行状态；

当任一网格计数器为5时，重置该网格计数器为0，并切换至避障状态。

所述障碍网格距离最小值模糊求解执行以下步骤：

保存避障前航向角；

统计各网格障碍最小距离；

根据模糊定义将障碍最小距离模糊化，并确定其隶属度；

根据制定的模糊规则确定模糊输出和后件隶属度，并利用重心法解模糊，确定航向角变化值；

障碍网格距离最小值进行模糊求解具体描述为：

判断第一次进入避障标志值是否为0，若为0则第一次进入避障，记录此时刻对应的航向角。

设定各网格最短距离值变量，初始化这些最短距离值变量为-1；

统计障碍网格障碍点距离最小值并赋值于相应变量。

根据模糊定义确定距离对应的模糊输入值，根据模糊控制规则确定输出模糊值。

根据模糊规则的符合程度确定激活强度。

根据输入可确定模糊集合D₁、D₂、D₃...D_N。设D_N对应的输入左、左前、前、右前、右方向对应的距离分别为dl、dlf、df、drf、dr，对应的隶属度可表示为μ(dl)、μ(dlf)、μ(df)、μ(drf)、μ(dr)。

激活强度α_N可表示为下式：

α_N＝min(μ(dl)，μ(dlf)，μ(df)，μ(drf)，μ(dr))

其中min为取最小值函数。

将激活强度作为模糊规则后件的强度。

根据后件的强度确定各模糊集合对应的模糊输出区域，将输出区域取最大值运算确定最终模糊输出的隶属度。

利用离散型重心法解模糊，确定航向角变化值，进而改变航向。

所述模糊定义描述为：

输入语言变量为距离值，距离值论域可划分为{NEAR、MID、FAR、TOO FAR}，NEAR——近距离；MID——中距离；FAR——远距离；TOO FAR——无障碍

则距离隶属度函数表示为：

μ(d)＝1，d＝-1

输出语言变量为航向角变化值ΔH，ΔH论域划分为{NB、NM、NS、PS、PM、PB}，

NB——负大角度；NM——负适中角度；NS——负小角度；PB——正大角度；PM——正适中角度；PS——正小角度；

则航向角变化值隶属度函数表示为：

μ(ΔH)＝{-2-6×ΔH/π，-π/2≤ΔH≤-π/3

所述模糊规则描述为：

所述无障碍判断并返回原航向角执行以下步骤：

无障碍计数器数值大于设定阈值，切换无障碍航行状态函数；

航向角分别减小为当前航向与避障前航向角差值的1/3逐步回归至原航向，继续执行航行任务；

无障碍判断并返回原航向角具体描述为：

根据声纳的探测结果确定在30个周期内无障碍区域，终止避障状态；

设定航向角为避障前航向角，并以当前航向与避障前航向角差值均分为3部分，缓慢回到原航向，避免水下机器人再次遇到障碍而无法切换至避障状态。

根据水下机器人当前位置与目标点位置确定航向角，并以无障碍航行状态航行。

本发明在设计过程中可分为三模块：障碍判断模块；模糊逻辑决策模块；避障状态与无障碍航行切换模块。

障碍判断模块：

1、建立UDP接收函数，接受视景软件传递的声纳信息，按角度与距离划分为5×3网格；

2、初始化网格计数器与无障碍计数器，若网格内障碍点数大于像素点总数的1/3，相应网格计数器值增加1；

3、网格计数器超过设定阈值，进入避障函数；

模糊逻辑决策模块：

1、统计各网格障碍最小距离；

2、将障碍最小距离模糊化，并确定其隶属度；

3、根据制定的模糊规则确定模糊输出和后件隶属度，并利用重心法解模糊，确定航向角变化值；

避障状态与无障碍航行切换模块

1、无障碍计数器数值大于设定阈值，切换无障碍航行状态函数；

2、返回至避障前原航向角，继续执行航行任务；

如图2所示，本发明一种基于模糊逻辑的自主水下机器人路径规划方法实例，包括以下几个步骤：

第一步：障碍判断模块，描述如下：

编写RecData()函数用于接收视景软件发送的声纳信息；初始化障碍计数器与无障碍计数器l_far_counter；l_middle_counter；l_near_counter；lf_far_counter；lf_middle_counter；lf_near_counter；f_far_counter；f_middle_counter；f_near_counter；rf_far_counter；rf_middle_counter；rf_near_counter；r_far_counter、r_middle_counter、r_near_counter；nothing_counter；

统计各网格像素点数，若障碍像素点数大于网格像素点数的1/3，则该网格计数器值增加1，当网格计数器为5时，进入避障状态函数。

第二步：模糊逻辑决策模块，描述如下：

记录当前时刻的航向角(before_avoid_heading)，统计障碍网格的最小距离值(dl；dlf；df；drf；dr)，确定模糊输入，根据隶属度函数确定相应的隶属度，根据模糊规则确定模糊输出，根据强度转移法确定后件隶属度，然后利用离散重心法确定模糊输出对应的具体值作为航向角变化量。

第三步：避障状态与无障碍航行切换模块，描述如下：

在nothing_counter增加到30时，说明在30个周期内均未存在障碍网格，则切换至无障碍航行函数，设定航向角变化分别为当前航向与避障前航向角差值的1/3、2/3、1逐步回归至原航向，继续执行无障碍航行任务。

Claims (6)

1.一种基于模糊逻辑的自主水下机器人路径规划方法，其特征在于，包括以下步骤：

a)获取图像及划分图像的网格以及障碍判断，用于将声纳获取到的图像进行网格划分以及判断网格内是否存在障碍，若存在，切换为避障状态；

b)模糊求解存在障碍点的网格中障碍点与潜器的距离最小值，用于在避障状态下确定航向角变化值；

步骤b)包括：

1)保存避障状态前的航向角；

2)统计存在障碍点的网格中障碍点与潜器的距离最小值；

3)根据模糊定义将障碍点距离最小值模糊化，并确定隶属度；

4)根据模糊规则确定模糊输出和隶属度，进而确定航向角变化值；

c)判断是否无障碍并返回原航向角；

所述模糊定义为：

输入语言变量为障碍点距离，障碍点距离论域划分为{NEAR、MID、FAR、TOO FAR}，其中，NEAR为近距离，MID为中距离，FAR为远距离，TOO FAR为无障碍，

则距离隶属度函数表示为：

μ(d)＝1,d＝-1

其中，d为存在障碍点的网格中障碍点与潜器的最小距离；

输出语言变量为航向角变化值ΔH，ΔH论域划分为{NB、NM、NS、PS、PM、PB}，其中NB为负大角度，NM为负适中角度，NS为负小角度，PB为正大角度，PM为正适中角度，PS为正小角度，

则航向角变化值隶属度函数表示为：

μ(ΔH)＝{-2-6×ΔH/π,-π/2≤ΔH≤-π/3

2.根据权利要求1所述的一种基于模糊逻辑的自主水下机器人路径规划方法，其特征在于，步骤a)包括：

1)将声纳获取到的图像进行网格划分，每个网格代表不同距离和角度下的潜器周围环境；

2)每个网格设定一个障碍计数器，另外单独设置一个无障碍计数器，初始化所有障碍计数器和无障碍计数器,设定其数值均为0；

3)统计每个网格内的像素点数以及障碍点数，并赋值；

4)判断网格内是否存在障碍。

3.根据权利要求2所述的一种基于模糊逻辑的自主水下机器人路径规划方法，其特征在于，所述障碍点数为根据声纳图像采集到的障碍点的数量。

4.根据权利要求2所述的一种基于模糊逻辑的自主水下机器人路径规划方法，其特征在于，步骤4)内容如下：

设网格图像总像素点数为pixel_sum，障碍点数为pixel_obs，若任一网格内pixel_obs≥pixel_sum/3，则该网格的障碍计数器值增加1，无障碍计数器值减1；

若各网格均满足pixel_obs＜pixel_sum/3，则无障碍计数器值增加1，所有网格障碍计数器值减1；

当无障碍计数器值为30时，重置无障碍计数器值为0，切换至无障碍航行状态；

当任一网格障碍计数器值为5时，重置该网格障碍计数器值为0，并切换至避障状态。

5.根据权利要求1所述的一种基于模糊逻辑的自主水下机器人路径规划方法，其特征在于，所述模糊规则描述为：

6.根据权利要求1所述的一种基于模糊逻辑的自主水下机器人路径规划方法，其特征在于，步骤c)包括：

1)无障碍计数器数值大于设定阈值，切换为无障碍航行状态；

2)航向角减小为当前航向与避障前航向角差值的1/3逐步回归至原航向，继续执行航行任务。

Images