CN116222543A - 用于机器人环境感知的多传感器融合地图构建方法及系统 - Google Patents
用于机器人环境感知的多传感器融合地图构建方法及系统 Download PDFInfo
- Publication number
- CN116222543A CN116222543A CN202310458414.8A CN202310458414A CN116222543A CN 116222543 A CN116222543 A CN 116222543A CN 202310458414 A CN202310458414 A CN 202310458414A CN 116222543 A CN116222543 A CN 116222543A
- Authority
- CN
- China
- Prior art keywords
- map
- point cloud
- data
- laser radar
- cloud data
- 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
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 32
- 238000010276 construction Methods 0.000 title claims abstract description 18
- 230000008447 perception Effects 0.000 title claims abstract description 10
- 238000005259 measurement Methods 0.000 claims abstract description 26
- 238000000034 method Methods 0.000 claims description 47
- 238000005457 optimization Methods 0.000 claims description 29
- 238000000605 extraction Methods 0.000 claims description 16
- 238000012545 processing Methods 0.000 claims description 14
- 238000001914 filtration Methods 0.000 claims description 13
- 239000013598 vector Substances 0.000 claims description 10
- 230000003287 optical effect Effects 0.000 claims description 5
- 238000007781 pre-processing Methods 0.000 claims description 5
- 230000008859 change Effects 0.000 claims description 4
- 238000001514 detection method Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 230000033001 locomotion Effects 0.000 abstract description 9
- 238000013473 artificial intelligence Methods 0.000 abstract description 2
- 230000000007 visual effect Effects 0.000 description 12
- 230000008569 process Effects 0.000 description 10
- 238000004364 calculation method Methods 0.000 description 6
- 238000007500 overflow downdraw method Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 230000007547 defect Effects 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 238000005286 illumination Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 239000011159 matrix material Substances 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 241001270131 Agaricus moelleri Species 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000001960 triggered effect Effects 0.000 description 1
Images
Classifications
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
-
- 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/1652—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 ranging devices, e.g. LIDAR or RADAR
-
- 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
- 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/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/13—Edge detection
-
- 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/269—Analysis of motion using gradient-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
- G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
- G06T7/337—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving reference images or patches
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/50—Depth or shape recovery
- G06T7/55—Depth or shape recovery from multiple images
- G06T7/593—Depth or shape recovery from multiple images from stereo images
-
- 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
- G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
-
- 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/10004—Still image; Photographic image
- G06T2207/10012—Stereo images
-
- 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/20112—Image segmentation details
- G06T2207/20164—Salient point detection; Corner detection
-
- 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/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
-
- 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
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
本发明属于人工智能技术领域,提供了一种用于机器人环境感知的多传感器融合地图构建方法及系统,针对传统需要将数据分布传入不同的SLAM算法当中,本发明通过双目相机采集左右目图像信息,通过双目视觉技术能够估计机器人的姿态和场景的深度信息,构建地图。同时,利用激光雷达获取高精度的深度信息,优化地图,提高地图的准确性和鲁棒性。其次,利用IMU测量信息,可以进行机器人的运动估计和误差补偿,提高四足机器人运动的精度和鲁棒性。最后,将双目相机、激光雷达和IMU传感器的测量信息进行融合,实现了四足机器人更加准确和鲁棒的定位与建图。
Description
技术领域
本发明属于人工智能技术领域,尤其涉及用于机器人环境感知的多传感器融合地图构建方法及系统。
背景技术
本部分的陈述仅仅是提供了与本发明相关的背景技术信息,不必然构成在先技术。
近年来,即时定位和建图(Simultaneous Localization and Mapping,简称SLAM)已经成为四足机器人进行环境感知的一项主流技术,并且目前行业内领先的服务机器人企业,大多数都采用了SLAM技术。在不具备先验信息的环境中,四足机器人搭载视觉等相关传感器后,SLAM可以帮助四足机器人构建室内的环境地图,从而实现自主行走等功能。SLAM技术又分为激光SLAM和视觉SLAM。两者各有其优缺点,纯激光雷达再通过无特征的隧道或笔直的走廊时,性能会变差,甚至系统失败。而视觉SLAM观测范围有限,并且在光照条件不好的情况下,会发生漂移,并且误差非常大。
传统的用于四足机器人环境感知的多传感器融合方法存在的技术问题是:
(1)在一些复杂环境下,比如光线不好、环境变化快、存在动态障碍物等情况下,多传感器融合的结果可能会受到很大的干扰和误差,从而导致整个系统性能下降甚至失效。
(2)传统的激光SLAM与视觉SLAM的地图融合方法仅仅是利用两种SLAM建立后的地图进行融合,而在数据处理过程中,需要将数据分布传入不同的SLAM算法当中,当一种算法失效时,后续的地图构建过程将无法完成。
(3)传统的多传感器融合方法对传感器之间的协作和同步要求较高。在数据采集、传输和处理过程中,各种传感器之间需要严格的同步和协作,一旦出现了数据的不一致或者传感器之间的冲突,就会导致整个系统的错误和故障。另外当多传感器融合方法在处理大规模、高维度数据时,需要高性能的计算平台和算法,但是这样的计算平台和算法往往比较昂贵,不适合普及应用。
发明内容
为了解决上述背景技术中存在的至少一项技术问题,本发明提供用于机器人环境感知的多传感器融合地图构建方法及系统,其通过双目相机采集左右目图像信息,通过双目视觉技术能够估计机器人的姿态和场景的深度信息。同时,利用激光雷达获取高精度的深度信息,将两者的数据信息结合,构建以及优化地图,其次,利用IMU测量信息,可以进行机器人的运动估计和误差补偿,提高四足机器人运动的精度和鲁棒性。最后,将双目相机、激光雷达和IMU传感器的测量信息进行融合,实现了四足机器人更加准确和鲁棒的定位与建图。
为了实现上述目的,本发明采用如下技术方案:
本发明的第一个方面提供用于机器人环境感知的多传感器融合地图构建方法,包括如下步骤:
获取双目图像数据、激光雷达点云数据和IMU测量数据;
基于双目图像数据进行特征提取得到图像特征;
基于图像特征估计四足机器人的姿态和场景的深度信息得到初始场景地图;
结合激光雷达点云数据对初始场景地图进行优化得到第二场景地图;
基于第二场景地图,融合双目图像数据、激光雷达点云数据和IMU测量数据,并构建约束优化问题,求解得到四足机器人的状态,基于四足机器人的状态,对第二场景地图进行配准,更新四足机器人的位姿和地图,得到最终的环境地图。
进一步地,所述基于双目图像数据进行特征提取得到图像特征包括:
利用基于快速角点检测器的方法通过比较待检测点周围的像素灰度检测得到关键点;
基于关键点,通过计算二进制描述子进行特征提取得到左右目图像数据对应的两个特征点集合;
基于两个特征点集合,采用特征匹配方法,使用描述符距离比值来判断两个特征点是否匹配,若该比值小于预设阈值,则左右目图像中的对应特征点为匹配点;
利用光流法来追踪匹配点在相邻帧之间的位置变化,修正匹配点的位置以及位姿估计,基于修正后的匹配点提取图像特征。
进一步地,对于每个匹配点,利用双目相机的几何约束关系,得到对应的3D点,其表达式为:
进一步地,所述结合激光雷达点云数据对初始场景地图进行优化得到第二场景地图具体包括:
将激光雷达点云数据通过双目视觉获得的相机姿态和内参进行投影,得到点云在图像坐标系中的投影位置;
通过最近邻匹配点云算法计算出该位置对应像素在双目视觉深度图中的深度值;
基于该深度值进行深度关联,对初始地图中的深度信息进行更新得到第二场景地图。
进一步地,所述通过最近邻匹配点云算法计算出该位置对应像素在双目视觉深度图中的深度值包括:
通过将两个点云中的每个点的特征向量进行比较,计算每个点与第二个点云中的所有点之间的距离,找到每个点的最近邻点。
进一步地,所述约束优化问题的表达式为:
其中,分别表示双目图像数据、激光雷达点云数据和IMU测量数据以及它们之间对应关系,/>分别表示双目图像数据、激光雷达点云数据和IMU测量数据特征提取误差项,/>表示数据关联的误差项,/>表示视觉数据中的第/>个元素,/>表示激光雷达数据中的第/>个元素,/>表示 IMU 数据中的第/>个元素,/>表示多传感器关联数据中的第/>个对应关系。
进一步地,在获取激光雷达点云数据后,对激光雷达点云数据进行预处理,包括:
利用体素滤波的方法将点云数据离散化为体素格子,并对每个体素格子内的点云数据进行采样,得到滤波后的点云数据;
再利用半径滤波法只保留设定距离内的点云数据,最后利用统计滤波的方法对点云数据进行均值滤波,剔除噪声以及冗余点,过滤掉不在相机坐标系中的激光点云,降低点云投影到相机坐标系下深度注册的处理量。
进一步地,基于滑动窗口的优化方法,采用滑动窗口来限制状态向量的大小,只保留部分时刻的状态变量。
本发明的第二个方面提供用于机器人环境感知的多传感器融合地图构建系统,包括:
数据获取模块,其被配置为:获取双目图像数据、激光雷达点云数据和IMU测量数据;
特征提取模块,其被配置为:基于双目图像数据进行特征提取得到图像特征;
多源信息联合优化模块,其被配置为:
基于图像特征估计四足机器人的姿态和场景的深度信息得到初始场景地图;
结合激光雷达点云数据对初始场景地图进行优化得到第二场景地图;
基于第二场景地图,融合双目图像数据、激光雷达点云数据和IMU测量数据,并构建约束优化问题,求解得到四足机器人的状态,基于四足机器人的状态,对第二场景地图进行配准,更新四足机器人的位姿和地图,得到最终的环境地图。
进一步地,所述多源信息联合优化模块中,所述结合激光雷达点云数据对初始场景地图进行优化得到第二场景地图具体包括:
将激光雷达点云数据通过双目视觉获得的相机姿态和内参进行投影,得到点云在图像坐标系中的投影位置;
通过最近邻匹配点云算法计算出该位置对应像素在双目视觉深度图中的深度值;
基于该深度值进行深度关联,对初始地图中的深度信息进行更新得到第二场景地图。
与现有技术相比,本发明的有益效果是:
1、本发明针对传统激光SLAM和视觉SLAM地图融合方法中算法失效导致地图构建失败的缺陷,提出了利用因子图优化算法融合双目图像数据、激光雷达点云数据和IMU测量数据,并构建约束优化问题的解决方案,该方法可以求解得到四足机器人的状态,对第二场景地图进行配准,更新机器人的位姿和地图,从而不受算法失效的影响,能够不间断地完成地图的构建。
2、本发明采用激光视觉惯性紧耦合的技术手段,对传统SLAM松耦合方法中难以处理两个独立地图之间的不一致性、容易受到环境变化和噪声的影响,同时需要进行复杂的地图融合和优化过程等缺点进行了有效解决,显著提升了系统的鲁棒性和实时性。
3、针对视觉SLAM难以处理大量特征点、容易受到光照变化和遮挡影响的缺点,本发明通过将激光雷达数据与相机数据进行联合处理,可以有效地减少特征提取和匹配的工作量,降低系统运算复杂度,提高系统实时性。同时,通过利用惯性测量单元数据对相机位姿进行优化,可以进一步提高系统的定位和重建精度。
4、传统的SLAM框架由于缺乏对多传感器数据进行融合的能力,面临着数据融合、校准、处理等诸多挑战。为了解决这些问题,本发明提出了一种多传感器融合框架,利用这种框架将不同类型的数据进行融合,从而进一步提高SLAM系统的精度和鲁棒性。
附图说明
构成本发明的一部分的说明书附图用来提供对本发明的进一步理解,本发明的示意性实施例及其说明用于解释本发明,并不构成对本发明的不当限定。
图1是本发明实施例提供的用于机器人环境感知的多传感器融合地图构建方法流程示意图;
图2是本发明实施例提供的用于机器人环境感知的多传感器融合地图构建系统框图;
图3是本发明实施例提供的双目相机特征提取模型;
图4是本发明实施例提供的视觉子系统系统框架;
图5是本发明实施例提供的激光子系统系统框架;
图6是本发明实施例提供的多传感器融合因子图框架。
具体实施方式
下面结合附图与实施例对本发明作进一步说明。
应该指出,以下详细说明都是例示性的,旨在对本发明提供进一步的说明。除非另有指明,本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是,这里所使用的术语仅是为了描述具体实施方式,而非意图限制根据本发明的示例性实施方式。如在这里所使用的,除非上下文另外明确指出,否则单数形式也意图包括复数形式,此外,还应当理解的是,当在本说明书中使用术语“包含”和/或“包括”时,其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
针对传统的激光SLAM与视觉SLAM的地图融合方法仅仅是利用两种SLAM建立后的地图进行融合,而在数据处理过程中,需要将数据分布传入不同的SLAM算法当中,当一种算法失效时,后续的地图构建过程将无法完成。因此,提出了将双目相机、激光雷达、IMU的数据进行融合的方法,在低纹理、光照弱等复杂环境中仍能够保持较高的定位精度,通过双目视觉技术精确的估计四足机器人的位姿信息,提供四足机器人在未知环境中的定位精度。该方法分别构建了视觉惯性里程计、激光惯性里程计以及IMU里程计,随后将视觉惯性里程计与IMU里程计结合形成视觉子系统,将激光惯性里程计与IMU里程计结合形成激光子系统。
本发明通过过双目相机采集左右目图像信息,通过双目视觉技术能够估计机器人的姿态和场景的深度信息,构建地图。同时,利用激光雷达获取高精度的深度信息,优化地图,其次,利用IMU测量信息,可以进行机器人的运动估计和误差补偿,提高四足机器人运动的精度和鲁棒性。最后,将双目相机、激光雷达和IMU传感器的测量信息进行融合,实现了四足机器人更加准确和鲁棒的定位与建图。
实施例一
如图1所示,本实施例提供用于机器人环境感知的多传感器融合地图构建方法,包括如下步骤:
步骤1:获取左右目图像数据、激光雷达点云数据和IMU数据;
本实施例中,所述左右目图像数据是通过双目相机RealSense D435i获取的,通过这款相机左右目红外相机获取双目图像,通过采用其强大的RealSense模块,实现相机的调用和图像信息的获取。
所述激光点云数据是通过16线3D激光雷达Velodyne获取的。通过这款激光雷达,可以获取丰富的点云信息,从而完成双目相机数据与激光雷达数据的信息融合。
所述IMU数据是通过四足机器人自身获取的,利用IMU信息对视觉子系统以及激光子系统进行误差补偿,显著提高了整个系统的精度以及鲁棒性。
观测数据处理分为两个部分,视觉子系统以及激光子系统;
其中,视觉子系统为:
为了获取未知环境的图像信息,提出了一种基于ORB的特征提取以及匹配的方法,首先对左右目图像进行特征提取,双目相机特征提取模型如图6所示,利用Oriented FAST角点检测器通过比较待检测点周围的像素灰度来检测角点,采取BRIEF二进制描述符(描述符的值由周围像素的灰度值大小关系来决定)来提取特征点,最后采用光流法对特征进行匹配具体如下:
步骤2:如图3所示,基于双目图像数据进行特征提取得到图像特征,基于图像特征估计四足机器人的姿态和场景的深度信息得到初始场景地图;
步骤201:利用Oriented FAST角点检测器通过比较待检测点周围的像素灰度来检测角点;
特征点的选择是通过FAST角点检测器进行的;FAST角点检测器是一种快速的角点检测算法,它能够快速地检测出图像中的角点,同时具有较好的鲁棒性。
FAST角点检测器被用来检测每个图像中的关键点,然后通过计算ORB描述子来提取特征,对其进行特征描述子的计算,最终得到两个特征点集合:
步骤202:采取BRIEF二进制描述符(描述符的值由周围像素的灰度值大小关系来决定)来提取特征点;
本实施例中,选择使用ORB特征描述子,ORB特征描述子是一种既具有旋转不变性,又具有尺度不变性的二进制描述子,计算效率高,对噪声和光照变化的鲁棒性较好。
所述采用ORB特征描述子进行计算的步骤包括:
1)构建尺度空间金字塔和DOG(Difference of Gaussian)金字塔;
2)利用BRIEF算法生成二进制描述子。
BRIEF算法的计算公式如下:
ORB描述子的计算公式如下:
步骤203:最后采用光流法对特征进行匹配。
在特征匹配方面,本实施例采用一种基于词袋模型的特征匹配方法,该方法可以将特征描述子聚类,通过计算描述符之间的Hamming距离,可以对关键点进行匹配。
另外,利用光流法来追踪匹配点在相邻帧之间的位置变化,从而修正匹配点的位置以及位姿估计,进一步优化了系统的精度以及鲁棒性,尤其在面对四足机器人快速移动、震动大、低纹理等复杂场景时。
通过双目摄像头,可以得到对应的左右两个摄像头中的像素坐标以及深度信息,通过三角测量公式,可以将这些信息转换为点云中的三维坐标。
最终得到的点云可以用来表示场景的结构,为后续的姿态估计和地图构建提供重要的信息。
此外还采用了基于优化的匹配策略,具体来说,在特征匹配时,选择将相邻两帧之间的激光点云和图像进行匹配,得到相对位姿变换,最后将相对位姿变换与之前的绝对位姿进行累积,从而得到整个位姿,视觉子系统框架如图4所示。
激光子系统
步骤3:对激光雷达点云进行去畸变预处理,提出了一种深度关联方法对视觉信息以及激光点云信息进行深度融合,具体的操作如下:
由于激光雷达扫描的数据存在畸变,首先对激光雷达获取的数据进行预处理,选择基于实测数据对激光雷达点云进行去畸变处理,设相机坐标系下畸变前的点云坐标为,畸变后点云坐标为/>,通过公式(7)对激光雷达点云数据进行去畸变处理:
提出了一种以IMU作为中心的坐标系,避免了相机与lidar点云的复杂坐标转换,直接通过标定获取Lidar-与IMU的转换矩阵即可。
利用体素滤波的方法将点云数据离散化为体素格子,并对每个体素格子内的点云数据进行采样,得到滤波后的点云数据;再利用半径滤波法只保留设定距离内的点云数据,最后利用统计滤波的方法对点云数据进行均值滤波,剔除噪声以及冗余点。过滤掉不在相机坐标系中的Lidar激光点云,降低点云投影到相机坐标系下深度注册的处理量。
步骤4:结合激光雷达点云数据对初始场景地图进行优化得到第二场景地图;
为了视觉子系统与激光子系统更好的融合,提出了一种深度关联的方法,通过将双目视觉和激光雷达的深度信息进行匹配,具体来说,将激光雷达的扫描的点云数据通过双目视觉获得的相机姿态和内参进行投影,得到点云在图像坐标系中的投影位置,选择通过最近邻匹配点云算法来计算出该位置对应像素在双目视觉深度图中的深度值。
所述深度值计算公式如下:
在后续的优化过程中,深度值被用在重投影误差项的计算中,用于衡量地图点和图像点之间的误差。具体公式为:
其中,为重投影误差项,/>为图像点坐标,/>为相机内参矩阵,/>为相机位姿,为相机姿态四元数,/>为地图点的深度值,/>为地图点在世界坐标系下的坐标,/>为四元数的旋转矩阵。深度值/>用于计算相机到地图点的距离,从而计算出地图点在相机坐标系下的坐标。最终,地图点在相机坐标系下的坐标会被转换成图像坐标系下的坐标,与对应的图像点坐标进行比较,计算重投影误差项。
通过将两个点云中的每个点的特征向量进行比较,找到每个点的最邻近点,并将其匹配在一起,通过如下公式计算两个点云中每个点的距离:
其中,表示第一个点云中的第/>个点,/>表示第二个点云中的/>个点,/>和分别表示两个点的特征向量/>表示/>范数(欧几里得距离) 通过计算每个点与第二个点云中的所有点之间的距离,可以找到每个点的最近邻点,从而完成深度关联。
多传感器数据融合模块
步骤5:基于第二场景地图,利用因子图优化算法融合双目图像数据、激光雷达点云数据和IMU测量数据,并构建约束优化问题,求解得到四足机器人的状态,基于四足机器人的状态,对第二场景地图进行配准,更新四足机器人的位姿和地图,得到最终的环境地图。
本实施例提出了一个新的因子图框架以此来对四足机器人的运动模型和传感器模型进行优化,具体方法如下:
首先将多个传感器数据进行融合,通过新建立的因子图框架来估计四足机器人的状态和环境地图,选择将视觉数据、激光雷达数据、IMU数据进行融合,以此来提高系统的鲁棒性和精度。将假设为激光雷达数据、双目相机数据、IMU数据的集合,利用因子图优化算法,来将传感器数据进行融合,以最小化成本函数:
通过将传感器数据转化为因子图,利用因子图优化算法来求解四足机器人自身状态和地图,从而实现传感器的融合。
因子图的联合概率分布可以表示为:
式中,为因子函数表示传感器测量数据与地图之间的关系,/>为四足机器人在地图上的状态,/>为地图信息,/>为四足机器人在/>时的初始状态,/>为四足机器人的初始状态分布,/>为四足机器人状态的因子函数;
具体的,双目图像、IMU数据和激光雷达的数据融合可以表示为如下的优化问题:
其中,分别表示双目数据、激光数据、IMU数据、以及它们之间对应关系,/>分别表示双目数据、激光雷达数据、以及IMU数据特征提取误差项。/>表示数据关联的误差项,通过调整对应的参数,来达到最优的优化结果;/>代表针对视觉特征中的第/>个样本;/>代表针对激光雷达特征中的第/>个样本;/>代表针对 IMU 数据中的第/>个样本;/>代表针对数据关联误差项的第/>个,同时,/>表示视觉数据中的第/>个元素,/>表示激光雷达数据中的第/>个元素,/>表示 IMU 数据中的第/>个元素,/>表示多传感器关联数据中的第/>个对应关系。
由于四足机器人的不断运动,状态向量不断变化,在状态估计过程中,随着时间的推移,状态变量会不断增加,同时估计误差也会不断累积。为了避免出现过大的误差,提出了基于滑动窗口的优化方法,采用滑动窗口来限制状态向量的大小,只保留部分时刻的状态变量,减少计算量,从而提高了计算效率,通过该优化方法能够保证算法在实现高定位精度的同时,实现较高的计算效率,多传感器融合框架如图6所示。
利用基于滑动窗口的方法来对轨迹进行估计,每次滑动一个固定的窗口大小。在每个窗口内,算法利用Lidar、Visual和Inertial三个传感器的数据进行轨迹估计,并通过平滑和映射算法进行优化。如果一个时刻的传感器数据出现异常,比如某个传感器的数据突然失效或出现严重的漂移,那么该时刻的轨迹估计结果就会与其他时刻的轨迹估计结果出现较大的残差,将这个轨迹残差命名为。/>
当超过一定阈值时,就会触发一个失败检测机制。具体来说,当某个时刻的超过阈值时,算法会将该时刻所在的窗口向前或向后滑动一定的距离,使得该时刻不再位于当前窗口内。然后,算法会重新对新的窗口进行轨迹估计和优化,以确保整个轨迹的连续性和准确性。
通过上面的处理,利用激光雷达数据进行地图初始化,获得初始地图。然后,通过处理双目相机视觉数据,将新的路标点/>添加到地图当中,得到更新后的地图。最后,利用四足机器人内部的IMU数据估计四足机器人状态/>,并将其与地图/>进行配准,从而更新地图,实现环境地图的构建。
实施例二
参照图2,本实施例提供用于机器人环境感知的多传感器融合地图构建系统,包括:
数据获取模块,其被配置为:获取双目图像数据、激光雷达点云数据和IMU测量数据;
特征提取模块,其被配置为:基于双目图像数据进行特征提取得到图像特征;
多源信息联合优化模块,其被配置为:
基于图像特征估计四足机器人的姿态和场景的深度信息得到初始场景地图;
结合激光雷达点云数据对初始场景地图进行优化得到第二场景地图;
基于第二场景地图,融合双目图像数据、激光雷达点云数据和IMU测量数据,并构建约束优化问题,求解得到四足机器人的状态,基于四足机器人的状态,对第二场景地图进行配准,更新四足机器人的位姿和地图,得到最终的环境地图。
其中,多源信息联合优化模块中,所述结合激光雷达点云数据对初始场景地图进行优化得到第二场景地图具体包括:
将激光雷达点云数据通过双目视觉获得的相机姿态和内参进行投影,得到点云在图像坐标系中的投影位置;
通过最近邻匹配点云算法计算出该位置对应像素在双目视觉深度图中的深度值;
基于该深度值进行深度关联,对初始地图中的深度信息进行更新得到第二场景地图。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (12)
1.用于机器人环境感知的多传感器融合地图构建方法,其特征在于,包括如下步骤:
获取双目图像数据、激光雷达点云数据和IMU测量数据;
基于双目图像数据进行特征提取得到图像特征;
基于图像特征估计四足机器人的姿态和场景的深度信息得到初始场景地图;
结合激光雷达点云数据对初始场景地图进行优化得到第二场景地图;
基于第二场景地图,融合双目图像数据、激光雷达点云数据和IMU测量数据,并构建约束优化问题,求解得到四足机器人的状态,基于四足机器人的状态,对第二场景地图进行配准,更新四足机器人的位姿和地图,得到最终的环境地图。
2.根据权利要求1所述的用于机器人环境感知的多传感器融合地图构建方法,其特征在于,所述基于双目图像数据进行特征提取得到图像特征包括:
利用基于快速角点检测器的方法通过比较待检测点周围的像素灰度检测得到关键点;
基于关键点,通过计算二进制描述子进行特征提取得到左右目图像数据对应的两个特征点集合;
基于两个特征点集合,采用特征匹配方法,使用描述符距离比值来判断两个特征点是否匹配,若该比值小于预设阈值,则左右目图像中的对应特征点为匹配点;
利用光流法来追踪匹配点在相邻帧之间的位置变化,修正匹配点的位置以及位姿估计,基于修正后的匹配点提取图像特征。
5.如权利要求1所述的用于机器人环境感知的多传感器融合地图构建方法,其特征在于,所述结合激光雷达点云数据对初始场景地图进行优化得到第二场景地图具体包括:
将激光雷达点云数据通过双目视觉获得的相机姿态和内参进行投影,得到点云在图像坐标系中的投影位置;
通过最近邻匹配点云算法计算出该位置对应像素在双目视觉深度图中的深度值;
基于该深度值进行深度关联,对初始地图中的深度信息进行更新得到第二场景地图。
6.根据权利要求4所述的用于机器人环境感知的多传感器融合地图构建方法,其特征在于,所述通过最近邻匹配点云算法计算出该位置对应像素在双目视觉深度图中的深度值包括:
通过将两个点云中的每个点的特征向量进行比较,计算每个点与第二个点云中的所有点之间的距离,找到每个点的最近邻点。
9.根据权利要求1所述的用于机器人环境感知的多传感器融合地图构建方法,其特征在于,在获取激光雷达点云数据后,对激光雷达点云数据进行预处理,包括:
利用体素滤波的方法将点云数据离散化为体素格子,并对每个体素格子内的点云数据进行采样,得到滤波后的点云数据;
再利用半径滤波法只保留设定距离内的点云数据,最后利用统计滤波的方法对点云数据进行均值滤波,剔除噪声以及冗余点,过滤掉不在相机坐标系中的激光点云,降低点云投影到相机坐标系下深度注册的处理量。
10.根据权利要求1所述的用于机器人环境感知的多传感器融合地图构建方法,其特征在于,在求解构建约束优化问题时,基于滑动窗口的优化方法,采用滑动窗口来限制状态向量的大小,只保留部分时刻的状态变量。
11.用于机器人环境感知的多传感器融合地图构建系统,其特征在于,包括:
数据获取模块,其被配置为:获取双目图像数据、激光雷达点云数据和IMU测量数据;
特征提取模块,其被配置为:基于双目图像数据进行特征提取得到图像特征;
多源信息联合优化模块,其被配置为:
基于图像特征估计四足机器人的姿态和场景的深度信息得到初始场景地图;
结合激光雷达点云数据对初始场景地图进行优化得到第二场景地图;
基于第二场景地图,融合双目图像数据、激光雷达点云数据和IMU测量数据,并构建约束优化问题,求解得到四足机器人的状态,基于四足机器人的状态,对第二场景地图进行配准,更新四足机器人的位姿和地图,得到最终的环境地图。
12.根据权利要求9所述的用于机器人环境感知的多传感器融合地图构建系统,其特征在于,所述多源信息联合优化模块中,所述结合激光雷达点云数据对初始场景地图进行优化得到第二场景地图具体包括:
将激光雷达点云数据通过双目视觉获得的相机姿态和内参进行投影,得到点云在图像坐标系中的投影位置;
通过最近邻匹配点云算法计算出该位置对应像素在双目视觉深度图中的深度值;
基于该深度值进行深度关联,对初始地图中的深度信息进行更新得到第二场景地图。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310458414.8A CN116222543B (zh) | 2023-04-26 | 2023-04-26 | 用于机器人环境感知的多传感器融合地图构建方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310458414.8A CN116222543B (zh) | 2023-04-26 | 2023-04-26 | 用于机器人环境感知的多传感器融合地图构建方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116222543A true CN116222543A (zh) | 2023-06-06 |
CN116222543B CN116222543B (zh) | 2023-07-28 |
Family
ID=86580801
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310458414.8A Active CN116222543B (zh) | 2023-04-26 | 2023-04-26 | 用于机器人环境感知的多传感器融合地图构建方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116222543B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116630442A (zh) * | 2023-07-19 | 2023-08-22 | 绘见科技(深圳)有限公司 | 一种视觉slam位姿估计精度评估方法及装置 |
CN117470230A (zh) * | 2023-10-23 | 2024-01-30 | 广州创源机器人有限公司 | 基于深度学习的视觉激光传感器融合定位算法 |
CN117765048A (zh) * | 2024-02-22 | 2024-03-26 | 吉林大学 | 一种基于跨模态融合的水下目标三维配准方法 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111045017A (zh) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
CN111258313A (zh) * | 2020-01-20 | 2020-06-09 | 深圳市普渡科技有限公司 | 多传感器融合slam系统及机器人 |
CN112304307A (zh) * | 2020-09-15 | 2021-02-02 | 浙江大华技术股份有限公司 | 一种基于多传感器融合的定位方法、装置和存储介质 |
CN113436260A (zh) * | 2021-06-24 | 2021-09-24 | 华中科技大学 | 基于多传感器紧耦合的移动机器人位姿估计方法和系统 |
WO2021232470A1 (zh) * | 2020-05-19 | 2021-11-25 | 北京数字绿土科技有限公司 | 基于多传感器融合的slam制图方法、系统 |
CN114018236A (zh) * | 2021-09-30 | 2022-02-08 | 哈尔滨工程大学 | 一种基于自适应因子图的激光视觉强耦合slam方法 |
CN115272596A (zh) * | 2022-07-08 | 2022-11-01 | 同济大学 | 一种面向单调无纹理大场景的多传感器融合slam方法 |
CN115479598A (zh) * | 2022-08-23 | 2022-12-16 | 长春工业大学 | 基于多传感器融合的定位与建图方法及紧耦合系统 |
WO2023000294A1 (zh) * | 2021-07-23 | 2023-01-26 | 深圳市大疆创新科技有限公司 | 位姿估计方法、激光雷达-惯性里程计、可移动平台及存储介质 |
-
2023
- 2023-04-26 CN CN202310458414.8A patent/CN116222543B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111045017A (zh) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
CN111258313A (zh) * | 2020-01-20 | 2020-06-09 | 深圳市普渡科技有限公司 | 多传感器融合slam系统及机器人 |
WO2021232470A1 (zh) * | 2020-05-19 | 2021-11-25 | 北京数字绿土科技有限公司 | 基于多传感器融合的slam制图方法、系统 |
CN112304307A (zh) * | 2020-09-15 | 2021-02-02 | 浙江大华技术股份有限公司 | 一种基于多传感器融合的定位方法、装置和存储介质 |
CN113436260A (zh) * | 2021-06-24 | 2021-09-24 | 华中科技大学 | 基于多传感器紧耦合的移动机器人位姿估计方法和系统 |
WO2023000294A1 (zh) * | 2021-07-23 | 2023-01-26 | 深圳市大疆创新科技有限公司 | 位姿估计方法、激光雷达-惯性里程计、可移动平台及存储介质 |
CN114018236A (zh) * | 2021-09-30 | 2022-02-08 | 哈尔滨工程大学 | 一种基于自适应因子图的激光视觉强耦合slam方法 |
CN115272596A (zh) * | 2022-07-08 | 2022-11-01 | 同济大学 | 一种面向单调无纹理大场景的多传感器融合slam方法 |
CN115479598A (zh) * | 2022-08-23 | 2022-12-16 | 长春工业大学 | 基于多传感器融合的定位与建图方法及紧耦合系统 |
Non-Patent Citations (2)
Title |
---|
李志;梁启星;李彬;荣学文;范永;: "基于彩色摄像头的四足机器人自主循迹算法的研究与实现", 齐鲁工业大学学报, no. 05 * |
涂金戈;谢仁平;赵鹏程;: "一种双目直接法视觉里程计", 测绘地理信息, no. 02 * |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116630442A (zh) * | 2023-07-19 | 2023-08-22 | 绘见科技(深圳)有限公司 | 一种视觉slam位姿估计精度评估方法及装置 |
CN116630442B (zh) * | 2023-07-19 | 2023-09-22 | 绘见科技(深圳)有限公司 | 一种视觉slam位姿估计精度评估方法及装置 |
CN117470230A (zh) * | 2023-10-23 | 2024-01-30 | 广州创源机器人有限公司 | 基于深度学习的视觉激光传感器融合定位算法 |
CN117765048A (zh) * | 2024-02-22 | 2024-03-26 | 吉林大学 | 一种基于跨模态融合的水下目标三维配准方法 |
Also Published As
Publication number | Publication date |
---|---|
CN116222543B (zh) | 2023-07-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111561923B (zh) | 基于多传感器融合的slam制图方法、系统 | |
Han et al. | Deepvio: Self-supervised deep learning of monocular visual inertial odometry using 3d geometric constraints | |
CN109307508B (zh) | 一种基于多关键帧的全景惯导slam方法 | |
WO2021233029A1 (en) | Simultaneous localization and mapping method, device, system and storage medium | |
CN112902953B (zh) | 一种基于slam技术的自主位姿测量方法 | |
CN116222543B (zh) | 用于机器人环境感知的多传感器融合地图构建方法及系统 | |
CN111156998B (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
CN110033489B (zh) | 一种车辆定位准确性的评估方法、装置及设备 | |
CN112785702A (zh) | 一种基于2d激光雷达和双目相机紧耦合的slam方法 | |
CN108519102B (zh) | 一种基于二次投影的双目视觉里程计算方法 | |
CN108615246B (zh) | 提高视觉里程计系统鲁棒性和降低算法计算消耗的方法 | |
CN112734765A (zh) | 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质 | |
CN111127540B (zh) | 一种三维虚拟空间自动测距方法及系统 | |
CN111623773B (zh) | 一种基于鱼眼视觉和惯性测量的目标定位方法及装置 | |
CN112767546B (zh) | 移动机器人基于双目图像的视觉地图生成方法 | |
CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
Zhen et al. | LiDAR-enhanced structure-from-motion | |
CN116619358A (zh) | 一种矿山自主探测机器人自适应定位优化与建图方法 | |
CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
CN117115271A (zh) | 无人机飞行过程中的双目相机外参数自标定方法及系统 | |
CN112945233A (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
CN114897988B (zh) | 一种铰链式车辆中的多相机定位方法、装置及设备 | |
CN116128966A (zh) | 一种基于环境物体的语义定位方法 | |
CN112767481A (zh) | 一种基于视觉边缘特征的高精度定位及建图方法 | |
CN112767482A (zh) | 一种多传感器融合的室内外定位方法及系统 |
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 |