本实用新型属于海洋装备领域，涉及水下机器人导航技术，特别涉及一种水下机器人多设备组合导航系统。
背景技术：
由于海洋海面下环境恶劣，人类的潜水深度受到海水外压等因素影响只能在一定深度内下潜，潜水作业深度有限，水下机器人日益成为海洋资源勘探、海洋生物研究、海洋环境观测、水下目标搜寻与救助打捞等开发海洋的重要工具。
导航设备是水下机器人获取自身定位信息、实现路径规划和导航的基础设备之一，与陆基和空中使用的机器人不同，由于无线电波在水中传播衰减迅速，传统的卫星导航无法在水下机器人装备中使用。
目前水下机器人大多采用惯性导航装置、计程仪、全球卫星定位系统、声学定位系统等设备进行导航定位，声学定位系统需要外界船只跟踪或海域中有较多的声定位信标配合，声学定位作业距离有限，限制了水下机器人使用的灵活性且提高了水下机器人的保障要求，但惯性导航装置在无其他导航设备补偿时定位误差随着水下机器人在水下航行时间逐步累积和发散，因此水下机器人惯性导航装置一般与计程仪配合使用；计程仪在一定水深范围正常打底时输出的速度误差较小，但随着水下机器人向深远海航行，计程仪将无法打底而切换成对流工作模式，对流工作模式时计程仪测试精度急剧下降，直接影响计程仪与惯性导航装置的组合导航定位精度；目前大多水下机器人会采取上浮至水面通过全球卫星定位系统进行校准以消除水下长时间航行带来的导航误差，但由于需要频繁上浮和下潜，将浪费水下机器人能源和有效作业时间，同时降低了水下机器人的隐蔽性。
因此水下机器人长时间在水下自主航行，若需要减少上浮至水面带来的隐蔽性降低、能源消耗增加和作业时间减少，高精度的组合导航技术始终是水下机器人在大深度海域航行需要解决的主要关键技术之一。
cn110044378a记载了一种用于水下深潜器的光纤捷联惯性导航高精度定位系统及方法，包括全球定位系统模块、捷联惯性导航接收模块、多普勒计程仪、地形地磁传感器、信息融合滤波模块、导航解算模块，所述全球定位系统模块用于在载体入水前确定载体速度、位置及姿态信息；捷联惯性导航接收模块用于在载体入水后测量载体速度、位置及姿态信息；多普勒计程仪用于测量载体速度，地形地磁传感器用于测量载体的位置、姿态信息；信息融合滤波模块用于对捷联惯性导航接收模块输出的载体速度、位置及姿态信息与多普勒计程仪、地形地磁传感器输出的载体速度、位置及姿态信息进行融合、滤波校正；导航解算模块用于将捷联惯性导航接收模块输出的数据和融合校正处理后的数据进行解算处理，得到准确的导航速度、位置、姿态信息。
cn110068331a记载了一种水下导航定位设备及系统，包括：水听器、处理器以及声源发生器；声源发生器预先固定在水下设备所在水域内的指定位置处，声源发生器用于发送调制声波信号，调制声波信号中携带有发送所述调制声波信号的时间信息；水听器设置在水下设备的外表面上，水听器用于探测声源发生器发送的调制声波信号；处理器与水听器电连接，处理器用于根据水听器接收的调制声波信号中的时间信息确定声源发生器与水听器间的距离，根据声源发生器的位置信息以及声源发生器与水听器间的距离，确定水下设备的水下位置。
cn105787489a记载了一种一种基于水下地貌图像的匹配导航算法，采用基于水下地貌图像的匹配导航算法，将实时图和待与实时图匹配的基准图都转化为圆形模板，提取两个圆形模板的不变矩特征作为匹配的内容；以实时图与基准图之间的不变矩特征向量的欧氏距离作为相似性度量；欧氏距离最小时所对应的基准图的位置即为实时图的匹配位置；采用粒子群优化算法代替遍历的搜索方式；利用多种群思想改进粒子群优化算法；搜索到实时图的匹配位置后，利用最大互相关法确定航向角。
2008年10月的《指挥控制与仿真》期刊上的文章《gps坐标系的转换及其在姿态求解中的应用》在欧拉角旋转的基础上推导了导航坐标系以及载体坐标系之间的转换关系，推导出相应的转换矩阵，并在此基础上对载体求定的方法进行了推导与说明。
技术实现要素：
本实用新型所要解决的技术问题是提供一种水下机器人多设备组合导航系统及方法，针对目前常见水下导航技术存在的问题，将捷联惯性导航装置，全球卫星定位系统，多普勒计程仪，深度计，海流计，推进电机,电子海图，导航计算机等多个设备组合使用，抑制水下机器人深海航行时，计程仪对流模式测试精度不高导致的捷联惯性导航装置与计程仪组合导航精度不高问题，显著提高水下机器人深水区航行的导航精度。
为解决上述技术问题，本实用新型所采用的技术方案是：一种水下机器人多设备组合导航系统，它包括捷联惯性导航装置，其特征在于，捷联惯性导航装置与全球卫星定位系统、多普勒计程仪、深度计、海流计、推进电机相连；
全球卫星定位系统用于在水下机器人在岸基或布放船只上未下潜前实时获得水下机器人的绝对位置和速度等信息，并发送给捷联惯性导航装置进行粗对准和精对准，获取水下机器人初始经纬度信息并解算姿态矩阵，或上浮后获得水下机器人的绝对位置和速度等信息消除惯性导航装置长时间导航误差；
多普勒计程仪用于在水下机器人布放入水后实时获得水下机器人对水底的绝对速度信息和离底高度信息，多普勒计程仪与捷联惯性导航装置形成水下机器人速度阻尼反馈机制；
深度计用于实时采集水下机器人航行深度信息，并发送给捷联惯性导航装置，深度计与捷联惯性导航装置形成水下机器人高度方向阻尼反馈机制；
海流计用于采集水下机器人在水面下海流速度和方向，并发送给导航捷联惯性导航装置，用于修正推进电机转速静水下对应的航速误差；
推进电机，输出矩推动水下机器人航行，并发送推进电机转速给捷联惯性导航装置，该转速和水下机器人航行速度对应关系可根据理论和水下机器人水下实际航行经验得到；在多普勒计程仪和捷联惯性导航装置组合导航时，以多普勒计程仪和捷联惯性导航装置组合导航时输出的速度信息作为基准，可对推进电机加海流计推断的水下机器人航行速度进行速度刻度因子在线计算，可进一步优化和提升推进电机转速与海流计形成的模型精度。
上述的捷联惯性导航装置包含陀螺仪和加速度计，陀螺仪用于测量水下机器人的角速度率，加速度计用于测量水下机器人的速度信息。
上述的捷联惯性导航装置内前有电子海图，可根据经纬度信息查询水下机器人当前位置的水深、航线障碍物等海图信息，以验证捷联惯性导航装置组合导航计算是否正常。
使用上述系统的一种导航方法，上述的捷联惯性导航装置内置推进电机转速与水下机器人静水匀速航行速度的对应关系，当水下机器人水下航行时，捷联惯性导航装置可根据推进电机转速和接收到海流计输出的海流速度和海流方向信息，可组合多普勒计程仪数据推算出水下机器人x、y轴航行速度。
水下机器人在浅水航行、多普勒计程仪对底工作时，捷联惯性导航装置与多普勒计程仪、深度计组合导航；水下机器人在深水航行、多普勒计程仪无法有效对底工作时，捷联惯性导航装置与推进电机和海流计、深度计组合导航，由于多普勒计程仪打底深度较低，随着水下机器人航行水域深度加深，一旦超过多普勒计程仪最大打底深度，多普勒计程仪将工作在对流模式，对流模式下测试精度急剧下降从而导致多普勒计程仪与捷联惯性导航装置组合导航精度急剧下降。这时捷联惯性导航装置将根据自身纯惯性导航计算的水下机器人位置信息、姿态信息和速度信息，结合接收的推进电机转速信息、海流计输出的海流速度和方向综合对比，经过卡尔曼滤波后形成精确的组合导航定位信息。
上述导航系统工作的具体步骤为：
步骤s1：水下机器人操控设备操控捷联惯性导航装置上电，自检通过后开始工作。
步骤s2：捷联惯性导航装置根据全球卫星定位系统输出的经纬度、速度等信息进行初始对准，完成捷联惯性导航装置自身的姿态、位置和速度推算并向水下机器人航行控制中心输出导航信息。
步骤s3：捷联惯性导航装置接收到多普勒计程仪输出信息后，在浅水区航行时多普勒计程仪对底工作，捷联惯性导航装置与多普勒计程仪、深度计组合导航，同时对推进电机加海流计输出的组合速度进行误差在线标定，计算出推进电机加海流计输出的组合速度刻度因子。
步骤s4：当水下机器人航行到深水区超过多普勒计程仪打底深度时，捷联惯性导航装置与推进电机加海流计、深度计组合导航。
步骤s5：组合导航输出前调用捷联惯性导航装置内置的电子海图，对水下机器人当前经纬度下的深度、航线障碍物等海图信息等信息进行校验。
步骤s6：校验通过后，捷联惯性导航装置向水下机器人航行控制中心输出组成导航信息，当航行到设定的校准点时，水下机器人上浮到水面进行校准。
步骤s7：当水下机器人自主航行到航路终点或作业完成时，水下机器人上浮等待回收，完成组合导航。
上述的当水下机器人水下航行时，捷联惯性导航装置可根据组合的多普勒计程仪数据推算出水下机器人x、y轴载体坐标系航行速度
上述步骤s3的具体计算过程为：
在载体坐标系b系与导航坐标系n系间的变换矩阵为
假设水下机器人在航行中遇到地理坐标系下海流流速为vc，海流流向ψc，则流速vc在载体坐标系下的分量为
vxc＝vccos(ψc-φ)
vyc＝vcsin(ψc-φ)
则水下机器人在载体坐标系下的速度分量
vx＝vxtz vxc＝vtzcosφ vccos(ψc-φ)
vy＝vytz vxc＝vtzsinφ vcsin(ψc-φ)
在静水和水下机器人平衡时其测试误差来源主要来源于刻度因素误差和随机测量误差，若考虑刻度因子误差和随机误差，推进电机转速与速度值可表示为：
式中k为标度因素误差，为随机常数，δvtz为随机测量误差；
与目前通用的水下机器人导航系统相比，本发明的有益效果是：
1)应用前景广阔，本发明中水下机器人多设备组合导航系统在大多数海洋应用领域，如水下勘探与搜寻、水下大深度远距离科考、海洋地质调查与深海生物学研究等提供重要工程应用技术支持，因为水下机器人自身的高精度导航是水下机器人自身作业和航行的基础和前提。
2)解决大深度导航精度差问题，本发明利用多普勒计程仪在浅水区测速精度高，但针对多普勒计程仪对流时测试精度低的问题，利用推进电机转速对应航速基本恒定、海流计输出的海流速度和方向综合计算，经过卡尔曼滤波后形成精确的组合导航定位信息。当校准发现航路偏差后，惯性导航装置内嵌安装的电子海图软件可进一步验证导航输出的正确与否，提高了组合导航系统的精度。
3)减少能源消耗和作业时间，本发明中利用水下机器人深海航行时推进电机转速对应航速基本恒定、海流计输出的海流速度，经过卡尔曼滤波后形成精确的组合导航精度，可减少水下机器人上浮校准次数增加其隐蔽性降，并减少了频繁上浮下潜引起的能源消耗和有效作业时间减少。
附图说明
下面结合附图和实施例对本发明作进一步说明：
图1为本实用新型的多设备组合导航系统组成图；
图2为本实用新型中多设备组合导航原理示意图；
图3为本实用新型中多设备组合导航控制解算流程图；
图4为捷联惯性导航装置的连接示意图。
图中：捷联惯性导航装置1、全球卫星定位系统2、多普勒计程仪3、深度计4、海流计5、推进电机6、电子海图7。
具体实施方式
如图1中，一种水下机器人多设备组合导航系统，它包括捷联惯性导航装置1，其特征在于，捷联惯性导航装置1与全球卫星定位系统2、多普勒计程仪3、深度计4、海流计5、推进电机6相连；
全球卫星定位系统2用于在水下机器人在岸基或布放船只上未下潜前实时获得水下机器人的绝对位置和速度等信息，并发送给捷联惯性导航装置1进行粗对准和精对准，获取水下机器人初始经纬度信息并解算姿态矩阵，或上浮后获得水下机器人的绝对位置和速度等信息消除惯性导航装置长时间导航误差；
多普勒计程仪3用于在水下机器人布放入水后实时获得水下机器人对水底的绝对速度信息和离底高度信息，多普勒计程仪3与捷联惯性导航装置1形成水下机器人速度阻尼反馈机制；
深度计4用于实时采集水下机器人航行深度信息，并发送给捷联惯性导航装置1，深度计4与捷联惯性导航装置1形成水下机器人高度方向阻尼反馈机制；
海流计5用于采集水下机器人在水面下海流速度和方向，并发送给导航捷联惯性导航装置1，用于修正推进电机转速静水下对应的航速误差；
推进电机6，输出矩推动水下机器人航行，并发送推进电机7转速给捷联惯性导航装置1，该转速和水下机器人航行速度对应关系可根据理论和水下机器人水下实际航行经验得到；在多普勒计程仪3和捷联惯性导航装置1组合导航时，以多普勒计程仪3和捷联惯性导航装置1组合导航时输出的速度信息作为基准，可对推进电机6加海流计5推断的水下机器人航行速度进行速度刻度因子在线计算，可进一步优化和提升推进电机6转速与海流计5形成的模型精度。
上述捷联惯性导航装置1装置中电路结构采用通用架构，其惯性传感器采用中星测控公司cs-imu-10惯性测量单元，集成了x、y、z三轴陀螺仪和加速度计，捷联惯性导航装置计算机板采用深圳盛博的scm9032系统核心模块 sem/csdcan总线接口模块，基于英特尔n455处理器、can通讯接口、串口和网口数据通信接口，可内置操作系统。
上述全球卫星定位系统2为市售的产品，例如深圳北天通讯的bs-7953ngps接收器。
上述多普勒计程仪3为市售的产品，例如nortek公司的dvl500khz计程仪。
上述的深度计4为市售的产品，例如seabird公司sbe50数字式压力传感器。
上述的海流计5为市售的产品，例如trdi公司os-38相控阵海洋调查型adcp。
上述推进电机6为市售的产品，例如济南预立直驱电机有限公司水下500米电机。
如图4中，上述的捷联惯性导航装置1包含陀螺仪和加速度计，陀螺仪用于测量水下机器人的角速度率，加速度计用于测量水下机器人的速度信息。
如图1中，上述的捷联惯性导航装置1内前有电子海图7，可根据经纬度信息查询水下机器人当前位置的水深、航线障碍物等海图信息，以验证捷联惯性导航装置1组合导航计算是否正常。
如图3、4中所示，使用上述系统的一种导航方法，上述的捷联惯性导航装置1内置推进电机6转速与水下机器人静水匀速航行速度的对应关系，当水下机器人水下航行时，捷联惯性导航装置1可根据推进电机6转速和接收到海流计5输出的海流速度和海流方向信息，可组合多普勒计程仪3数据推算出水下机器人x、y轴航行速度。
水下机器人在浅水航行、多普勒计程仪对底工作时，捷联惯性导航装置1与多普勒计程仪、深度计4组合导航；水下机器人在深水航行、多普勒计程仪无法有效对底工作时，捷联惯性导航装置1与推进电机6和海流计5、深度计4组合导航，由于多普勒计程仪3打底深度较低，随着水下机器人航行水域深度加深，一旦超过多普勒计程仪3最大打底深度，多普勒计程仪3将工作在对流模式，对流模式下测试精度急剧下降从而导致多普勒计程仪3与捷联惯性导航装置1组合导航精度急剧下降。这时捷联惯性导航装置1将根据自身纯惯性导航计算的水下机器人位置信息、姿态信息和速度信息，结合接收的推进电机转速信息、海流计输出的海流速度和方向综合对比，经过卡尔曼滤波后形成精确的组合导航定位信息。
上述导航系统工作的具体步骤为：
步骤s1：水下机器人操控设备操控捷联惯性导航装置1上电，自检通过后开始工作。
步骤s2：捷联惯性导航装置1根据全球卫星定位系统2输出的经纬度、速度等信息进行初始对准，完成捷联惯性导航装置1自身的姿态、位置和速度推算并向水下机器人航行控制中心输出导航信息。
步骤s3：捷联惯性导航装置1接收到多普勒计程仪3输出信息后，在浅水区航行时多普勒计程仪3对底工作，捷联惯性导航装置1与多普勒计程仪3、深度计4组合导航，同时对推进电机6加海流计5输出的组合速度进行误差在线标定，计算出推进电机6加海流计5输出的组合速度刻度因子。
步骤s4：当水下机器人航行到深水区超过多普勒计程仪3打底深度时，捷联惯性导航装置1与推进电机6加海流计5、深度计4组合导航。
步骤s5：组合导航输出前调用捷联惯性导航装置1内置的电子海图7，对水下机器人当前经纬度下的深度、航线障碍物等海图信息等信息进行校验。
步骤s6：校验通过后，捷联惯性导航装置1向水下机器人航行控制中心输出组成导航信息，当航行到设定的校准点时，水下机器人上浮到水面进行校准。
步骤s7：当水下机器人自主航行到航路终点或作业完成时，水下机器人上浮等待回收，完成组合导航。
上述的当水下机器人水下航行时，捷联惯性导航装置1可根据组合的多普勒计程仪3数据推算出水下机器人x、y轴航行速度
上述步骤s3的具体计算过程为：
在载体坐标系b系与导航坐标系n系间的变换矩阵为
假设水下机器人在航行中遇到地理坐标系下海流流速为vc，海流流向ψc，则流速vc在载体坐标系下的分量为
vxc＝vccos(ψc-φ)
vyc＝vcsin(ψc-φ)
则水下机器人在载体坐标系下的速度分量
vx＝vxtz vxc＝vtzcosφ vccos(ψc-φ)
vy＝vytz vxc＝vtzsinφ vcsin(ψc-φ)
在静水和水下机器人平衡时其测试误差来源主要来源于刻度因素误差和随机测量误差，若考虑刻度因子误差和随机误差，推进电机转速与速度值可表示为：
式中k为标度因素误差，为随机常数，δvtz为随机测量误差；
得出推进电机转速与海流计测量的海流速度刻度因子后，即可以(1 k)vtz作为捷联惯性导航装置1在深水航行中的推进电机转速推算的航行速度阻尼，可以避免由于深水航行时多普勒计程仪对流模式下测试精度急剧下降从而导致多普勒计程仪3与捷联惯性导航装置1组合导航精度急剧下降的问题。
1.一种水下机器人多设备组合导航系统，它包括捷联惯性导航装置（1），其特征在于，捷联惯性导航装置（1）与全球卫星定位系统（2）、多普勒计程仪（3）、深度计（4）、海流计（5）、推进电机（6）相连；
所述全球卫星定位系统（2），用于在水下机器人未下潜前或上浮后实时获得水下机器人的绝对位置信息和速度信息，并发送给捷联惯性导航装置（1）；
所述多普勒计程仪（3），用于在水下机器人布放入水后实时获得水下机器人对水底的绝对速度信息和离底高度信息，并发送给捷联惯性导航装置（1）；
所述深度计（4），用于实时采集水下机器人航行深度信息，并发送给捷联惯性导航装置（1）；
所述海流计（5），用于采集水下机器人在水面下海流速度和方向，并发送给导航捷联惯性导航装置（1）；
所述推进电机（6），输出扭矩推动水下机器人航行，并发送推进电机（6）转速给捷联惯性导航装置（1）。
2.根据权利要求1所述的一种水下机器人多设备组合导航系统，其特征是：所述的捷联惯性导航装置（1）包含陀螺仪和加速度计，陀螺仪用于测量水下机器人的角速度率，加速度计用于测量水下机器人的速度信息。
3.根据权利要求2所述的一种水下机器人多设备组合导航系统，其特征是：所述的捷联惯性导航装置（1）内置有电子海图（7），可根据经纬度信息查询水下机器人当前位置水深、航线障碍物等海图信息，以验证捷联惯性导航装置（1）组合导航计算是否正常。
技术总结