水下机器人定位方法技术

本发明专利技术提供一种水下机器人定位方法

【技术实现步骤摘要】
水下机器人定位方法、装置、电子设备和存储介质


[0001]本专利技术涉及视觉图像
，尤其涉及一种水下机器人定位方法
、
装置
、
电子设备和存储介质
。

技术介绍

[0002]水下机器人在海洋资源勘探和近海平台检修中发挥着越来越重要的作用
。
为了顺利完成任务，水下机器人对自身进行准确定位至关重要
。
传统的定位方法通常利用声呐
、
惯性测量单元
(Inertial Measurement Unit
，
IMU)、
深度传感器和多普勒测速仪
(Doppler Velocity Log
，
DVL)
等传感器进行水下机器人的导航和状态估计
。
近年来，由于视觉传感器能够以较低的成本提供丰富的图像信息，基于视觉的同步定位建图
(Visual
‑
Based Simultaneous Localization and Mapping
，
V
‑
SLAM)
系统受到广泛关注，水下视觉同步定位建图
(Simultaneous Localization and Mapping
，
SLAM)
系统也在许多研究中得到了验证
。
[0003]DVL
可以通过发射声波信号并测量它们从底部反射回来时的多普勒频移来获得
X
‑/>Y
‑
Z
方向的线速度，这些测量值包含位置变化的信息，可用于机器人的位置估计
。
虽然已经有一些工作尝试将视觉传感器与
DVL
传感器进行融合，但这些方法是将
DVL
速度测量值与陀螺仪和深度传感器融合，由于采用了进行累积计算的航迹推算方法，不可避免地导致了误差累积问题，因此，这些融合方法不能在视觉里程计的基础上显著提高定位精度
。

技术实现思路

[0004]针对现有技术存在的问题，本专利技术提供一种水下机器人定位方法
、
装置
、
电子设备和存储介质
。
[0005]第一方面，本专利技术提供一种水下机器人定位方法，包括：
[0006]基于多普勒测速仪
DVL
的速度测量数据，获取机器人在第一帧时刻的第一速度分量以及在第二帧时刻的
DVL
误差项；所述第一帧时刻为所述第二帧时刻的上一帧时刻；
[0007]基于所述机器人在所述第一帧时刻的第一速度分量，确定所述机器人在所述第二帧时刻的初始位姿估计值；
[0008]基于所述机器人在所述第二帧时刻的
DVL
误差项，对所述机器人在所述第二帧时刻的初始位姿估计值进行优化，确定所述机器人在所述第二帧时刻的最终位姿估计值
。
[0009]可选地，所述基于
DVL
的速度测量数据，获取机器人在第一帧时刻的第一速度分量，包括：
[0010]基于所述机器人在所述第一帧时刻的第二速度分量与所述机器人从视觉坐标系变换到
DVL
坐标系的旋转分量的乘积
、
所述机器人从所述
DVL
坐标系变换到所述视觉坐标系的平移分量与所述
DVL
在所述第一帧时刻与所述第二帧时刻之间在所述
DVL
坐标系中的位移之间的差值以及所述机器人从所述视觉坐标系变换到所述
DVL
坐标系的平移分量，获取所述机器人在所述第一帧时刻的第一速度分量
。
[0011]可选地，所述机器人在所述第一帧时刻的第一速度分量基于以下公式确定：
[0012][0013]其中，所述表示所述机器人在所述第一帧时刻的第一速度分量，所述表示所述机器人在所述第一帧时刻的第二速度分量，所述表示所述机器人从所述视觉坐标系变换到所述
DVL
坐标系的旋转分量，所述表示所述机器人从所述
DVL
坐标系变换到所述视觉坐标系的平移分量，所述表示所述
DVL
在所述第一帧时刻与所述第二帧时刻之间在所述
DVL
坐标系中的位移，所述表示所述机器人从所述视觉坐标系变换到所述
DVL
坐标系的平移分量
。
[0014]可选地，所述基于所述机器人在所述第一帧时刻的第一速度分量，确定所述机器人在所述第二帧时刻的初始位姿估计值，包括：
[0015]基于所述机器人在所述第一帧时刻的第一速度分量
、
所述机器人在所述第一帧时刻的第二速度分量和所述机器人在所述第一帧时刻的最终位姿估计值，确定所述机器人在所述第二帧时刻的初始位姿估计值
。
[0016]可选地，所述基于
DVL
的速度测量数据，获取机器人在第二帧时刻的
DVL
误差项，包括：
[0017]基于所述
DVL
在所述第一帧时刻与所述第二帧时刻之间在
DVL
坐标系中的位移之间的差值
、
所述机器人从视觉坐标系变换到所述
DVL
坐标系的旋转分量
、
所述机器人在所述第一帧时刻的旋转分量
、
所述机器人在所述第二帧时刻相对于所述视觉坐标系的旋转分量
、
所述机器人从所述视觉坐标系变换到所述
DVL
坐标系的平移分量
、
所述机器人在所述第二帧时刻相对于所述视觉坐标系的平移分量
、
所述机器人在所述第一帧时刻的平移分量
、
所述机器人从所述
DVL
坐标系变换到所述视觉坐标系的平移分量，获取所述机器人在所述第二帧时刻的
DVL
误差项
。
[0018]可选地，所述机器人在所述第二帧时刻的
DVL
误差项基于以下公式确定：
[0019][0020]其中，所述表示所述机器人在所述第二帧时刻的
DVL
误差项，所述表示所述
DVL
在所述第一帧时刻与所述第二帧时刻之间在所述
DVL
坐标系中的位移，所述表示所述机器人从所述视觉坐标系变换到所述
DVL
坐标系的旋转分量，所述表示所述机器人在所述第一帧时刻的旋转分量，所述表示所述机器人在所述第二帧时刻相对于所述视觉坐标系的旋转分量，所述表示所述机器人从所述视觉坐标系变换到所述
DVL
坐标系的平移分量，所述表示所述机器人在所述第二帧时刻相对于所述视觉坐标系的平移分量，所述表示所述机器人在所述第一帧时刻的平移分量，所述表示所述机器人从所述
DVL
坐标系变换到所述视觉坐标系的平移分量
。
[0021]可选地，所述本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
一种水下机器人定位方法，其特征在于，包括：基于多普勒测速仪
DVL
的速度测量数据，获取机器人在第一帧时刻的第一速度分量以及在第二帧时刻的
DVL
误差项；所述第一帧时刻为所述第二帧时刻的上一帧时刻；基于所述机器人在所述第一帧时刻的第一速度分量，确定所述机器人在所述第二帧时刻的初始位姿估计值；基于所述机器人在所述第二帧时刻的
DVL
误差项，对所述机器人在所述第二帧时刻的初始位姿估计值进行优化，确定所述机器人在所述第二帧时刻的最终位姿估计值
。2.
根据权利要求1所述的水下机器人定位方法，其特征在于，所述基于
DVL
的速度测量数据，获取机器人在第一帧时刻的第一速度分量，包括：基于所述机器人在所述第一帧时刻的第二速度分量与所述机器人从视觉坐标系变换到
DVL
坐标系的旋转分量的乘积
、
所述机器人从所述
DVL
坐标系变换到所述视觉坐标系的平移分量与所述
DVL
在所述第一帧时刻与所述第二帧时刻之间在所述
DVL
坐标系中的位移之间的差值以及所述机器人从所述视觉坐标系变换到所述
DVL
坐标系的平移分量，获取所述机器人在所述第一帧时刻的第一速度分量
。3.
根据权利要求2所述的水下机器人定位方法，其特征在于，所述机器人在所述第一帧时刻的第一速度分量基于以下公式确定：其中，所述表示所述机器人在所述第一帧时刻的第一速度分量，所述表示所述机器人在所述第一帧时刻的第二速度分量，所述表示所述机器人从所述视觉坐标系变换到所述
DVL
坐标系的旋转分量，所述表示所述机器人从所述
DVL
坐标系变换到所述视觉坐标系的平移分量，所述表示所述
DVL
在所述第一帧时刻与所述第二帧时刻之间在所述
DVL
坐标系中的位移，所述表示所述机器人从所述视觉坐标系变换到所述
DVL
坐标系的平移分量
。4.
根据权利要求1所述的水下机器人定位方法，其特征在于，所述基于所述机器人在所述第一帧时刻的第一速度分量，确定所述机器人在所述第二帧时刻的初始位姿估计值，包括：基于所述机器人在所述第一帧时刻的第一速度分量
、
所述机器人在所述第一帧时刻的第二速度分量和所述机器人在所述第一帧时刻的最终位姿估计值，确定所述机器人在所述第二帧时刻的初始位姿估计值
。5.
根据权利要求1所述的水下机器人定位方法，其特征在于，所述基于
DVL
的速度测量数据，获取机器人在第二帧时刻的
DVL
误差项，包括：基于所述
DVL
在所述第一帧时刻与所述第二帧时刻之间在
DVL
坐标系中的位移之间的差值
、
所述机器人从视觉坐标系变换到所述
DVL
坐标系的旋转分量
、
所述机器人在所述第一帧时刻的旋转分量
、
所述机器人在所述第二帧时刻相对于所述视觉坐标系的旋转分量
...

【专利技术属性】
技术研发人员：吴正兴，李朋，黄雨培，闫帅铮，李思捷，谭民，喻俊志，
申请(专利权)人：中国科学院自动化研究所，
类型：发明
国别省市：