CN113744337B - 一种融合视觉、imu与声纳的同步定位与建图方法 - Google Patents
一种融合视觉、imu与声纳的同步定位与建图方法 Download PDFInfo
- Publication number
- CN113744337B CN113744337B CN202111048106.5A CN202111048106A CN113744337B CN 113744337 B CN113744337 B CN 113744337B CN 202111048106 A CN202111048106 A CN 202111048106A CN 113744337 B CN113744337 B CN 113744337B
- Authority
- CN
- China
- Prior art keywords
- sonar
- camera
- point cloud
- image
- imu
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 27
- 238000013507 mapping Methods 0.000 title claims abstract description 20
- 230000001360 synchronised effect Effects 0.000 title claims abstract description 13
- 230000004927 fusion Effects 0.000 claims abstract description 8
- 239000013598 vector Substances 0.000 claims abstract description 8
- 230000000007 visual effect Effects 0.000 claims abstract description 8
- 238000001514 detection method Methods 0.000 claims abstract description 7
- 230000009466 transformation Effects 0.000 claims abstract description 5
- 238000004422 calculation algorithm Methods 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000005259 measurement Methods 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 9
- 238000001914 filtration Methods 0.000 claims description 8
- 238000003384 imaging method Methods 0.000 claims description 8
- 238000006243 chemical reaction Methods 0.000 claims description 7
- 230000008569 process Effects 0.000 claims description 6
- 238000011065 in-situ storage Methods 0.000 claims description 5
- 238000012545 processing Methods 0.000 claims description 4
- 238000013519 translation Methods 0.000 claims description 4
- 238000012795 verification Methods 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000002146 bilateral effect Effects 0.000 claims description 3
- 238000004891 communication Methods 0.000 claims description 3
- 239000013078 crystal Substances 0.000 claims description 3
- 230000007246 mechanism Effects 0.000 claims description 3
- 238000007689 inspection Methods 0.000 claims description 2
- 238000000605 extraction Methods 0.000 abstract description 3
- 230000005484 gravity Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000000295 complement effect Effects 0.000 description 2
- 238000003708 edge detection Methods 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 description 1
- 238000007476 Maximum Likelihood Methods 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000003066 decision tree Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 238000007637 random forest analysis Methods 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
- 230000009469 supplementation Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/86—Combinations of sonar systems with lidar systems; Combinations of sonar systems with systems not using wave reflection
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/23—Clustering techniques
- G06F18/232—Non-hierarchical techniques
- G06F18/2321—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions
- G06F18/23213—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions with fixed number of clusters, e.g. K-means clustering
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/005—Tree description, e.g. octree, quadtree
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/80—Geometric correction
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20024—Filtering details
- G06T2207/20028—Bilateral filtering
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20212—Image combination
- G06T2207/20221—Image fusion; Image merging
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Data Mining & Analysis (AREA)
- Automation & Control Theory (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- General Engineering & Computer Science (AREA)
- Bioinformatics & Computational Biology (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Multimedia (AREA)
- Computer Graphics (AREA)
- Evolutionary Biology (AREA)
- Software Systems (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Probability & Statistics with Applications (AREA)
- Computer Networks & Wireless Communication (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
本发明提供了一种融合视觉、IMU与声纳的同步定位与建图方法,通过求取局部时间与空间场的载体位姿变换再与IMU数据互补,同时利用图像的纹理信息辅助识别与框取环境中的目标信息,形成有界的体素边框,回环部分先利用声纳进行大范围回环,然后在相机图像融合的点云数据中,对已经聚类的目标特征选用三维矢量参数进行定义,建立词袋模型(BoW)用分类器判断,利用k‑d树对目标特征进行快速搜索,完成粗略匹配,最后使用迭代最近点(ICP)完成精确匹配完成回环检测,可以使原本稀疏的声纳点云图像通过与视觉图像融合,获得带有颜色信息的,并能对选定目标信息补充使之成为更易于框取的图像,有利于对特定目标的提取与识别,同时获得被测物体的距离信息。
Description
技术领域
本发明涉及同步定位与建图方法的技术领域,具体涉及一种融合视觉、IMU与声纳的同步定位与建图方法。
背景技术
声纳、视觉与IMU是水下机器人同步定位与建图的重要技术手段,但是由于水下环境的特殊性,视觉能见度距离有限,但可以提供临近场景的丰富环境信息,而声纳可以不受水下浑浊度,湍流等因素的影响,可提供准确的环境目标距离信息,但采样率低,易受水下环境噪声且在近处数据缺失的影响,声纳测量中存在的噪声是阻碍现有的声纳数据与视觉融合的主要原因之一,且数据形式大多是稀疏的,同时利用IMU与视觉互补形成一个传统的VIO,协同提供较高精度的位姿估计,再利用声纳为主视觉辅助融合建图,使之获得包含视觉信息的地图,IMU数据存在零偏,对于静止与缓慢运动测量误差大,但却能对于短时间的快速运动提供较好的估计,相机数据漂移较少,但在快速运动时会出现运动模糊,可以有效的修正IMU数据,因此,将水下声纳、IMU和视觉信息融合可有效提高水下SLAM的准确性与鲁棒性。
发明内容
本发明为了克服声纳点云图像过于稀疏且没有纹理信息且声纳点云图像存在缺失,不利于对特定目标框取识别,声纳点云图由于采样率低,载体运动情况下存在一定的畸变等问题,因此提供了一种融合视觉、IMU与声纳的同步定位与建图方法。
本发明解决其技术问题所采用的技术方案是:
一种融合视觉、IMU与声纳的同步定位与建图方法,包括以下步骤:
S1:进行相机标定,获得相机内参外参,求解声纳坐标系与相机坐标系之间的关系,获得声纳与相机的位姿变换,使相机与声纳实现同空间信息获取,完成联合标定;
S2:设定同步晶振触发方式,使用特定电路硬件记录时间戳信息,根据声纳传感器与摄像头的采样频率设定触发规则,在融合时刻进行同时触发,以解决时间同步问题;
S3:采用与相机松耦合的IMU,用来测量物体在三维空间中的角速度和加速度并对自身做出运动估计,用于运动预测与对基于视觉得出的位姿估计结果融合,其主要方案为基于卡尔曼滤波的方法;
S4:初始化,将相机进行平移,根据相机获取的图像对当前相机姿态进行运动估计,用于在缓慢运动的情况下去除声纳的点云畸变。
S5:对相机图像利用Tenengrad梯度算法进行模糊检验,设定阈值T,当超过阈值时认为存在运动模糊,此时利用IMU完成因相机快速运动造成运动模糊无法进行位姿估计时的运动补偿,完成点云畸变去除;
S6:将声纳点云图像体素栅格滤波,建立八叉树地图,将相机图像特征点映射到声纳点云图像中,从图像的纹理信息分析环境目标特征,构建有界约束框架。
S7:对框取目标的声纳点云图像与相机图像进行分辨率匹配,采用高斯过程回归对目标点云进行插值,完成数据关联获得带有声纳提供深度信息的目标的相机图像,相机针孔模型中,将一个三维点投影到一个二维成像平面,设现实空间中存在一点P坐标为[X,Y,Z]T,投影后P’坐标为[X’,Y’,Z’]T,f为焦距,
像素坐标系为成像平面加上一个缩放α(μ),β(υ)与一个原点平移[Cx,Cy]T,可得P’的像素坐标[μ,υ]T
μ=αX′+Cx (3)
υ=βY′+Cy (4)
将αf合并为fx,把βf合并为fy,得:
在声纳模型中,声纳投影的模型直接建立在球坐标系上
由于声纳会丢失标高信息φ;
坐标系间的转换,将2个传感器用一个3*3的旋转矩阵与一个3维的平移向量完成刚体转换:
Ps=RPc+T (9)
得:
立体通信约束:
未知量φ可以消除:
最终得到:
通过下式,可得到相机像素Zc的场景深度:
此时完成数据关联,获得带有声纳提供深度信息的目标物的相机图像;
S8:使用声纳点云图像进行初步的回环检测,利用进行过插值处理的点云图像建立submap(子图),提取稀疏的点云关键帧与submap匹配,匹配后进行下一步的图像精匹配;
S9:对融合后图像提取稀疏关键帧,针对框架约束的点云信息,采用K-means++算法进行聚类,设定以直线度,平面度,散度这三维为代表的矢量特征建立词袋模型,通过分类器进行判断,利用k-d树对特征目标进行快速搜索,完成第一步匹配,然后通过使用迭代最近点(ICP)完成进一步的匹配,完成回环检测。
本发明的进一步改进在于:对于步骤S4中对当前相机姿态进行估计的操作包括以下步骤:
S4-1:提取ORB特征点;
S4-2:使用FLANN匹配特征点;
S4-3:进行对极几何约束计算获得相机的基础矩阵F与本质矩阵E;
S4-4:对本质矩阵E进行奇异值分解得到运动参数[R,t],从IMU提供的数据中获得初始尺度与重力方向。
本发明的进一步改进在于:所述步骤S5中利用视觉与IMU互补进行运动补偿的操作包括以下步骤:
S5-1:计算相机图像的Tenengrad梯度函数;
S5-2:设定阈值T,判断图像是否存在运动模糊;
S5-3:若无运动模糊,将基于相机图像进行位姿估计,用于去除缓慢运动下的声纳点云畸变;
S5-4:若存在运动模糊,将提取运动模糊前邻近且不连续的三帧图像,与运动模糊后邻近且不连续的三帧图像进行比对,若存在明显相似部分则认为相机运动为原地抖动,此时将利用相似帧进行位姿估计去除声纳点云畸变;若无相似部分,则认为相机做了快速运动,此时将利用IMU进行运动补偿。
本发明的进一步改进在于:所述步骤S6中的相机图像的特征点映射到声纳点云图像实施过程包括以下步骤:
S6-1:获取拥有相同时间戳的声纳点云图像与相机图像;
S6-2:将三维点云进行分割栅格,设定栅格大小,栅格数;
S6-3:对每个栅格内的点云进行双边滤波,去除部分点云,达到向下采样同时保全点云几何结构;
S6-4:将滤波过的声纳点云图像转换到相机坐标系中;
S6-5:运用针孔模型将三维点云映射到图像坐标系中,接着离散化到像素坐标系中;
S6-6:判断二维像素平面内的声纳三维点云是否在二维相机图像平面中;
S6-7:将存在该平面内的三维点云保留并对齐;
S6-8:用RANSAC算法进行校验;
S6-9:完成映射。
本发明的进一步改进在于:步骤S9中的提取稀疏关键帧过程包括以下步骤:
S9-1:设定一个最小关键帧间隔,使关键帧尽量稀疏;
S9-2:检测到回环后,将成功回环的相似关键帧聚类,避免无效回环;
S9-3:设立验证机制,仅当一段时间持续检测到回环,认为回环成功。
本发明的进一步改进在于:步骤S5中若存在运动模糊,且存在明显相似部分则认为相机运动为原地抖动,将利用相似帧进行位姿估计去除声纳点云畸变;若无相似部分,此时将利用IMU进行运动补偿。
本发明的进一步改进在于:所述利用IMU进行运动补偿的操作包括以下步骤:
S5-4-1:积分一个声纳帧扫描起始和结束时刻之间的IMU测量;
S5-4-2:利用每个激光测量点的时间戳对IMU积分结果进行线性插值;
S5-4-3:将每个点投影到激光帧的起始时刻,完成相机数据失效时的运动补偿。
与现有技术相比,本发明的有益效果为:
本发明为一种融合视觉、IMU与声纳的同步定位与建图方法,通过视觉与IMU松耦合,弥补了视觉数据无效时无法获得位姿信息的问题,同时可以使原本稀疏的声纳点云图像通过与视觉图像融合,获得带有颜色信息的,并能对选定目标信息补充使之成为更易于框取的图像,有利于对特定目标的提取与识别,同时获得被测物体的距离信息。
附图说明
图1是本发明方法的流程图;
图2是本发明的传感器联合标定示意图。
具体实施方式
为了使本发明的目的,技术方案与优点更明确,因此对本发明进一步详细说明,一种融合视觉、IMU与声纳的图像处理方法,包括以下步骤:
S1:相机的标定,获得内参外参;相机与声纳的联合标定:将相机标定中的世界坐标系替换为声纳坐标系,求出两者之间的关系,构建极大似然估计模型,用LM算法(Levenberg-Marquardt Algorithm)求得最优解。得到一个刚体旋转,实现相机与声纳数据的对齐,完成联合标定。
S2:声纳的扫描周期约为6s一次,而相机却1s有10帧图像,为了实现数据同步,设定同步晶振触发方式,使用特定电路硬件记录时间戳信息,根据声纳传感器与摄像头的采样频率设定触发规则,在融合时刻进行同时触发,以解决时间同步问题。
S3:采用松耦合的IMU,用来测量物体在三维空间中的角速度和加速度并对自身做出运动估计,用于运动预测与对基于视觉得出的位姿估计结果融合,其主要方案为基于卡尔曼滤波的方法。
S4:初始化,将相机进行一定程度的平移,根据相机获取的图像对当前相机姿态进行估计,操作为在图像中进行ORB特征点的提取,ORB特征点由关键点(Oriented Fast)与描述子(BRIEF)构成。
关键点的尺度信息由构建的图像金字塔进行匹配获得,旋转信息通过计算特征点附近的图像灰度质心获得。
使用FLANN(快速近似最邻近)匹配特征点,FLANN是一个对大数据集合和高维特征进行最近邻近搜索的合集,其中的算法主要包括随机k-d树算法,优先搜索k-means树算法和层次聚类树;
通过对极几何约束构建关于本质矩阵E的方程,先求解得到一个E的初值,用八点法和SVD恢复相机的[R,t],从IMU提供的数据中获得初始尺度与重力方向。
将得到的运动矩阵用于补偿去除当相机缓慢运动情况下的声纳点云图像的畸变。将由于载体运动所造成的基准坐标系不同的每列声纳点的坐标,乘上基于运动矩阵得出的相对转换,转换成在初始坐标系下的声纳点。
S5:计算相机图像的Tenengrad梯度函数,采用Sobel算子提取水平与垂直方向的梯度值,计算像素点处于Sobel水平与垂直方向边缘检测算子的卷积,设定边缘检测阈值T,判断图像是否存在运动模糊;若无运动模糊,将基于相机图像进行位姿估计,用于去除缓慢运动下的声纳点云畸变;若存在运动模糊,将提取运动模糊前邻近且不连续的3帧图像,与运动模糊后邻近且不连续的3帧图像进行比对;若存在明显相似部分则认为相机运动为原地抖动,此时将利用相似帧进行位姿估计去除声纳点云畸变;若无相似部分,则认为相机做了快速运动,此时将利用IMU进行运动补偿。
IMU进行运动补偿,首先积分一个声纳帧扫描起始和结束时刻之间的IMU测量,然后利用每个激光测量点的时间戳对IMU积分结果进行线性插值,从而将每个点投影到声纳帧的起始时刻,实现运动补偿并完成相机快速运动造成运动模糊情况下的点云畸变去除。
S6:对声纳点云图像预处理,去除深度值无效的点,利用统计滤波器方法去除孤立点,将声纳3D点云图像用体素栅格滤波,通过建立一个三维体素格栅,用体素中所有点的重心来表示近似的点,达到向下采样同时保全点云几何结构,使分割栅格更容易,接着设定栅格大小,栅格数,此时每个栅格中都有一定数量的点云,然后采用双边滤波,采取临近采样点的加权平均来修正当前采样点的位置,同时有选择的剔除部分差异过大的相邻采样点,剔除部分点云并建立八叉树地图以便于目标配对。
将视觉图像纹理映射到点云图像上,选取拥有相同时间戳的三维点云和图像,将三维点云转换到相机坐标系中,通过针孔模型将三维点云映射到图像坐标,接着离散化到像素坐标系,确定尺寸范围,三维点云对齐,用RANSAC算法进行校验,完成映射,对于点云图像缺失部分,进行最邻近插值算法(NNIA),将成像数据对应回波点的极坐标值,计算各点在笛卡尔坐标下的位置并将成像数据表示的回波点像素值填充到对应坐标位置,完成插值,从而便于后续视觉图像信息的补全,最终获得了包含视觉信息的地图。
对相机图像进行灰度处理,利用图像梯度算法提取边框实现对目标物的框取,使目标物图像易于提取,通过上述的映射关系得到该目标物的点云信息,并利用二维相机图像完成对目标物点云信息的补充。
S7:对框取目标的声纳点云图像与相机图像进行分辨率匹配,再采用高斯过程回归对目标点云进行插值。
相机针孔模型中,将一个三维点投影到一个二维成像平面,设现实空间中存在一点P坐标为[X,Y,Z]T,投影后P’坐标为[X’,Y’,Z’]T,f为焦距。
像素坐标系为成像平面加上一个缩放α(μ),β(υ)与一个原点平移[Cx,Cy]T,可得P’的像素坐标[μ,v]T
μ=αX′+Cx (3)
υ=βY′+Cy (4)
将αf合并为fx,把βf合并为fy,得:
在声纳模型中,声纳投影的模型直接建立在球坐标系上
由于声纳会丢失标高信息φ
坐标系间的转换,将2个传感器用一个3*3的旋转矩阵与一个3维的平移向量完成刚体转换。
Ps=RPc+T (9)
得:
立体通信约束:
未知量φ可以消除:
最终得到:
通过下式,可得到相机像素Zc的场景深度:
此时完成数据关联,获得带有声纳提供深度信息的目标物的相机图像。
S8:使用声纳点云图像进行初步的回环检测,利用进行过插值处理的点云图像建立submap(子图),提取稀疏的点云关键帧与submap进行匹配,一旦发现匹配就立刻进行下一步的图像精匹配。
S9:对融合后的图像提取关键帧,同时设定一个最小关键帧间隔,使关键帧尽量稀疏,采用K-means++算法对点云进行聚类,从而实现分割,特征提取通过计算聚类Ci的矢量特征fi={fi1,fi2,fi3}建立词袋模型,分别代表直线度,平面度,散度这三维的几何特性。
采用由随机森林算法驱动的分类器进行判断,利用k-d树对所有特征向量进行快速搜索,得到潜在匹配对象,由一系列的决策树分类投票,最终得到匹配的特征目标,完成第一步匹配。
使用迭代最近点(ICP)求解,先对点云进行滤波、清理部分数据等预处理;应用Least Squares来求解变换,找出最近点;调整一些对应点对的权重;剔除不合理对应点对;计算loss,最小化loss,求解当前最优变换;回到第二步进行迭代,直到收敛。完成较为精细的匹配,完成回环检测。
当检测到回环后,将成功回环的相似关键帧聚类,避免无效回环,同时设立验证机制,仅当一段时间持续检测到回环,认为回环成功。
Claims (6)
1.一种融合视觉、IMU与声纳的同步定位与建图方法,其特征在于,包括以下步骤:
S1:进行相机标定,获得相机内参外参,求解声纳坐标系与相机坐标系之间的关系,获得声纳与相机的位姿变换,使相机与声纳实现同空间信息获取,完成联合标定;
S2:设定同步晶振触发方式,使用特定电路硬件记录时间戳信息,根据声纳传感器与摄像头的采样频率设定触发规则,在融合时刻进行同时触发;
S3:采用与相机松耦合的IMU,用来测量物体在三维空间中的角速度和加速度并对自身做出运动估计,用于运动预测与对基于视觉得出的位姿估计结果融合;
S4:初始化,将相机进行平移,根据相机获取的图像对当前相机姿态进行运动估计,用于在缓慢运动的情况下去除声纳的点云畸变;
S5:对相机图像利用Tenengrad梯度算法进行模糊检验,设定阈值T,当超过阈值时认为存在运动模糊,利用IMU完成点云畸变去除;
S6:将声纳点云图像体素栅格滤波,建立八叉树地图,将相机图像特征点映射到声纳点云图像中,构建有界约束框架;
S7:对框取目标的声纳点云图像与相机图像进行分辨率匹配,采用高斯过程回归对目标点云进行插值,完成数据关联获得带有声纳提供深度信息的目标的相机图像,相机针孔模型中,将一个三维点投影到一个二维成像平面,设现实空间中存在一点P坐标为[X,Y,Z]T,投影后P’坐标为[X’,Y’,Z’]T,f为焦距,
像素坐标系为成像平面加上一个缩放α(μ),β(υ)与一个原点平移[Cx,Cy]T,可得P’的像素坐标[μ,υ]T
μ=αX′+Cx (3)
υ=βY′+Cy (4)
将αf合并为fx,把βf合并为fy,得:
在声纳模型中,声纳投影的模型直接建立在球坐标系上
坐标系间的转换,将2个传感器用一个3*3的旋转矩阵与一个3维的平移向量完成刚体转换:
Ps=RPc+T (9)
得:
立体通信约束:
未知量φ可以消除:
最终得到:
通过下式,可得到相机像素Zc的场景深度:
此时完成数据关联,获得带有声纳提供深度信息的目标物的相机图像;
S8:使用声纳点云图像进行初步的回环检测,利用进行过插值处理的点云图像建立submap,提取稀疏的点云关键帧与submap匹配,匹配后进行下一步的图像精匹配;
S9:对融合后图像提取稀疏关键帧,针对框架约束的点云信息,采用K-means++算法进行聚类,设定以直线度,平面度,散度这三维为代表的矢量特征建立词袋模型,通过分类器进行判断,利用k-d树对特征目标进行快速搜索,完成第一步匹配,然后通过使用迭代最近点完成进一步的匹配,完成回环检测。
2.根据权利要求1所述的方法,其特征在于,所述步骤S5中利用视觉与IMU互补进行运动补偿的操作包括以下步骤:
S5-1:计算相机图像的Tenengrad梯度函数;
S5-2:设定阈值T,判断图像是否存在运动模糊;
S5-3:若无运动模糊,将基于相机图像进行位姿估计,用于去除缓慢运动下的声纳点云畸变;
S5-4:若存在运动模糊,将提取运动模糊前邻近且不连续的三帧图像,与运动模糊后邻近且不连续的三帧图像进行比对,若存在明显相似部分则认为相机运动为原地抖动,此时将利用相似帧进行位姿估计去除声纳点云畸变;若无相似部分,则认为相机做了快速运动,此时将利用IMU进行运动补偿。
3.根据权利要求1所述的方法,其特征在于,所述步骤S6中的相机图像的特征点映射到声纳点云图像实施过程包括以下步骤:
S6-1:获取拥有相同时间戳的声纳点云图像与相机图像;
S6-2:将三维点云进行分割栅格,设定栅格大小,栅格数;
S6-3:对每个栅格内的点云进行双边滤波,去除部分点云,达到向下采样同时保全点云几何结构;
S6-4:将滤波过的声纳点云图像转换到相机坐标系中;
S6-5:运用针孔模型将三维点云映射到图像坐标系中,接着离散化到像素坐标系中;
S6-6:判断二维像素平面内的声纳三维点云是否在二维相机图像平面中;
S6-7:将存在该平面内的三维点云保留并对齐;
S6-8:用RANSAC算法进行校验;
S6-9:完成映射。
4.根据权利要求1所述的方法,其特征在于,步骤S9中的提取稀疏关键帧过程包括以下步骤:
S9-1:设定一个最小关键帧间隔,使关键帧尽量稀疏;
S9-2:检测到回环后,将成功回环的相似关键帧聚类,避免无效回环;
S9-3:设立验证机制,仅当一段时间持续检测到回环,认为回环成功。
5.根据权利要求1所述的方法,其特征在于,步骤S5中若存在运动模糊,且存在明显相似部分则认为相机运动为原地抖动,将利用相似帧进行位姿估计去除声纳点云畸变;若无相似部分,此时将利用IMU进行运动补偿。
6.根据权利要求5所述的方法,其特征在于,所述利用IMU进行运动补偿的操作包括以下步骤:
S5-4-1:积分一个声纳帧扫描起始和结束时刻之间的IMU测量;
S5-4-2:利用每个激光测量点的时间戳对IMU积分结果进行线性插值;
S5-4-3:将每个点投影到激光帧的起始时刻,完成相机数据失效时的运动补偿。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111048106.5A CN113744337B (zh) | 2021-09-07 | 2021-09-07 | 一种融合视觉、imu与声纳的同步定位与建图方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111048106.5A CN113744337B (zh) | 2021-09-07 | 2021-09-07 | 一种融合视觉、imu与声纳的同步定位与建图方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113744337A CN113744337A (zh) | 2021-12-03 |
CN113744337B true CN113744337B (zh) | 2023-11-24 |
Family
ID=78737050
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111048106.5A Active CN113744337B (zh) | 2021-09-07 | 2021-09-07 | 一种融合视觉、imu与声纳的同步定位与建图方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113744337B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114241269B (zh) * | 2022-02-18 | 2022-07-08 | 聚时科技(江苏)有限公司 | 一种用于岸桥自动控制的集卡视觉融合定位系统 |
CN114897942B (zh) * | 2022-07-15 | 2022-10-28 | 深圳元戎启行科技有限公司 | 点云地图的生成方法、设备及相关存储介质 |
CN115100298B (zh) * | 2022-08-25 | 2022-11-29 | 青岛杰瑞工控技术有限公司 | 一种用于深远海视觉养殖的光-声图像融合方法 |
CN116339336A (zh) * | 2023-03-29 | 2023-06-27 | 北京信息科技大学 | 电动农机集群协同作业方法、装置及系统 |
CN116468858B (zh) * | 2023-06-19 | 2023-08-15 | 中色蓝图科技股份有限公司 | 一种基于人工智能的地图融合方法及系统 |
CN116844068B (zh) * | 2023-09-01 | 2023-12-26 | 山东省地质矿产勘查开发局第五地质大队(山东省第五地质矿产勘查院) | 建筑测绘方法、系统、计算机设备及存储介质 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112562052A (zh) * | 2020-12-03 | 2021-03-26 | 广东工业大学 | 一种面向近岸水域的实时定位与建图方法 |
CN112946660A (zh) * | 2021-01-28 | 2021-06-11 | 西北工业大学 | 一种基于多波束前视声呐的同时定位与建图方法 |
CN113313172A (zh) * | 2021-05-31 | 2021-08-27 | 江苏科技大学 | 一种基于高斯分布聚类的水下声呐图像匹配方法 |
CN113345018A (zh) * | 2021-05-31 | 2021-09-03 | 湖南大学 | 一种动态场景下的激光单目视觉融合定位建图方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2018048353A1 (en) * | 2016-09-09 | 2018-03-15 | Nanyang Technological University | Simultaneous localization and mapping methods and apparatus |
-
2021
- 2021-09-07 CN CN202111048106.5A patent/CN113744337B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112562052A (zh) * | 2020-12-03 | 2021-03-26 | 广东工业大学 | 一种面向近岸水域的实时定位与建图方法 |
CN112946660A (zh) * | 2021-01-28 | 2021-06-11 | 西北工业大学 | 一种基于多波束前视声呐的同时定位与建图方法 |
CN113313172A (zh) * | 2021-05-31 | 2021-08-27 | 江苏科技大学 | 一种基于高斯分布聚类的水下声呐图像匹配方法 |
CN113345018A (zh) * | 2021-05-31 | 2021-09-03 | 湖南大学 | 一种动态场景下的激光单目视觉融合定位建图方法 |
Non-Patent Citations (1)
Title |
---|
融合激光与视觉点云信息的定位与建图方法;张伟伟;陈超;徐军;;计算机应用与软件(第07期);114-119 * |
Also Published As
Publication number | Publication date |
---|---|
CN113744337A (zh) | 2021-12-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113744337B (zh) | 一种融合视觉、imu与声纳的同步定位与建图方法 | |
CN110223348B (zh) | 基于rgb-d相机的机器人场景自适应位姿估计方法 | |
CA2326816C (en) | Face recognition from video images | |
CN112304307A (zh) | 一种基于多传感器融合的定位方法、装置和存储介质 | |
CN108711166A (zh) | 一种基于四旋翼无人机的单目相机尺度估计方法 | |
CN114424250A (zh) | 结构建模 | |
CN103559711A (zh) | 基于三维视觉系统图像特征和三维信息的运动估计方法 | |
CN112419497A (zh) | 基于单目视觉的特征法与直接法相融合的slam方法 | |
Eichhardt et al. | Affine correspondences between central cameras for rapid relative pose estimation | |
CN111899345B (zh) | 一种基于2d视觉图像的三维重建方法 | |
CN111325828A (zh) | 一种基于三目相机的三维人脸采集方法及装置 | |
CN108021857B (zh) | 基于无人机航拍图像序列深度恢复的建筑物检测方法 | |
CN116188417A (zh) | 基于slam和图像处理的裂缝检测及其三维定位方法 | |
CN114549549B (zh) | 一种动态环境下基于实例分割的动态目标建模跟踪方法 | |
CN116978009A (zh) | 基于4d毫米波雷达的动态物体滤除方法 | |
CN115222884A (zh) | 一种基于人工智能的空间对象分析及建模优化方法 | |
CN117523461B (zh) | 一种基于机载单目相机的运动目标跟踪与定位方法 | |
CN110197104B (zh) | 基于车辆的测距方法及装置 | |
CN113536959A (zh) | 一种基于立体视觉的动态障碍物检测方法 | |
CN109443319A (zh) | 基于单目视觉的障碍物测距系统及其测距方法 | |
CN112767459A (zh) | 基于2d-3d转换的无人机激光点云与序列影像配准方法 | |
CN111160362B (zh) | 一种fast特征均匀化提取与帧间特征误匹配去除方法 | |
Nouduri et al. | Deep realistic novel view generation for city-scale aerial images | |
EP1580684B1 (en) | Face recognition from video images | |
CN116878524A (zh) | 一种基于金字塔l-k光流与多视角几何约束的动态slam稠密地图构建方法 |
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 |