一种具备路径规划功能的搜救ROV控制系统及方法

一种具备路径规划功能的搜救rov控制系统及方法
技术领域
1.本发明涉及救援领域，具体涉及一种具备路径规划功能的搜救rov控制系统及方法。


背景技术：

2.rov很早就被应用于救援工作之中，它可以在从海面到海底的各种水深状况下工作，但在实际救援过程中，搜索工作是救援的前提，通常采用自主式水下机器人(auv)在失事区域进行大范围扫描，当搜索到失事物时，auv就返回，换成rov对失事物进行打捞作业。这种将搜索与救援分为两部分的救援方式，显然不能满足救援任务快速的要求。
3.南京邮电大学赵静等提出的一种基于改进遗传算法的移动机器人路径规划方法公开了一种基于改进遗传算法的移动机器人路径规划方法，步骤包括：使用栅格地图对机器人移动空间进行环境建模；设置算法参数并初始化种群；利用路径长度函数和平滑度函数构造适应度函数；引入精英保留策略，即在进行轮盘赌选择时，将最优个体保留至下一代，继续进行交叉变异操作；采用自适应交叉率和变异率对种群进行动态调整；判断进化次数是否达到最大，若是，则输出最优解，若不是，则重复上述步骤。此发明在应用于移动机器人路径规划时不仅增强了其寻找最优解的能力，而且提高了其收敛速度，减少了转弯次数。但对于搜救水下机器人，尤其是搜救rov机器人有很多不足。由于rov机器人本身带缆的限制，对机器人的避障能力要求更高，上述技术方案在此方面并无过多要求，应用于rov中极有可能造成脐带缆的缠绕，不利于搜救工作的进行。


技术实现要素：

4.发明目的：本发明的目的是提供一种能够远离障碍物、拥有更好的初始路径、拥有高质量和光滑度更好的路径的搜救rov控制系统；本发明的另一目的是提供一种搜救rov控制方法。
5.技术方案：本发明的搜救rov控制系统，包括水下环境栅格地图模型建立模块，用以通过水下机器人的工作环境构建水下环境栅格地图模型；通过采用改进的遗传算法对路径进行规划得到良好路径，改进的遗传算法对路径进行规划的过程为：采用a*算法初始化种群、设计路径适应度函数、对路径进行交叉、变异、选择操作；路径优化模块，用以采用bezier优化算子对良好路径进行优化，得到最优路径；控制模块，用以通过最优路径控制水下机器人进行搜救。
6.进一步的，在水下环境栅格地图模型建立模块中，
7.在水下环境栅格地图模型中，黑格表示障碍物，白格代表水下机器人的自由空间，使用序号标记法进行栅格标记，按照从左到右、从下到上的原则依次加1进行标记；在水下环境栅格地图模型中，s，2，3，
…
，e为栅格节点，每个栅格节点为一个路径点；其中，s为水下机器人起点位置，e为水下机器人终点位置；给定栅格序号t(i，j)与直角坐标点p(i，j)[x(i，j)，y(i，j)]的关系如下：
[0008][0009]
其中，mod表示取模算法的行为，floor表示整数除法的行为。
[0010]
进一步的，在路径规划模块中，
[0011]
采用a*算法初始化种群：随机选取起点坐标和终点坐标，并将起点坐标和终点坐标加入染色体编码中；在水下环境栅格地图模型中加入一个无障碍随机点作为目标点；利用a*算法分别建立起点坐标点到目标点以及目标点到终点坐标点的路径，在路径建立过程中避免障碍点和外围区域；将起点坐标点和终点坐标点连接起来，对路径节点一模一样的进行删除，最终生成一个路径集合；
[0012]
设计路径适应度函数：对路径集合中的每个路径点进行采样，计算各个路径点到最近障碍点的距离，当该距离小于数值p时，路径的适应度增加；当该距离大于等于数值p时，路径的适应度减少，其中p大于等于水下环境栅格地图模型的栅格边长的一半；选取适应度最小的路径作为良好路径：
[0013]
fitness(i)＝ω1length(p)+ω2num
[0014]
其中，fitness(i)为适应度函数，length(p)为路径长度，num为路径上距离障碍点小于p的路径点数量；ω1，ω2为路径长度与靠近障碍路径点数量的比例系数，当ω1》ω2时，表示保障路径长度较短更为优先，当ω1《ω2时，表示远离障碍保障运行安全更为优先；
[0015]
对路径进行交叉、变异操作：个体为路径，且一个个体就是一条路径，进行交叉、变异的操作，产生更多的子代个体；
[0016]
交叉概率以pc表示，交叉操作在路径规划中指将已搜索到的两条父代个体在相交的点进行交换，交换后保留较短路径，摒弃较长路径；在进行交叉操作后，子代个体的适应度低于父代个体的适应度；
[0017]
变异概率以pm表示，变异操作在路径规划中指将已搜索到的父代个体以概率pm进行翻转，再结合交叉操作得到适应度更低的子代个体；
[0018]
交叉概率和变异概率的自适应模型为：
[0019][0020][0021]
其中，pc和pm分别为交叉概率和变异概率；分别为每一代的最大交叉概率和最小交叉概率，分别为每一代的最大变异概率和最小变异概率；f为每一代适应度函数值、f
avg
为适应度函数平均值、f
max
为适应度函数平均值的最大值、f
min
为适应度函数平均值的最小值；
[0022]
当个体的适应度低于适应度平均值时，个体的交叉变异概率保持最大值当个体的适应度高于适应度平均值时，适应度越大，交叉变异概率越低；
[0023]
对路径进行选择操作：选择父代种群中适应度最小的最精英个体作为良好路径，将良好路径保留下来用于与子代种群中的最精英个体相比较，若子代最精英个体的适应度小于良好路径的适应度，则将子代最精英个体更新为良好路径；否则，依然保留父代最精英个体为良好路径。
[0024]
进一步的，在路径优化模块中，
[0025]
假定存在一条含有m个控制点p0，p1，p2，
…
，pm的贝塞尔曲线，该曲线的数学表达式如下所示：
[0026][0027]
其中，m∈n+，t为归一化时间变量，pi＝(xi,yi)
t
为第i个控制点的坐标向量，xi和yi分别为x和y坐标的分量，为bernstein多项式，是bezier曲线表达式的基础函数，展开式为：使用上述贝塞尔曲线的数学表达式对良好路径进行优化，得到最优路径。
[0028]
本发明的搜救rov控制方法，包括：
[0029]
(1)通过水下机器人的工作环境构建水下环境栅格地图模型；
[0030]
(2)通过采用改进的遗传算法对路径进行规划得到良好路径，改进的遗传算法对路径进行规划的过程为：采用a*算法初始化种群、设计路径适应度函数、对路径进行交叉、变异、选择操作；
[0031]
(3)采用bezier优化算子对良好路径进行优化，得到最优路径；
[0032]
(4)通过最优路径控制水下机器人进行搜救。
[0033]
进一步的，在步骤(1)中，
[0034]
在水下环境栅格地图模型中，黑格表示障碍物，白格代表水下机器人的自由空间，使用序号标记法进行栅格标记，按照从左到右，从下到上的原则依次加1进行标记，在水下环境栅格地图模型中，s，2，3，
…
，e为栅格节点，每个栅格节点为一个路径点；其中，s为水下机器人起点位置，e为水下机器人终点位置；给定栅格序号t(i，j)与直角坐标点p(i，j)[x(i，j)，y(i，j)]的关系如下：
[0035][0036]
其中，mod表示取模算法的行为，floor表示整数除法的行为。
[0037]
进一步的，在步骤(2)中，
[0038]
采用a*算法初始化种群：随机选取起点坐标和终点坐标，并将起点坐标和终点坐标加入染色体编码中；在水下环境栅格地图模型中加入一个无障碍随机点作为目标点；利用a*算法分别建立起点坐标点到目标点以及目标点到终点坐标点的路径，在路径建立过程中避免障碍点和外围区域；将起点坐标点和终点坐标点连接起来，对路径节点一模一样的进行删除，最终生成一个路径集合；
[0039]
设计路径适应度函数：对路径集合中的每个路径点进行采样，计算各个路径点到最近障碍点的距离，当该距离小于数值p时，路径的适应度增加；当该距离大于等于数值p
时，路径的适应度减少，其中p大于等于水下环境栅格地图模型的栅格边长的一半；选取适应度最小的路径作为良好路径：
[0040]
fitness(i)＝ω1length(p)+ω2num
[0041]
其中，fitness(i)为适应度函数，length(p)为路径长度，num为路径上距离障碍点小于p的路径点数量；ω1，ω2为路径长度与靠近障碍路径点数量的比例系数，当ω1》ω2时，表示保障路径长度较短更为优先，当ω1《ω2时，表示远离障碍保障运行安全更为优先；
[0042]
进行交叉、变异操作：个体为路径，且一个个体就是一条路径，进行交叉、变异的操作，产生更多的子代个体；
[0043]
交叉概率以pc表示，交叉操作在路径规划中指将已搜索到的两条父代个体在相交的点进行交换，交换后保留较短路径，摒弃较长路径；在进行交叉操作后，子代个体的适应度低于父代个体的适应度；
[0044]
变异概率以pm表示，变异操作在路径规划中指将已搜索到的父代个体以概率pm进行翻转，再结合交叉操作得到适应度更低的子代个体；
[0045]
交叉概率和变异概率的自适应模型为：
[0046][0047][0048]
其中，pc和pm分别为交叉概率和变异概率；为每一代的最大交叉概率和最小交叉概率，为每一代的最大变异概率和最小变异概率；f，f
avg
，f
max
和f
min
分别为每一代适应度函数值、适应度函数平均值、适应度函数平均值的最大值和适应度函数平均值的最小值；
[0049]
当个体的适应度低于适应度平均值时，个体的交叉变异概率保持最大值当个体的适应度高于适应度平均值时，适应度越大，交叉变异概率越低；
[0050]
对路径进行选择操作：选择父代种群中适应度最小的最精英个体作为良好路径，将良好路径保留下来用于与子代种群中的最精英个体相比较，若子代最精英个体的适应度小于良好路径的适应度，则将子代最精英个体更新为良好路径；否则，依然保留父代最精英个体为良好路径。
[0051]
进一步的，在步骤(3)中，
[0052]
假定存在一条含有m个控制点p0，p1，p2，
…
，pm的贝塞尔曲线，该曲线的数学表达式如下所示：
[0053][0054]
其中，m∈n+，t为归一化时间变量，pi＝(xi,yi)
t
为第i个控制点的坐标向量，xi和yi分别为x和y坐标的分量，为bernstein多项式，是bezier曲线表达式的基础函数，展
开式为：
[0055]
使用上述贝塞尔曲线的数学表达式对良好路径进行优化，得到最优路径。
[0056]
本发明提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述搜救rov控制方法的步骤。
[0057]
本发明提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述搜救rov控制方法的步骤。
[0058]
有益效果：本发明与现有技术相比，其显著优点是：1、能够远离障碍物：利用路径长度和路径节点距最近障碍物的距离构造适应度函数以达到远离障碍物的目的；2、拥有更好的初始路径：在改进的遗传算法中，通过引入a*算法以得到更好的初始路径；3、拥有高质量和光滑度更好的路径：通过引入bezier优化算子，利用bezier曲线的连续性和平滑性的优点，改善路径的质量和光滑度。
附图说明
[0059]
图1是本发明的流程图；
[0060]
图2是水下环境栅格地图模型图；
[0061]
图3是采用改进的遗传算法和bezier优化算子对路径进行规划的流程图；
[0062]
图4是路径适应度函数的流程图；
[0063]
图5是水下机器人控制系统框图；
[0064]
图6是路径规划功能架构图；
[0065]
图7是自动规划路径的流程图。
具体实施方式
[0066]
以下结合附图对本发明的具体实施方式进行说明。
[0067]
实施例1
[0068]
如图1所示，所述的搜救rov控制系统，包括：
[0069]
水下环境栅格地图模型建立模块，用以通过水下机器人的工作环境构建水下环境栅格地图模型。
[0070]
如图2所示，在水下环境栅格地图模型中，黑格表示障碍物，白格代表水下机器人的自由空间，使用序号标记法进行栅格标记，按照从左到右、从下到上的原则依次加1进行标记；在水下环境栅格地图模型中，s，2，3，
…
，e为栅格节点，每个栅格节点为一个路径点；其中，s为水下机器人起点位置，e为水下机器人终点位置；给定栅格序号t(i，j)与直角坐标点p(i，j)[x(i，j)，y(i，j)]的关系如下：
[0071][0072]
其中，mod表示取模算法的行为，floor表示整数除法的行为。
[0073]
如图3所示，路径规划模块，用以通过采用改进的遗传算法对路径进行规划得到良好路径，改进的遗传算法对路径进行规划的过程为：采用a*算法初始化种群、设计路径适应
度函数、对路径进行交叉、变异、选择操作。
[0074]
采用a*算法初始化种群：随机选取起点坐标和终点坐标，并将起点坐标和终点坐标加入染色体编码中；在水下环境栅格地图模型中加入一个无障碍随机点作为目标点；利用a*算法分别建立起点坐标点到目标点以及目标点到终点坐标点的路径，在路径建立过程中避免障碍点和外围区域；将起点坐标点和终点坐标点连接起来，对路径节点一模一样的进行删除，最终生成一个路径集合。
[0075]
如图4所示，设计路径适应度函数：对路径集合中的每个路径点进行采样，计算各个路径点到最近障碍点的距离，当该距离小于数值p时，路径的适应度增加；当该距离大于等于数值p时，路径的适应度减少，其中p大于等于水下环境栅格地图模型的栅格边长的一半；选取适应度最小的路径作为良好路径：
[0076]
fitness(i)＝ω1length(p)+ω2num
[0077]
其中，fitness(i)为适应度函数，length(p)为路径长度，num为路径上距离障碍点小于p的路径点数量；ω1，ω2为路径长度与靠近障碍路径点数量的比例系数，当ω1》ω2时，表示保障路径长度较短更为优先，当ω1《ω2时，表示远离障碍保障运行安全更为优先。
[0078]
对路径进行交叉、变异操作：个体为路径，且一个个体就是一条路径，进行交叉、变异的操作，产生更多的子代个体。
[0079]
交叉概率以pc表示，交叉操作在路径规划中指将已搜索到的两条父代个体在相交的点进行交换，交换后保留较短路径，摒弃较长路径；在进行交叉操作后，子代个体的适应度低于父代个体的适应度。
[0080]
变异概率以pm表示，变异操作在路径规划中指将已搜索到的父代个体以概率pm进行翻转，再结合交叉操作得到适应度更低的子代个体。
[0081]
交叉概率和变异概率的自适应模型为：
[0082][0083][0084]
其中，pc和pm分别为交叉概率和变异概率；分别为每一代的最大交叉概率和最小交叉概率，分别为每一代的最大变异概率和最小变异概率；f为每一代适应度函数值、f
avg
为适应度函数平均值、f
max
为适应度函数平均值的最大值、f
min
为适应度函数平均值的最小值。
[0085]
当个体的适应度低于适应度平均值时，个体的交叉变异概率保持最大值当个体的适应度高于适应度平均值时，适应度越大，交叉变异概率越低。
[0086]
对路径进行选择操作：选择父代种群中适应度最小的最精英个体作为良好路径，将良好路径保留下来用于与子代种群中的最精英个体相比较，若子代最精英个体的适应度小于良好路径的适应度，则将子代最精英个体更新为良好路径；否则，依然保留父代最精英个体为良好路径。
[0087]
如图3所示，路径优化模块，用以采用bezier优化算子对良好路径进行优化，得到最优路径。
[0088]
假定存在一条含有m个控制点p0，p1，p2，
…
，pm的贝塞尔曲线，该曲线的数学表达式如下所示：
[0089][0090]
其中，m∈n+，t为归一化时间变量，pi＝(xi,yi)
t
为第i个控制点的坐标向量，xi和yi分别为x和y坐标的分量，为bernstein多项式，是bezier曲线表达式的基础函数，展开式为：
[0091]
使用上述贝塞尔曲线的数学表达式对良好路径进行优化，得到最优路径。
[0092]
控制模块，用以通过最优路径控制水下机器人进行搜救。
[0093]
如图5所示，水下机器人控制系统分为水面控制系统和水下控制系统。
[0094]
水面控制系统包含供电系统和监控系统。水面电源箱一端连接脐带缆，负责给水下机器人本体运动提供所需的动力，另一端通过网线和监控电脑相连，内置路由器负责连接下位机和上位机；监控电脑作为终端实时显示水下机器人本体当前的姿态、深度、温湿度、压力、电压电流等信息，与此同时机器人可选择两种控制方式：手柄控制、路径规划(自动控制)。
[0095]
水下控制系统包括能量控制系统、运动控制系统、视频观察系统、安全监测系统和信息采集系统。其中，能量控制系统负责进行电力传输，给不同的模块进行供电，还负责变压电路和驱动电路；运动控制系统驱动电机正反装来控制水下机器人运动；视频观察系统包括摄像头(云台摄像头)和水下灯，摄像头负责对周围环境进行观测，当环境较暗时打开水下灯辅助照明；安全监测系统负责实时采集电子耐压舱内报警模块信息，当发生故障时发出报警；信息采集系统负责实时采集水下机器人耐压仓内各类传感器数据及gps数据并上传给上位机监控系统。
[0096]
如图6所示，路径规划功能需要水面监控系统和水下控制系统共同实现。
[0097]
如图7所示，路径规划负责对水下机器人进行自动规划路径，流程如下：
[0098]
1、打开路径规划窗口，切换到自动规划模式。
[0099]
2、环境信息处理，首先获得当前海域的地图模型，并在地图模型上进行禁航区设置并进行图像膨胀腐蚀处理，得到最终的栅格地图模型。
[0100]
3、选择路径规划的目标点，并以水下机器人的当前位置作为路径规划的起始点，目标位置为终点。
[0101]
4、判断所选取目标点是否合理，不合理将重新选择。
[0102]
5、调用引入a*算法的路径规划接口，传入地图模型、起始点、目标点，进行路径规划。
[0103]
6、获取规划路径节点，进行任务规划。
[0104]
实施例2
[0105]
如图1所示，所述的搜救rov控制方法，包括：
[0106]
(1)通过水下机器人的工作环境构建水下环境栅格地图模型。
[0107]
如图2所示，在水下环境栅格地图模型中，黑格表示障碍物，白格代表水下机器人的自由空间，使用序号标记法进行栅格标记，按照从左到右、从下到上的原则依次加1进行标记；在水下环境栅格地图模型中，s，2，3，
…
，e为栅格节点，每个栅格节点为一个路径点；其中，s为水下机器人起点位置，e为水下机器人终点位置；给定栅格序号t(i，j)与直角坐标点p(i，j)[x(i，j)，y(i，j)]的关系如下：
[0108][0109]
其中，mod表示取模算法的行为，floor表示整数除法的行为。
[0110]
(2)如图3所示，通过采用改进的遗传算法对路径进行规划得到良好路径，改进的遗传算法对路径进行规划的过程为：采用a*算法初始化种群、设计路径适应度函数、对路径进行交叉、变异、选择操作。
[0111]
采用a*算法初始化种群：随机选取起点坐标和终点坐标，并将起点坐标和终点坐标加入染色体编码中；在水下环境栅格地图模型中加入一个无障碍随机点作为目标点；利用a*算法分别建立起点坐标点到目标点以及目标点到终点坐标点的路径，在路径建立过程中避免障碍点和外围区域；将起点坐标点和终点坐标点连接起来，对路径节点一模一样的进行删除，最终生成一个路径集合。
[0112]
如图4所示，设计路径适应度函数：对路径集合中的每个路径点进行采样，计算各个路径点到最近障碍点的距离，当该距离小于数值p时，路径的适应度增加；当该距离大于等于数值p时，路径的适应度减少，其中p大于等于水下环境栅格地图模型的栅格边长的一半；选取适应度最小的路径作为良好路径：
[0113]
fitness(i)＝ω1length(p)+ω2num
[0114]
其中，fitness(i)为适应度函数，length(p)为路径长度，num为路径上距离障碍点小于p的路径点数量；ω1，ω2为路径长度与靠近障碍路径点数量的比例系数，当ω1》ω2时，表示保障路径长度较短更为优先，当ω1《ω2时，表示远离障碍保障运行安全更为优先。
[0115]
对路径进行交叉、变异操作：个体为路径，且一个个体就是一条路径，进行交叉、变异的操作，产生更多的子代个体。
[0116]
交叉概率以pc表示，交叉操作在路径规划中指将已搜索到的两条父代个体在相交的点进行交换，交换后保留较短路径，摒弃较长路径；在进行交叉操作后，子代个体的适应度低于父代个体的适应度。
[0117]
变异概率以pm表示，变异操作在路径规划中指将已搜索到的父代个体以概率pm进行翻转，再结合交叉操作得到适应度更低的子代个体。
[0118]
交叉概率和变异概率的自适应模型为：
[0119][0120]
[0121]
其中，pc和pm分别为交叉概率和变异概率；分别为每一代的最大交叉概率和最小交叉概率，分别为每一代的最大变异概率和最小变异概率；f为每一代适应度函数值、f
avg
为适应度函数平均值、f
max
为适应度函数平均值的最大值、f
min
为适应度函数平均值的最小值。
[0122]
当个体的适应度低于适应度平均值时，个体的交叉变异概率保持最大值当个体的适应度高于适应度平均值时，适应度越大，交叉变异概率越低。
[0123]
对路径进行选择操作：选择父代种群中适应度最小的最精英个体作为良好路径，将良好路径保留下来用于与子代种群中的最精英个体相比较，若子代最精英个体的适应度小于良好路径的适应度，则将子代最精英个体更新为良好路径；否则，依然保留父代最精英个体为良好路径。
[0124]
(3)如图3所示，采用bezier优化算子对良好路径进行优化，得到最优路径。
[0125]
假定存在一条含有m个控制点p0，p1，p2，
…
，pm的贝塞尔曲线，该曲线的数学表达式如下所示：
[0126][0127]
其中，m∈n+，t为归一化时间变量，pi＝(xi,yi)
t
为第i个控制点的坐标向量，xi和yi分别为x和y坐标的分量，为bernstein多项式，是bezier曲线表达式的基础函数，展开式为：
[0128]
使用上述贝塞尔曲线的数学表达式对良好路径进行优化，得到最优路径。
[0129]
(4)通过最优路径控制水下机器人进行搜救。
[0130]
如图5所示，水下机器人控制系统分为水面控制系统和水下控制系统。
[0131]
水面控制系统包含供电系统和监控系统。水面电源箱一端连接脐带缆，负责给水下机器人本体运动提供所需的动力，另一端通过网线和监控电脑相连，内置路由器负责连接下位机和上位机；监控电脑作为终端实时显示水下机器人本体当前的姿态、深度、温湿度、压力、电压电流等信息，与此同时机器人可选择两种控制方式：手柄控制、路径规划(自动控制)。
[0132]
水下控制系统包括能量控制系统、运动控制系统、视频观察系统、安全监测系统和信息采集系统。其中，能量控制系统负责进行电力传输，给不同的模块进行供电，还负责变压电路和驱动电路；运动控制系统驱动电机正反装来控制水下机器人运动；视频观察系统包括摄像头(云台摄像头)和水下灯，摄像头负责对周围环境进行观测，当环境较暗时打开水下灯辅助照明；安全监测系统负责实时采集电子耐压舱内报警模块信息，当发生故障时发出报警；信息采集系统负责实时采集水下机器人耐压仓内各类传感器数据及gps数据并上传给上位机监控系统。
[0133]
如图6所示，路径规划功能需要水面监控系统和水下控制系统共同实现。
[0134]
如图7所示，路径规划负责对水下机器人进行自动规划路径，流程如下：
[0135]
1、打开路径规划窗口，切换到自动规划模式。
[0136]
2、环境信息处理，首先获得当前海域的地图模型，并在地图模型上进行禁航区设
置并进行图像膨胀腐蚀处理，得到最终的栅格地图模型。
[0137]
3、选择路径规划的目标点，并以水下机器人的当前位置作为路径规划的起始点，目标位置为终点。
[0138]
4、判断所选取目标点是否合理，不合理将重新选择。
[0139]
5、调用引入a*算法的路径规划接口，传入地图模型、起始点、目标点，进行路径规划。
[0140]
6、获取规划路径节点，进行任务规划。
[0141]
本发明提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述搜救rov控制方法的步骤。
[0142]
本发明提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述搜救rov控制方法的步骤。