一种水下机器人定位导航系统及定位导航方法与流程

本发明涉及机器人导航技术领域，更具体地，涉及一种水下机器人定位导航系统及定位导航方法。

背景技术：

目前，地球大部分地域是海洋，海洋不仅蕴藏着丰富的资源，而且随着科技的进步，各国的科学家对海洋进行未知探索。水下机器人不仅应用于海洋，还可以应用于河流、湖泊，水下机器人在水产养殖方面也有重要贡献，它灵活的潜入水底通过自身携带的传感器进行实时监测水质状况，包括PH值、溶解氧、浊度、温度、电导率等。基于这些原因，各国相继开展了水下机器人方面的研究，水下机器人技术得到了快速发展。

随着对水下机器人研究的不断深入，出现了一系列定位的方法，有特征点标记法、惯性定位导航法等，但是都存在一定的局限性，特征标记法利用声波回声定位选取最近的三个点，再利用三点定位法计算出机器人的位置，这种方法需要对实验区域提前布置，不能适应于各种环境；惯性定位导航法可以在任意区域，但是随着距离的增长，误差会不断累加。

技术实现要素：

本发明提供一种克服上述问题或者至少部分地解决上述问题的一种水下机器人定位导航系统及定位导航方法。

根据本发明的一个方面，提供一种水下机器人定位导航系统，包括水下机器人和控制台，所述水下机器人位于水面下，所述控制台位于地面或船舰上，所述水下机器人包括定位信息采集模块、避障信息采集模块和控制模块；

所述定位信息采集模块，用于采集水下机器人在水面下的位置坐标信息，并发送给控制模块；

所述避障信息采集模块，用于对水下障碍物进行检测，并将障碍物检测信息发送给控制模块；

所述控制模块，用于根据水下机器人的位置坐标信息和水下障碍物检测信息，控制水下机器人前行；

所述控制台，用于对水下机器人的位置坐标信息和水下障碍物检测信息进行显示。

在上述技术方案的基础上，本发明还可以作如下改进。

进一步的，所述定位信息采集模块包括GPS接收器、多普勒声纳传感器、惯性测量单元和超声波测高仪，所述控制台中包括GPS发射端，所述GPS发射端与所述GPS接收器组成GPS模块；

所述多普勒声纳传感器，用于测量水下机器人的三维运动速度信息；

所述惯性测量单元，用于测量水下机器人的位姿信息和加速度信息；

所述控制模块，用于根据水下机器人的所述三维运动速度信息、加速度信息和水下机器人入水时刻的位置坐标，计算出任意时刻，水下机器人在水面下的第一位置坐标信息(x1，y1，z1)；还用于根据计算出来的水下机器人的第二位置坐标信息(x2，y2，z2)对计算出的水下机器人在水面下的第一位置坐标信息(x1，y1，z1)进行校正，得到校正后的水下机器人的位置坐标(x，y，z)；

所述超声波测高仪，用于测量水下机器人距离水面的高度；

所述GPS模块，用于结合所述超声波测高仪，对水下机器人的第二位置坐标信息(x2，y2，z2)进行计算。

进一步的，所述多普勒声纳传感器中包括发射换能器和接收换能器，所述接收换能器包括多个水听器；

所述发射换能器，用于向水下方发射多束声波信号；

所述接收换能器，用于采用多个水听器接收海底反射的每一束回波信号；

所述多普勒声纳传感器，还用于根据发射的每一束声波信号的频率和接收的相应的每一束回波信号的频率，计算出水下机器人的三维运动速度。

进一步的，所述避障信息采集模块包括红外线传感器和摄像头；

所述红外线传感器，用于对水下机器人前方障碍物的位置坐标信息进行检测；

所述摄像头，用于根据所述障碍物的位置坐标信息，对障碍物的类型进行分析；

所述控制模块，用于根据所述障碍物的位置坐标信息以及障碍物的类型控制水下机器人前行。

根据本发明的另一个方面，提供了一种基于水下机器人定位导航系统的定位导航方法，包括：

采集水下机器人在水面下的位置坐标信息以及水下机器人前方障碍物信息；

根据水下机器人的位置坐标信息和前方障碍物信息，控制水下机器人在水下前行。

进一步的，所述采集水下机器人在水面下的位置坐标信息具体包括：

分别测量水下机器人的三维运动速度和三维加速度；

根据水下机器人的三维运动速度、三维加速度以及水下机器人入水时刻的位置，计算出任意时刻，水下机器人在水面下的第一位置坐标信息(x1，y1，z1)。

进一步的，所述测量水下机器人的三维运动速度包括：

采用多普勒声纳传感器向水下发射多束声波信号，并接收海底反射的相对应的多束回波信号；

根据接收的每一束回波信号的频率与对应的发射的每一束声波信号的频率之间的频移，计算得到水下机器人的三维运动速度。

进一步的，还包括：

通过控制台上的GPS发射端和水下机器人上的GPS接收器，以及超声波测高仪，对水下机器人的第二位置坐标信息(x2，y2，z2)进行计算；

采用所述第二位置坐标信息(x2，y2，z2)校正所述第一位置坐标信息(x1，y1，z1)，得到校正后的水下机器人的位置坐标信息。

进一步的，所述采集水下机器人前方障碍物信息具体包括：

采用红外线传感器测量水下机器人正前方障碍物的位置信息；

根据所述障碍物的位置信息，利用摄像头对障碍物进行拍摄，并根据拍摄得到的障碍物的照片分析障碍物的类型。

进一步的，所述根据水下机器人的位置坐标信息和前方障碍物信息，控制水下机器人在水下前行具体包括：

当水下机器人前方障碍物在安全距离外时，控制水下机器人继续前行；

当水下机器人前方障碍物在安全距离内时，根据所述障碍物的类型，控制水下机器人是继续前行或者转弯避开障碍物。

本发明提供的一种水下机器人定位导航系统及定位导航方法，定位信息采集模块和避障信息采集模块分别对水下机器人的位置信息和障碍物信息进行检测，根据水下机器人的位置坐标信息和水下障碍物检测信息，控制水下机器人前行。本发明利用多传感器信息融合技术实现了全时段高精度定位导航及避障，保证了水下机器人在水面下前行的安全性。

附图说明

图1为本发明一个实施例的水下机器人定位导航系统结构框图；

图2为本发明一个实施例的水下机器人定位导航方法流程图；

图3为多普勒声纳传感器发射四束声波信号的示意图；

图4为多普勒声纳传感器的模型坐标系示意图；

图5为惯性测量单元测量的水下机器人的位姿参数分布图；

图6为通过GPS模块和超声波测高仪测量水下机器人的位置坐标信息的示意图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

参见图1，提供了本发明一个实施例的水下机器人定位导航系统，包括水下机器人和控制台，所述水下机器人位于水面下，所述控制台位于地面或船舰上，水下机器人和控制台之间通过无线信号传输。

其中，水下机器人包括定位信息采集模块、避障信息采集模块和控制模块。定位信息采集模块，用于采集水下机器人在水面下的位置坐标信息，并发送给控制模块；避障信息采集模块，用于对水下障碍物进行检测，并将障碍物检测信息发送给控制模块；控制模块，用于根据水下机器人的位置坐标信息和水下障碍物检测信息，控制水下机器人前行；控制台，用于对水下机器人的位置坐标信息和水下障碍物检测信息进行显示。

在上述实施例的基础上，本发明的一个实施例中，所述定位信息采集模块包括GPS接收器、多普勒声纳传感器、惯性测量单元和超声波测高仪，所述控制台中包括GPS发射端，所述GPS发射端与所述GPS接收器组成GPS模块。

其中，所述多普勒声纳传感器，用于测量水下机器人的三维运动速度信息；所述惯性测量单元，用于测量水下机器人的位姿信息和加速度信息；所述控制模块，用于根据水下机器人的所述三维运动速度信息、加速度信息和水下机器人入水时刻的位置坐标，计算出任意时刻，水下机器人在水面下的第一位置坐标信息(x1，y1，z1)；还用于根据计算出来的水下机器人的第二位置坐标信息(x2，y2，z2)对计算出的水下机器人在水面下的第一位置坐标信息(x1，y1，z1)进行校正，得到校正后的水下机器人的位置坐标；所述超声波测高仪，用于测量水下机器人距离水面的高度；所述GPS模块，用于结合所述超声波测高仪，对水下机器人的第二位置坐标信息(x2，y2，z2)进行计算。

在上述各实施例的基础上，本发明的一个实施例中，所述多普勒声纳传感器中包括发射换能器和接收换能器，所述接收换能器包括多个水听器。其中，发射换能器，用于向水下方发射多束声波信号；接收换能器，用于采用多个水听器接收海底反射的每一束回波信号；多普勒声纳传感器，还用于根据发射的每一束声波信号的频率和接收的每一束回波信号的频率，计算出水下机器人的三维运动速度。其中，惯性测量单元可以直接得到水下机器人的位姿信息和三维加速度信息。

在上述各个实施例的基础上，本发明的一个实施例中，避障信息采集模块包括红外线传感器和摄像头。其中，红外线传感器，用于对水下机器人前方障碍物的位置坐标信息进行检测；摄像头，用于根据所述障碍物的位置坐标信息，对障碍物的类型进行分析；控制模块，用于根据所述障碍物的位置坐标信息以及障碍物的类型控制水下机器人前行。

参见图2，提供了本发明一个实施例的基于水下机器人的定位导航系统的定位导航方法，包括采集水下机器人在水面下的位置坐标信息以及水下机器人前方障碍物信息；根据水下机器人的位置坐标信息和前方障碍物信息，控制水下机器人在水下前行。

本实施例提供的是基于上述各实施例的水下机器人的定位导航系统进行的定位导航方法，通过多传感器对水下机器人的位置信息和水下机器人前方的障碍物信息，根据水下机器人的位置信息和前方障碍物信息，控制水下机器人在水下前行。

在上述实施例的基础上，本发明的一个实施例中，分别测量水下机器人的三维速度和三维加速度；根据水下机器人的三维速度、三维加速度以及水下机器人入水时刻的位置坐标信息，计算出任意时刻，水下机器人在水面下的第一位置坐标信息(x1，y1，z1)。

其中，采用多普勒声纳传感器测量水下机器人在水下行进的三维运动速度，以及采用惯性测量单元测量水下机器人的位姿信息和三维加速度。

多普勒声纳传感器中包括发射换能器和接收换能器，发射换能器向水下方同时发射四束声波波束，可参见图3，每一束声波波束与水平面夹角均为θ，每两束声波波束之间的夹角均为90°。接收换能器采用4个水听器来接收海底返回的四束回波信号。根据每一个水听器接收的回波信息的频率与对应的发射的每一束声波波束频率之间的频移，来计算水下机器人的三维运动速度。

具体为，多普勒声纳传感器的发射换能器向水下发射四束声波波束，每一束声波波束与水平面的夹角为θ，声波发射的频率都为f0，接收换能器接收到的四束回波的频率为f1、f2、f3和f4,则四束回波的频移为Δf1、Δf2、Δf3和Δf4，设水下机器人在每一个声波波束方向上的速度为V1、V2、V3和V4，则根据如下公式可计算得到V1、V2、V3和V4。

本实施例中的四束声波波束两两之间的夹角为90°，即为正交配置，根据四波束多普勒声纳原理建立空间三维坐标，可参见图4，其中OX,OY,OZ为坐标系正方向，则水下机器人的三维速度可通过如下公式计算得到：

在实际对水下机器人的三维运动速度进行测量的过程中，由于水下环境复杂，即使当某一方向的水听器没有接收到回波信号，利用另外三个方程可以算出水下机器人的三维运动速度，从而保证了一些复杂、特殊情况下能够有效地工作，提高了系统的可靠性。

采用惯性测量单元测得水下机器人的位姿信息，如图5所示，水下机器人的位姿信息包括与X轴方向的夹角α、与Y轴方向的夹角β以及与Z轴方向的夹角γ，以及通过惯性测量单元还可以测量得到水下机器人在三维方向上的三轴加速度a1、a2和a3。假设水下机器人入水时刻的位置坐标为(0,0,0)，那么在任意时刻t，水下机器人在水下的第一位置坐标(X,Y,Z)可以根据下面的计算公式得到：

在上述各个实施例的基础上，本发明的一个实施例中，还包括：通过控制台上的GPS发射端和水下机器人上的GPS接收器，以及超声波测高仪，对水下机器人的第二位置坐标信息(x2，y2，z2)进行计算；采用所述第二位置坐标信息(x2，y2，z2)校正所述第一位置坐标信息(x1，y1，z1)，得到校正后的水下机器人的位置坐标信息。

其中，通过多普勒声纳传感器和惯性测量单元的测量数据计算得到的水下机器人的位置坐标的精确度不是很高，因此，本实施例采用GPS模块和超声波测高仪来对水下机器人的第二位置坐标(x2，y2，z2)进行计算，由于采用GPS模块定位出的水下机器人的位置坐标更准确，因此，利用计算出的水下机器人的第二位置坐标(x2，y2，z2)对计算出的第一位置坐标(x1，y1，z1)进行校正，得到校正后的水下机器人的位置坐标信息(x，y，z)，利用校正后的水下机器人的位置坐标信息(x，y，z)控制水下机器人的前行。

其中，计算水下机器人的第二位置坐标的过程为，在控制台上安装一个GPS发射端，在水下机器人上安装一个GPS接收器。其中，控制台处的GPS发射端在位于不同位置坐标的三个点向GPS接收端发射信号，GPS接收端在位于同一个位置接收信号，可参见图6。

根据GPS无线传输原理如下：

如图6所示，其中L1,L2,L3是三个信号发射点到信号接收端的距离，h是超声波测高仪所测的水下机器人距离水面的高度，h即为上述公式中的Z，由上述公式可联立得到水下机器人在水下的第二位置坐标(X2，Y2，Z2)。

采用计算得到的水下机器人在水下的第二位置坐标(x2，y2，z2)校正计算得到的第一位置坐标(x1，y1，z1)，由于采用GPS信号计算得到的水下机器人的位置坐标的数据要比采用多普勒声纳传感器和超声波测高仪测量得到的水下机器人的位置坐标数据要稀疏一些，但是精确度要高一些，因此，采用通过GSP信号计算的水下机器人的多个第二位置坐标(x2，y2，z2)去替换更新计算得到的水下机器人的很多个第一位置坐标(x1，y1，z1)中对应的位置坐标，将更新后的水下机器人的位置坐标作为最后计算得到的水下机器人的位置坐标(x，y，z)。

在上述各个实施例的基础上，本发明的另一个实施例中，所述采集水下机器人前方障碍物信息具体包括：采用红外线传感器测量水下机器人正前方障碍物的位置信息；根据所述障碍物的位置信息，利用摄像头对障碍物进行拍摄，并根据拍摄得到的障碍物的照片分析障碍物的类型。

其中，利用红外线传感器向水下机器人前方发射红外线，当水下机器人前方有障碍物时，会将红外线反射回来，根据红外传感器测距原理利用时间t和光在水中的传播速度V求得障碍物距离水下机器人的距离d，其中，可由下述公式计算求得距离d：

求得了障碍物距离水下机器人的距离d后，判断该障碍物是否在水下机器人的安全距离之外，当水下机器人前方障碍物在安全距离外时，控制模块控制水下机器人继续前行。当水下机器人前方障碍物在安全距离内时，则将水下机器人的前方障碍物的位置信息传输给摄像头，根据障碍物位置信息，摄像头会聚焦这一点并提取连续的几张图片，该摄像头的视频录制格式为P制，每秒25帧，然后对图片特征进行分析判断障碍物是小型生物还是大型生物，如果是小型生物就继续前行，否则就转弯避开障碍物。

本发明提供的一种水下机器人的定位导航系统及定位导航方法，定位信息采集模块和避障信息采集模块分别对水下机器人的位置信息和障碍物信息进行检测，根据水下机器人的位置坐标信息和水下障碍物检测信息，控制水下机器人前行，多传感器信息融合技术实现了全时段高精度定位导航及避障，保证了水下机器人在水面下前行的安全性。

最后，本申请的方法仅为较佳的实施方案，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。