[发明专利]一种水下机器人定位导航系统及定位导航方法

权利要求书

1.一种水下机器人定位导航系统，其特征在于，包括水下机器人和控制台，所述水下机器人位于水面下，所述控制台位于地面或船舰上，所述水下机器人与所述控制台通过无线传输，所述水下机器人包括定位信息采集模块、避障信息采集模块和控制模块；

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

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

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

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

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

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

所述惯性测量单元，用于测量水下机器人三维加速度信息；

所述控制模块，用于根据水下机器人的所述三维运动速度信息、三维加速度信息和水下机器人入水时刻的位置坐标，计算出任意时刻，水下机器人在水面下的第一位置坐标信息(x1，y1，z1)；还用于根据计算出来的水下机器人的第二位置坐标信息(x2，y2，z2)对计算出的水下机器人在水面下的第一位置坐标信息(x1，y1，z1)进行校正，得到校正后的水下机器人的位置坐标(x3，y3，z3)；其中，对于不同时刻，对应有多个第一位置坐标信息(x1，y1，z1)和多个第二位置坐标信息(x2，y2，z2)，采用多个第二位置坐标信息(x2，y2，z2)替换对应时刻的多个第一位置坐标信息(x1，y1，z1)，得到校正后的水下机器人的多个位置坐标(x3，y3，z3)；

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

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

其中，控制台处的GPS发射端在位于不同位置坐标的三个点(x4,y4,z4)、(x5,y5,z5)和(x6,y6,z6)向GPS接收端发射信号，GPS接收端在位于同一个位置接收信号，根据GPS无线传输原理如下：

其中，L1,L2,L3是三个信号发射点到信号接收端的距离，h是超声波测高仪所测的水下机器人距离水面的高度，h＝z4＝z5＝z6，由上述公式可联立得到水下机器人在水下的第二位置坐标(x2，y2，z2)。

2.如权利要求1所述的定位导航系统，其特征在于，所述多普勒声纳传感器中包括发射换能器和接收换能器，所述接收换能器包括多个水听器；

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

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

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

3.如权利要求1所述的定位导航系统，其特征在于，所述避障信息采集模块包括红外线传感器和摄像头；

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

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

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