CN112665584A - 一种基于多传感器融合的水下机器人定位与构图方法 - Google Patents

一种基于多传感器融合的水下机器人定位与构图方法 Download PDF

Info

Publication number
CN112665584A
CN112665584A CN202011601957.3A CN202011601957A CN112665584A CN 112665584 A CN112665584 A CN 112665584A CN 202011601957 A CN202011601957 A CN 202011601957A CN 112665584 A CN112665584 A CN 112665584A
Authority
CN
China
Prior art keywords
road sign
sonar
underwater robot
camera
sequence
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202011601957.3A
Other languages
English (en)
Other versions
CN112665584B (zh
Inventor
严浙平
王迪
吴迪
刘睿
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202011601957.3A priority Critical patent/CN112665584B/zh
Publication of CN112665584A publication Critical patent/CN112665584A/zh
Application granted granted Critical
Publication of CN112665584B publication Critical patent/CN112665584B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明属于水下机器人定位技术领域，具体涉及一种基于多传感器融合的水下机器人定位与构图方法。本发明克服了水下环境恶劣而导致传感器无法精确使用的问题，利用水下机器人安装的视觉传感器和声觉传感器，结合传感器工作原理，优化所得的路标信息，再与惯性测量元件相互作用下，获得水下机器人的航行状态，进而达成传感器融合的效果。本发明通过相机、声呐和惯性测量元件三个传感器融合，通过紧耦合的方式对水下机器人的状态估计和周围环境路标进行了滤除干扰和优化，使得水下机器人在航行任务的过程中，能够根据融合声、光传感器的数据信息，达到优化路标的目的来提升水下航行的续航时间，增强了同步定位与构图系统的实时性和鲁棒性。

Description

一种基于多传感器融合的水下机器人定位与构图方法
技术领域
本发明属于水下机器人定位技术领域，具体涉及一种基于多传感器融合的水下机器人定位与构图方法。
背景技术
水下定位技术是水下机器人领域的重要研究方向，是使水下机器人进行有效任务作业的关键。当水下机器人处于一个完全陌生的环境时，需要具体的环境信息用以自主航行，但由于水下环境的复杂多变，仅凭单成本传感器无法实现高精度的航海任务。
发明内容
本发明的目的在于克服水下环境恶劣而导致传感器无法精确使用的问题，提供一种基于多传感器融合的水下机器人定位与构图方法。
本发明的目的通过如下技术方案来实现：包括以下步骤：
步骤1：获取相机在时间间隔t内测量的路标序列
Figure BDA0002871346620000011
路标序列
Figure BDA0002871346620000012
中的路标
Figure BDA0002871346620000013
为相机测量的第i个路标点在大地坐标系下的坐标；
步骤2：根据声呐在时间间隔t内测量的路标的物理信息z＝[γj,θj]，计算声呐在时间间隔t内测量的路标序列
Figure BDA0002871346620000014
路标序列
Figure BDA0002871346620000015
中的路标
Figure BDA0002871346620000016
为声呐测量的第j个路标点在大地坐标系下的坐标，
Figure BDA0002871346620000017
Figure BDA0002871346620000018
其中，γj为声呐到第j个路标点的距离；θj为声呐与第j个路标点的角度；TWS为声呐坐标系到大地坐标系下的转移矩阵；
步骤3：计算声呐在时间间隔t内测量的路标序列
Figure BDA0002871346620000019
的均值，得到均值路标
Figure BDA00028713466200000110
步骤4：根据路标序列
Figure BDA00028713466200000111
中各路标
Figure BDA00028713466200000112
与均值路标
Figure BDA00028713466200000113
之间的马氏距离
Figure BDA00028713466200000114
对路标序列
Figure BDA00028713466200000115
进行筛选，仅保留满足
Figure BDA00028713466200000116
的路标
Figure BDA00028713466200000117
Figure BDA00028713466200000118
其中，S为相机和声纳路标之间的协方差矩阵；‖DΔl‖2为设定的信赖区间；
步骤5：根据筛选后的路标序列
Figure BDA00028713466200000119
结合惯性测量元件进行水下机器人的定位与构图。
从筛选后的相机测量的路标序列
Figure BDA00028713466200000120
中选取特征点用来定位；
ZPuv＝KPw
其中，K为相机内参；Z为路标到相机的距离，通过相机成像原理获得；Puv为特征点的像素坐标；Pw为水下机器人的坐标。
本发明的有益效果在于：
本发明克服了水下环境恶劣而导致传感器无法精确使用的问题。本发明利用水下机器人安装的视觉传感器和声觉传感器，结合传感器工作原理，优化所得的路标信息；再与惯性测量元件相互作用下，获得水下机器人的航行状态，进而达成传感器融合的效果。本发明通过相机、声呐和惯性测量元件三个传感器融合，通过紧耦合的方式对水下机器人的状态估计和周围环境路标进行了滤除干扰和优化，使得水下机器人在航行任务的过程中，能够根据融合声、光传感器的数据信息，达到优化路标的目的来提升水下航行的续航时间，增强了同步定位与构图系统的实时性和鲁棒性。
附图说明
图1本发明的整体框架图。
图2本发明中多传感器融合的流程图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明属于水下机器人定位领域，具体涉及水下机器人在航行时对于同步定位与构图的精确性问题，通过对多传感器获取的里程计进行数据融合，避免了由于水下强干扰等因素使单个传感器无法精确使用的问题，用以满足水下机器人长时间的航海作业的需求。
本发明的目的是克服水下环境恶劣而导致传感器无法精确使用的问题，提供一种基于多传感器融合的水下机器人定位与构图方法。本发明利用了水下机器人安装的视觉传感器和声觉传感器，结合传感器工作原理，优化所得的路标信息；再与惯性测量元件相互作用下，获得水下机器人的航行状态，进而达成传感器融合的效果。本发明通过相机、声呐和惯性测量元件三个传感器融合，通过紧耦合的方式对水下机器人的状态估计和周围环境路标进行了滤除干扰和优化，提高了定位与构图的快速性和鲁棒性。本发明使得水下机器人在航行任务的过程中，能够根据融合声、光传感器的数据信息，达到优化路标的目的来提升水下航行的续航时间，增强了同步定位与构图系统的实时性和鲁棒性。
当水下机器人处于无先验信息的环境时，根据水下机器人安装的传感元件，通过对传感器数据进行读取，分析所传入的信息用来研究水下机器人的运动等状态。见图1为整体方案结构图。
对于视觉传感器相机来说，需要根据一张张连续运动的图像，从中推断相机的运动，以及周围环境的情况。因此通过相机拍摄的照片作为视觉里程计，其中照片中的像素点可作为图像特征点用来标记路标环境。
对于声学传感器声呐来说，由声学传感器声呐通过发出并接受声波可以获取周遭标志物的物理位置，声呐所测得的周围环境物理信息序列对视觉所测得的图像信息中路标点进行优化运算，在给定一个信赖区间内，通过计算上述传感器间的差异，获得所需优化后的路标点，进而将从路标点中选出特征点。
单独使用相机作为传感器时，由于相机受图像遮挡、运动物体干扰等缺点，本质上无法得知相机自身发生了运动，还是外界条件发生了变化，因此很难处理动态的障碍物。当运动过快时，相机会出现运动模糊，或者两帧间距重叠区域太少以至于无法进行特征匹配，因此相机也有无法匹配到快速运动的缺点。而单独使用惯性测量元件时，虽然可以测得角速度和加速度，但是这些量都存在着明显的漂移，使得积分两次得到的位姿数据非常不可靠。有存在零点漂移和积分位姿容易发散等问题的出现都不利于机器人的应用。于是，上述两个传感器存在一定互补的性质，既可利用视觉定位信息来估计慢速运动下惯性测量元件的零偏，减少由零偏导致的发散和累计误差，又可将惯性测量元件为视觉提供快速运动时的定位。
因此，通过对惯性测量元件得出的数据进行预积分处理后，采用的融合方案为紧耦合。之所以不采用松耦合方案是因为：松耦合方案中的视觉内部没有全局优化惯性测量元件的信息，在整体层面来看不是最优解。
结合图2，水下机器人的同步定位与构图方法流程图如下：
如图2所示，对于视觉传感器相机来说，需要根据一张张连续运动的图像，从中推断相机的运动，以及周围环境的情况。某一时间间隔下，通过双目相机获得视觉里程计下的路标序列记为
Figure BDA0002871346620000031
声学传感器收发的声学信息为周围环境的物理测量值，声呐测量物理信息为z＝[γ,θ]，其中γ为声呐到声学路标的距离，θ为路标的角度。因此可以获得在大地坐标系下路标的坐标为
ls＝TWS[γcos(θ),γsin(θ),0]
其中，TWS为声呐坐标系到大地坐标系下的转移矩阵。同一时间间隔下，通过声呐获得的路标序列分别记为
Figure BDA0002871346620000032
由于声呐的工作机制可知，在发射声波和接收声波之间存在一个小的时间差t，以该时间差t为单位，在若干倍时间差的时间间隔内，求得声呐所测量的路标序列
Figure BDA0002871346620000033
的均值为
Figure BDA0002871346620000034
其中，时间差的倍数不宜过大，容易造成精度不准。
通过相机得到路标序列
Figure BDA0002871346620000035
和声呐均值
Figure BDA0002871346620000036
所得的距离的差值
Figure BDA0002871346620000037
满足
Figure BDA0002871346620000038
即为满足优化条件的路标。
在某一时间间隔下的路标间的差值用马氏距离
Figure BDA0002871346620000041
表示，其中S为相机和声纳路标之间的协方差矩阵。
对于给定的信赖区间为‖DΔl‖2，D为系数矩阵，若D为单位阵，则相当于把Δl约束于球中，若D为非负对角阵，则相当于约束到椭球中，通过调节信赖区间的系数来确认路标范围。
给定一个序列
Figure BDA0002871346620000042
若有满足优化条件的视觉序列
Figure BDA0002871346620000043
则
Figure BDA0002871346620000044
所得序列
Figure BDA0002871346620000045
为优化后的路标序列，通过该序列进行构图。
得到优化后的相机序列，从中选取特征点用来定位。已知相机内参为K，Z为路标到相机的距离，可通过相机成像原理获得。特征点的像素坐标为Puv，Pw为机器人的坐标，即为所求机器人位姿信息。
ZPuv＝KPw
对于惯性测量元件和优化后的特征点融合到状态中，采用紧耦合的方式。机器人的位姿状态xR＝[pT,qT,vT,bg,ba]T，其中p为机器人的位置，q为机器人的姿态，v为惯性测量元件的线速度，bg和ba分别为陀螺仪和加速度计的偏差。对应的误差状态向量表示为δxR＝[δqT,δqT,δvT,δbg,δba]T，将所有状态都视为优化变量，采用最小化重投影误差的方式进行优化。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于多传感器融合的水下机器人定位与构图方法，其特征在于，包括以下步骤：
步骤1：获取相机在时间间隔t内测量的路标序列
Figure FDA0002871346610000011
路标序列
Figure FDA0002871346610000012
中的路标
Figure FDA0002871346610000013
为相机测量的第i个路标点在大地坐标系下的坐标；
步骤2：根据声呐在时间间隔t内测量的路标的物理信息z＝[γj，θj]，计算声呐在时间间隔t内测量的路标序列
Figure FDA0002871346610000014
路标序列
Figure FDA0002871346610000015
中的路标
Figure FDA0002871346610000016
为声呐测量的第j个路标点在大地坐标系下的坐标，
Figure FDA0002871346610000017
Figure FDA0002871346610000018
其中，γj为声呐到第j个路标点的距离；θj为声呐与第j个路标点的角度；TWS为声呐坐标系到大地坐标系下的转移矩阵；
步骤3：计算声呐在时间间隔t内测量的路标序列
Figure FDA0002871346610000019
的均值，得到均值路标
Figure FDA00028713466100000120
步骤4：根据路标序列
Figure FDA00028713466100000110
中各路标
Figure FDA00028713466100000111
与均值路标
Figure FDA00028713466100000112
之间的马氏距离
Figure FDA00028713466100000113
对路标序列
Figure FDA00028713466100000114
进行筛选，仅保留满足
Figure FDA00028713466100000115
的路标
Figure FDA00028713466100000116
Figure FDA00028713466100000117
其中，S为相机和声纳路标之间的协方差矩阵；||DΔl||2为设定的信赖区间；
步骤5：根据筛选后的路标序列
Figure FDA00028713466100000118
结合惯性测量元件进行水下机器人的定位与构图。
从筛选后的相机测量的路标序列
Figure FDA00028713466100000119
中选取特征点用来定位；
ZPuv＝KPw
其中，K为相机内参；Z为路标到相机的距离，通过相机成像原理获得；Puv为特征点的像素坐标；Pw为水下机器人的坐标。
CN202011601957.3A 2020-12-30 2020-12-30 一种基于多传感器融合的水下机器人定位与构图方法 Active CN112665584B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011601957.3A CN112665584B (zh) 2020-12-30 2020-12-30 一种基于多传感器融合的水下机器人定位与构图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011601957.3A CN112665584B (zh) 2020-12-30 2020-12-30 一种基于多传感器融合的水下机器人定位与构图方法

Publications (2)

Publication Number Publication Date
CN112665584A true CN112665584A (zh) 2021-04-16
CN112665584B CN112665584B (zh) 2022-07-15

Family

ID=75410561

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011601957.3A Active CN112665584B (zh) 2020-12-30 2020-12-30 一种基于多传感器融合的水下机器人定位与构图方法

Country Status (1)

Country Link
CN (1) CN112665584B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114228959A (zh) * 2021-12-29 2022-03-25 中国科学院沈阳自动化研究所 一种基于声学路标与光学路标组合辅助导航的水下机器人极地冰下回收方法
CN115100298A (zh) * 2022-08-25 2022-09-23 青岛杰瑞工控技术有限公司 一种用于深远海视觉养殖的光-声图像融合方法
CN115242881A (zh) * 2022-08-01 2022-10-25 湖南大学无锡智能控制研究院 基于声光混合通信的多水下机器人任务分配方法及系统
CN116540238A (zh) * 2022-09-07 2023-08-04 长江水利委员会水文局 基于测深数据及声学影像融合的水下岸坡工情监测方法

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4031544A (en) * 1975-08-11 1977-06-21 Edo Western Corporation Sonar/television system for use in underwater exploration
CN105352496A (zh) * 2015-11-17 2016-02-24 中国海洋大学 基于声呐辅助自主导航的auv导航方法
CN105787489A (zh) * 2016-03-04 2016-07-20 哈尔滨工程大学 一种基于水下地貌的匹配导航算法
CN105841687A (zh) * 2015-01-14 2016-08-10 上海智乘网络科技有限公司 室内定位方法和系统
CN108344999A (zh) * 2018-01-09 2018-07-31 浙江大学 一种用于水下机器人导航的声呐地图构建与重定位方法
US10043076B1 (en) * 2016-08-29 2018-08-07 PerceptIn, Inc. Visual-inertial positional awareness for autonomous and non-autonomous tracking
US20190007673A1 (en) * 2011-08-04 2019-01-03 Trx Systems, Inc. Mapping and tracking system with features in three-dimensional space
CN109543694A (zh) * 2018-09-28 2019-03-29 天津大学 一种基于点特征稀疏策略的视觉同步定位与地图构建方法
CN111444777A (zh) * 2020-03-03 2020-07-24 哈尔滨工程大学 一种同时标记阴影的前视声呐目标检测标记方法
CN111486845A (zh) * 2020-04-27 2020-08-04 中国海洋大学 基于海底地形匹配的auv多策略导航方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4031544A (en) * 1975-08-11 1977-06-21 Edo Western Corporation Sonar/television system for use in underwater exploration
US20190007673A1 (en) * 2011-08-04 2019-01-03 Trx Systems, Inc. Mapping and tracking system with features in three-dimensional space
CN105841687A (zh) * 2015-01-14 2016-08-10 上海智乘网络科技有限公司 室内定位方法和系统
CN105352496A (zh) * 2015-11-17 2016-02-24 中国海洋大学 基于声呐辅助自主导航的auv导航方法
CN105787489A (zh) * 2016-03-04 2016-07-20 哈尔滨工程大学 一种基于水下地貌的匹配导航算法
US10043076B1 (en) * 2016-08-29 2018-08-07 PerceptIn, Inc. Visual-inertial positional awareness for autonomous and non-autonomous tracking
CN108344999A (zh) * 2018-01-09 2018-07-31 浙江大学 一种用于水下机器人导航的声呐地图构建与重定位方法
CN109543694A (zh) * 2018-09-28 2019-03-29 天津大学 一种基于点特征稀疏策略的视觉同步定位与地图构建方法
CN111444777A (zh) * 2020-03-03 2020-07-24 哈尔滨工程大学 一种同时标记阴影的前视声呐目标检测标记方法
CN111486845A (zh) * 2020-04-27 2020-08-04 中国海洋大学 基于海底地形匹配的auv多策略导航方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
ANTHONY SPEARS 等: "Acoustic sonar and video sensor fusion for landmark detection in an under-ice environment", 《2014 OCEANS-ST. JOHN"S》, 8 January 2015 (2015-01-08), pages 1 - 8 *
ZHEPING YAN 等: "Using Conditional Adversarial Networks to Deblur the Sonar Image of the Unknown Motion Blur Kernels", 《IOP CONFERENCE SERIES: MATERIALS SCIENCE AND ENGINEERING》, 31 December 2019 (2019-12-31), pages 1 - 9 *
严浙平 等: "基于前视声呐的水下移动障碍物运动参数预测", 《华中科技大学学报（自然科学版）》, vol. 47, no. 5, 31 May 2019 (2019-05-31), pages 105 - 109 *
赵亮 等: "基于视觉同时定位与地图构建数据关联优化算法", 《计算机应用》, vol. 34, no. 2, 10 February 2014 (2014-02-10), pages 576 - 579 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114228959A (zh) * 2021-12-29 2022-03-25 中国科学院沈阳自动化研究所 一种基于声学路标与光学路标组合辅助导航的水下机器人极地冰下回收方法
CN115242881A (zh) * 2022-08-01 2022-10-25 湖南大学无锡智能控制研究院 基于声光混合通信的多水下机器人任务分配方法及系统
CN115242881B (zh) * 2022-08-01 2023-06-13 湖南大学无锡智能控制研究院 基于声光混合通信的多水下机器人任务分配方法及系统
CN115100298A (zh) * 2022-08-25 2022-09-23 青岛杰瑞工控技术有限公司 一种用于深远海视觉养殖的光-声图像融合方法
CN116540238A (zh) * 2022-09-07 2023-08-04 长江水利委员会水文局 基于测深数据及声学影像融合的水下岸坡工情监测方法
CN116540238B (zh) * 2022-09-07 2023-10-13 长江水利委员会水文局 基于测深数据及声学影像融合的水下岸坡工情监测方法

Also Published As

Publication number Publication date
CN112665584B (zh) 2022-07-15

Similar Documents

Publication Publication Date Title
CN112665584B (zh) 一种基于多传感器融合的水下机器人定位与构图方法
US10788830B2 (en) Systems and methods for determining a vehicle position
CN109710724B (zh) 一种构建点云地图的方法和设备
US6778928B2 (en) Method of calibrating a sensor system
Lee et al. Robust mobile robot localization using optical flow sensors and encoders
CN107909614B (zh) 一种gps失效环境下巡检机器人定位方法
CN108375370A (zh) 一种面向智能巡防无人机的复合导航系统
US20080319664A1 (en) Navigation aid
CN111121754A (zh) 移动机器人定位导航方法、装置、移动机器人及存储介质
JP2022518911A (ja) 乗り物のセンサとカメラアレイからの構造化された地図データの生成
WO2015173034A1 (en) Method and system for determining a position relative to a digital map
CN108692701B (zh) 基于粒子滤波器的移动机器人多传感器融合定位方法
WO2020146102A1 (en) Robust lane association by projecting 2-d image into 3-d world using map information
CN110533719B (zh) 基于环境视觉特征点识别技术的增强现实定位方法及装置
CN111025366B (zh) 基于ins及gnss的网格slam的导航系统及方法
CN110388919B (zh) 增强现实中基于特征图和惯性测量的三维模型定位方法
KR20200083301A (ko) 이동하는 오브젝트 센서의 정렬을 교정하기 위한 방법
CN108603933A (zh) 用于融合具有不同分辨率的传感器输出的系统和方法
CN112819711B (zh) 一种基于单目视觉的利用道路车道线的车辆反向定位方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
Khoshelham et al. Vehicle positioning in the absence of GNSS signals: Potential of visual-inertial odometry
KR100564236B1 (ko) 이동 로봇의 자기위치인식 장치 및 방법
CN111402702A (zh) 地图构建方法、装置及系统
CN115585805A (zh) 车辆定位方法、定位装置、定位系统以及车辆
CN114646993A (zh) 一种基于gnss、视觉以及imu进行精确定位的数据融合算法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant