您的当前位置：首页一种基于无人艇与多水下机器人的协同控制系统及方法[发明专利]

一种基于无人艇与多水下机器人的协同控制系统及方法[发明专利]

来源：九壹网

(19)中华人民共和国国家知识产权局

(12)发明专利申请

(10)申请公布号 CN 111045453 A(43)申请公布日 2020.04.21

(21)申请号 201911353444.2(22)申请日 2019.12.25

(71)申请人南京工程学院

地址 211167 江苏省南京市江宁科学园弘

景大道1号(72)发明人郭铁铮　陈巍　夏细明　杨雪　

陈　郭覃　李佩娟　陈凯杰　陈伟　刘静　(74)专利代理机构南京纵横知识产权代理有限

公司 32224

代理人俞翠华(51)Int.Cl.

G05D 1/10(2006.01)

权利要求书5页说明书16页附图5页

(54)发明名称

一种基于无人艇与多水下机器人的协同控制系统及方法(57)摘要

本发明公开了一种基于无人艇与多水下机器人的协同控制系统及方法，系统包括无人艇和多个机器人；无人艇基于获得的所有水下勘查搜索目标的地理位置、水下地形信息和所有机器人的实时地理位置，生成与各机器人对应的水下勘查搜索目标和路径规划信息；各机器人接收由无人艇发送的水下勘查搜索目标和路径规划信息，按照路径规划信息进行搜索目标的勘查，在执行搜索目标勘查过程中，当到达某水下可疑目标附近时，采集该水下可疑目标的图像信息并与接收到的水下勘查搜索目标进行比对，直至完成可疑目标和搜索目标的图像视频拍摄和数据采集。本发明采用SOM网络解决无人艇与多水下机器人的多机协作、信息共享、多任务分配和实时路径规划问题。

CN 111045453 ACN 111045453 A

权　利　要　求　书

1/5页

1.一种基于无人艇与多水下机器人的协同控制系统，其特征在于，包括：无人艇和多个机器人；

所述无人艇基于获得的所有水下勘查搜索目标的地理位置、水下地形信息和所有机器人的实时地理位置，动态生成与各机器人对应的水下勘查搜索目标和路径规划信息；

各机器人接收由无人艇发送的水下勘查搜索目标和路径规划信息，按照所述路径规划信息进行目标搜索，当遇到障碍物时，根据所述路径规划信息绕开障碍物；在执行搜索目标勘察的过程中，当到达某水下可疑目标附近时，采集该水下可疑目标的图像信息并与接收到的水下勘查搜索目标进行比对识别，直至完成可疑目标和接收到的搜索目标的图像视频拍摄和相关数据采集。

2.根据权利要求1所述的一种基于无人艇与多水下机器人的协同控制系统，其特征在于：所述无人艇中设有GPS导航单元、第一航姿参考单元、USBL超短基线收发基阵单元、第一声波通信单元和主控制器；各机器人中设有图像采集单元、USBL应答器单元、多波束声呐单元、第二航姿参考单元、深度传感器、从控制器和第二声波通信单元；

所述GPS导航单元用于获取USBL超短基线收发基阵单元的大地坐标位置；所述第一航姿参考单元获取无人艇的任意时刻的横摇、纵摇及艏向数据；所述USBL超短基线收发基阵单元发出声脉冲信号；所述USBL应答器单元收到所述声脉冲后发送反馈声脉冲信号，其与所述USBL基阵探头共同构成完整的USBL超短基线基阵系统；

所述主控制器分别与所述GPS导航单元、第一航姿参考单元、USBL超短基线收发基阵单元和第一声波通信单元相连；

所述多波束声呐单元用于获得水下地形信息；

所述第二航姿参考单元用于获取水下机器人的任意时刻的三维坐标速度向量和加速度向量；

所述深度传感器用于获取机器人的水下深度；所述从控制器分别与所述图像采集单元、USBL应答器单元、多波束声呐单元、第二航姿参考单元、深度传感器和第二声波通信单元相连，所述第二声波通信单元与所述第一声波通信单元相连。

3.根据权利要求2所述的一种基于无人艇与多水下机器人的协同控制系统，其特征在于，定义无人艇的坐标系为无人艇坐标系；定义USBL超短基线收发基阵单元的坐标系为USBL基阵坐标系，其中，所述无人艇坐标系以向下垂直于无人艇甲板方向为Z轴，船艏方向为X轴，且所述无人艇坐标系与USBL基阵坐标系的原点为同一点；

各机器人的实时地理位置也即USBL应答器单元的实时地理位置的计算公式为：

其中，(Rx，Ry，Rz)为各机器人在大地坐标下的位置；(x2，y2，z2)为各机器人在USBL基阵坐标系的位置；R1为USBL基阵坐标向大地坐标系变换的旋转矩阵，

CN 111045453 A

权　利　要　求　书

2/5页

其中，(αβγm)分别为无人艇任意时刻的横摇、纵摇及艏向数据；m、m、(x1，y1，z1)为USBL超短基线收发基阵单元在大地坐标中的位置：

其中，(x0，y0，z0)为USBL超短基线收发基阵单元在无人艇坐标系中的位置；R2为USBL基阵坐标向无人艇坐标的旋转矩阵，

式中，(αβγe)分别为USBL超短基线收发基阵单元安装的横向偏差、纵向偏差及艏e、e、向偏差。

4.根据权利要求1所述的一种基于无人艇与多水下机器人的协同控制系统，其特征在于，当所述主控制器获得所有搜索目标的地理位置以及所有机器人的实时地理位置后，则所述主控制器将所有搜索目标作为SOM网络的输入神经元，输入层神经元的三维权向量代表搜索目标的位置坐标，并将所有机器人作为SOM网络的输出神经元，输出层神经元的三维权向量代表机器人的位置坐标；基于设定的目标函数，计算出与输入层各神经元相对应的获胜神经元，即计算出与各搜索目标相对应的机器人；

所述主控制器对获胜神经元的权值进行更新调整，执行对对应的机器人的路径规划，当所有机器人位置更新到对应的搜索目标位置后，动态任务分配和路径规划结束。

5.根据权利要求4所述的一种基于无人艇与多水下机器人的协同控制系统，其特征在于：所述获胜神经元的计算公式为：

Pj←min{Dij+S(x，y，z)}

S(x，y，z)＝[Vj(x，y，z)-Vo(x，y，z)]·t其中，Pj表示相对于输入神经元Ti的获胜神经元；Dij表示某一时刻输入神经元Ti和输出神经元Rj之间的欧式距离，Ti＝(Tix，Tiy，Tiz)表示输入神经元的位置坐标；Rj＝(Rjx，Rjy，Rjz)是输出神经元的位置坐标；S(x，y，z)为海流影响因子计算公式，Vj(x，y，z)为机器人速度向量，VO(x，y，z)为海流速度向量，t为机器人从出发点到搜索目标位置的行进时间；Cmax表示机器人在源充电的前提下，仅依靠自身携带的能源，能够航行的最大距离；C表示机器人巡航的安全距离；

其中，Dj表示第j个输出神经元的实际运行距离；S表示多机器人在完成某一全局任务的过程中每个机器人的平均移动距离。

CN 111045453 A

权　利　要　求　书

3/5页

6.根据权利要求4所述的一种基于无人艇与多水下机器人的协同控制系统，其特征在于：将工作环境抽象为有限的栅格地图，所述路径规划的计算公式为：

g＝(1-β)t·g0

Nj＝cosΔθj

其中，Rj(t)和Rj(t+1)分别输出神经元的t和t+1时刻的坐标，δ为网络学习速率，Ti为输入神经元Ti的坐标；当机器人与搜索目标点的距离Dij≤Dmin时，表示机器人已到达对应的搜索目标，路径规划结束，SOM网络无需更新权值；当机器人与搜索目标点的距离Dij＞Dmin时，按上式更新权值；

f(dm，g)是领域函数；

dm＝|Nm-Nj|是第m个神经元与获胜神经元之间的距离，r是领域半径，t是迭代次数；β是变化率，决定每一次迭代的时间；β和g0为常数；Pn表示机器人下一个行驶位置；c是一个正常数；k是与当前栅格Pc相邻的栅格个数；Fj为综合栅格信度函数；

Mj表示第j个栅格的位置信度函数值，Δθj是机器人当前位置和下一可能位置连线与机器人当前位置和搜索目标位置连线夹角，Nj是机器人与搜索目标之间的方向信度函数。

7.一种基于无人艇与多水下机器人的协同控制方法，其特征在于，包括：利用无人艇基于获得的所有水下勘查搜索目标的地理位置、水下地形信息和所有机器人的实时地理位置，动态生成与各机器人对应的水下勘查搜索目标和路径规划信息；

利用各机器人接收由无人艇发送的水下勘查搜索目标和路径规划信息，按照所述路径规划信息进行目标搜索，当遇到障碍物时，根据所述路径规划信息绕开障碍物；在执行搜索目标勘察的过程中，当到达某水下可疑目标附近时，采集该水下可疑目标的图像信息并与接收到的水下勘查搜索目标进行比对识别，直至完成可疑目标和接收到的搜索目标的图像视频拍摄和相关数据采集。

8.根据权利要求7所述的一种基于无人艇与多水下机器人的协同控制方法，其特征在于：定义无人艇的坐标系为无人艇坐标系；定义USBL超短基线收发基阵单元的坐标系为USBL基阵坐标系，其中，所述无人艇坐标系以向下垂直于无人艇甲板方向为Z轴，船艏方向为X轴，且所述无人艇坐标系与USBL基阵坐标系的原点为同一点；

各机器人的实时地理位置也即USBL应答器单元的实时地理位置的计算公式为：

CN 111045453 A

权　利　要　求　书

4/5页

其中，(Rx，Ry，Rz)为各机器人在大地坐标下的位置；(x2，y2，z2)为各机器人在USBL基阵坐标系的位置；R1为USBL基阵坐标向大地坐标系变换的旋转矩阵，

其中，(αβγm)分别为无人艇任意时刻的横摇、纵摇及艏向数据；m、m、

(x1，y1，z1)为USBL超短基线收发基阵单元在大地坐标中的位置：

其中，(x0，y0，z0)为USBL超短基线收发基阵单元在无人艇坐标系中的位置；R2为USBL基阵坐标向无人艇坐标的旋转矩阵，

式中，(αβγe)分别为USBL超短基线收发基阵单元安装的横向偏差、纵向偏差及艏e、e、向偏差。

9.根据权利要求7所述的一种基于无人艇与多水下机器人的协同控制方法，其特征在于：当所述主控制器获得所有搜索目标的地理位置以及所有机器人的实时地理位置后，则所述主控制器将所有搜索目标作为SOM网络的输入神经元，输入层神经元的三维权向量代表搜索目标的位置坐标，并将所有机器人作为SOM网络的输出神经元，输出层神经元的三维权向量代表机器人的位置坐标；基于设定的目标函数，计算出与输入层各神经元相对应的获胜神经元，即计算出与各搜索目标相对应的机器人；

10.根据权利要求7所述的一种基于无人艇与多水下机器人的协同控制方法，其特征在于：所述获胜神经元的计算公式为：

Pj←min{Dij+S(x，y，z)}

CN 111045453 A

权　利　要　求　书

5/5页

其中，Dj表示第j个输出神经元的实际运行距离；S表示多机器人在完成某一全局任务的过程中每个机器人的平均移动距离。

将工作环境抽象为有限的栅格地图，所述路径规划的计算公式为：

g＝(1-β)t·g0

Nj＝cosΔθj其中，Rj(t)和Rj(t+1)分别输出神经元的t和t+1时刻的坐标，δ为网络学习速率，Ti为输入神经元Ti的坐标；当机器人与搜索目标点的距离Dij≤Dmin时，表示机器人已到达对应的搜索目标，路径规划结束，SOM网络无需更新权值；当机器人与搜索目标点的距离Dij＞Dmin时，按上式更新权值；

f(dm，g)是领域函数；

CN 111045453 A

说　明　书

1/16页

一种基于无人艇与多水下机器人的协同控制系统及方法

技术领域

[0001]本发明属于海空界面机器人协作技术领域，具体涉及一种基于无人艇与多水下机器人的协同控制系统及方法。

背景技术

[0002]在人类生存的地球上，海洋占据了地球表面积的71％，远大于陆地所占比重。随着人类对海洋认识和探索的不断加深，海洋科学技术得到加速发展。人们逐渐意识到海洋开发的重要性，尤其对于当前21世纪正面临的环境不断恶化，人口不断增多，资源不断减少三大难题，探索开发海洋资源受到越来越多的关注。[0003]自治水下机器人(Autonomous Underwater Vehicle，AUV)是海洋资源和水下环境勘查的重要工具，当面临复杂的水域环境和越来越多样化的任务需求时，单个AUV由于自带的能源动力有限、观察范围小、作业工具单一等条件，已经无法完成多样化的任务需求，多AUV系统成为提高作业效率、扩大作业范围的有效途径和方法。对于多AUV系统，需要解决AUV的多机协作、信息共享、路径规划等问题，亟需建立能够起到“大脑”作用的主控系统，而水面无人艇(Unmanned Surface Vehicle，USV)作为海空界面的智能运载平台、立体化海洋空间的关键节点，可成为多AUV系统的总控平台和通讯中继，解决多机协作和任务分配等难题。无人艇与多AUV系统的结合，对提高任务的执行效率和可靠性具有很好的效果。[0004]目前USV与多AUV的协同控制还存在以下技术难题：(1)动态任务实时分配问题；(2)路径规划和避障问题。在动态任务分配时，如果仅仅依靠任务目标点坐标与AUV坐标的最近原则来分，则可能出现某个AUV被分配过多任务而其他AUV得不到任务的情况。而在AUV的路径规划和行进避障方面，海洋环境、海流速度和AUV的运动学约束对AUV路径规划的影响，目前很少有相关资料涉及。发明内容

[0005]针对上述问题，本发明提出一种基于无人艇与多水下机器人的协同控制系统及方法，采用自组织映射神经网络方法解决了无人艇与多水下机器人的多机协作、信息共享、多任务分配和实时路径规划问题。[0006]为了实现上述技术目的，达到上述技术效果，本发明通过以下技术方案实现：[0007]第一方面，本发明提供了一种基于无人艇与多水下机器人的协同控制系统，包括：无人艇和多个机器人；

[0008]所述无人艇基于获得的所有水下勘查搜索目标的地理位置、水下地形信息和所有机器人的实时地理位置，动态生成与各机器人对应的水下勘查搜索目标和路径规划信息；[0009]各机器人接收由无人艇发送的水下勘查搜索目标和路径规划信息，按照所述路径规划信息进行目标搜索，当遇到障碍物时，根据所述路径规划信息绕开障碍物；在执行搜索目标勘察的过程中，当到达某水下可疑目标附近时，采集该水下可疑目标的图像信息并与接收到的水下勘查搜索目标进行比对识别，直至完成可疑目标和接收到的搜索目标的图像

CN 111045453 A

说　明　书

2/16页

视频拍摄和相关数据采集。[0010]可选地，所述无人艇中设有GPS导航单元、第一航姿参考单元、USBL超短基线收发基阵单元、第一声波通信单元和主控制器；各机器人中设有图像采集单元、USBL应答器单元、多波束声呐单元、第二航姿参考单元、深度传感器、从控制器和第二声波通信单元；[0011]所述GPS导航单元用于获取USBL超短基线收发基阵单元的大地坐标位置；[0012]所述第一航姿参考单元获取无人艇的任意时刻的横摇、纵摇及艏向数据；[0013]所述USBL超短基线收发基阵单元发出声脉冲信号；所述USBL应答器单元收到所述声脉冲后发送反馈声脉冲信号，其与所述USBL基阵探头共同构成完整的USBL超短基线基阵系统；

[0014]所述主控制器分别与所述GPS导航单元、第一航姿参考单元、USBL超短基线收发基阵单元和第一声波通信单元相连；

[0015]所述多波束声呐单元用于获得水下地形信息；

[0016]所述第二航姿参考单元用于获取水下机器人的任意时刻的三维坐标速度向量和加速度向量；

[0017]所述深度传感器用于获取机器人的水下深度；[0018]所述从控制器分别与所述图像采集单元、USBL应答器单元、多波束声呐单元、第二航姿参考单元、深度传感器和第二声波通信单元相连，所述第二声波通信单元与所述第一声波通信单元相连。[0019]可选地，定义无人艇的坐标系为无人艇坐标系；定义USBL超短基线收发基阵单元的坐标系为USBL基阵坐标系，其中，所述无人艇坐标系以向下垂直于无人艇甲板方向为Z轴，船艏方向为X轴，且所述无人艇坐标系与USBL基阵坐标系的原点为同一点；

[0020]各机器人的实时地理位置也即USBL应答器单元的实时地理位置的计算公式为：

[0021][0022]

其中，(Rx，Ry，Rz)为各机器人在大地坐标下的位置；(x2，y2，z2)为各机器人在USBL

基阵坐标系的位置；R1为USBL基阵坐标向大地坐标系变换的旋转矩阵，

[0023][0024][0025][0026][0027][0028][0029]

其中，(αβγm)分别为无人艇任意时刻的横摇、纵摇及艏向数据；m、m、

(x1，y1，z1)为USBL超短基线收发基阵单元在大地坐标中的位置：

其中，(x0，y0，z0)为USBL超短基线收发基阵单元在无人艇坐标系中的位置；R2为USBL基阵坐标向无人艇坐标的旋转矩阵，

CN 111045453 A[0030]

说　明　书

3/16页

式中，(αβγe)分别为USBL超短基线收发基阵单元安装的横向偏差、纵向偏差e、e、

及艏向偏差。

[0031]可选地，当所述主控制器获得所有搜索目标的地理位置以及所有机器人的实时地理位置后，则所述主控制器将所有搜索目标作为SOM网络的输入神经元，输入层神经元的三维权向量代表搜索目标的位置坐标，并将所有机器人作为SOM网络的输出神经元，输出层神经元的三维权向量代表机器人的位置坐标；基于设定的目标函数，计算出与输入层各神经元相对应的获胜神经元，即计算出与各搜索目标相对应的机器人；[0032]所述主控制器对获胜神经元的权值进行更新调整，执行对对应的机器人的路径规划，当所有机器人位置更新到对应的搜索目标位置后，动态任务分配和路径规划结束。[0033]可选地，所述获胜神经元的计算公式为：[0034]Pj←min{Dij+S(x，y，z)}

[0035]

S(x，y，z)＝[Vj(x，y，z)-Vo(x，y，z)]·t[0037]其中，Pj表示相对于输入神经元Ti的获胜神经元；Dij表示某一时刻输入神经元Ti和输出神经元Rj之间的欧式距离，Ti＝(Tix,Tiy,Tiz)表示输入神经元的位置坐标；Rj＝(Rjx,Rjy,Rjz)是输出神经元的位置坐标；S(x，y，z)为海流影响因子计算公式，Vj(x,y,z)为机器人速度向量，VO(x,y,z)为海流速度向量，t为机器人从出发点到搜索目标位置的行进时间；Cmax表示机器人在源充电的前提下，仅依靠自身携带的能源，能够航行的最大距离；C表示机器人巡航的安全距离；

[0036][0038][0039]

其中，Dj表示第j个输出神经元的实际运行距离；S表示多机器人在完成某一全局

任务的过程中每个机器人的平均移动距离。[0040]可选地，将工作环境抽象为有限的栅格地图，所述路径规划的计算公式为：

[0041]

[0042][0043][0044][0045][0046]

g＝(1-β)t·g0

Nj＝cosΔθj[0047]其中，Rj(t)和Rj(t+1)分别输出神经元的t和t+1时刻的坐标，δ为网络学习速率，Ti为输入神经元Ti的坐标；当机器人与搜索目标点的距离Dij≤Dmin时，表示机器人已到达对应

CN 111045453 A

说　明　书

4/16页

的搜索目标，路径规划结束，SOM网络无需更新权值；当机器人与搜索目标点的距离Dij>Dmin时，按上式更新权值；[0048]f(dm，g)是领域函数；

[0049]dm＝|Nm-Nj|是第m个神经元与获胜神经元之间的距离，r是领域半径，t是迭代次数；

[0050]β是变化率，决定每一次迭代的时间；β和g0为常数；[0051]Pn表示机器人下一个行驶位置；c是一个正常数；k是与当前栅格Pc相邻的栅格个数；

[0052]Fj为综合栅格信度函数；

[0053]Mj表示第j个栅格的位置信度函数值，△θj是机器人当前位置和下一可能位置连线与机器人当前位置和搜索目标位置连线夹角，Nj是机器人与搜索目标之间的方向信度函数。[0054]第二方面，一种基于无人艇与多水下机器人的协同控制方法，包括：[0055]利用无人艇基于获得的所有水下勘查搜索目标的地理位置、水下地形信息和所有机器人的实时地理位置，动态生成与各机器人对应的水下勘查搜索目标和路径规划信息；[0056]利用各机器人接收由无人艇发送的水下勘查搜索目标和路径规划信息，按照所述路径规划信息进行目标搜索，当遇到障碍物时，根据所述路径规划信息绕开障碍物；在执行搜索目标勘察的过程中，当到达某水下可疑目标附近时，采集该水下可疑目标的图像信息并与接收到的水下勘查搜索目标进行比对识别，直至完成可疑目标和接收到的搜索目标的图像视频拍摄和相关数据采集。[0057]可选地，定义无人艇的坐标系为无人艇坐标系；定义USBL超短基线收发基阵单元的坐标系为USBL基阵坐标系，其中，所述无人艇坐标系以向下垂直于无人艇甲板方向为Z轴，船艏方向为X轴，且所述无人艇坐标系与USBL基阵坐标系的原点为同一点；

[0058]各机器人的实时地理位置也即USBL应答器单元的实时地理位置的计算公式为：

[0059][0060]

其中，(Rx，Ry，Rz)为各机器人在大地坐标下的位置；(x2，y2，z2)为各机器人在USBL

基阵坐标系的位置；R1为USBL基阵坐标向大地坐标系变换的旋转矩阵，

[0061][0062][0063][00][0065][0066]

其中，(αβγm)分别为无人艇任意时刻的横摇、纵摇及艏向数据；m、m、

(x1，y1，z1)为USBL超短基线收发基阵单元在大地坐标中的位置：

其中，(x0，y0，z0)为USBL超短基线收发基阵单元在无人艇坐标系中的位置；R2为USBL基阵坐标向无人艇坐标的旋转矩阵，

CN 111045453 A

说　明　书

5/16页

[0067]

式中，(αβγe)分别为USBL超短基线收发基阵单元安装的横向偏差、纵向偏差e、e、

及艏向偏差。

[0069]可选地，当所述主控制器获得所有搜索目标的地理位置以及所有机器人的实时地理位置后，则所述主控制器将所有搜索目标作为SOM网络的输入神经元，输入层神经元的三维权向量代表搜索目标的位置坐标，并将所有机器人作为SOM网络的输出神经元，输出层神经元的三维权向量代表机器人的位置坐标；基于设定的目标函数，计算出与输入层各神经元相对应的获胜神经元，即计算出与各搜索目标相对应的机器人；[0070]所述主控制器对获胜神经元的权值进行更新调整，执行对对应的机器人的路径规划，当所有机器人位置更新到对应的搜索目标位置后，动态任务分配和路径规划结束。[0071]可选地，所述获胜神经元的计算公式为：[0072]Pj←min{Dij+S(x，y，z)}

[0073]

[0068]

S(x，y，z)＝[Vj(x，y，z)-Vo(x，y，z)]·t[0075]其中，Pj表示相对于输入神经元Ti的获胜神经元；Dij表示某一时刻输入神经元Ti和输出神经元Rj之间的欧式距离，Ti＝(Tix,Tiy,Tiz)表示输入神经元的位置坐标；Rj＝(Rjx,Rjy,Rjz)是输出神经元的位置坐标；S(x，y，z)为海流影响因子计算公式，Vj(x,y,z)为机器人速度向量，VO(x,y,z)为海流速度向量，t为机器人从出发点到搜索目标位置的行进时间；Cmax表示机器人在源充电的前提下，仅依靠自身携带的能源，能够航行的最大距离；C表示机器人巡航的安全距离；

[0074][0076][0077]

其中，Dj表示第j个输出神经元的实际运行距离；S表示多机器人在完成某一全局

任务的过程中每个机器人的平均移动距离。[0078]将工作环境抽象为有限的栅格地图，所述路径规划的计算公式为：

[0079]

[0080][0081][0082]

g＝(1-β)t·g0

CN 111045453 A[0083]

说　明　书

6/16页

Nj＝cosΔθj

[0085]其中，Rj(t)和Rj(t+1)分别输出神经元的t和t+1时刻的坐标，δ为网络学习速率，Ti为输入神经元Ti的坐标；当机器人与搜索目标点的距离Dij≤Dmin时，表示机器人已到达对应的搜索目标，路径规划结束，SOM网络无需更新权值；当机器人与搜索目标点的距离Dij>Dmin时，按上式更新权值；[0086]f(dm，g)是领域函数；

[0087]dm＝|Nm-Nj|是第m个神经元与获胜神经元之间的距离，r是领域半径，t是迭代次数；

[0088]β是变化率，决定每一次迭代的时间；β和g0为常数；[00]Pn表示机器人下一个行驶位置；c是一个正常数；k是与当前栅格Pc相邻的栅格个数；

[0090]Fj为综合栅格信度函数；

[0091]Mj表示第j个栅格的位置信度函数值，△θj是机器人当前位置和下一可能位置连线与机器人当前位置和搜索目标位置连线夹角，Nj是机器人与搜索目标之间的方向信度函数。

[0092]与现有技术相比，本发明的有益效果：

[0093]当面临复杂的水域环境和越来越多样化的任务需求时，单个AUV由于自带的能源动力有限、观察范围小、作业工具单一等条件，已经无法完成多样化的任务需求，多AUV系统成为提高作业效率、扩大作业范围的有效途径和方法。USV作为海空界面的智能运载平台，可成为多AUV系统的总控平台和通讯中继，解决多机协作和任务分配等难题。[0094]1、本发明完成任务的实时性强。USV具有海-空界面协同运行的优势，实时高速接收指挥中心的控制命令，并将命令分解、下发至各个AUV。[0095]2、本发明完成任务的能力强。针对比较复杂的任务，将其分解成多个小的任务模块，降低单个AUV的负担，AUV配置携带不同传感器和工具，从而完成较为复杂的任务工作。[0096]3、本发明完成任务的效率高。多个AUV同时分别执行任务，可以有效地提高完成任务目标的效率。另外在能源供应方面，单个AUV的续航时间短，受到能源条件，有些任务无法一次性完成，而多AUV系统可以充分发挥数量的优势。[0097]4、本发明完成任务的可靠性高。多AUV系统不会因为某个AUV的故障而导致任务无法执行，从而提高了执行任务的可靠性。

[0084]

附图说明

[0098]为了使本发明的内容更容易被清楚地理解，下面根据具体实施例并结合附图，对本发明作进一步详细的说明，其中：

[0099]图1为本发明一种实施例的基于无人艇与多水下机器人的协同控制系统的结构示意图；

[0100]图2为本发明一种实施例的基于无人艇与多水下机器人的协同控制方法的流程示意图；

CN 111045453 A[0101][0102][0103][0104][0105][0106][0107]

说　明　书

7/16页

图3为本发明一种实施例的USBL超短基线基阵系统的原理及坐标系图；图4为本发明一种实施例的SOM网络结构图；

图5为本发明一种实施例的栅格路径方向选择示意图；图6为本发明一种实施例的各栅格方向信度函数的示意图；图7为本发明一种实施例的领域权重改变示意图；图8为本发明一种实施例的领域权重改变算法流程图；图9为本发明一种实施例的水下勘查搜索目标、AUV和障碍物水下三维分布情况之

一；

图10为本发明一种实施例的水下勘查搜索目标数多于AUV的动态任务分配和路径

规划示意图；

[0109]图11为本发明一种实施例的水下勘查搜索目标、AUV和障碍物水下三维分布情况之二；

[0110]图12为本发明一种实施例的水下勘查搜索目标数少于AUV的动态任务分配和路径规划示意图。

具体实施方式

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

[0112]下面结合附图对本发明的应用原理作详细的描述。[0113]实施例1

[0114]本发明实施例中提供了一种基于无人艇与多水下机器人的协同控制系统，如图1所示，包括无人艇和多个机器人；

[0115]所述无人艇基于获得的所有水下勘查搜索目标的地理位置、水下地形信息和所有机器人的实时地理位置，动态生成与各机器人对应的水下勘查搜索目标和路径规划信息；[0116]各机器人接收由无人艇发送的水下勘查搜索目标和路径规划信息，按照所述路径规划信息进行目标搜索，当遇到障碍物时，根据所述路径规划信息绕开障碍物；在执行搜索目标勘察的过程中，当到达某水下可疑目标附近时，采集该水下可疑目标的图像信息并与接收到的水下勘查搜索目标进行比对识别，直至完成可疑目标和接收到的搜索目标的图像视频拍摄和相关数据采集。

[0117]在本发明实施例的一种具体实施方式中，所述无人艇中设有GPS导航单元、第一航姿参考单元(AHRS)、USBL超短基线收发基阵单元(USBL收发基列)、LTE-Wifi通信单元、RF无线通信单元、PC工控机单元、第一声波通信单元、数据存储单元和主控制器单元；各机器人中设有图像采集单元、USBL应答器单元、多波束声呐单元、第二航姿参考单元(AHRS)、第二声波通信单元、深度传感器、数据存储单元和从控制器单元；

[0118]所述GPS导航单元用于获取USBL超短基线收发基阵单元的大地坐标位置，也即USV的大地坐标位置信息，并通过串口通信接口(如RS232)与主控制器相连；[0119]所述第一航姿参考单元用于获取无人艇的任意时刻的横摇、纵摇及艏向数据，通过UART接口与主控制器相连；

[0108]

CN 111045453 A[0120]

说　明　书

8/16页

所述USBL超短基线收发基阵单元发出声脉冲信号；所述USBL应答器单元收到所述

声脉冲后发送反馈声脉冲信号，其与所述USBL超短基线收发基阵单元共同构成完整的USBL超短基线基阵系统，实现USV与AUV之间的精准定位，通过串口通信接口(RS232/RS422)与主控制器相连；

[0121]所述主控制器还分别与所述LTE-Wifi通信模块、RF无线通信模块、PC工控机单元、数据存储单元、第一声波通信单元相连，实时交互数据和命令下发；所述LTE-Wifi通信模块和RF无线通信模块实现USV与指挥中心的无线通信；所述PC工控机单元实现水下机器人的多任务分配和路径规划，通过UART接口与主控制器相连，无人艇中的数据存储器用于保存USV的运行数据和AUV的上传数据；

[0122]所述第二航姿参考单元用于获取机器人的角度值，以及机器人任意时刻的三维坐标速度向量和加速度向量，且与所述从控制器相连；[0123]所述多波束声呐单元用于获得水下地形信息，且与所述从控制器相连；[0124]所述深度传感器为AUV提供深度数据，通过模数转换器ADC与AUV的从控制器相连；在具体实施过程中，所述深度传感器可以采用深度针；[0125]所述从控制器还分别与所述图像采集单元、USBL应答器单元、数据存储单元和第二声波通信单元相连，所述第二声波通信单元与所述第一声波通信单元相连，实现USV与AUV之间的声波通信，在具体使用过程中，所述第一声波通信单元和第二声波通信单元均可以采用现有技术中的声波通信模块；数据存储器保存AUV的图像和数据信息。[0126]USV接收指挥中心指令，获得水下勘查任务目标地理位置，USV行驶至任务目标附近海域，布放携带的多个AUV并进行多任务分配，各AUV接收由USV发送的水下勘查搜索目标和实时路径规划信息，按照所述路径规划信息进行目标搜索，当遇到障碍物时，根据路径规划信息绕开障碍物，达到某水下可疑目标附近时，通过图像采集单元(比如摄像机)对水下可疑目标进行确认，当所述可疑目标无法匹配接收到的水下勘查搜索目标时，拍摄图像并留存供以后核对。AUV再抵近附近的其他水下目标进行识别，当与接收到的水下勘查搜索目标一致时，则拍摄目标的详细视频并存储。完成任务后，AUV返回并回收至USV。任务流程图如图2所示。

[0127]在本发明实施例的一种具体实施方式中，在任务执行过程中，USV的位置由GPS导航单元确定。USV和AUV之间的相对位置由安装在USV和AUV上的USBL超短基线基阵系统确定。所述超短基线基阵系统由USBL超短基线收发基阵单元和USBL应答器单元组成，USBL超短基线收发基阵中的探头安装于USBL超短基线收发基阵前端。所述USBL超短基线收发基阵单元安装在USV上，所述USBL应答器单元固定在各AUV上。所述USBL超短基线收发基阵中的探头发出一个声脉冲，AUV中的USBL应答器单元收到后返回声脉冲信号，收发基阵接收到后，测量出X、Y、Z方向的相位差，并根据声波的到达时间计算出AUV到USBL超短基线基阵单元的距离。为获得AUV的绝对位置(大地坐标/地理坐标)，需将GPS导航单元信号、声基阵信号和航姿参考单元信号相结合，USV通过GPS导航单元获得大地坐标位置，第一航姿参考单元获取USV任意时刻的横摇(roll)、纵摇(pitch)及艏向(yaw)数据，用于实时校正USBL基阵坐标系的姿态，通过测量数据和各个坐标系的转换，最终实现水下AUV的三维实时高精度定位。

[0128]为此，本发明中定义无人艇的坐标系为无人艇坐标系；定义USBL超短基线收发基

CN 111045453 A

说　明　书

9/16页

阵单元的坐标系为USBL基阵坐标系，其中，所述无人艇坐标系以向下垂直于无人艇甲板方向为Z轴，船艏方向为X轴，且所述无人艇坐标系与USBL基阵坐标系的原点为同一点，可避免坐标系的平移，有利于坐标的计算和转换。[0129]如图3所示，在USBL基阵坐标系中，θ为AUV的方位角，即AUV水下位置在XY平面上与X轴的夹角，r为AUV在XY平面上的水平斜距，R为超短基线收发基阵单元到AUV的距离，α、β为测量值。USBL超短基线收发基阵单元在USV坐标系中的值为(x0,y0,z0)，USV利用第二航姿参考单元可得横摇角α纵摇角β艏向角γm，超短基线收发基阵单元在大地坐标中的位置计m、m、为(x1,y1,z1)。根据坐标旋转原则确定超短基线收发基阵单元在大地坐标系的值为：

[0130][0131][0132]

其中，R1为USBL基阵坐标向大地坐标变换的旋转矩阵，公式如下：

超短基线收发基阵单元在USV上安装是有方向性的，由于在安装时，USBL基阵坐标系与无人艇坐标系之间不可能完全重合，存在系统性偏差，要保证获取高精度的定位数据，必须进行校准改正。根据超短基线收发基阵单元安装偏差的方向可以分为横向偏差(roll error)、纵向偏差(pitch error)和艏向偏差(yaw error)。

[0134]通过超短基线收发基阵单元可以测得USBL应答器单元在USBL基阵坐标下的位置(x2,y2,z2)，USBL应答器单元(AUV)在大地坐标下的位置为(Rx,Ry,RZ)，则有：

[0135][0136][0137]

[0133]

R2为USBL基阵坐标向无人艇坐标的旋转矩，如下式：

式中，(αβ纵向偏差及艏e,e,γe)分别为超短基线收发基阵单元安装的横向偏差、向偏差。

[0139]通过式(3)可获得每个水下机器人AUV相对于大地坐标系的绝对位置，从而USV为AUV提供导航和路径规划信息。

[0140]为了完成对工作环境中所有任务目标的访问，首先将任务目标分配给不同的AUV，在无海流的环境下，最小能量消耗等价于最短路径，但是在海流环境影响下，能量消耗需要考虑海流因素的影响。同时，考虑每个AUV的任务均衡，避免一些AUV的任务过重，超出能力范围；其次，任务分配完成之后进行路径规划，由于环境中存在障碍物，因此每个AUV规划的路径都必须顺利避开障碍物；最后，在AUV向目标行进的过程中航行速度应满足运动学约束条件，避免发生速度跳变，保证每个AUV的规划路径都能实现。当水下三维工作空间中所有的目标都有AUV到达时，动态任务分配和路径规划结束。综上描述，多AUV系统的控制方法主要考虑以下因素：(1)执行任务时的任务均衡性和AUV能量；(2)任务路径规划中的障碍

[0138]

CN 111045453 A

说　明　书

10/16页

物自动避障；(3)AUV任务执行过程中的运动学约束；(4)海流因素对多任务分配和路径规划的影响。

[0141]多任务分配和路径规划与神经网络原理相似，故采用自组织映射神经网络方法(Self-Organizing Map，SOM)解决无人艇与多水下机器人的协同控制问题。常规SOM方法如下：

[0142]SOM网络由输入层和输出层组成，输入层通过权向量将外界信息汇集到输出层各神经元，竞争层(也即输出层)负责对输入模式进行“比较”、“分析”，寻找规律并归类。图4所示为多AUV系统的SOM模型，它有两层神经元组成：第1层为输入层，代表三维工作区中任务目标，输入层神经元的三维权向量(权值)代表该任务目标的位置坐标；第2层为输出层，代表三维工作区中的AUV，输出层神经元的三维权向量(权值)代表该AUV的位置坐标，输出层的神经元与输入层的神经元一一连接，连接权重构成三维权向量。[0143]SOM可以实现输入数据的聚类，将水下勘查搜索目标作为输入层神经元，AUV作为输出层神经元，则SOM学习过程即为水下勘查搜索目标点位置坐标与AUV位置坐标的比较、竞争和学习过程。在SOM工作过程中，首先将输入层中水下勘查搜索目标的位置坐标与输出层中不同AUV的位置坐标进行比较，根据目标函数竞争取得获胜神经元(也即对应的AUV)，获胜神经元(AUV)将获得对应输入的水下勘查搜索目标点(任务目标)；建立和计算领域函数，在学习过程中，对获胜神经元和领域神经元的权值进行更新调整，即对应AUV路径规划，随着邻域不断减小，输出神经元(AUV)趋向于任务目标点所在的区域，当三维水下工作空间中所有的目标都有AUV到达时，即所有AUV位置更新到对应的任务目标位置时，动态任务分配和路径规划结束，流程图如图8所示。[0144]为此，在本发明实施例的一种具体实施方式中，当所述主控制器获得所有搜索目标的地理位置以及所有机器人的实时地理位置后，则所述主控制器将所有搜索目标作为SOM网络的输入神经元，输入层神经元的三维权向量代表搜索目标的位置坐标，并将所有机器人作为SOM网络的输出神经元，输出层神经元的三维权向量代表机器人的位置坐标；基于设定的目标函数，计算出与输入层各神经元相对应的获胜神经元，即计算出与各搜索目标相对应的机器人。

[0145]所述主控制器对获胜神经元的权值进行更新调整，执行对对应的机器人的路径规划，当所有机器人位置更新到对应的搜索目标位置后，动态任务分配和路径规划结束。[0146]具体地，所述获胜神经元实时计算过程具体为：[0147]获胜神经元由目标函数公式(4)给出，Pj表示相对于输入神经元Ti的获胜神经元，在无海流影响的情况下，获胜神经元即为到输入神经元(任务目标)距离最小的输出神经元(AUV)，在考虑海流影响的情况下，需增加海流影响因子S(x,y,z)。[0148]Pj→min{Dij+S(x，y，z)} (4)

[0149]

S(x，y，z)＝[Vj(x，y，z)-Vo(x，y，z)]·t (6)[0151]式(5)中，Dij表示某一时刻输入神经元Ti和输出神经元Rj之间的欧式距离，Ti＝(Tix,Tiy,Tiz)表示输入神经元的位置坐标(任务目标的位置坐标)；Rj＝(Rjx,Rjy,Rjz)是输出神经元的位置坐标(AUV的位置坐标)；式(6)为海流影响因子计算公式，Vj(x,y,z)为AUV速度向量，VO(x,y,z)为海流速度向量，t为AUV从出发点到目标位置的行进时间。

[0150]

CN 111045453 A[0152]

说　明　书

11/16页

为了避免单个AUV的任务过重，在算法中加入AUV的负担均衡。设定AUV都具有相同

的水下巡航能力且携带相同能源。Cmax表示AUV在源充电的前提下，仅依靠自身携带的能源，能够航行的最大距离。C表示AUV巡航的安全距离，即AUV的移动距离在[0,C]的范围时，不用担心能源不足或者用尽的情况。Dj表示第j个输出神经元(AUV)的实际运行距离。S表示多AUV系统在完成某一全局任务的过程中，每个AUV的平均移动距离。

[0153][0154]

参数Q控制AUV的负担均衡。基于3种不同的情况，式(5)可修改为：

[0155]

公式(8)中，第1种情况是当AUV的移动距离小于其安全距离，即0≤Dj＜C时，不需要考虑AUV自身携带的能源制约。当AUV的移动距离大于其安全距离，即C≤Dj＜Cmax时，负担均衡函数会被自动调用，解决AUV运行过程中的能源短缺问题。对于AUV在向目标行进过程中会因为能源短缺而停止时，AUV的任务分配判据会被置为无穷大∞，表示在整个多AUV系统的任务分配中，该AUV永远不会被选为获胜神经元。

[0157]为了使AUV能够自动避开障碍物并且克服速度跳变，引入栅格信度函数对SOM的权值进行调节，提出一种基于栅格信度自组织函数的自组织映射算法。栅格信度函数包括两部分：位置信度函数与方向信度函数。

[0158]为了防止AUV的速度超出运动学的约束条件，将工作环境抽象为有限的栅格地图。依据每个栅格的性质赋不同的信度函数值，即位置信度函数值，表示环境信息。这里以二维的栅格地图为例说明怎样对栅格进行信度赋值。图5所示，是一个二维的栅格地图，工作空间被分成了9个栅格，其中灰色栅格表示被障碍物占领，白色栅格表示自由空间，根据栅格位置信息由式(9)进行栅格位置信度函数Mj赋值，如栅格2被赋值为-1，其余栅格被赋值为+1。

[0159]

[0156]

式中Mj表示第j个栅格的位置信度函数值。

[0161]为了保证工作效率，再引入方向信度函数使得AUV始终向着目标方向运动，方向信度函数是表示AUV运动方向和目标方向的相近程度。AUV运动方向和目标方向越靠近，方向信度函数越大；AUV运动方向离目标方向越远时，方向信度函数越小。其定义为：[0162]Nj＝cosΔθ (10)j[0163]其中，△θj是关于AUV当前位置和下一可能位置连线与AUV当前位置和目标位置连线夹角，Nj是AUV与目标之间的方向信度函数。式中：△θπ]，若自治水下机器人AUV前j∈[0,方是目标，则△θyj＝1；若AUV相反方向是目标，则△θπ，yj＝-1。假设AUV在中心栅j＝0，j＝格位置，目标位于与水平正方向成60°的位置，图6给出了AUV各个相邻栅格的方向信度函数值。

[01]将工作环境离散化以后，路径规划问题就演变为寻找AUV的下一个行驶位置，只有准确找出了该位置，才能使AUV自主规划出一条切实可行的无碰撞行驶路径，为了避开障碍

[0160]

CN 111045453 A

说　明　书

12/16页

物并且能够快速的到达对应的目标位置，根据栅格位置信度函数和方向信度函数，再定义一个综合栅格信度函数Fj，路径选择的原则是AUV始终向着栅格信度函数值最大的方向运动。其定义为

[0165]

式中：Pn表示AUV下一个行驶位置；c是一个正常数，取值为0.6；k是与当前栅格Pc相邻的栅格个数。

[0167]在选出获胜神经元后，下一步是设计一个领域函数，用来确定在领域范围内受获胜神经元影响的输入神经元。[0168]邻域函数决定了输入神经元对于获胜神经元和相邻神经元的影响力(吸引力强度)。获胜神经元所受影响最大，邻近神经元所受影响逐渐降低，邻近区域以外的神经元不受影响，影响力大小决定了在某次迭代过程中邻域神经元权重调整的大小。[0169]三维空间下的获胜神经元领域是一个球体，球心的坐标是获胜神经元的位置坐标。领域函数决定了输入神经元对获胜神经元和相邻神经元的影响力，其中获胜神经元所受到的影响是最大的，其相邻的神经元所受的影响逐渐降低，邻近区域以外的神经元不受到影响，影响力的大小决定了迭代过程中神经元权重调整的大小，领域函数定义如下：

[0170][0171]

[0166]

dm＝|Nm-Nj|是第m个神经元与获胜神经元之间的距离，r是领域半径，t是迭代次

数。

g＝(1-β)t·g0 (13)[0173]其中，β是变化率，决定每一次迭代的时间。β和g0为常数，通过修改β和g0的值可以调整领域中AUV的运动步长，从而控制计算精度和运算时间。[0174]在获胜神经元和领域函数确定后，下一步是控制获胜神经元(AUV)和相邻神经元向目标(输入神经元)的位置坐标移动。为了克服传统SOM算法不能避障和超过运动学约束的问题，改进SOM算法的权值(坐标)按照下式进行更新：

[0172][0175]

式(14)中，Rj(t)是输出神经元(AUV)的t时刻坐标，Rj(t+1)是输出神经元(AUV)的t+1时刻坐标，δ为网络学习速率，当AUV与目标点的距离Dij≤Dmin时，表示AUV已到达目标，路径规划结束，SOM无需更新权值；当AUV与目标点的距离Dij>Dmin时，按式(14)更新权值，和传统的SOM算法比较，提出的新算法采用AUV下一步的位置代替目标位置对SOM的权值进行更新。由于AUV下一步的运动位置综合考虑了障碍物的位置和任务目标的方向，因此能够保证AUV在向着目标运动的过程中自动避开障碍物。另外，对工作空间进行栅格化，AUV下一步的运动被在相邻的栅格，AUV的单步航行距离必然满足运动学约束条件，所提出的栅格信度SOM算法自然克服了速度跳变。

[0177]在获胜神经元及其邻域函数确定以后，获胜神经元和相邻神经元向输入神经元移动，其他神经元保持不动，反映在SOM的学习中即为权值的调整。如图7所示，圆点表示某时

[0176]

CN 111045453 A

说　明　书

13/16页

刻机器人的位置，作为输出层神经元；三角框表示目标位置，作为输入层神经元。R11距离输入神经元(任务目标点)最近，成为获胜神经元，R12是获胜神经元周围的惟一相邻神经元，处于领域半径以内。R11和R12根据规则向目标点移动并改变权向量，其他神经元权值不变，算法的循环流程如图8所示，初始化SOM神经网络后，任务目标位置依次输入到网络。对于给定的一个任务目标，流程如下：(1)、通过目标函数竞争选出该轮次获胜神经元；(2)、确定获胜神经元的邻域；(3)、利用公式(14)调整获胜神经元及其领域神经元的权向量；(4)、判断获胜神经元的权值是否持续更新，若有更新，则移动神经元，神经元重新进入目标函数竞争下一轮的获胜神经元；若无更新，则获胜神经元已达到任务目标，循环结束。[0178]实施例2

[0179]基于与实施例1相同的发明构思，本发明实施例中提供了一种基于无人艇与多水下机器人的协同控制方法，包括：[0180]利用无人艇基于获得的所有水下勘查搜索目标的地理位置、水下地形信息和所有机器人的实时地理位置，动态生成与各机器人对应的水下勘查搜索目标和路径规划信息；[0181]利用各机器人接收由无人艇发送的水下勘查搜索目标和路径规划信息，按照所述路径规划信息进行目标搜索，当遇到障碍物时，根据所述路径规划信息绕开障碍物；在执行搜索目标勘察的过程中，当到达某水下可疑目标附近时，采集该水下可疑目标的图像信息并与接收到的水下勘查搜索目标进行比对识别，直至完成可疑目标和接收到的搜索目标的图像视频拍摄和相关数据采集。

[0182]在本发明实施例的一种具体实施方式中，定义无人艇的坐标系为无人艇坐标系；定义USBL超短基线收发基阵单元的坐标系为USBL基阵坐标系，其中，所述无人艇坐标系以向下垂直于无人艇甲板方向为Z轴，船艏方向为X轴，且所述无人艇坐标系与USBL基阵坐标系的原点为同一点；

[0183]各机器人的实时地理位置也即USBL应答器单元的实时地理位置的计算公式为：

[0184][0185]

其中，(Rx，Ry，Rz)为各机器人在大地坐标下的位置；(x2，y2，z2)为各机器人在USBL

基阵坐标系的位置；R1为USBL基阵坐标向大地坐标系变换的旋转矩阵，

[0186][0187][0188][01][0190][0191]

其中，(αβγm)分别为无人艇任意时刻的横摇、纵摇及艏向数据；m、m、

(x1，y1，z1)为USBL超短基线收发基阵单元在大地坐标中的位置：

其中，(x0，y0，z0)为USBL超短基线收发基阵单元在无人艇坐标系中的位置；R2为USBL基阵坐标向无人艇坐标的旋转矩阵，

CN 111045453 A

说　明　书

14/16页

[0192]

式中，(αβγe)分别为USBL超短基线收发基阵单元安装的横向偏差、纵向偏差e、e、

及艏向偏差。

[0194]在本发明实施例的一种具体实施方式中，当所述主控制器获得所有搜索目标的地理位置以及所有机器人的实时地理位置后，则所述主控制器将所有搜索目标作为SOM网络的输入神经元，输入层神经元的三维权向量代表搜索目标的位置坐标，并将所有机器人作为SOM网络的输出神经元，输出层神经元的三维权向量代表机器人的位置坐标；基于设定的目标函数，计算出与输入层各神经元相对应的获胜神经元，即计算出与各搜索目标相对应的机器人；

[0195]所述主控制器对获胜神经元的权值进行更新调整，执行对对应的机器人的路径规划，当所有机器人位置更新到对应的搜索目标位置后，动态任务分配和路径规划结束。[0196]在本发明实施例的一种具体实施方式中，所述获胜神经元的计算公式为：[0197]Pj←min{Dij+S(x，y，z)} (4)

[0193][0198]

S(x，y，z)＝[Vj(x，y，z)-Vo(x，y，z)]·t (6)[0200]其中，Pj表示相对于输入神经元Ti的获胜神经元；Dij表示某一时刻输入神经元Ti和输出神经元Rj之间的欧式距离，Ti＝(Tix,Tiy,Tiz)表示输入神经元的位置坐标；Rj＝(Rjx,Rjy,Rjz)是输出神经元的位置坐标；S(x，y，z)为海流影响因子计算公式，Vj(x,y,z)为机器人速度向量，VO(x,y,z)为海流速度向量，t为机器人从出发点到搜索目标位置的行进时间；Cmax表示机器人在源充电的前提下，仅依靠自身携带的能源，能够航行的最大距离；C表示机器人巡航的安全距离；

[0199][0201][0202]

其中，Dj表示第j个输出神经元的实际运行距离；S表示多机器人在完成某一全局

任务的过程中每个机器人的平均移动距离。[0203]将工作环境抽象为有限的栅格地图，所述路径规划的计算公式为：

[0204]

[0205][0206][0207]

g＝(1-β)t·g0 (13)

CN 111045453 A[0208]

说　明　书

15/16页

Nj＝cosΔθ (10)j

[0210]其中，Rj(t)和Rj(t+1)分别输出神经元的t和t+1时刻的坐标，δ为网络学习速率，Ti为输入神经元Ti的坐标；当机器人与搜索目标点的距离Dij≤Dmin时，表示机器人已到达对应的搜索目标，路径规划结束，SOM网络无需更新权值；当机器人与搜索目标点的距离Dij>Dmin时，按上式更新权值；

[0211]dm＝|Nm-Nj|是第m个神经元与获胜神经元之间的距离，r是领域半径，t是迭代次数；

[0212]β是变化率，决定每一次迭代的时间；β和g0为常数；[0213]Pn表示机器人下一个行驶位置；c是一个正常数；k是与当前栅格Pc相邻的栅格个数；

[0214]Fj为综合栅格信度函数；

[0215]Mj表示第j个栅格的位置信度函数值，△θj是机器人当前位置和下一可能位置连线与机器人当前位置和搜索目标位置连线夹角，Nj是机器人与搜索目标之间的方向信度函数。

[0216]其余部分均与实施例1相同。

[0217]仿真实验设置三维水下工作空间的大小为200米×200米×200米，其中随机的分布着障碍物、任务目标和AUV。每个AUV都视为质点，不考虑其形状。通过仿真实验，证明改进的SOM算法纳入了海流因素并成功实施任务均衡，在任务分配和路径规划中自动避开障碍物，行进速度符合运动学约束。[0218]任务目标、AUV和障碍物水下三维分布情况如图9所示，其中R1-R5表示AUV，T1-T7表示目标，O1-O3为障碍物。由于工作环境中分布着5个AUV，7个目标，目标的数量超过了AUV的数量，因此部分AUV将分配到多于一个目标。传统SOM算法通过反复计算和比较每个AUV和目标的距离，进行任务分配。AUV总是沿着直线选择最近的目标访问，不考虑海流、障碍物和运动学约束等问题。每个AUV沿着最近的直线路径和最快的速度到达目标，这在实际情况中很难实现，AUV可能会与障碍物发生碰撞，并且航行速度会超出约束条件。[0219]根据改进栅格信度SOM算法，SOM权值的更新考虑了AUV的方向信度函数和位置信度函数信息，使得AUV能够绕过障碍物向着目标运动。此外，AUV总是选择栅格信度函数最大的相邻的栅格作为下一步运动位置，有效地了AUV的速度，防止了速度跳变的发生。图10显示了目标数量多于AUV情况下的任务分配和路径规划结果。首先，SOM算法进行任务分配，将R1分配给目标T1，R2分配给目标T2，R3分配给目标T3和T4，R4分配给目标T6和T5，而R5分配给目标T7，图中显示规划路径无碰撞。如果采用传统的SOM算法进行任务分配和路径规划，R5沿着直线运动必将和障碍物O2发生碰撞，不能到达目标T7。[0220]改进的SOM算法不仅能够处理目标数量多于AUV的情况，而且能够解决目标数量少于AUV的情况。为了节省能量，没有分配任务的AUV将保持静止。图11和图12给出了AUV数量多于目标任务数量的仿真结果，这里有7个AUV随机的分布在三维的工作空间，但是只有5个目标需要访问。图11为任务目标、AUV和障碍物水下三维分布情况，图12为目标数少于AUV的动态任务分配和路径规划结果，从图中可以发现离目标最近的4个AUV分配去访问，而距离

[0209]

CN 111045453 A

说　明　书

16/16页

较远的R6和R7未分配任务，保持静止。

[0221]以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

CN 111045453 A

说　明　书　附　图

1/5页

图1

图2

CN 111045453 A

说　明　书　附　图

2/5页

图3

图4

图5

CN 111045453 A

说　明　书　附　图

3/5页

图6

图7

CN 111045453 A

说　明　书　附　图

4/5页

图8

图9

CN 111045453 A

说　明　书　附　图

5/5页

图10

图11

图12

因篇幅问题不能全部显示，请点此查看更多更全内容

查看全文