本发明涉及路径规划技术领域，尤其是一种无人船路径规划方法、装置、设备和存储介质。
背景技术：
无人船是一种具有自主规划、自主航行能力，并可自主完成环境感知、目标探测等任务的小型水面平台。相对于无人机，无人船研究起步较晚，但是其发展迅速。无人船被广泛应用于水文监测、地貌测绘、军事侦查打击等领域。随着无人船在水域测量领域的广泛应用，如何规划无人船的测量路径，则成为关注的焦点。通常，船只在航行过程中，会遇到各种各样的障碍物。考虑到无人船上并没有工作人员来查看周围环境状况以及时避开障碍物，导致河面上的障碍物极容易对无人船带来危险。因此无人船的路径规划就显得尤为重要。无人船的路径规划是指无人船在静动态障碍物并存的水域环境中，寻找一条从给定起点到终点，且满足测量需求的运动路径，使其在测量航行过程中能安全可靠地避开所有障碍物，并到达指定目标点。
人工势场法因其算法原理简单，算法结构简洁明了，所得路径较为平滑的优势，在避障问题中拥有着较为广泛的应用。但传统的人工势场法无法对动态障碍物进行避碰，并且容易陷入局部极值问题，并且有可能由于目标点与障碍物距离太近而出现目标不可达的问题。而现有的无人船的测量路径规划中，大多是已经规划好全局路径，无人船按照指定规划好的路径行走的，从而忽略了动态障碍物以及非全局路径最优的情况，因此寻求更优的改进的人工势场法解决上述问题显得尤为重要。
技术实现要素：
为解决上述问题，本发明的目的在于提供一种无人船路径规划方法、装置、设备和存储介质，以人工势场法为基础，结合了相对速度和回归搜索算法实现了无人船的动静态避障和全局最优路径的规划，并引入改进的引力势场函数和逃逸力来改进无人船目标不可达和局部极值问题。
本发明解决其问题所采用的技术方案是：
第一方面，本发明实施例提出了一种无人船路径规划方法，包括：
获取无人船路径规划所需的信息；
建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径；
利用回归搜索算法获取全局最优路径；
判断无人船是否陷入局部极值，以构造逃逸力使得逃出局部极值来实现避障，直至无人船到达目标点位置。
进一步，获取无人船路径规划所需的信息包括：通过侧扫声纳和船载设备分别实时采集无人船路径规划所需的目标点位置信息、河面环境信息、障碍物位置信息以及无人船位置信息。
进一步，所述建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径，其中，改进的引力场函数如下：
其中，ζ是正参数,qgoal＝(xgoal,ygoal)t是目标点位置，q＝(x,y)t是无人船位置，d(q,qgoal)＝||qgoal-q||是无人船位置和目标点位置的相对距离，d是正参数且d<d(qobstacle,qgoal)，fatt(q)是目标对无人船的吸引力，
进一步，所述建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径，其中，传统的斥力场函数如下：
其中，η为正参数，d0为障碍物的影响距离，di(q)为每个障碍物中机器人到最近点的距离；
斥力是斥力势场函数的负梯度，如下:
其中，
结合相对速度的斥力场函数如下：
其中，n是障碍物的个数，θi是相对速度和相对距离的夹角；
由相对速度确定的对应斥力为：
进一步，所述建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径，其中，将引力场函数和斥力场函数叠加得到合力势场函数如下：
得到的合力为：
进一步，所述利用回归搜索算法获取全局最优路径包括如下步骤：
s1、计算当前状态下的人工势场合力f(q)；
s2、无人船向人工势场合力f(q)指示的方向前进一步；
s3、将当前坐标保存到ti；
s4、连续点ti∈{t1，t2，···，tn}是改进的人工势场方法的计划路径；
s5、重复步骤s3直到到达目标位置；
s6、用回归搜索ti；
s7、从起点t1开始，与后一个点tj,j∈{2,3,4,…n}相连接成为线l1,j；
s8、如果l1,j没有穿过任何障碍物并且b大于b0，保存l1,j并j＝j+1，直到j＝n+1,返回到步骤s7；
s9、否则，保存l1,j-1，并跳到步骤s10；
s10、如果tj-1不是最后一个点，将点tj-1作为新的起始点并返回到步骤s7；
s11、否则跳到步骤s12；
s12、获得全局最优路径；
s13、无人船沿着全局最优路径移动。
第二方面，本发明实施例还提出了一种无人船路径规划装置，包括：
获取信息模块，用于获取无人船路径规划所需的信息；
构建势力场模块，用于建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径；
路径寻优模块，用于利用回归搜索算法获取全局最优路径；
判断局部极值模块，用于判断无人船是否陷入局部极值，以构造逃逸力使得逃出局部极值来实现避障，直至无人船到达目标点位置。
第三方面，本发明实施例还提出了一种无人船路径规划设备，包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行本发明第一方面所述的方法。
第四方面，本发明实施例还提出了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行本发明第一方面所述的方法。
本发明实施例中提供的一个或多个技术方案，至少具有如下有益效果：本发明提供的一种无人船路径规划方法、装置、设备和存储介质，引入了相对速度人工势场法，可对动静态障碍物进行避碰，并且本发明通过引入新的引力函数和加入逃逸力帮助无人船在河面上改进目标不可达和陷入局部极值而无法动弹问题。本发明还结合回归搜索算法，规划出全局最优路径，大大减少了计划路径从无人船位置到目标位置距离，提高了无人船任务执行效率。
附图说明
下面结合附图和实例对本发明作进一步说明。
图1是本发明第一实施例中无人船路径规划方法的流程简图；
图2是本发明第一实施例中引力势场模型图；
图3是本发明第一实施例中引入相对速度的斥力势场模型图；
图4是本发明第一实施例中回归搜索算法模型图；
图5是本发明第一实施例中回归搜索算法流程图；
图6是本发明第一实施例中无人船路径规划方法的具体流程图；
图7是本发明第二实施例中无人船路径规划装置的结构简图；
图8是本发明第三实施例中无人船路径规划设备的结构简图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
需要说明的是，如果不冲突，本发明实施例中的各个特征可以相互结合，均在本发明的保护范围之内。另外，虽然在装置示意图中进行了功能模块划分，在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于装置中的模块划分，或流程图中的顺序执行所示出或描述的步骤。
下面结合附图，对本发明实施例作进一步阐述。
如图1所示，本发明的第一实施例提供了一种无人船路径规划方法，包括但不限于以下步骤：
s100:获取无人船路径规划所需的信息；
s200:建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径；
s300:利用回归搜索算法获取全局最优路径；
s400:判断无人船是否陷入局部极值，以构造逃逸力使得逃出局部极值来实现避障，直至无人船到达目标点位置。
优选地，在步骤s100中，采用侧扫声纳和船载设备分别实时采集无人船避障路径规划所需的目标点位置信息、河面环境信息、障碍物位置信息以及无人船位置信息；
在步骤s200中，势力场包含两部分，一部分是目标点对无人船产生的引力场uatt(q)，另一部分是障碍物对无人船产生的斥力场urep(q)，无人船在引力场与斥力场共同作用下产生的合力场中共同作用。其中，传统的引力场函数如下：
其中，ζ是正参数,qgoal＝(xgoal,ygoal)t是目标点位置，q＝(x,y)t是无人船位置，d(q,qgoal)＝||qgoal-q||是无人船位置和目标点位置的相对距离。
引力函数如下：
其中
传统的斥力场函数如下：
斥力是用来使无人船远离障碍物的。然而，当无人船远离障碍物时，不能影响无人船的运动，无人船的移动必须不受影响。其中urep,i(q)为每个障碍物的斥力势场，η为正参数，d0为障碍物的影响距离，di(q)为每个障碍物中无人船到最近点的距离。斥力是斥力势场函数的负梯度，如下:
其中，
如图2所示，本发明对引力势场函数进行了改进，改进的引力场函数为：
其中，ζ是正参数,qgoal＝(xgoal,ygoal)t是目标点位置，q＝(x,y)t是无人船位置，d(q,qgoal)＝||qgoal-q||是无人船位置和目标点位置的相对距离，d是一个正参数。要选择合适的d，必须满足以下条件:d<d(qobstacle,qgoal)。因此,引力为:
该功能的主要优点简述如下:
如果无人船达到目标，则d(q,qgoal)＝0，因此：
uatt(q)＝0
如果无人船离目标很远
在无人船距离目标点较远的地方，新的吸引力比传统的吸引力更强。而在距离目标点较近时，新的吸引力比传统的吸引力强，减少了目标点附近的障碍物的影响，解决了人工势场上目标不可达问题。
如图3所示，引入相对速度，加入由相对速度确定的斥力函数为，由相对速度确定的斥力势场函数如下：
其中，n是障碍物的个数，θi是相对速度和相对距离的夹角。如果逆时针旋转被定义为正方向,顺时针是负的,那么θi的范围是(-π，π)，当θi属于(-π/2，π/2),它表明无人船和障碍物正在接近。
由相对速度确定的对应斥力为：
将引力势场函数和斥力势场函数叠加得到合力势场函数:
因此，得到的合力为:
在步骤s300中，采用改良的基于apf的回归搜索算法，优化路径是通过连接apf产生的连续点计算。基于回归搜索方法，首先，作为起点之间的初始点t1与下一个点t2连接为直线l1，2。然后判断l1，2是否穿过任何障碍物，并且l1，2与障碍物之间的最短距离b是否大于b0。b为无人船到障碍物的最短距离，而b0是安全距离，也就是障碍物的影响范围，只要无人船到障碍物的最短距离不在b0的范围内也就是b大于b0时，无人船不受障碍物的影响。如图4所示，如果l1，2没有穿越任何障碍物并且b大于b0，将t1与t3重新连接为l1，3，并重复进行上述步骤。直到l1，i即ti是终点。由于ti不是最后一点，所以下一个起点是ti并且类似地与下一个点ti+1连接。最后的最佳路径是l1，i和li，n。
如图5所示，利用回归搜索算法获取全局最优路径包括如下步骤：
s1、计算当前状态下的人工势场合力f(q)；
s2、无人船向人工势场合力f(q)指示的方向前进一步；
s3、将当前坐标保存到ti；
s4、连续点ti∈{t1，t2，···，tn}是改进的人工势场方法的计划路径；
s5、重复步骤s3直到到达目标位置；
s6、用回归搜索ti；
s7、从起点t1开始，与后一个点tj,j∈{2,3,4,…n}相连接成为线l1,j；
s8、如果l1,j没有穿过任何障碍物并且b大于b0，保存l1,j并j＝j+1，直到j＝n+1,返回到步骤s7；
s9、否则，保存l1,j-1，并跳到步骤s10；
s10、如果tj-1不是最后一个点，将点tj-1作为新的起始点并返回到步骤s7；
s11、否则跳到步骤s12；
s12、获得全局最优路径；
s13、无人船沿着全局最优路径移动。
在步骤s400中，判断无人船是否陷入局部极值，以构造逃逸力使得逃出局部极值来实现避障，其中，无人船是否陷入局部极值的判断条件为：
其中，其中b和c是任意常数。如果条件满足，则可以确定无人船在局部最小位置被阻塞。因此，我们激活了逃跑的力量。如果检测到局部最小值，则无人船必须重新初始化势场。在这一步中，在该区域设置一个附加的势场，使无人船远离局部最小值。它被用来平稳地转动无人船以达到目标。新的斥力为经典斥力与提出的逃逸力方程之和:
附加势场表示为:
其中，
如图6所示，在步骤s400中，判断无人船是否陷入局部极值，若是，则构造逃逸力使得逃出局部极值，并且返回步骤s100，若没有陷入局部极值则判断无人船是否到达目标点位置，若是，则路径规划任务结束；否则，则继续执行步骤s300。
综上所述，与现有技术相比，本动态全局最优无人船路径规划方法的优点在于：
1、采用的改良人工势场法在传统的基于位置场的人工势场法的基础上引入了移动物体的相对速度带来的影响，加入了由相对速度确定的斥力函数，实现了人工势场可以避开动静态障碍物的功能。
2、采用的改良人工势场法重构了引力函数。重构后的优化引力函数平衡了吸引力和斥力的变化，解决了目标不可达问题。
3、引入了逃逸力的概念。当算法判断无人船陷入局部最小值时，激活逃逸力，使无人船逃离局部最小值。
4、改进算法在基于改良的人工势场法的基础上结合了回归搜索算法，使得到的路径为全局最优路径。
另外，如图7所示，本发明的第二实施例提供了一种无人船路径规划装置，包括：
获取信息模块110，用于获取无人船路径规划所需的信息；
构建势力场模块120，用于建立覆盖整个河面的坐标系,构建改进后引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小与方向，得到全局路径；
路径寻优模块130，用于利用回归搜索算法获取全局最优路径；
判断局部极值模块140，用于判断无人船是否陷入局部极值，以构造逃逸力使得逃出局部极值来实现避障，直至无人船到达目标点位置。
本实施例中的无人船路径规划装置与第一实施例中的无人船路径规划方法基于相同的发明构思，因此，本实施例中的无人船路径规划系统具有相同的有益效果：引入了相对速度人工势场法，可对动静态障碍物进行避碰，并且本发明通过引入新的引力函数和加入逃逸力帮助无人船在河面上改进目标不可达和陷入局部极值而无法动弹问题。本发明还结合回归搜索算法，规划出全局最优路径，大大减少了计划路径从无人船位置到目标位置距离，提高了无人船任务执行效率。
如图8所示，本发明的第三实施例还提供了一种无人船路径规划设备，包括：
至少一个处理器；
以及与所述至少一个处理器通信连接的存储器；
其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如上述第一实施例中任意一种无人船路径规划方法。
存储器作为一种非暂态计算机可读存储介质，可用于存储非暂态软件程序、非暂态性计算机可执行程序以及模块，如本发明实施例中的虚拟影像控制方法对应的程序指令/模块。处理器通过运行存储在存储器中的非暂态软件程序、指令以及模块，从而执行立体成像处理装置的各种功能应用以及数据处理，即实现上述任一方法实施例的无人船路径规划方法。
存储器可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据立体成像处理装置的使用所创建的数据等。此外，存储器可以包括高速随机存取存储器，还可以包括非暂态存储器，例如至少一个磁盘存储器件、闪存器件、或其他非暂态固态存储器件。在一些实施例中，存储器可选包括相对于处理器远程设置的存储器，这些远程存储器可以通过网络连接至该立体投影装置。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
所述一个或者多个模块存储在所述存储器中，当被所述一个或者多个处理器执行时，执行上述任意方法实施例中的无人船路径规划方法，例如第一实施例中的方法步骤s100至s400。
本发明的第四实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，该计算机可执行指令被一个或多个控制处理器执行，可使得上述一个或多个处理器执行上述方法实施例中的一种无人船路径规划方法，例如第一实施例中的方法步骤s100至s400。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。
通过以上的实施方式的描述，本领域普通技术人员可以清楚地了解到各实施方式可借助软件加通用硬件平台的方式来实现，当然也可以通过硬件。本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(read-onlymemory,rom)或随机存储记忆体(randomaccessmemory,ram)等。
以上是对本发明的较佳实施进行了具体说明，但本发明并不局限于上述实施方式，熟悉本领域的技术人员在不违背本发明精神的前提下还可作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。
技术特征：
1.一种无人船路径规划方法，其特征在于，包括：
获取无人船路径规划所需的信息；
建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径；
利用回归搜索算法获取全局最优路径；
判断无人船是否陷入局部极值，以构造逃逸力使得逃出局部极值来实现避障，直至无人船到达目标点位置。
2.根据权利要求1所述的一种无人船路径规划方法，其特征在于，所述获取无人船路径规划所需的信息包括：通过侧扫声纳和船载设备分别实时采集无人船路径规划所需的目标点位置信息、河面环境信息、障碍物位置信息以及无人船位置信息。
3.根据权利要求1所述的一种无人船路径规划方法，其特征在于，所述建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径，其中，改进的引力场函数如下：
其中，ζ是正参数,qgoal＝(xgoal,ygoal)t是目标点位置，q＝(x,y)t是无人船位置，d(q,qgoal)＝||qgoal-q||是无人船位置和目标点位置的相对距离，d是正参数且d<d(qobstacle,qgoal)，fatt(q)是目标对无人船的吸引力，
4.根据权利要求1所述的一种无人船路径规划方法，其特征在于，所述建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径，其中，传统的斥力场函数如下：
其中，η为正参数，d0为障碍物的影响距离，di(q)为每个障碍物中机器人到最近点的距离；
斥力是斥力势场函数的负梯度，如下:
其中，
结合相对速度的斥力场函数如下：
其中，n是障碍物的个数，θi是相对速度和相对距离的夹角；
由相对速度确定的对应斥力为：
5.根据权利要求1所述的一种无人船路径规划方法，其特征在于，所述建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径，其中，将引力场函数和斥力场函数叠加得到合力势场函数如下：
得到的合力为：
6.根据权利要求1所述的一种无人船路径规划方法，其特征在于，
所述利用回归搜索算法获取全局最优路径包括如下步骤：
s1、计算当前状态下的人工势场合力f(q)；
s2、无人船向人工势场合力f(q)指示的方向前进一步；
s3、将当前坐标保存到ti；
s4、连续点ti∈{t1，t2，···，tn}是人工势场计划路径；
s5、重复步骤s3直到到达目标位置；
s6、用回归搜索ti；
s7、从起点t1开始，与后一个点tj,j∈{2,3,4,…n}相连接成为线l1,j；
s8、如果l1,j没有穿过任何障碍物并且b大于b0，保存l1,j并j＝j+1，直到j＝n+1,返回到步骤s7；
s9、否则，保存l1,j-1，并跳到步骤s10；
s10、如果tj-1不是最后一个点，将点tj-1作为新的起始点并返回到步骤s7；
s11、否则跳到步骤s12；
s12、获得全局最优路径；
s13、无人船沿着全局最优路径移动。
7.根据权利要求1所述的一种无人船路径规划方法，其特征在于，所述判断无人船是否陷入局部极值，以构造逃逸力使得逃出局部极值来实现避障，其中，无人船是否陷入局部极值的判断条件为：
其中，其中b和c是任意常数。
8.一种无人船路径规划装置，其特征在于，包括：
获取信息模块，用于获取无人船路径规划所需的信息；
构建势力场模块，用于建立覆盖整个河面的坐标系,构建改进的引力势场函数、传统的斥力势场函数以及结合相对速度的斥力势场函数，计算其合力大小和方向，得到全局路径；
路径寻优模块，用于利用回归搜索算法获取全局最优路径；
判断局部极值模块，用于判断无人船是否陷入局部极值，以构造逃逸力使得逃出局部极值来实现避障，直至无人船到达目标点位置。
9.一种无人船路径规划设备，其特征在于，包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如权利要求1-6任一项所述的方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行如权利要求1-6任一项所述的方法。
技术总结
本发明公开了一种无人船路径规划方法、装置、设备和存储介质，无人船在河面上执行任务，遇到漂流障碍物和静态固态障碍物可能发生碰撞，将会对无人船造成极大威胁。为解决无人船在河面上安全避障并且安全到达目标，本发明引入了相对速度人工势场法，可对动静态障碍物进行避碰，并且本发明通过引入新的引力函数和加入逃逸力帮助无人船在河面上改进目标不可达和陷入局部极值而无法动弹问题。最后，本发明结合回归搜索算法，规划出全局最优路径，大大减少了计划路径从无人船位置到目标位置距离，提高了无人船任务执行效率。
技术研发人员：翟懿奎;张俊亮;黄灏文;余翠琳;陈家聪;柯琪锐;梁艳阳;王宏民
受保护的技术使用者：五邑大学
技术研发日：.10.31
技术公布日：.02.28
