CN114003047A - 一种小型无人船路径规划方法 - Google Patents

Abstract

本发明涉及船舶自动控制及导航技术领域，提供一种小型无人船路径规划方法，包括以下步骤：S1：初始化环境信息，确定小型无人船的起始点位置、目标点位置及环境中的障碍物的信息；S2：确定小型无人船相对于当前位置的下一位置，将下一位置重新设置为当前位置；S3：判断当前位置是否为目标点位置，如是则结束路径规划，如否，则跳转到步骤S4；S4：判断当前位置是否为局部极小值位置，如否，则跳转到步骤S2，如是，则跳转到步骤S5；S5：规划小型无人船脱离局部极小值位置的路径，在小型无人船脱离局部极小值位置后跳转到步骤S2。本发明提供的方法能够有效地解决目标不可达问题，并能够使小型无人船更快地脱离局部极小值位置。

Description

一种小型无人船路径规划方法

技术领域

本申请涉及船舶自动控制及导航技术领域，具体地，涉及一种基于改进的人工势场法的小型无人船路径规划方法。

背景技术

小型无人船（Unmanned Surface Vehicle，简称USV）因其机动性高、模块化和智能化等优势，在海洋及湖泊、河道的水面调查任务中得到广泛的利用。USV的性能主要取决于其自主导航也就是其路径规划的能力。

目前针对包括USV在内的运动物体的常用路径规划算法主要有：Dijkstra算法、A*算法（也称作A Star算法）、人工势场法、神经网络算法、栅格法、遗传算法以及上述算法的改进算法与融合算法。其中，人工势场法（Artificial Potential Field，简称APF）作为一种局部规划算法具有模型结构简单、计算冗余量小、实时性强等优点而得到广泛的研究与应用。该方法的核心思想是假设运动物体向目标点运动的过程中，其运动空间是二维的，通过在运动物体的规划运动范围内构建人工势场(该势场为人为构造的虚拟势场)，目标点对于运动物体产生全局的引力作用(即由人工势场产生的虚拟引力)，相反，环境中的障碍物对运动物体产生斥力影响(即由人工势场产生的虚拟斥力)，运动物体所受到目标点的引力与环境中的障碍物对其产生的斥力形成合力来引导运动物体的运动。同时引入梯度的概念，即在人工势场环境下，引力为引力场的负梯度，斥力为斥力场的负梯度，运动物体受到的合力为当前位置总势场的负梯度。

但是，在使用传统的人工势场法进行小型无人船路径规划时仍存在着一定的缺陷，主要包括：

(1) 当目标点距离障碍物过近时，在目标点位置小型无人船所受到的引力小于障碍物对其产生的斥力，从而导致目标不可达问题；

(2) 当小型无人船在环境中受到的障碍物对其产生的斥力或斥力的合力与所受到的引力大小相等，方向相反时，则会陷入局部极小值陷阱。

同时，现有的对于人工势场法的改进算法计算负载较大，并没有考虑到小型无人船的低成本、高机动性等本身的特点，不适用于小型无人船的应用。

综上，如何解决现有的路径规划方法所存在的缺陷并将其应用于小型无人船，成为小型无人船路径规划的重点及难点。对于提升小型无人船航行效率、高安全性及高效水面调查具有极为重要的意义。

发明内容

本申请的目的在于解决上述小型无人船路径规划方法中存在的缺陷，提供一种基于改进的人工势场法的小型无人船路径规划方法。

本申请的实施例可以通过以下技术方案实现：

一种小型无人船路径规划方法，基于改进的人工势场法对小型无人船进行路径规划，包括以下步骤：

S1：初始化环境信息，确定所述小型无人船的起始点位置、目标点位置及环境中的障碍物的信息，所述障碍物的信息包括障碍物的位置和障碍物的最大作用范围；

S2：基于改进的人工势场法确定所述小型无人船相对于当前位置的下一位置，将所述下一位置重新设置为当前位置；

S3：判断所述当前位置是否为目标点位置，如判断结果为是，则结束路径规划，如判断结果为否，则跳转到步骤S4；

S4：判断所述当前位置是否为局部极小值位置，如判断结果为否，则跳转到步骤S2，如判断结果为是，则跳转到步骤S5；

S5：规划所述小型无人船脱离所述局部极小值位置的路径，在所述小型无人船脱离所述局部极小值位置后跳转到步骤S2。

进一步地，所述步骤S1中基于改进的人工势场法确定所述小型无人船相对于当前位置的下一位置，进一步包括以下步骤：

根据所述当前位置和所述目标点位置确定所述小型无人船受到所述目标点的引力；根据所述当前位置、所述障碍物的信息和所述目标点位置确定所述小型无人船受到所述障碍物的斥力；根据所述当前位置、所述引力和所述斥力确定所述下一位置。

进一步地，所述斥力随所述当前位置与所述目标点位置之间距离的减小而减小。

进一步地，所述斥力包括由所述障碍物指向所述当前位置的分量和由所述当前位置指向所述目标点位置的分量

进一步地，所述引力由下式(1)确定：

(1)

其中，

为所述当前位置，

为所述引力，

为所述目标点位置，

为引力势场正比例增益系数；

所述斥力由下式(2)确定：

(2)

其中，

为所述斥力，

为所述障碍物的位置，

为所述当前位置与所述障碍物的位置之间的距离，

为所述障碍物的最大作用范围，

为由所述障碍物的位置指向所述当前位置的分量，

为由所述当前位置指向所述目标点位置的分量，具体地，所述

和所述

由下式(3)、(4)确定：

(3)

(4)

其中，

为正比例斥力增益系数，

为斥力势场调节因子，

为正整数且大小根据所述小型无人船与所述障碍物之间的距离确定。

进一步地，所述步骤S5进一步包括以下步骤：

S51：根据所述当前位置和所述障碍物的信息生成虚拟障碍物并确定所述虚拟障碍物的信息，所述虚拟障碍物的信息包括所述虚拟障碍物的位置和所述虚拟障碍物的最大作用范围；

S52：根据所述当前位置和所述目标点位置确定所述引力，根据所述当前位置、所述障碍物的信息和所述目标点位置确定所述斥力，根据所述当前位置、所述虚拟障碍物的信息确定所述小型无人船受到所述虚拟障碍物的虚拟斥力；

S53：根据所述当前位置，所述引力、所述斥力以及所述虚拟斥力确定所述小型无人船相对于当前位置的下一位置，将所述下一位置重新设置为当前位置；

S54：判断所述当前位置是否脱离所述虚拟障碍物的最大作用范围，如判断结果为是，则删除所述虚拟障碍物后跳转到步骤S2，如否，则跳转到步骤S52。

进一步地，所述步骤S51中根据所述当前位置和所述障碍物的信息生成虚拟障碍物，进一步包括以下步骤：

以所述当前位置为原点建立二维坐标系，其中，所述二维坐标系的y轴正方向为所述当前位置朝向所述目标点位置的方向，x轴正方向为所述y轴正方向顺时针旋转90°的方向；在所述二维坐标系的第三象限或第四象限随机生成虚拟障碍物，其中，所述虚拟障碍物与所述原点的距离小于所述障碍物的最大作用范围。

优选地，所述虚拟斥力根据所述当前位置、所述虚拟障碍物的位置以及所述虚拟障碍物的最大作用范围确定。

优选地，所述虚拟斥力在所述当前位置与所述虚拟障碍物的距离小于所述虚拟障碍物的最大作用范围时为常数。

本申请的实施例提供的一种小型无人船路径规划方法至少具有以下有益效果：

(1) 本实施例提供的方法对小型无人船受到的障碍物的斥力进行改进，将斥力分解为从障碍物指向小型无人船的分量和从小型无人船指向目标点的分量，并在斥力中引入当前位置与目标点位置的相对距离参数，且该距离参数随着小型无人船与目标点距离的接近而减小，使得小型无人船在受到的障碍物影响的同时，还受到与目标点距离的约束，且斥力随着小型无人船逐渐靠近目标点而逐渐减小，从而有效地解决了目标不可达问题；

(2) 本实施例提供的方法在小型无人船陷入局部极小值时，采用在侧后方生成虚拟障碍物的方法，使得小型无人船获得额外的朝向侧前方的虚拟斥力，使得小型无人船能够脱离局部极小值且无需在短时间内进行大角度转弯操作；

(3) 本实施例提供的方法中，虚拟障碍物在小型无人船脱离其最大作用范围后自动释放消失，从而减小了后续计算量且对后续的航向不会产生任何影响。

(4) 在本申请的优选的实施例中，虚拟斥力在小型无人船处于虚拟障碍物的最大作用范围内保持为常数，使得虚拟斥力不随小型无人船与虚拟障碍物距离增大而减小，从而可以加快小型无人船脱离局部最小值，有效地缩短了小型无人船的规划路径。

附图说明

图1为本申请实施例提供的一种小型无人船路径规划方法的流程图；

图2为本申请实施例中小型无人船受力的示意图；

图3为本申请实施例中小型无人船处于局部极小值位置时受力的示意图；

图4为本申请实施例中步骤S5的流程图；

图5为本申请实施例中生成虚拟障碍物及小型无人船受力的示意图。

具体实施方式

以下，结合本申请的实施例并参照附图对本申请的技术方案进行清楚、完整的描述，应当说明的是，以下所描述的实施例用于使本技术领域的技术人员更好地理解本申请的技术方案，而不是代表本申请的所有实施例。基于本申请中的实施例，本技术领域的普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应当属于本申请保护的范围。

本申请的说明书、权利要求书和附图中所使用的“第一”、“第二”等表述是为了区分类似的单元或对象，而不是用于限定特定的顺序或先后次序，也不能理解为指示或暗示相对重要性。此外，术语“包括”、“具有”以及他们的任何可以替换的表述，意图在于覆盖不排他的包含，例如包含了一系列步骤或单元的过程、方法不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法固有的其他步骤或单元。

除非另有明确的规定和限定，若出现术语“设置”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，可以是直接相连，也可以通过中间媒介间接连接，可以是两个元件内部的连通。对于本领域的技术人员而言，可以具体理解上述术语在本申请中的具体含义。

如图1所示，本申请的实施例提供一种小型无人船路径规划方法，基于改进的人工势场法对小型无人船进行路径规划，包括以下步骤：

S1：初始化环境信息，确定所述小型无人船的起始点位置、目标点位置及环境中的障碍物的信息，所述障碍物的信息包括障碍物的位置和障碍物的最大作用范围；

S2：基于改进的人工势场法确定所述小型无人船相对于当前位置的下一位置，将所述下一位置重新设置为当前位置；

S3：判断所述当前位置是否为目标点位置，如判断结果为是，则结束路径规划，如判断结果为否，则跳转到步骤S4；

S4：判断所述当前位置是否为局部极小值位置，如判断结果为否，则跳转到步骤S2，如判断结果为是，则跳转到步骤S5；

S5：规划所述小型无人船脱离所述局部极小值位置的路径，在所述小型无人船脱离所述局部极小值位置后跳转到步骤S2。

以下结合本申请的优选的实施例对上述各步骤进行详细说明。

在本申请的实施例中，步骤S1为初始化环境信息，确定上述小型无人船的起始点位置、目标点位置及环境中的障碍物的信息，其中，障碍物可能有多个，障碍物的信息包括障碍物的位置以及障碍物的最大作用范围，确定上述信息后将上述小型无人船的起始点位置设置为小型无人船的当前位置。

在本申请的实施例中，步骤S2为求取小型无人船相对于当前位置的下一位置并将其重新设置为当前位置的步骤，具体地，包括以下步骤：

第一步，根据当前位置和目标点位置确定小型无人船受到目标点的引力，设当前位置为

，目标点位置为

，则目标点与处于当前位置的小型无人船之间的引力场

可以表示为：

其中，

为引力势场正比例增益系数，由于引力为引力场的负梯度，则小型无人船受到目标点的引力

可以表示为：

，

引力的方向为自小型无人船的当前位置指向目标；

第二步，根据当前位置、障碍物的信息和目标点位置确定小型无人船受到障碍物的斥力，设障碍物的位置为

，障碍物的最大作用范围为

，当前位置与障碍物的位置之间的距离为

，则障碍物与小型无人船之间的斥力场

可以通过改进后的斥力场函数表示为：

其中，

为正比例斥力增益系数，

为无人船与目标点的相对距离参数，

为斥力势场调节因子，

为正整数且大小根据所述小型无人船与所述障碍物之间的距离确定。在无人船未进入障碍物的最大作用范围时，无人船不受障碍物的斥力场的影响，仅受目标点的引力进行运动，由上式求得无人船受到障碍物的斥力

为：

其中，

上式中

的方向为自障碍物的位置指向小型无人船的当前位置，

的位置为自小型无人船的当前位置指向目标点位置，且

和

的大小均随着小型无人船逐步靠近目标点而减小；

第三步，根据当前位置、引力和斥力确定下一位置，如图2所示，小型无人船受到的自当前位置指向目标点的力为

，受到的自障碍物的位置指向当前位置的力为

，小型无人船受到的合力

的方向即为小型无人船自当前位置向下一位置运动的方向，运动的步长可以为预先设置的固定值，也可以根据小型无人船受到的合力的大小进行调整；

第四步，小型无人船运动到下一位置，并将下一位置重新设置为当前位置。

步骤S2采用改进的人工势场法进行小型无人船的局部路径规划，对小型无人船受到的障碍物的斥力进行改进，将斥力分解为从障碍物指向小型无人船的分量和从小型无人船指向目标点的分量，并在斥力中引入当前位置与目标点位置的相对距离参数

和

，且该距离参数随着小型无人船与目标点距离的接近而减小，使得小型无人船在受到的障碍物影响的同时，还受到与目标点距离的约束，且斥力随着小型无人船逐渐靠近目标点而逐渐减小，从而有效地解决了目标不可达问题。

在本申请的实施例中，步骤S3判断当前位置是否为目标点位置，如判断结果为是，则代表小型无人船已经到达目标点，则结束路径规划，如判断结果为否，则跳转到步骤S4继续判断小型无人船是否陷入局部最小值区域。

在本申请的实施例中，步骤S4判断当前位置是否陷入局部最小值位置，具体地，如图3所示，局部极小值位置为小型无人船受到的自当前位置指向目标点的力

和自目标点指向当前位置的力

大小相等、方向相反的位置，如果判断结果为是，则跳转到步骤S5规划小型无人船脱离局部极小值位置的路径，如果判断结果为否，则跳转到步骤S2继续计算下一位置。

在本申请的实施例中，步骤S5对陷入局部极小值位置的无人船进行路径规划，使其脱离局部极小值位置，并在脱离后重新执行步骤S2。具体地，如图4所示，步骤S5包括以下步骤：

S51：根据所当前位置和所述障碍的物信息生成虚拟障碍物并确定所述虚拟障碍物的信息，所述虚拟障碍物的信息包括所述虚拟障碍物的位置和所述虚拟障碍物的最大作用范围；

S52：根据所述当前位置和所述目标点位置确定所述引力，根据所述当前位置、所述障碍物的信息和所述目标点位置确定所述斥力，根据所述当前位置、所述虚拟障碍物的信息确定所述小型无人船受到所述虚拟障碍物的虚拟斥力；

S53：根据所述当前位置，所述引力、所述斥力以及所述虚拟斥力确定所述小型无人船相对于当前位置的下一位置，将所述下一位置重新设置为当前位置；

S54：判断所述当前位置是否脱离所述虚拟障碍物的最大作用范围，如判断结果为是，则删除所述虚拟障碍物后跳转到步骤S2，如否，则跳转到步骤S52。

具体地，在本申请的实施例中，当小型无人船陷入局部最小值位置后，首先，如图5所示，以当前位置为原点建立二维坐标系，其中，二维坐标系的y轴正方向为当前位置朝向目标点位置的方向，x轴正方向为y轴正方向顺时针旋转90°的方向；

然后在二维坐标系的第三象限或第四象限随机生成虚拟障碍物

，其中，虚拟障碍物

与原点的距离小于障碍物的最大作用范围

；同时确定虚拟障碍物

的位置和虚拟障碍物

的最大作用范围，在本实施例的一些具体实施方式中，虚拟障碍物

的最大作用范围可以与障碍物的最大作用范围

相同；

获得上述信息后分别计算小型无人船受到目标点的引力、受到障碍物的斥力和受到虚拟障碍物

的虚拟斥力

，其中，引力、斥力的计算与步骤S2中对应步骤相同，在此不再赘述；

在本申请实施例的一些优选的实施方案中，虚拟斥力

的方向为自虚拟障碍物的位置指向当前位置，虚拟斥力

的大小在当前位置未脱离虚拟障碍物的最大作用范围时为一预先确定的常数；

确定上述引力、斥力和虚拟斥力后，根据上述各作用力的合力确定小型无人船相对于当前位置的下一位置并将其重新设置为当前位置，然后判断当前位置是否已脱离虚拟障碍物的最大作用范围，如果已经脱离虚拟障碍物的最大作用范围，则删除虚拟障碍物（即虚拟斥力在

在当前位置已经脱离虚拟障碍物的最大作用范围时即消失），并跳转到步骤S2，如果未脱离虚拟障碍物的最大作用范围，则跳转到步骤S52。

本实施例的方法在小型无人船陷入局部极小值时，采用在侧后方生成虚拟障碍物的方法，使得小型无人船获得额外的朝向侧前方的虚拟斥力，使得小型无人船能够脱离局部极小值且无需在短时间内进行大角度转弯操作；虚拟障碍物在小型无人船脱离其最大作用范围后自动释放消失，从而减小了后续计算量且对后续的航向不会产生任何影响。

在本实施例的优选的实施方式中，虚拟斥力在小型无人船处于虚拟障碍物的最大作用范围内保持为常数，使得虚拟斥力不随小型无人船与虚拟障碍物距离增大而减小，从而可以加快小型无人船脱离局部最小值，有效地缩短了小型无人船的规划路径。

以上对本申请的具体实施方式作了详细介绍，对于本技术领域的技术人员来说，在不脱离本申请原理的前提下，还可以对本申请进行若干改进和修饰，这些改进和修饰也属于本申请权利要求的保护范围。

Claims (9)

1.一种小型无人船路径规划方法，基于改进的人工势场法对小型无人船进行路径规划，其特征在于，包括以下步骤：

S1：初始化环境信息，确定所述小型无人船的起始点位置、目标点位置及环境中的障碍物的信息，所述障碍物的信息包括障碍物的位置和障碍物的最大作用范围；

S2：基于改进的人工势场法确定所述小型无人船相对于当前位置的下一位置，将所述下一位置重新设置为当前位置；

S3：判断所述当前位置是否为目标点位置，如判断结果为是，则结束路径规划，如判断结果为否，则跳转到步骤S4；

S4：判断所述当前位置是否为局部极小值位置，如判断结果为否，则跳转到步骤S2，如判断结果为是，则跳转到步骤S5；

S5：规划所述小型无人船脱离所述局部极小值位置的路径，在所述小型无人船脱离所述局部极小值位置后跳转到步骤S2。

2.如权利要求1所述的小型无人船路径规划方法，其特征在于：

所述步骤S2中基于改进的人工势场法确定所述小型无人船相对于当前位置的下一位置，进一步包括以下步骤：

根据所述当前位置和所述目标点位置确定所述小型无人船受到所述目标点的引力；

根据所述当前位置、所述障碍物的信息和所述目标点位置确定所述小型无人船受到所述障碍物的斥力；

根据所述当前位置、所述引力和所述斥力确定所述下一位置。

3.如权利要求2所述的小型无人船路径规划方法，其特征在于：

所述斥力随所述当前位置与所述目标点位置之间距离的减小而减小。

4.如权利要求2所述的小型无人船路径规划方法，其特征在于：

所述斥力包括由所述障碍物指向所述当前位置的分量和由所述当前位置指向所述目标点位置的分量。

5.如权利要求4所述的小型无人船路径规划方法，其特征在于：

所述引力由下式(1)确定：

(1)

其中，

为所述当前位置，

为所述引力，

为所述目标点位置，

为引力势场正比例增益系数；

所述斥力由下式(2)确定：

(2)

其中，

为所述斥力，

为所述障碍物的位置，

为所述当前位置与所述障碍物的位置之间的距离，

为所述障碍物的最大作用范围，

为由所述障碍物的位置指向所述当前位置的分量，

为由所述当前位置指向所述目标点位置的分量，具体地，所述

和所述

由下式(3)、(4)确定：

(3)

(4)

其中，

为正比例斥力增益系数，

为斥力势场调节因子，

为正整数且大小根据所述小型无人船与所述障碍物之间的距离确定。

6.如权利要求5所述的小型无人船路径规划方法，其特征在于，所述步骤S5进一步包括以下步骤：

S51：根据所述当前位置和所述障碍物的信息生成虚拟障碍物并确定所述虚拟障碍物的信息，所述虚拟障碍物的信息包括所述虚拟障碍物的位置和所述虚拟障碍物的最大作用范围；

S52：根据所述当前位置和所述目标点位置确定所述引力，根据所述当前位置、所述障碍物的信息和所述目标点位置确定所述斥力，根据所述当前位置、所述虚拟障碍物的信息确定所述小型无人船受到所述虚拟障碍物的虚拟斥力；

S53：根据所述当前位置，所述引力、所述斥力以及所述虚拟斥力确定所述小型无人船相对于当前位置的下一位置，将所述下一位置重新设置为当前位置；

S54：判断所述当前位置是否脱离所述虚拟障碍物的最大作用范围，如判断结果为是，则删除所述虚拟障碍物后跳转到步骤S2，如否，则跳转到步骤S52。

7.如权利要求6所述的小型无人船路径规划方法，其特征在于，所述步骤S51中根据所述当前位置和所述障碍物的信息生成虚拟障碍物，进一步包括以下步骤：

以所述当前位置为原点建立二维坐标系，其中，所述二维坐标系的y轴正方向为所述当前位置朝向所述目标点位置的方向，x轴正方向为所述y轴正方向顺时针旋转90°的方向；

在所述二维坐标系的第三象限或第四象限随机生成虚拟障碍物，其中，所述虚拟障碍物与所述原点的距离小于所述障碍物的最大作用范围。

8.如权利要求7所述的小型无人船路径规划方法，其特征在于：

所述虚拟斥力根据所述当前位置、所述虚拟障碍物的位置以及所述虚拟障碍物的最大作用范围确定。

9.如权利要求8所述的小型无人船路径规划方法，其特征在于：

所述虚拟斥力在所述当前位置与所述虚拟障碍物的位置的距离小于所述虚拟障碍物的最大作用范围时为常数。

Images