CN112462786A - 一种基于模糊控制策略双窗口算法的无人艇避碰方法 - Google Patents

Abstract

本发明提供一种基于模糊控制策略双窗口算法的无人艇避碰方法，包括：S1、确定无人艇的起始点和目标范围，计算速度窗口的线速度范围和角速度范围；S2、通过视觉传感器更新视觉窗口信息，计算得到当前的密集度和距最近物的距离；S3、基于当前的密集度和距最近物的距离，采用模糊控制器计算速度权值参数和安全性权值参数，并代入评价函数；S4、更新速度窗口，计算预测轨迹，对所述评价函数进行归一化处理，根据评价函数选择合适的速度组合作为下一时刻无人艇航行速度；S5、执行所述下一时刻无人艇航行速度，判断是否到达目标区域，若是，则结束；否则返回步骤S1。解决了无人艇通过密集障碍物环境时出现的局部最优问题以及无人艇选择过激速度的问题。

Description

一种基于模糊控制策略双窗口算法的无人艇避碰方法

技术领域

本发明涉及无人艇避障技术领域，具体而言，尤其涉及一种基于模糊控制策略双窗口算法的无人艇避碰方法。

背景技术

无人水面艇是一种轻型智能水面运载工具，具有体积小、造价低、速度快、机动性强等特点。在我国的环境检侧、科研勘探、水下测绘、搜索救援、安全巡逻等领域均有应用。自主导航是无人水面艇的核心技术之一。在实际海洋环境中，特别是航行过程中遇到的岛屿、暗礁、灯塔、浮标和航行的船只等复杂环境下,无人水面艇会因为一些易变因素，很难获得环境的完整信息。在局部地图信息已知情况下，局部动态路径规划方法是实现无人水面艇自主导航的首选方法，使其能够在环境有较大变化的情况下自主完成避障和新的路径规划。经过国内外学者多年的研究，目前已经有很多成熟的算法用于水面无人艇的路径规划，比如人工势场法，动态窗口法，遗传算法和模糊逻辑算法等。

基于动态窗口法，Fox等提出了更完善的动态窗口法(DWA)，充分考虑了物理限制、环境约束以及当前速度等因素。DWA算法首先根据无人艇外形尺寸和安全距离，将障碍物进行圆形膨化，然后根据当前速度和加速度建立一个预选速度窗口，再通过评价函数优化得到下一时刻的最优速度(包括速率和方向)。评价函数综合考虑了航向角、速率和障碍物距离3个因素，同时考虑了真实无人艇自身物理约束(最大线速度、最大角度)、环境约束，该方式能够直接获得无人艇的期望线速度和角速度，得到的轨迹比较平滑，适合真实无人艇的运行，有效解决了围绕障碍物绕行的问题。

但是，传统DWA算法没有考虑无人艇实际航行过程中通过各种传感器获取局部信息的范围限制，并且没有规定结束对单个障碍物避障的边界。算法评价函数权值固定，无人艇无法适应在密集障碍物海域航行时复杂的海况。

发明内容

根据上述提出的技术问题，而提供一种基于模糊控制策略双窗口算法的无人艇避碰方法。本发明设计了一个感知窗口来模拟传感器的感知范围，对感知范围内的障碍物进行处理，提高运行效率，并将感知窗口作为结束当前单个障碍物避障的边界，障碍物离开感知窗口即结束对其避障。在此基础上，引入模糊控制策略的双窗口算法，根据不同的局部海况信息，动态调整评价函数中权值参数，使算法规划的路径更加安全合理，其中为表征障碍物的分布情况，还设计了一种描述障碍物密集程度的函数。

本发明采用的技术手段如下：

一种基于模糊控制策略双窗口算法的无人艇避碰方法，包括如下步骤：

S1、确定无人艇的起始点和目标范围，计算速度窗口的线速度范围和角速度范围；

S2、通过视觉传感器更新视觉窗口信息，计算得到当前的密集度和距最近物的距离；

S3、基于当前的密集度和距最近物的距离，采用模糊控制器计算速度权值参数和安全性权值参数，并代入评价函数；

S4、更新速度窗口，计算预测轨迹，对所述评价函数进行归一化处理，根据评价函数选择合适的速度组合作为下一时刻无人艇航行速度；

S5、执行所述下一时刻无人艇航行速度，判断是否到达目标区域，若是，则结束；否则返回步骤S1。

进一步地，所述步骤S1具体包括：

S11、在地图上，确定无人艇的起始位置和目标位置；

S12、设定判定无人艇是否到达目标位置的距离范围；

S13、在给定无人艇的最大线速度v_max、最大角速度ω_max、当前线速度v_curr、当前角速度ω_curr和时间间隔d_t的条件下，速度组合V_s(v,ω)和V_d(v,w)满足：

V_s＝{(v,ω)|0≤v≤v_max,-ω_max≤ω≤ω_max}

其中，v表示线速度,ω表示角速度，

表示线加速度，

表示角加速度，d_t表示时间间隔。

进一步地，所述步骤S2具体包括：

S21、通过视觉传感器获得当前的障碍物信息；

S22、基于获得的障碍物信息，计算出视觉窗口内障碍物的密集度和距最近物的距离，其中，障碍物的密集度计算公式如下：

intensity＝δ·n+ε·a^instance+μ·b^area.

其中，intensity表示密集度，n表示障碍物个数，instance表示相距最近的两个障碍物的距离，area表示障碍物组成多边形的面积，δ、ε、μ表示各个部分的占比参数。

进一步地，所述步骤S3具体包括：

S31、根据计算的障碍物的密集度和距最近物的距离，制定模糊逻辑控制策略，动态调节动态窗口中的安全性权值参数和速度权值参数；其中，模糊逻辑控制器选用Mamdani型模糊逻辑控制器，采用两输入单输出结构；

S32、将计算得到的速度权值参数和安全性权值参数代入评价函数中，进而选择最优的速度。

进一步地，所述步骤S4具体包括：

S41、通过当前的速度，更新速度窗口；

S42、根据速度窗口里的速度矢量计算对应的预测轨迹，其中，每个预测轨迹都对应着评价函数中3个输入heading、dist和v，分别求和后取平均值作归一化处理；

S43、根据评价函数选择合适的速度组合作为下一时刻无人艇航行速度。

进一步地，所述步骤S5具体包括：

无人艇根据自身当前的位置，判断自己是否处于目标区域范围内。

进一步地，所述步骤S31中制定的模糊逻辑控制策略，具体包括：

当密集度为较小，且无人艇距离障碍物的最短距离较小时，采用较小的安全性权值参数和较大的速度权值参数；无人艇距离障碍物的最短距离中等时，采用较小的安全性权值参数和较大的速度权值参数；无人艇距离障碍物的最短距离较大时，采用较小的安全性权值参数和较大的速度权值参数；

当密集度为较大，且无人艇距离障碍物的最短距离较小时，采用较大的安全性权值参数和较小的速度权值参数；无人艇距离障碍物的最短距离中等时，采用中等的安全性权值参数和较小的速度权值参数；无人艇距离障碍物的最短距离较大时，采用中等的安全性权值参数和较大的速度权值参数。

较现有技术相比，本发明具有以下优点：

1、本发明提供的无人艇避碰方法，针对无人艇通过密集障碍物环境时出现的局部最优问题，设置最小速度，当无人艇陷入局部最优时就以最小速度航行，跳出局部最优。

2、本发明提供的无人艇避碰方法，设置最小安全距离的下限，一旦无人艇与障碍物的记录小于该值，则放弃该组速度组合，解决无人艇选择过激速度的问题。

3、本发明提供的无人艇避碰方法，针对视觉传感器的特点，设计了更加符合视觉传感器的视觉窗口。

4、本发明提供的无人艇避碰方法，针对评价函数中的权值参数一旦确定,就无法适应各种复杂环境或动态环境的问题，提出了模糊控制双窗口DWA算法，可以根据不同的障碍物信息，动态调整评价函数中权值参数。

基于上述理由本发明可在无人艇避障等领域广泛推广。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明方法流程图。

图2为本发明实施例提供的无人艇部分预测轨迹示意图。

图3为本发明实施例提供的无人艇航行路径示意图。

图4为本发明实施例提供的视觉窗口示意图。

图5为本发明实施例提供的密集度示意图。

图6为本发明实施例提供的模糊逻辑变量的隶属度函数示意图。

图7为本发明实施例提供的控制规则的输出界面。

图8为本发明实施例提供的β＝1，γ＝10时的航行轨迹与速度示意图。

图9为本发明实施例提供的β＝10，γ＝1时的航行轨迹与速度示意图。

图10为本发明实施例提供的动态参数时的航行轨迹与速度示意图。

图11为本发明实施例提供的不同权值参数下的无人艇避障轨迹对比图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

如图1所示，本发明提供了一种基于模糊控制策略双窗口算法的无人艇避碰方法，包括如下步骤：

S1、确定无人艇的起始点和目标范围，计算速度窗口的线速度范围和角速度范围；

具体实施时，作为本发明优选的实施方式，所述步骤S1具体包括：

S11、在地图上，确定无人艇的起始位置和目标位置；

S12、设定判定无人艇是否到达目标位置的距离范围；

S13、在给定无人艇的最大线速度v_max、最大角速度ω_max、当前线速度v_curr、当前角速度ω_curr和时间间隔d_t的条件下，速度组合V_s(v,ω)和V_d(v,w)满足：

V_s＝{(v,ω)|0≤v≤v_max,-ω_max≤ω≤ω_max}

其中，v表示线速度,ω表示角速度，

表示线加速度，

表示角加速度，d_t表示时间间隔。

S2、通过视觉传感器更新视觉窗口信息，计算得到当前的密集度和距最近物的距离；

具体实施时，作为本发明优选的实施方式，所述步骤S2具体包括：

S21、通过视觉传感器获得当前的障碍物信息；

S22、基于获得的障碍物信息，计算出视觉窗口内障碍物的密集度和距最近物的距离，其中，障碍物的密集度计算公式如下：

intensity＝δ·n+ε·a^instance+μ·b^area.

其中，intensity表示密集度，n表示障碍物个数，instance表示相距最近的两个障碍物的距离，area表示障碍物组成多边形的面积，δ、ε、μ表示各个部分的占比参数。

S3、基于当前的密集度和距最近物的距离，采用模糊控制器计算速度权值参数和安全性权值参数，并代入评价函数；

具体实施时，作为本发明优选的实施方式，所述步骤S3具体包括：

S31、根据计算的障碍物的密集度和距最近物的距离，制定模糊逻辑控制策略，动态调节动态窗口中的安全性权值参数和速度权值参数；其中，模糊逻辑控制器选用Mamdani型模糊逻辑控制器，采用两输入单输出结构；制定的模糊逻辑控制策略，具体包括：

当密集度为较小，且无人艇距离障碍物的最短距离较小时，采用较小的安全性权值参数和较大的速度权值参数；无人艇距离障碍物的最短距离中等时，采用较小的安全性权值参数和较大的速度权值参数；无人艇距离障碍物的最短距离较大时，采用较小的安全性权值参数和较大的速度权值参数；

当密集度为较大，且无人艇距离障碍物的最短距离较小时，采用较大的安全性权值参数和较小的速度权值参数；无人艇距离障碍物的最短距离中等时，采用中等的安全性权值参数和较小的速度权值参数；无人艇距离障碍物的最短距离较大时，采用中等的安全性权值参数和较大的速度权值参数。

S32、将计算得到的速度权值参数和安全性权值参数代入评价函数中，进而选择最优的速度。

S4、更新速度窗口，计算预测轨迹，对所述评价函数进行归一化处理，根据评价函数选择合适的速度组合作为下一时刻无人艇航行速度；

具体实施时，作为本发明优选的实施方式，所述步骤S4具体包括：

S41、通过当前的速度，更新速度窗口；

S42、根据速度窗口里的速度矢量计算对应的预测轨迹，其中，每个预测轨迹都对应着评价函数中3个输入heading、dist和v，分别求和后取平均值作归一化处理；

S43、根据评价函数选择合适的速度组合作为下一时刻无人艇航行速度。

S5、执行所述下一时刻无人艇航行速度，判断是否到达目标区域，若是，则结束；否则返回步骤S1。

具体实施时，作为本发明优选的实施方式，所述步骤S5具体包括：

无人艇根据自身当前的位置，判断自己是否处于目标区域范围内。

下面结合附图和实施例对本发明作进一步地说明。

参见图2-11，本发明的技术方案是这样实现的：

DWA算法是对无人艇的速度进行控制。算法会建立一个速度空间，并通过约束对其优化，包括速度和加速度的约束和目标与障碍物的约束。

因为无人艇受动力学限制，所以其速度和加速度都是有一定限制的。在给定无人艇的最大线速度v_max、最大角速度ω_max、当前线速度v_curr、当前角速度ω_curr和时间间隔d_t的条件下，速度组合V_s(v，ω)和V_d(v，w)满足：

V_s＝{(v，ω)|0≤v≤v_max，-ω_max≤ω≤ω_max}

其中，v表示线速度，ω表示角速度，

表示线加速度，

表示角加速度，dt表示时间间隔。

即建立了算法的速度窗口，实际上是这两个速度集合的合成：

V_r＝V_s∩V_d.

为了保证无人艇的安全性，所以考虑其在行驶过程中的制动距离(对无人艇和障碍物都进行了安全距离的扩展)，计算公式如下：

其中：stopDist为制动距离，v_n为后n个时刻的速度，m为速度为0时的时刻。在速度窗口中去掉不符合的速度组合，保证选择的线速度计算出的制动距离大于无人艇距离障碍物的最短距离。

无人艇通过评价函数对下一时刻的速度做预测，选择满足各种约束的最优速度。评价函数考虑了无人艇的航行速度、航向和安全性。定义如下：

G(v，ω)＝α·heading(v，ω)+β·dist(v，ω)+γ·v(v，ω).

G(v，w)值最大的v和w即为最优速度。其中，heading用于评价无人艇的航向，无人艇航向完全指向目标点时有最大值；dist用于评价无人艇的安全性，即预测轨迹中无人艇距离障碍物的最小距离；v用于评价无人艇的航行速度，即轨迹中t+1时刻的线速度，结果尽量选择动态窗口V_r中线速度大的值；α，β，γ为3个权值参数，分别对应航向、安全性和航行速度。将评价函数的三个参数归一化处理到[0，1]之间，避免任何一项值过大。

无人艇的轨迹可以认为是由n个时段的n个小轨迹组成的，其运动学方程如下：

X(x′，y′)＝[x+d_t·cos(θ)·v，y+d_t·sin(θ)·v]

θ′＝θ+d_t*ω.

其中，x是无人艇当前横坐标；y是无人艇当前纵坐标；X是下一时刻对无人艇的位置信息更新；θ是当前无人艇的朝向，θ′是下一时刻无人艇的朝向。无人艇多个预测轨迹示意图如图2所示。根据上式可以计算出每一个预测轨迹的G值，选择G值最大的轨迹的速度作为下一时刻的最优速度。

经典DWA算法中的的三个评价函数权值参数都是固定的，尤其是当无人艇遇到密集障碍物时，想要找到可以兼顾三个方面并且可以适应各种障碍物分布的权值参数是很难的，无人艇采用低参数时路径短、迭代次数少、运行时间少，但是安全性低，采用高参数时随着安全性增加，迭代次数和运行时间就会增加。而且在有特别密集障碍物的环境中，如果权值参数选择不当还会陷入局部最优，出现没有预测路径的情况。还有在航行过程中，无人艇为了保证航向和速度时，可能出现选择过激速度的情况(即无人艇与障碍物几乎碰撞)。

下面将航向权值参数和速度权值参数固定，调节安全性权值参数(即上述β)进行仿真。图3为无人艇在不同安全性权值参数下的航行路径。其中：Distance表示路径长度；Steps表示迭代次数；Time表示航行时间；Ssfe Dist表示无人艇距离障碍物的最短距离。

当无人艇采用低参数时，路径短(117.50m)、迭代次数少(136此)、运行时间少(14.046s)，但是安全性极低(0.56m)，几乎和障碍物发生了碰撞；采用高参数时随着安全性(2.04)增加迭代次数(145次)和运行时间(15.887s)就会增加，造成无人艇的航行效率降低。

针对无人艇通过密集障碍物环境时出现的局部最优问题，设置最小速度，当无人艇陷入局部最优时就以最小速度航行，跳出局部最优；同时设置最小安全距离的下限，一旦无人艇与障碍物的记录小于该值，则放弃该组速度组合，解决无人艇选择过激速度的问题；针对视觉传感器的特点，设计了更加符合视觉传感器的视觉窗口；针对评价函数中的权值参数一旦确定，就无法适应各种复杂环境或动态环境的问题，提出了模糊控制双窗口DWA算法，可以根据不同的障碍物信息，动态调整评价函数中的安全性权值参数(β)和速度权值参数(γ)，还提出一种描述障碍物密集程度的方法。

为了适应视觉传感器，设置一个类似视线范围的扇形区域，无人艇的航向在此区域中心，随无人艇航行运动，如图4所示，视觉窗口通过视觉传感器可以探测到的最大距离为半径的扇形窗口，进入窗口内的障碍物再进行避障，提高了算法的效率。

通过视觉传感器更新视觉窗口信息，计算得到当前的密集度和距最近物的距离；

与范围内障碍物的个数以及障碍物的分布相关，其中，分布情况用相距最近的两个障碍物的距离和障碍物组成多边形的面积来表示，均采用指数函数的形式，具体公式如下：

intensity＝δ·n+ε·a^instance+μ·b^area.

其中，intensity表示密集度，n表示障碍物个数，instance表示相距最近的两个障碍物的距离，area表示障碍物组成多边形的面积，δ、ε、μ表示各个部分的占比参数，经过多次仿真实验调整，δ、ε、μ取值分别为0.3、0.25、0.5，a和b取值分别为1/50和1/2。

从图5中可知，当视觉窗口中出现两个障碍物时密集度为0.6157，五个障碍物时为1.9600，七个障碍物时为2.5600，可以有效地表示范围内障碍物的密集程度。

通过本文提出的密集度和无人艇距离障碍物的最短距离制定了模糊逻辑控制来动态调节动态窗口中的安全性权值参数和速度权值参数。模糊逻辑控制策略的详细控制策略如下：

当密集度为较小时，无人艇距离障碍物的最短距离较小时，采用较小的安全性权值参数和较大的速度权值参数；无人艇距离障碍物的最短距离中等时，采用较小的安全性权值参数和较大的速度权值参数；无人艇距离障碍物的最短距离较大时，采用较小的安全性权值参数和较大的速度权值参数。

当密集度为较大时，无人艇距离障碍物的最短距离较小时，采用较大的安全性权值参数和较小的速度权值参数；无人艇距离障碍物的最短距离中等时，采用中等的安全性权值参数和较小的速度权值参数；无人艇距离障碍物的最短距离较大时，采用中等的安全性权值参数和较大的速度权值参数。

本实施例中，模糊逻辑控制器选用Mamdani型模糊逻辑控制器，采用两输入两输出结构。模糊逻辑控制的输入子集如表1所示，输出子集如表2所示。模糊逻辑变量的隶属度函数如图3所示，模糊逻辑控制器的逻辑控制规则如表3所示。

表1模糊逻辑控制输入子集

表2模糊逻辑控制输出子集

表3模糊规则

为了验证本发明方法的有效性，在不同权值下进行了对比实验，对不同环境下障碍物分布情况进行对比实验，在保证安全性的情况下对时间、路程等进行定性和定量比较。在相同环境下进行避障效果对比实验：

实施例1：

强调航行速度时，需要将β设置较小，γ设置较大，即牺牲安全性，保证速度。实验设置固定β为1，γ为10，实验结果如图8所示。图8(a)是无人艇航行轨迹图,整个轨迹长度为118.10m,迭代次数为84次,时间为10.938s。在通过密集障碍物区域时,无人艇过于接近障碍物,安全性较低(安全距离为2.01m)。图8(b)为航行过程中的线速度，可以看出无人艇基本保持最高速度航行，就算遇到障碍使速度降低也会迅速加速到最高速度。图8(c)为航行过程中的角速度，对应轨迹图可以看出，无人艇航行过程中没有出现急转弯的现象，转角最大为0.2(角度制为11°左右)左右，轨迹相对平缓。

实施例2：

强调航行安全性时，需要将β设置较大，γ设置较小，即牺牲速度，保证安全性。实验设置固定β为10，γ为1，实验结果如图9所示。图9(a)是无人艇航行轨迹图,轨迹长度为117.40m,迭代次数为92次,时间为12.727s，安全距离为3.53m。轨迹长度、迭代次数和时间均高于上述实验，但是安全性较好(安全距离为3.53m)。图9(b)为航行过程中的线速度，可以看出相比于上述实验，无人艇在遇到障碍物时会放慢速度，在部分时间是处于低速航行的状态。航行过程中的角速度与上述实验相似。

实施例3：

当采用动态权值参数时，既保证速度，又能保证安全性，实验结果如图10所示。图10(a)是无人艇航行轨迹图,轨迹长度为118.80m,迭代次数为83次,时间为11.410s，安全距离为2.79m。对比前两个实验，无人艇可以在保证安全性的前提下，缩短迭代次数和运行时间，使无人艇具有更高的航行效率。图10(b)为航行过程中的线速度，图10(c)为航行过程中的角速度，可以看出相比于前两个实验，在遇到障碍物时会在保证速度的情况下选择合适的速度迅速避开障碍物，没有出现速度特别小的情况。图11为三种权值参数下的轨迹对比，可以看出动态参数时的轨迹可以安全的穿越复杂障碍物，并且路径合理。

对实验结果进行定量分析，由各图中数据可知，本发明方法能够在保证安全性的前提下使无人艇的航行效率得到提高，相比于β＝10，γ＝1时的情况，迭代次数降低了10.84％，运行时间降低了11.54％，轨迹长度变化不大。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (7)

1.一种基于模糊控制策略双窗口算法的无人艇避碰方法，其特征在于，包括如下步骤：

S1、确定无人艇的起始点和目标范围，计算速度窗口的线速度范围和角速度范围；

S2、通过视觉传感器更新视觉窗口信息，计算得到当前的密集度和距最近物的距离；

S3、基于当前的密集度和距最近物的距离，采用模糊控制器计算速度权值参数和安全性权值参数，并代入评价函数；

S4、更新速度窗口，计算预测轨迹，对所述评价函数进行归一化处理，根据评价函数选择合适的速度组合作为下一时刻无人艇航行速度；

S5、执行所述下一时刻无人艇航行速度，判断是否到达目标区域，若是，则结束；否则返回步骤S1。

2.根据权利要求1所述的基于模糊控制策略双窗口算法的无人艇避碰方法，其特征在于，所述步骤S1具体包括：

S11、在地图上，确定无人艇的起始位置和目标位置；

S12、设定判定无人艇是否到达目标位置的距离范围；

S13、在给定无人艇的最大线速度v_max、最大角速度ω_max、当前线速度v_curr、当前角速度ω_curr和时间间隔d_t的条件下，速度组合V_s(v,ω)和V_d(v,w)满足：

V_s＝{(v,ω)|0≤v≤v_max,-ω_max≤ω≤ω_max}

其中，v表示线速度,ω表示角速度，

表示线加速度，

表示角加速度，d_t表示时间间隔。

3.根据权利要求1所述的基于模糊控制策略双窗口算法的无人艇避碰方法，其特征在于，所述步骤S2具体包括：

S21、通过视觉传感器获得当前的障碍物信息；

S22、基于获得的障碍物信息，计算出视觉窗口内障碍物的密集度和距最近物的距离，其中，障碍物的密集度计算公式如下：

intensity＝δ·n+ε·a^instance+μ·b^area.

其中，intensity表示密集度，n表示障碍物个数，instance表示相距最近的两个障碍物的距离，area表示障碍物组成多边形的面积，δ、ε、μ表示各个部分的占比参数。

4.根据权利要求1所述的基于模糊控制策略双窗口算法的无人艇避碰方法，其特征在于，所述步骤S3具体包括：

S31、根据计算的障碍物的密集度和距最近物的距离，制定模糊逻辑控制策略，动态调节动态窗口中的安全性权值参数和速度权值参数；其中，模糊逻辑控制器选用Mamdani型模糊逻辑控制器，采用两输入单输出结构；

S32、将计算得到的速度权值参数和安全性权值参数代入评价函数中，进而选择最优的速度。

5.根据权利要求1所述的基于模糊控制策略双窗口算法的无人艇避碰方法，其特征在于，所述步骤S4具体包括：

S41、通过当前的速度，更新速度窗口；

S42、根据速度窗口里的速度矢量计算对应的预测轨迹，其中，每个预测轨迹都对应着评价函数中3个输入heading、dist和v，分别求和后取平均值作归一化处理；

S43、根据评价函数选择合适的速度组合作为下一时刻无人艇航行速度。

6.根据权利要求1所述的基于模糊控制策略双窗口算法的无人艇避碰方法，其特征在于，所述步骤S5具体包括：

无人艇根据自身当前的位置，判断自己是否处于目标区域范围内。

7.根据权利要求4所述的基于模糊控制策略双窗口算法的无人艇避碰方法，其特征在于，所述步骤S31中制定的模糊逻辑控制策略，具体包括：

当密集度为较小，且无人艇距离障碍物的最短距离较小时，采用较小的安全性权值参数和较大的速度权值参数；无人艇距离障碍物的最短距离中等时，采用较小的安全性权值参数和较大的速度权值参数；无人艇距离障碍物的最短距离较大时，采用较小的安全性权值参数和较大的速度权值参数；

当密集度为较大，且无人艇距离障碍物的最短距离较小时，采用较大的安全性权值参数和较小的速度权值参数；无人艇距离障碍物的最短距离中等时，采用中等的安全性权值参数和较小的速度权值参数；无人艇距离障碍物的最短距离较大时，采用中等的安全性权值参数和较大的速度权值参数。

Images