[发明专利]一种基于多源信息融合的小型水下多机器人协同定位方法及系统

说明书

一种基于多源信息融合的小型水下多机器人协同定位方法及系统，属于多机器人协同定位技术领域，用以解决小型水下机器人因尺寸小、供能有限而无法使用光纤陀螺、多普勒(DVL)及水声定位系统进行定位的问题。本发明将基于压力传感器的两台机器人垂直距离信息和基于环视立体感知装置即双目视觉定位的机器人三维空间位置信息融合，获取精确的水下机器人空间位置，在特殊的水下环境中，不需要依赖较大功率且比较笨重的定位设备，解决了小型水下机器人因尺寸小、供能有限而无法使用光纤陀螺、多普勒(DVL)及水声定位系统进行定位的问题，有效提高了小型水下多机器人相对协同定位的精度和鲁棒性。本发明为小型两栖机器人协同编队控制提供理论基础。

技术领域

本发明涉及多机器人协同定位技术领域，具体涉及一种基于多源信息融合的小型水下多机器人协同定位方法及系统。

背景技术

近年来，受自然界鱼群编队游行及鸟群编队飞行启发，研究人员提出相对协同定位技术。机器人对外感知及通信范围有限，在有效测量范围内，机器人通过视觉、红外等传感器对相邻近机器人进行感知或定位，通过多次跳跃式通信机制传递位置及姿态信息，可以实现小型多机器人相对协同定位。

2014年，受自然界鸟群编队飞行启发，瑞士苏黎世联邦理工大学研究人员采用ARToolkit码对无人机进行标识，利用机载视觉感知传感器和通信设备结合卡尔曼滤波算法对多无人机进行相对位姿估计，进而完成完全分布式的Leader-follower编队飞行控制，进行了室内机室外编队飞行试验。2015和2016年，针对无人机GPS定位受限的环境，宾夕法尼亚大学研究人员提出一种基于无人机相对定位的无人机群系统，无需借助额外全局定位系统，无人机通过机载单目视觉对相邻贴有标识的无人机进行定位。在真实环境中，完成领航跟随编队稳定实验、无人机群系统稳定和监控场景的部署实验。

为实现自主小型AUV近距离编队跟踪系统，2015年，西班牙赫罗纳大学研究人员提出了基于视觉和主动信号灯的多AUV近距离相对协同定位系统，领航者AUV下方搭载一台全景视觉相机，跟随者AUV机体上方固定四个主动信号灯，信号灯的机器人上的位置固定且已知。领航者AUV通过视觉手段对领航者AUV信号灯检测并进行位置和姿态估计。但是基于信号灯的定位方法需要检测到所有信号灯，如果单个信号灯检测错误或者被遮挡，将无法对领航者AUV进行定位。

由于水下环境的特殊性，声波的衰减快，机器人潜入水中将无法接收GPS信号，卫星定位导航系统应用受限。绝对定位法(水声定位法、惯性/航位推算法、海底地形匹配等方法)依赖的水声通信设备、惯导设备、DVL和声呐设备都需要较大功率，且比较笨重，无法应用于小型两栖多机器人。

发明内容

鉴于以上问题，本发明提出一种基于多源信息融合的小型水下多机器人协同定位方法及系统，用以解决小型水下机器人因尺寸小、供能有限而无法使用光纤陀螺、多普勒(DVL)及水声定位系统进行定位的问题。

根据本发明一方面，提出一种基于多源信息融合的小型水下多机器人协同定位方法，该方法包括下述步骤：

步骤一、获取传感器数据；其中，所述传感器数据包括定位机器人水下压力传感器值、被定位机器人水下压力传感器值、包含被定位机器人的图像序列；

步骤二、根据定位机器人水下压力传感器值和被定位机器人水下压力传感器值计算获得定位机器人和被定位机器人之间的垂直距离；

步骤三、根据包含被定位机器人的图像序列计算获得被定位机器人的三维空间位置坐标；

步骤四、将定位机器人和被定位机器人之间的垂直距离与被定位机器人的三维空间位置坐标进行信息融合，获得被定位机器人最终的三维空间位置。

进一步地，步骤二的具体过程包括：

步骤二一、建立定位机器人和被定位机器人压力差与垂直距离的线性方程；

定位机器人和被定位机器人之间的压力差与垂直距离的关系式为：