CN110850873A - 无人船路径规划方法、装置、设备和存储介质 - Google Patents

Abstract

本发明公开了一种无人船路径规划方法、装置、设备和存储介质，无人船在河面上执行任务，遇到漂流障碍物和静态固态障碍物可能发生碰撞，将会对无人船造成极大威胁。为解决无人船在河面上安全避障并且安全到达目标，本发明引入了相对速度人工势场法，可对动静态障碍物进行避碰，并且本发明通过引入新的引力函数和加入逃逸力帮助无人船在河面上改进目标不可达和陷入局部极值而无法动弹问题。最后，本发明结合回归搜索算法，规划出全局最优路径，大大减少了计划路径从无人船位置到目标位置距离，提高了无人船任务执行效率。

Description

无人船路径规划方法、装置、设备和存储介质

技术领域

本发明涉及路径规划技术领域，尤其是一种无人船路径规划方法、 装置、设备和存储介质。

背景技术

无人船是一种具有自主规划、自主航行能力，并可自主完成环境 感知、目标探测等任务的小型水面平台。相对于无人机，无人船研究 起步较晚，但是其发展迅速。无人船被广泛应用于水文监测、地貌测 绘、军事侦查打击等领域。随着无人船在水域测量领域的广泛应用， 如何规划无人船的测量路径，则成为关注的焦点。通常，船只在航行 过程中，会遇到各种各样的障碍物。考虑到无人船上并没有工作人员 来查看周围环境状况以及时避开障碍物，导致河面上的障碍物极容易 对无人船带来危险。因此无人船的路径规划就显得尤为重要。无人船 的路径规划是指无人船在静动态障碍物并存的水域环境中，寻找一条 从给定起点到终点，且满足测量需求的运动路径，使其在测量航行过 程中能安全可靠地避开所有障碍物，并到达指定目标点。

人工势场法因其算法原理简单，算法结构简洁明了，所得路径较 为平滑的优势，在避障问题中拥有着较为广泛的应用。但传统的人工 势场法无法对动态障碍物进行避碰，并且容易陷入局部极值问题，并 且有可能由于目标点与障碍物距离太近而出现目标不可达的问题。而 现有的无人船的测量路径规划中，大多是已经规划好全局路径，无人 船按照指定规划好的路径行走的，从而忽略了动态障碍物以及非全局 路径最优的情况，因此寻求更优的改进的人工势场法解决上述问题显 得尤为重要。

发明内容

为解决上述问题，本发明的目的在于提供一种无人船路径规划方 法、装置、设备和存储介质，以人工势场法为基础，结合了相对速度 和回归搜索算法实现了无人船的动静态避障和全局最优路径的规划， 并引入改进的引力势场函数和逃逸力来改进无人船目标不可达和局 部极值问题。

本发明解决其问题所采用的技术方案是：

第一方面，本发明实施例提出了一种无人船路径规划方法，包括：

获取无人船路径规划所需的信息；

建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的 斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和 方向，得到全局路径；

利用回归搜索算法获取全局最优路径；

判断无人船是否陷入局部极值，以构造逃逸力使得逃出局部极值 来实现避障，直至无人船到达目标点位置。

进一步，获取无人船路径规划所需的信息包括：通过侧扫声纳和 船载设备分别实时采集无人船路径规划所需的目标点位置信息、河面 环境信息、障碍物位置信息以及无人船位置信息。

进一步，所述建立覆盖整个河面的坐标系,构建改进的引力势场 函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算 其合力大小和方向，得到全局路径，其中，改进的引力场函数如下：

其中，ζ是正参数,q_goal＝(x_goal,y_goal)^t是目标点位置，q＝(x,y)^t是 无人船位置，d(q,q_goal)＝||q_goal-q||是无人船位置和目标点位置的相 对距离，D是正参数且D<d(q_obstacle,q_goal)，F_att(q)是目标对无人船的吸 引力，

是吸引势函数的负梯度，当无人船到达目 标时，其收敛到零。

进一步，所述建立覆盖整个河面的坐标系,构建改进的引力势场 函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算 其合力大小和方向，得到全局路径，其中，传统的斥力场函数如下：

其中，η为正参数，d₀为障碍物的影响距离，d_i(q)为每个障碍物 中机器人到最近点的距离；

斥力是斥力势场函数的负梯度，如下:

其中，

并且q_c＝(x_cl,y_c)^t是障碍物最靠近的点；

结合相对速度的斥力场函数如下：

其中，n是障碍物的个数，θ_i是相对速度和相对距离的夹角；

由相对速度确定的对应斥力为：

进一步，所述建立覆盖整个河面的坐标系,构建改进的引力势场 函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算 其合力大小和方向，得到全局路径，其中，将引力场函数和斥力场函 数叠加得到合力势场函数如下：

得到的合力为：

进一步，所述利用回归搜索算法获取全局最优路径包括如下步骤：

S1、计算当前状态下的人工势场合力F(q)；

S2、无人船向人工势场合力F(q)指示的方向前进一步；

S3、将当前坐标保存到T_i；

S4、连续点T_i∈{T₁，T₂，···，T_n}是改进的人工势场方法的计划 路径；

S5、重复步骤S3直到到达目标位置；

S6、用回归搜索T_i；

S7、从起点T₁开始，与后一个点T_j,j∈{2,3,4,…n}相连接成为线 L_1,j；

S8、如果L_1,j没有穿过任何障碍物并且B大于B₀，保存L_1,j并j＝j+1， 直到j＝n+1,返回到步骤S7；

S9、否则，保存L_1,j-1，并跳到步骤S10；

S10、如果T_j-1不是最后一个点，将点T_j-1作为新的起始点并返回 到步骤S7；

S11、否则跳到步骤S12；

S12、获得全局最优路径；

S13、无人船沿着全局最优路径移动。

第二方面，本发明实施例还提出了一种无人船路径规划装置，包 括：

获取信息模块，用于获取无人船路径规划所需的信息；

构建势力场模块，用于建立覆盖整个河面的坐标系,构建改进的 引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函 数，计算其合力大小和方向，得到全局路径；

路径寻优模块，用于利用回归搜索算法获取全局最优路径；

判断局部极值模块，用于判断无人船是否陷入局部极值，以构造 逃逸力使得逃出局部极值来实现避障，直至无人船到达目标点位置。

第三方面，本发明实施例还提出了一种无人船路径规划设备，包 括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指 令被所述至少一个处理器执行，以使所述至少一个处理器能够执行本 发明第一方面所述的方法。

第四方面，本发明实施例还提出了一种计算机可读存储介质，所 述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行 指令用于使计算机执行本发明第一方面所述的方法。

本发明实施例中提供的一个或多个技术方案，至少具有如下有益 效果：本发明提供的一种无人船路径规划方法、装置、设备和存储介 质，引入了相对速度人工势场法，可对动静态障碍物进行避碰，并且 本发明通过引入新的引力函数和加入逃逸力帮助无人船在河面上改 进目标不可达和陷入局部极值而无法动弹问题。本发明还结合回归搜 索算法，规划出全局最优路径，大大减少了计划路径从无人船位置到 目标位置距离，提高了无人船任务执行效率。

附图说明

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

图1是本发明第一实施例中无人船路径规划方法的流程简图；

图2是本发明第一实施例中引力势场模型图；

图3是本发明第一实施例中引入相对速度的斥力势场模型图；

图4是本发明第一实施例中回归搜索算法模型图；

图5是本发明第一实施例中回归搜索算法流程图；

图6是本发明第一实施例中无人船路径规划方法的具体流程图；

图7是本发明第二实施例中无人船路径规划装置的结构简图；

图8是本发明第三实施例中无人船路径规划设备的结构简图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合 附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描 述的具体实施例仅用以解释本发明，并不用于限定本发明。

需要说明的是，如果不冲突，本发明实施例中的各个特征可以相 互结合，均在本发明的保护范围之内。另外，虽然在装置示意图中进 行了功能模块划分，在流程图中示出了逻辑顺序，但是在某些情况下， 可以以不同于装置中的模块划分，或流程图中的顺序执行所示出或描 述的步骤。

下面结合附图，对本发明实施例作进一步阐述。

如图1所示，本发明的第一实施例提供了一种无人船路径规划方 法，包括但不限于以下步骤：

S100:获取无人船路径规划所需的信息；

S200:建立覆盖整个河面的坐标系,构建改进的引力势场函数、 传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力 大小和方向，得到全局路径；

S300:利用回归搜索算法获取全局最优路径；

S400:判断无人船是否陷入局部极值，以构造逃逸力使得逃出局 部极值来实现避障，直至无人船到达目标点位置。

优选地，在步骤S100中，采用侧扫声纳和船载设备分别实时采 集无人船避障路径规划所需的目标点位置信息、河面环境信息、障碍 物位置信息以及无人船位置信息；

在步骤S200中，势力场包含两部分，一部分是目标点对无人船 产生的引力场U_att(q)，另一部分是障碍物对无人船产生的斥力场 U_rep(q)，无人船在引力场与斥力场共同作用下产生的合力场中共同作 用。其中，传统的引力场函数如下：

其中，ζ是正参数,q_goal＝(x_goal,y_goal)^t是目标点位置，q＝(x,y)^t是 无人船位置，d(q,q_goal)＝||q_goal-q||是无人船位置和目标点位置的相 对距离。

引力函数如下：

其中是引力势场函数的负梯度，当无人船到达目 标时，它收敛到零。

传统的斥力场函数如下：

斥力是用来使无人船远离障碍物的。然而，当无人船远离障碍物 时，不能影响无人船的运动，无人船的移动必须不受影响。其中 U_rep,i(q)为每个障碍物的斥力势场，η为正参数，d₀为障碍物的影响距 离，d_i(q)为每个障碍物中无人船到最近点的距离。斥力是斥力势场函 数的负梯度，如下:

其中，

并且q_c＝(x_cl,y_c)^t是障碍物最靠近的点。

如图2所示，本发明对引力势场函数进行了改进，改进的引力场 函数为：

其中，ζ是正参数,q_goal＝(x_goal,y_goal)^t是目标点位置，q＝(x,y)^t是 无人船位置，d(q,q_goal)＝||q_goal-q||是无人船位置和目标点位置的相 对距离，D是一个正参数。要选择合适的D，必须满足以下条 件:D<d(q_obstacle,q_goal)。因此,引力为:

该功能的主要优点简述如下:

如果无人船达到目标，则d(q,q_goal)＝0，因此：

U_att(q)＝0

如果无人船离目标很远则：

在无人船距离目标点较远的地方，新的吸引力比传统的吸引力更 强。而在距离目标点较近时，新的吸引力比传统的吸引力强，减少了 目标点附近的障碍物的影响，解决了人工势场上目标不可达问题。

如图3所示，引入相对速度，加入由相对速度确定的斥力函数为， 由相对速度确定的斥力势场函数如下：

其中，n是障碍物的个数，θ_i是相对速度和相对距离的夹角。如 果逆时针旋转被定义为正方向,顺时针是负的,那么θ_i的范围是(-π， π)，当θ_i属于(-π/2，π/2),它表明无人船和障碍物正在接近。

由相对速度确定的对应斥力为：

将引力势场函数和斥力势场函数叠加得到合力势场函数:

因此，得到的合力为:

上述方法实现了 人工势场法可以避开动态障碍物的功能。

在步骤S300中，采用改良的基于APF的回归搜索算法，优化路 径是通过连接APF产生的连续点计算。基于回归搜索方法，首先，作 为起点之间的初始点T₁与下一个点T₂连接为直线L_1，2。然后判断L_1，2是否穿过任何障碍物，并且L_1，2与障碍物之间的最短距离B是否大于B₀。B为无人船到障碍物的最短距离，而B₀是安全距离，也就是障碍 物的影响范围，只要无人船到障碍物的最短距离不在B₀的范围内也就 是B大于B₀时，无人船不受障碍物的影响。如图4所示，如果L_1，2没 有穿越任何障碍物并且B大于B₀，将T₁与T₃重新连接为L_1，3，并重复 进行上述步骤。直到L_1，i即T_i是终点。由于T_i不是最后一点，所以下一 个起点是T_i并且类似地与下一个点T_i+1连接。最后的最佳路径是L_1，i和 L_i，n。

如图5所示，利用回归搜索算法获取全局最优路径包括如下步骤：

S1、计算当前状态下的人工势场合力F(q)；

S2、无人船向人工势场合力F(q)指示的方向前进一步；

S3、将当前坐标保存到T_i；

S4、连续点T_i∈{T₁，T₂，···，T_n}是改进的人工势场方法的计划 路径；

S5、重复步骤S3直到到达目标位置；

S6、用回归搜索T_i；

S7、从起点T₁开始，与后一个点T_j,j∈{2,3,4,…n}相连接成为线L_1,j；

S8、如果L_1,j没有穿过任何障碍物并且B大于B₀，保存L_1,j并j＝j+1， 直到j＝n+1,返回到步骤S7；

S9、否则，保存L_1,j-1，并跳到步骤S10；

S10、如果T_j-1不是最后一个点，将点T_j-1作为新的起始点并返回 到步骤S7；

S11、否则跳到步骤S12；

S12、获得全局最优路径；

S13、无人船沿着全局最优路径移动。

在步骤S400中，判断无人船是否陷入局部极值，以构造逃逸力 使得逃出局部极值来实现避障，其中，无人船是否陷入局部极值的判 断条件为：

其中，其中b和c是任意常数。如果条件满足，则可以确定无人 船在局部最小位置被阻塞。因此，我们激活了逃跑的力量。如果检测 到局部最小值，则无人船必须重新初始化势场。在这一步中，在该区 域设置一个附加的势场，使无人船远离局部最小值。它被用来平稳地 转动无人船以达到目标。新的斥力为经典斥力与提出的逃逸力方程之 和:

附加势场表示为:

其中，

是逃逸力的分量，α是旋转角度它是随机取值在 [-π,0)∪(0,π]，

是一个单位矢量从障碍物指向无人船而是垂直 于

的单位向量。

如图6所示，在步骤S400中，判断无人船是否陷入局部极值， 若是，则构造逃逸力使得逃出局部极值，并且返回步骤S100，若没 有陷入局部极值则判断无人船是否到达目标点位置，若是，则路径规 划任务结束；否则，则继续执行步骤S300。

综上所述，与现有技术相比，本动态全局最优无人船路径规划方 法的优点在于：

1、采用的改良人工势场法在传统的基于位置场的人工势场法的 基础上引入了移动物体的相对速度带来的影响，加入了由相对速度确 定的斥力函数，实现了人工势场可以避开动静态障碍物的功能。

2、采用的改良人工势场法重构了引力函数。重构后的优化引力 函数平衡了吸引力和斥力的变化，解决了目标不可达问题。

3、引入了逃逸力的概念。当算法判断无人船陷入局部最小值时， 激活逃逸力，使无人船逃离局部最小值。

4、改进算法在基于改良的人工势场法的基础上结合了回归搜索 算法，使得到的路径为全局最优路径。

另外，如图7所示，本发明的第二实施例提供了一种无人船路径 规划装置，包括：

获取信息模块110，用于获取无人船路径规划所需的信息；

构建势力场模块120，用于建立覆盖整个河面的坐标系,构建改 进后引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势 场函数，计算其合力大小与方向，得到全局路径；

路径寻优模块130，用于利用回归搜索算法获取全局最优路径；

判断局部极值模块140，用于判断无人船是否陷入局部极值，以 构造逃逸力使得逃出局部极值来实现避障，直至无人船到达目标点位 置。

本实施例中的无人船路径规划装置与第一实施例中的无人船路 径规划方法基于相同的发明构思，因此，本实施例中的无人船路径规 划系统具有相同的有益效果：引入了相对速度人工势场法，可对动静 态障碍物进行避碰，并且本发明通过引入新的引力函数和加入逃逸力 帮助无人船在河面上改进目标不可达和陷入局部极值而无法动弹问 题。本发明还结合回归搜索算法，规划出全局最优路径，大大减少了 计划路径从无人船位置到目标位置距离，提高了无人船任务执行效率。

如图8所示，本发明的第三实施例还提供了一种无人船路径规划 设备，包括：

至少一个处理器；

以及与所述至少一个处理器通信连接的存储器；

其中，所述存储器存储有可被所述至少一个处理器执行的指令， 所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够 执行如上述第一实施例中任意一种无人船路径规划方法。

存储器作为一种非暂态计算机可读存储介质，可用于存储非暂态 软件程序、非暂态性计算机可执行程序以及模块，如本发明实施例中 的虚拟影像控制方法对应的程序指令/模块。处理器通过运行存储在 存储器中的非暂态软件程序、指令以及模块，从而执行立体成像处理 装置的各种功能应用以及数据处理，即实现上述任一方法实施例的无 人船路径规划方法。

存储器可以包括存储程序区和存储数据区，其中，存储程序区 可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存 储根据立体成像处理装置的使用所创建的数据等。此外，存储器可以 包括高速随机存取存储器，还可以包括非暂态存储器，例如至少一个 磁盘存储器件、闪存器件、或其他非暂态固态存储器件。在一些实施 例中，存储器可选包括相对于处理器远程设置的存储器，这些远程存 储器可以通过网络连接至该立体投影装置。上述网络的实例包括但不 限于互联网、企业内部网、局域网、移动通信网及其组合。

所述一个或者多个模块存储在所述存储器中，当被所述一个或 者多个处理器执行时，执行上述任意方法实施例中的无人船路径规划 方法，例如第一实施例中的方法步骤S100至S400。

本发明的第四实施例还提供了一种计算机可读存储介质，所述计 算机可读存储介质存储有计算机可执行指令，该计算机可执行指令被 一个或多个控制处理器执行，可使得上述一个或多个处理器执行上述 方法实施例中的一种无人船路径规划方法，例如第一实施例中的方法 步骤S100至S400。

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

通过以上的实施方式的描述，本领域普通技术人员可以清楚地了 解到各实施方式可借助软件加通用硬件平台的方式来实现，当然也可 以通过硬件。本领域普通技术人员可以理解实现上述实施例方法中的 全部或部分流程是可以通过计算机程序来指令相关的硬件来完成，所 述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可 包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、 光盘、只读存储记忆体(Read-Only Memory,ROM)或随机存储记忆体 (Random Access Memory,RAM)等。

以上是对本发明的较佳实施进行了具体说明，但本发明并不局限 于上述实施方式，熟悉本领域的技术人员在不违背本发明精神的前提 下还可作出种种的等同变形或替换，这些等同的变形或替换均包含在 本申请权利要求所限定的范围内。

Claims (10)

1.一种无人船路径规划方法，其特征在于，包括：

获取无人船路径规划所需的信息；

建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径；

利用回归搜索算法获取全局最优路径；

判断无人船是否陷入局部极值，以构造逃逸力使得逃出局部极值来实现避障，直至无人船到达目标点位置。

2.根据权利要求1所述的一种无人船路径规划方法，其特征在于，所述获取无人船路径规划所需的信息包括：通过侧扫声纳和船载设备分别实时采集无人船路径规划所需的目标点位置信息、河面环境信息、障碍物位置信息以及无人船位置信息。

3.根据权利要求1所述的一种无人船路径规划方法，其特征在于，所述建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径，其中，改进的引力场函数如下：

其中，ζ是正参数,q_goal＝(x_goal,y_goal)^t是目标点位置，q＝(x,y)^t是无人船位置，d(q,q_goal)＝||q_goal-q||是无人船位置和目标点位置的相对距离，D是正参数且D<d(q_obstacle,q_goal)，F_att(q)是目标对无人船的吸引力，

是吸引势函数的负梯度，当无人船到达目标时，其收敛到零。

4.根据权利要求1所述的一种无人船路径规划方法，其特征在于，所述建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径，其中，传统的斥力场函数如下：

其中，η为正参数，d₀为障碍物的影响距离，d_i(q)为每个障碍物中机器人到最近点的距离；

斥力是斥力势场函数的负梯度，如下:

其中，

并且q_c＝(x_cl,y_c)^t是障碍物最靠近的点；

结合相对速度的斥力场函数如下：

其中，n是障碍物的个数，θ_i是相对速度和相对距离的夹角；

由相对速度确定的对应斥力为：

5.根据权利要求1所述的一种无人船路径规划方法，其特征在于，所述建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径，其中，将引力场函数和斥力场函数叠加得到合力势场函数如下：

得到的合力为：

6.根据权利要求1所述的一种无人船路径规划方法，其特征在于，

所述利用回归搜索算法获取全局最优路径包括如下步骤：

S1、计算当前状态下的人工势场合力F(q)；

S2、无人船向人工势场合力F(q)指示的方向前进一步；

S3、将当前坐标保存到T_i；

S4、连续点T_i∈{T₁，T₂，···，T_n}是人工势场计划路径；

S5、重复步骤S3直到到达目标位置；

S6、用回归搜索T_i；

S7、从起点T₁开始，与后一个点T_j,j∈{2,3,4,…n}相连接成为线L_1,j；

S8、如果L_1,j没有穿过任何障碍物并且B大于B₀，保存L_1,j并j＝j+1，直到j＝n+1,返回到步骤S7；

S9、否则，保存L_1,j-1，并跳到步骤S10；

S10、如果T_j-1不是最后一个点，将点T_j-1作为新的起始点并返回到步骤S7；

S11、否则跳到步骤S12；

S12、获得全局最优路径；

S13、无人船沿着全局最优路径移动。

7.根据权利要求1所述的一种无人船路径规划方法，其特征在于，所述判断无人船是否陷入局部极值，以构造逃逸力使得逃出局部极值来实现避障，其中，无人船是否陷入局部极值的判断条件为：

其中，其中b和c是任意常数。

8.一种无人船路径规划装置，其特征在于，包括：

获取信息模块，用于获取无人船路径规划所需的信息；

构建势力场模块，用于建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径；

路径寻优模块，用于利用回归搜索算法获取全局最优路径；

判断局部极值模块，用于判断无人船是否陷入局部极值，以构造逃逸力使得逃出局部极值来实现避障，直至无人船到达目标点位置。

9.一种无人船路径规划设备，其特征在于，包括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如权利要求1-6任一项所述的方法。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行如权利要求1-6任一项所述的方法。

Images