一种水下机器人三维路径规划和控制方法

1.本发明属于水下机器人领域，涉及路径规划和控制技术，具体涉及一种转轴法和分布估计算法相融合的水下机器人三维路径规划和控制方法。


背景技术：

2.路径规划技术是自主水下机器人的关键技术之一，是指为了到达某个目标或完成某个任务，对所规划设备的航行方向、航行路线等进行预先计算、设定、优化的过程。路径规划技术在一定程度上标志着水下机器人智能化程度的高低。目前计算机控制技术、人工智能技术和先进控制理论的长足发展，国内外的学者将多种智能算法应用在auv(水下机器人)的路径规划上，例如人工势场法、快速步进法、a*算法、粒子群优化算法、蚁群优化算法、遗传算法、eda算法等各种路径规划方法。但传统的路径规划算法在实际应用中会有各种缺点，针对这些缺点，国内外研究人员对算法进行了各种优化。如sebag和ducoulombier在eda中使用高斯概率模型来处理连续搜索区间内的优化问题。由于单变量高斯概率模型的特性，使得采用该模型的eda能够很好地处理一些单峰优化问题。刘润东将一种直方图分布估计算法与搜索区间自适应收缩法结合并成功将其运用在auv路径规划上。夏桂梅等将一种mimic分布估计算法与转轴法相结合，弥补了mimic分布估计算法的局部求解能力不足的问题。刘建军等将基于fwh的分布估计算法与模式搜索法相结合，进一步提高了算法的最优解精度和收敛速度。
3.现有最佳方法为2020年华南理工大学硕士论文，基于分布估计算法的自助式水下航行器路径规划研究。主要针对静态海洋环境和动态海洋环境中的路径规划算法进行研究，提出了fwh算法处理auv路径规划问题，该方法在等宽度直方图分布估计算法的基础上，考虑到算法精度不高的问题，引入搜索区间自适应收缩的方法，提高了算法精度并加快收敛速度。但是，该方法存在的问题是，fwh算法的缺点是其计算量大，算法收敛速度慢，解决这一问题的一种方法是合理设置划分区间的数量，但不可避免算法的精度会受到影响。目前auv的导航装置大多采用gps惯性导航，其缺点是误差会累积越来越大，导致auv不能按照预设的规划出的路径准确到达目标区域或目标点。


技术实现要素：

4.发明目的：为了克服现有技术中存在的不足，提供一种转轴法和分布估计算法相融合的水下机器人三维路径规划和控制方法，其针对传统fwh算法的缺点，采用转轴法(rosenbrock法)进行改进，主要对fwh算法的更新候选解步骤进行优化，提高了路径规划的精度和速度。
5.技术方案：为实现上述目的，本发明提供一种水下机器人三维路径规划和控制方法，包括如下步骤：
6.s1：通过适应度函数评估路径质量，构造auv路径；
7.s2：通过rb-fwh算法对构造的auv路径进行规划，基于设计解的编码方式，获取到
规划好的auv路径；
8.s3：auv开始根据gps惯性导航和多普勒计程仪测量的auv的实时地理位置开始按照规划好的auv路径进行航行；
9.s4：当检测到障碍物时，将拍到的障碍物与水下环境高程模型中的障碍物特征进行比对，比对成功后，修正gps惯性导航和多普勒计程仪的数据，确定auv实际位置，如果偏离预设路径，则控制auv回到预设路线，继续按照预设的路径航行。
10.进一步地，所述步骤s1中适应度函数为：
11.f
fitness
＝l
length
s
smooth
o
security
12.其中，l
length
为路径长度，l
length
值越小表示路径越短，消耗能量越小；
13.s
smooth
表示路径平滑程度，s
smooth
值越小表示路径越平滑，更有利于auv实现预定路径航行；
14.o
security
表示路径安全性，其值为零则代表路径安全。
15.进一步地，所述步骤s1的适应度函数中l
length
、s
smooth
和o
security
的表达式分别如下：
[0016][0017]
其中，dis
pspt
代表起点与终点的欧氏距离，l
path
代表整条路径的实际长度；
[0018][0019]
其中，n
uneven
表示一条路径中弯曲程度超过auv最大转弯半径的路径段数量，如果一条路径弯曲程度超过auv最大转弯半径，则该条路径视为不可行路径；n
seg
表示一条路径中路径段的总数量；
[0020][0021]
其中，n
obt
表示落入不可航行区的路径点数量，n
profile
表示三维刨面的数量。
[0022]
进一步地，所述步骤s1中采用直线段的方法进行auv路径的构造，具体为：每一条路径都包含一系列的航路点，这些航路点两两之间构成一个路径段，再由这些路径段构成最终路径。
[0023]
在地图上寻找一定数量的航路点，然后再用直线段将相邻的航路点连接起来，构成最终路径。直线段路径构造比较便于实现，并且可以通过控制航路点的数量来确定路径中路径段的数量，方便寻找可行路径。若环境空间中障碍物分布比较均匀并且稀疏，则航路点的数量可以选取少一些，若环境空间中障碍物分布比较密集，则航路点数量可以选取的多一些，方便算法寻找可行路径。这些路径点分别位于环境的刨面中，由于实际情况下，auv存在转向角，机动能力有限，续航时间有限等因素，若路径不够平滑，则不适合auv航行。本发明中采用适应度函数评估路径质量，确定路径是否适合auv航行。
[0024]
进一步地，所述步骤s2中rb-fwh算法是通过rosenbrock算法对fwh算法的种群更新步骤进行优化获得，具体的优化过程为：
[0025]
a1：初始化种群；
[0026]
a2：构建概率模型；
[0027]
a3：采样概率模型，产生规模为m的参数候选解；
[0028]
a4：基于参数候选解，确定新的优秀候选解；
[0029]
a5：通过rosenbrock算法更新当前最优解；
[0030]
a6：更新概率向量，获得每个区间优秀解的概率，根据每个区间优秀解的概率的大小产生新的更具优势的候选解；
[0031]
a7：算法停止并给出结果。
[0032]
进一步地，所述步骤a1具体为：对参数候选解规模m，每个变量的区间等分数n，优秀候选解规模s(s《m)，学习概率α，迭代次数h等给定初始值；
[0033]
所述步骤a2具体为：对变量的连续空间[a,b]进行n等分，每个区间长度为开始时每个区间的取值概率相同都为
[0034]
所述步骤a3具体为：对每个个体的每一维变量进行取值，采用轮盘赌随机抽样法产生规模为m的参数候选解；
[0035]
所述步骤a4具体为：基于适应度函数，计算参数候选解的适应值，根据其适应值，对候选解排序，选择前s个适应度高的作为优秀候选解；
[0036]
所述步骤a5具体为：从当前s个群体中随机选择t个个体作为初始点进行rosenbrock搜索，将得到的新个体作为新一代群体的一部分，增加种群多样性；
[0037]
所述步骤a6具体为：对于变量某一维取值的连续空间，在它的n个划分小区间里统计每个区间优秀解的概率，概率为pi：
[0038][0039]
所述步骤a7具体为：当算法到达迭代次数或给定精度后，算法停止并给出结果。
[0040]
进一步地，所述步骤s2中auv路径的规划方法为：一个个体代表一个路径，个体中的每一维变量代表一个航路点。每个航路点由x/y/z三维坐标信息构成，其中y轴坐标已知，x/z轴坐标信息为算法待求解的值，基于解的编码方式，求解到x/z轴坐标信息；
[0041]
解的编码方式为：
[0042][0043]
进一步地，所述步骤s1中三维刨面图的建立方法为：
[0044]
环境信息包括可以航行位置和不可航行位置，用两种不同的颜色区分，x轴、y轴、z轴分别代表环境区域的长度，将构建好的三维环境模型沿y轴切成平行切面，形成三维环境刨面图，每个刨面图都由均匀方格组成，方格的个数应合理设置，若方格数少，则环境信息识别度低，算法精度下降，若方格数过多，环境信息识别度高，算法精度高，但算法速度减慢；将刨面图数字化，可航行区域用0代替，不可航行区域用1代替，然后用三维数组储存所有刨面图数据。
[0045]
进一步地，所述步骤s4中如果偏离预设路径，则控制主推、侧推、垂推螺旋桨使auv回到预设路线。
[0046]
有益效果：本发明与现有技术相比，具备如下优点：
[0047]
1、本发明提供的路径规划方法基于等宽度直方图分布估计算法，采用rosenbrock算法对fwh算法进行改进，对fwh算法的种群更新步骤进行优化，减少了等宽度直方图分布估计算法的迭代次数，避免迭代次数过多导致收敛速度慢的问题，使fwh算法在最优解精度和收敛速度方面有较大的提高，使算法在auv路径规划应用方面找到的路径更加精确合理，寻找最优路径的速度更快。
[0048]
2、本发明提供的路径控制方法采用水下摄像机、声呐、多普勒计程仪等传感器，与路径规划算法相结合，增加了auv在预设路径规划下的实际航行精度。
附图说明
[0049]
图1为本发明中rb-fwh算法的优化过程示意图；
[0050]
图2为本发明中auv路径的控制示意图；
[0051]
图3为本实施例提供的auv路径仿真图。
具体实施方式
[0052]
下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本技术所附权利要求所限定的范围。
[0053]
本发明提供一种水下机器人三维路径规划和控制方法，包括如下步骤：
[0054]
s1：通过适应度函数评估路径质量，构造auv路径；
[0055]
适应度函数为：
[0056]ffitness
＝l
length
s
smooth
o
security
[0057]
其中，l
length
为路径长度，l
length
值越小表示路径越短，消耗能量越小；
[0058]ssmooth
表示路径平滑程度，s
smooth
值越小表示路径越平滑，更有利于auv实现预定路径航行；
[0059]osecurity
表示路径安全性，其值为零则代表路径安全。
[0060]
l
length
、s
smooth
和o
security
的表达式分别如下：
[0061][0062]
其中，dis
pspt
代表起点与终点的欧氏距离，l
path
代表整条路径的实际长度；
[0063][0064]
其中，n
uneven
表示一条路径中弯曲程度超过auv最大转弯半径的路径段数量，如果一条路径弯曲程度超过auv最大转弯半径，则该条路径视为不可行路径；n
seg
表示一条路径中路径段的总数量；
[0065]
[0066]
其中，n
obt
表示落入不可航行区的路径点数量，n
profile
表示三维刨面的数量。
[0067]
三维刨面图的建立方法为：
[0068]
环境信息包括可以航行位置和不可航行位置，用两种不同的颜色区分，x轴、y轴、z轴分别代表环境区域的长度，将构建好的三维环境模型沿y轴切成平行切面，形成三维环境刨面图，每个刨面图都由均匀方格组成，方格的个数应合理设置，若方格数少，则环境信息识别度低，算法精度下降，若方格数过多，环境信息识别度高，算法精度高，但算法速度减慢；将刨面图数字化，可航行区域用0代替，不可航行区域用1代替，然后用三维数组储存所有刨面图数据。
[0069]
s2：通过rb-fwh算法对构造的auv路径进行规划，基于设计解的编码方式，获取到规划好的auv路径：
[0070]
本实施例中rb-fwh算法是通过rosenbrock算法对fwh算法的种群更新步骤进行优化获得，参照图1，其具体的优化过程为：
[0071]
a1：初始化种群：对参数候选解规模m，每个变量的区间等分数n，优秀候选解规模s(s《m)，学习概率α，迭代次数h等给定初始值；
[0072]
a2：构建概率模型：对变量的连续空间[a,b]进行n等分，每个区间长度为开始时每个区间的取值概率相同都为
[0073]
a3：采样概率模型：对每个个体的每一维变量进行取值，采用轮盘赌随机抽样法产生规模为m的参数候选解；
[0074]
a4：基于参数候选解，确定新的优秀候选解：基于适应度函数，计算参数候选解的适应值，根据其适应值，对候选解排序，选择前s个适应度高的作为优秀候选解；
[0075]
a5：通过rosenbrock算法更新当前最优解：从当前s个群体中随机选择t个个体作为初始点进行rosenbrock搜索，将得到的新个体作为新一代群体的一部分，增加种群多样性；
[0076]
a6：更新概率向量，获得每个区间优秀解的概率：对于变量某一维取值的连续空间，在它的n个划分小区间里统计每个区间优秀解的概率，概率为pi：
[0077][0078]
a7：当算法到达迭代次数或给定精度后，算法停止并给出结果。
[0079]
本实施例中采用直线段的方法进行auv路径的构造，具体为：每一条路径都包含一系列的航路点，这些航路点两两之间构成一个路径段，再由这些路径段构成最终路径。
[0080]
在地图上寻找一定数量的航路点，然后再用直线段将相邻的航路点连接起来，构成最终路径。直线段路径构造比较便于实现，并且可以通过控制航路点的数量来确定路径中路径段的数量，方便寻找可行路径。若环境空间中障碍物分布比较均匀并且稀疏，则航路点的数量可以选取少一些，若环境空间中障碍物分布比较密集，则航路点数量可以选取的多一些，方便算法寻找可行路径。这些路径点分别位于环境的刨面中，由于实际情况下，auv存在转向角，机动能力有限，续航时间有限等因素，若路径不够平滑，则不适合auv航行。本实施例中采用步骤s1的适应度函数评估路径质量，确定路径是否适合auv航行。
[0081]
auv路径的规划方法为：一个个体代表一个路径，个体中的每一维变量代表一个航路点。每个航路点由x/y/z三维坐标信息构成，其中y轴坐标已知，x/z轴坐标信息为算法待求解的值，基于解的编码方式，求解到x/z轴坐标信息；
[0082]
解的编码方式为：
[0083][0084]
s3：参照图2，auv开始根据gps惯性导航和多普勒计程仪测量的auv的实时地理位置开始按照规划好的auv路径进行航行；
[0085]
s4：当前视声呐检测到障碍物时，水下照明灯打开，水下摄像机对障碍物进行拍照，并与已知的水下环境高程模型中的障碍物特征进行比对，虽然惯性导航会随时间而误差累积，但此时仍可用gps惯性导航和多普勒计程仪测量的数据缩小水下环境模型中障碍物的比对范围，加快搜索速度，比对成功后，修正gps惯性导航和多普勒计程仪的数据，确定auv实际位置，如果偏离预设路径，则控制主推、侧推、垂推螺旋桨使得auv回到预设路线，继续按照预设的路径航行。
[0086]
本实施例还提供一种水下机器人三维路径规划和控制系统，该系统包括网络接口、存储器和处理器；其中，网络接口，用于在与其他外部网元之间进行收发信息过程中，实现信号的接收和发送；存储器，用于存储能够在所述处理器上运行的计算机程序指令；处理器，用于在运行计算机程序指令时，执行上述共识方法的步骤。
[0087]
本实施例还提供一种计算机存储介质，该计算机存储介质存储有计算机程序，在处理器执行所述计算机程序时可实现以上所描述的方法。所述计算机可读介质可以被认为是有形的且非暂时性的。非暂时性有形计算机可读介质的非限制性示例包括非易失性存储器电路(例如闪存电路、可擦除可编程只读存储器电路或掩膜只读存储器电路)、易失性存储器电路(例如静态随机存取存储器电路或动态随机存取存储器电路)、磁存储介质(例如模拟或数字磁带或硬盘驱动器)和光存储介质(例如cd、dvd或蓝光光盘)等。计算机程序包括存储在至少一个非暂时性有形计算机可读介质上的处理器可执行指令。计算机程序还可以包括或依赖于存储的数据。计算机程序可以包括与专用计算机的硬件交互的基本输入/输出系统(bios)、与专用计算机的特定设备交互的设备驱动程序、一个或多个操作系统、用户应用程序、后台服务、后台应用程序等。
[0088]
本领域内的技术人员应明白，本技术的实施例可提供为方法、系统、或计算机程序产品。因此，本技术可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本技术可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0089]
本技术是参照根据本技术实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实
现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0090]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0091]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0092]
基于上述方案，本发明实际提供了提出一种融合水下摄像机、gps惯性导航、前置声呐和多普勒计程仪与rb-fwh算法的路径规划控制器，为了验证本发明方法的效果，本实施例中将本发明提供的rb-fwh算法和传统的fwh算法同时应用于auv路径的规划，获取到如图3所示的，rb-fwh算法相比于fwh算法，能够获取到更优、更合理的auv路径。

技术特征：
1.一种水下机器人三维路径规划和控制方法，其特征在于，包括如下步骤：s1：通过适应度函数评估路径质量，构造auv路径；s2：通过rb-fwh算法对构造的auv路径进行规划，基于设计解的编码方式，获取到规划好的auv路径；s3：auv开始根据gps惯性导航和多普勒计程仪测量的auv的实时地理位置开始按照规划好的auv路径进行航行；s4：当检测到障碍物时，将拍到的障碍物与水下环境高程模型中的障碍物特征进行比对，比对成功后，修正gps惯性导航和多普勒计程仪的数据，确定auv实际位置，如果偏离预设路径，则控制auv回到预设路线，继续按照预设的路径航行。2.根据权利要求1所述的一种水下机器人三维路径规划和控制方法，其特征在于，所述步骤s1中适应度函数为：f
fitness
＝l
length
s
smooth
o
security
其中，l
length
为路径长度，s
smooth
表示路径平滑程度，o
security
表示路径安全性，其值为零则代表路径安全。3.根据权利要求2所述的一种水下机器人三维路径规划和控制方法，其特征在于，所述步骤s1的适应度函数中l
length
、s
smooth
和o
security
的表达式分别如下：其中，代表起点与终点的欧氏距离，l
path
代表整条路径的实际长度；其中，n
uneven
表示一条路径中弯曲程度超过auv最大转弯半径的路径段数量，如果一条路径弯曲程度超过auv最大转弯半径，则该条路径视为不可行路径；n
seg
表示一条路径中路径段的总数量；其中，n
obt
表示落入不可航行区的路径点数量，n
profile
表示三维刨面的数量。4.根据权利要求1所述的一种水下机器人三维路径规划和控制方法，其特征在于，所述步骤s1中采用直线段的方法进行auv路径的构造，具体为：每一条路径都包含一系列的航路点，这些航路点两两之间构成一个路径段，再由这些路径段构成最终路径。5.根据权利要求1所述的一种水下机器人三维路径规划和控制方法，其特征在于，所述步骤s2中rb-fwh算法是通过rosenbrock算法对fwh算法的种群更新步骤进行优化获得，具体的优化过程为：a1：初始化种群；a2：构建概率模型；a3：采样概率模型，产生规模为m的参数候选解；a4：基于参数候选解，确定新的优秀候选解；
a5：通过rosenbrock算法更新当前最优解；a6：更新概率向量，获得每个区间优秀解的概率；a7：算法停止并给出结果。6.根据权利要求5所述的一种水下机器人三维路径规划和控制方法，其特征在于，所述步骤a1具体为：对参数候选解规模m，每个变量的区间等分数n，优秀候选解规模s，学习概率α，迭代次数h给定初始值；所述步骤a2具体为：对变量的连续空间[a，b]进行n等分，每个区间长度为开始时每个区间的取值概率相同都为所述步骤a3具体为：对每个个体的每一维变量进行取值，采用轮盘赌随机抽样法产生规模为m的参数候选解；所述步骤a4具体为：基于适应度函数，计算参数候选解的适应值，根据其适应值，对候选解排序，选择前s个适应度高的作为优秀候选解；所述步骤a5具体为：从当前s个群体中随机选择t个个体作为初始点进行rosenbrock搜索，将得到的新个体作为新一代群体的一部分；所述步骤a6具体为：对于变量某一维取值的连续空间，在它的n个划分小区间里统计每个区间优秀解的概率，概率为p
i
：所述步骤a7具体为：当算法到达迭代次数或给定精度后，算法停止并给出结果。7.根据权利要求1所述的一种水下机器人三维路径规划和控制方法，其特征在于，所述步骤s2中auv路径的规划方法为：一个个体代表一个路径，个体中的每一维变量代表一个航路点。每个航路点由x/y/z三维坐标信息构成，其中y轴坐标已知，x/z轴坐标信息为算法待求解的值，基于解的编码方式，求解到x/z轴坐标信息；解的编码方式为：8.根据权利要求3所述的一种水下机器人三维路径规划和控制方法，其特征在于，所述步骤s1中三维刨面图的建立方法为：环境信息包括可以航行位置和不可航行位置，用两种不同的颜色区分，x轴、y轴、z轴分别代表环境区域的长度，将构建好的三维环境模型沿y轴切成平行切面，形成三维环境刨面图，每个刨面图都由均匀方格组成，将刨面图数字化，可航行区域用0代替，不可航行区域用1代替，然后用三维数组储存所有刨面图数据。9.根据权利要求3所述的一种水下机器人三维路径规划和控制方法，其特征在于，所述步骤s4中如果偏离预设路径，则控制主推、侧推、垂推螺旋桨使auv回到预设路线。

技术总结
本发明公开了一种水下机器人三维路径规划和控制方法，包括：通过适应度函数评估路径质量，构造AUV路径；通过RB-FWH算法对构造的AUV路径进行规划，基于设计解的编码方式，获取到规划好的AUV路径；AUV开始根据GPS惯性导航和多普勒计程仪测量的AUV的实时地理位置开始按照规划好的AUV路径进行航行；当检测到障碍物时，将拍到的障碍物与水下环境高程模型中的障碍物特征进行比对，比对成功后，修正GPS惯性导航和多普勒计程仪的数据，确定AUV实际位置，如果偏离预设路径，则控制AUV回到预设路线，继续按照预设的路径航行。本发明针对传统FWH算法的缺点，采用转轴法进行改进，主要对FWH算法的更新候选解步骤进行优化，有效提高了路径规划的精度和速度。划的精度和速度。划的精度和速度。


技术研发人员：戴晓强 许赫威 丁建军 赵源 郑宇航 曾庆军
受保护的技术使用者：江苏科技大学
技术研发日：2022.02.18
技术公布日：2022/5/25