CN117470259A - 一种子母式空地协同多传感器融合三维建图系统 - Google Patents

一种子母式空地协同多传感器融合三维建图系统 Download PDF

Info

Publication number
CN117470259A
CN117470259A CN202311438987.0A CN202311438987A CN117470259A CN 117470259 A CN117470259 A CN 117470259A CN 202311438987 A CN202311438987 A CN 202311438987A CN 117470259 A CN117470259 A CN 117470259A
Authority
CN
China
Prior art keywords
data
sub
map
local map
central
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.)
Pending
Application number
CN202311438987.0A
Other languages
English (en)
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.)
Zhengzhou University
Original Assignee
Zhengzhou 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 Zhengzhou University filed Critical Zhengzhou University
Priority to CN202311438987.0A priority Critical patent/CN117470259A/zh
Publication of CN117470259A publication Critical patent/CN117470259A/zh
Pending legal-status Critical Current

Links

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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • 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
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G01S17/894—3D imaging with simultaneous measurement of time-of-flight at a 2D array of receiver pixels, e.g. time-of-flight cameras or flash lidar
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/21—Design or setup of recognition systems or techniques; Extraction of features in feature space; Blind source separation
    • G06F18/213—Feature extraction, e.g. by transforming the feature space; Summarisation; Mappings, e.g. subspace methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/251—Fusion techniques of input or preprocessed data
    • 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/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • 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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Engineering & Computer Science (AREA)
  • Geometry (AREA)
  • Electromagnetism (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Software Systems (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Automation & Control Theory (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Computer Graphics (AREA)
  • Multimedia (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供了本发明提供了一种子母式空地协同多传感器融合三维建图系统，该系统采用中心式架构，将系统分为两部分：子端和中心端，其中无人车平台为中心端，无人机平台为子端，中心端和子端均装备有SLAM系统，由激光雷达、I MU和摄像头等组成，中心端和子端进行数据交互，接收子端传输的关键帧，地图点等数据，完成回环检测，地图融合，全局优化等非实时操作。通过对融合后的多种传感器数据进行处理能够克服单一传感器的限制，提供更准确、鲁棒和全面的定位和地图构建结果。

Description

一种子母式空地协同多传感器融合三维建图系统
技术领域
本发明涉及三维建图技术领域，具体涉及一种子母式空地协同多传感器融合三维建图系统。
背景技术
现在常见的对地面环境进行数据采集和建图的三维建图系统主要包括单一的对空平台、对地平台或者二者联合的空空、地地平台，然而单一平台、单一传感器往往存在视野范围小、数据单一、运算能力有限等问题，因此难以完成特殊场景的任务。针对上述三维建图存在的问题，越来越多的研究开始侧重于使用空地协同多传感器融合进行三维建图。由于空中机器人是俯视或倾斜视角，对场景内建筑物的顶部信息或传感器照射正面信息容易获取，对于建筑物背面或者环视区域却容易出现盲区，与此同时部分遮挡区域也会造成空中机器人采集数据建图不完整。地面机器人虽然无法获取较高建筑物顶部数据，但对物体环视区域或者部分空中机器人的遮挡区域却有很好的采集效果，同时地面机器人数据采集过程可以利用空中机器人建立的地图进行路径规划和自主导航。但是，现有技术中，在对空地协同多传感器的融合的过程中对数据的采集和处理存在诸多缺陷，使得融合后的数据不能够精准的完成三维地图的构建。
上述问题是目前亟待解决的。
发明内容
本发明要克服现有技术的上述至少一个缺点，提供了一种子母式空地协同多传感器融合三维建图系统，所述系统包括：中心端和子端，所述中心端和子端均装备有由激光雷达、IMU和摄像头组成的SLAM系统；数据获取模块：用于分别获取子端和中心端的三维激光点云数据、IMU惯导数据以及图像数据；数据预处理模块：用于对所述子端和中心端的三维激光点云数据、IMU惯导数据以及图像数据进行滤波、去噪以及平滑处理；特征提取模块：用于对所述子端和中心端的三维激光点云数据、IMU惯导数据以及图像数据进行特征提取分别生成与之对应的观测数据；后端优化模块：用于基于不同的观测数据进行多源数据的联合优化；关键帧选取模块：用于对优化后的数据进行关键帧选取；局部地图构建模块：用于分别对选取的子端和中心端的关键帧进行该帧局部地图的构建，并基于构建的该帧局部地图进行相邻帧局部地图的融合生成子端局部地图和中心端局部地图；全局地图构建模块：用于对子端局部地图和中心端局部地图进行融合生成全局地图；输出模块：用于对所述全局地图进行输出。
优选的，所述中心端包括无人车平台，所述子端包括无人机平台；无人机平台与无人车平台的SLAM系统同步进行环境数据的采集分别生成第一三维激光点云数据、第一IMU惯导数据、第一图像数据、第二三维激光点云数据、第二IMU惯导数据以及第二图像数据。
优选的，所述特征提取模块还用于：在子端侧，基于第一三维激光点云数据提取原始激光点云中的几何特征，利用IMU预报的位姿对载体运动过程中可能导致的点云畸变进行去除生成第一观测数据；基于第一IMU惯导数据，通过对两个相邻关键帧之间的IMU惯导数据进行预积分，生成第二观测数据；基于第一图像数据，进行视觉特征提取与跟踪生成第三观测数据；在中心端侧，基于第二三维激光点云数据提取原始激光点云中的几何特征，利用IMU预报的位姿对载体运动过程中可能导致的点云畸变进行去除生成第四观测数据；基于第二IMU惯导数据，通过对两个相邻关键帧之间的IMU惯导数据进行预积分，生成第五观测数据；基于第二图像数据，进行视觉特征提取与跟踪生成第六观测数据。
优选的，所述后端优化模块还用于：在子端侧，对第一观测数据、第二观测数据和第三观测数据构建残差项分别生成第一激光点云匹配残差、第一IMU预积分残差和第一视觉重投影残差；使用SLAM系统中的Ceres Solver库对所述第一激光点云匹配残差、第一IMU预积分残差和第一视觉重投影残差进行非线性最小二乘优化求解，完成多源数据的联合优化；在中心端侧，对第四观测数据、第五观测数据和第六观测数据构建残差项分别生成第二激光点云匹配残差、第二IMU预积分残差和第二视觉重投影残差；使用SLAM系统中的CeresSolver库对所述第二激光点云匹配残差、第二IMU预积分残差和第二视觉重投影残差进行非线性最小二乘优化求解，完成多源数据的联合优化。
优选的，所述关键帧选取模块还用于：在子端和中心端分别基于连续跟踪的特征点判断当前帧与上一帧的平均视差,如果平均视差大于设定的阈值,则判断当前帧为关键帧，并将关键帧存储至中心端的关键帧数据库中；所述系统还包括回环检测模块，用于对关键帧数据库进回环检测。
优选的，所述局部地图构建模块包括多源数据融合建图模块和局部地图融合模块；所述多源数据融合建图模块用于：在子端，通过对提取的第一三维激光点云数据和第一图像数据的特征点进行匹配；根据特征点的匹配结果，计算激光雷达和摄像头之间的相对位姿；采用非线性优化方法将第一三维激光点云和第一图像数据与第一IMU惯导数据进行融合，估计子端的位姿；根据位姿估计结果，完成在子端该帧局部地图的构建；在中心端，通过对提取的第二三维激光点云数据和第二图像数据的特征点进行匹配；根据特征点的匹配结果，计算激光雷达和摄像头之间的相对位姿；采用非线性优化方法将第二三维激光点云和第二图像数据与第二IMU惯导数据进行融合，估计中心端的位姿；根据位姿估计结果，完成在中心端该帧局部地图的构建；所述局部地图融合模块用于：采用基于字典机制的相似度检测算法分别对构建的该帧的子端局部地图和该帧的中心端局部地图进行重叠区域的检测，采用特征点匹配算法完成特征点的匹配；基于相邻两帧的IMU惯导数据分别对子端的无人机和中心端的无人车进行运动估计和位姿优化，进而完成子端和中心端的相邻两帧局部地图的融合分别生成子端局部地图和中心端局部地图；所述局部地图融合模块还用于分别对子端局部地图和中心端局部地图进行更新，并将更新后的子端局部地图存储在位于中心端的地图堆栈中。
优选的，所述平均视差计算公式为：式中,N表示连续跟踪特征点的个数；k表示特征标识ID；duk＝ujk－uik；dvk＝vjk－vik；u和v表示像素坐标；i和j分别表示上一帧和当前帧序号。
优选的，所述全局地图构建模块还用于：将子端局部地图和中心端局部地图的坐标系转换到统一的世界坐标系下；分别对子端局部地图和中心端局部地图进行特征点线的提取；基于所述提取的特征点，采用基于字典机制的相似度检测算法进行子端和中心端局部地图重叠区域的检测；根据重叠区域的检测结果，对子端和中心端的局部地图的位姿进行优化和更新；对优化更新后的子端和中心端的局部地图进行特征点的匹配以完成局部地图融合生成全局地图。
优选的，所述对优化更新后的子端和中心端的局部地图进行特征点的匹配以完成局部地图融合生成全局地图包括：通过融合了3D-HARRIS关键点检测算法与方向直方图签名特征描述子算法分别对优化更新后的子端和中心端局部地图进行特征描述；通过采样一致性算法分别获得子端局部地图和中心端局部地图的初始位姿；通过ICP算法分别获取子端局部地图和中心端局部地图精确的位姿变换结果进而完成对子端局部地图和中心端局部地图的融合生成全局地图。
优选的，所述系统还包括地图融合优化模块，所述融合优化模块置于中心端，用于利用子端无人机和中心端无人车提供的IMU惯导数据，计算出两者的相对位姿关系，进行位置与位姿融合优化。
本发明的有益效果是：本发明提供了一种子母式空地协同多传感器融合三维建图系统，该系统采用中心式架构，将系统分为两部分：子端和中心端，其中无人车平台为中心端，无人机平台为子端，中心端和子端均装备有SLAM系统，由激光雷达、IMU和摄像头等组成，中心端和子端进行数据交互，接收子端传输的关键帧，地图点等数据，完成回环检测，地图融合，全局优化等非实时操作。并且该系统是一种子母式结构，由子端(无人机)和中心端(无人车)组成，利用空地平台协同进行多传感器数据的采集，其中无人车平台在限制较多的区域内执行任务并进行前期测量建图，无人机平台提供更高的视野和快速移动能力，两种平台数据互为补充，共同完成三维地图的构建。通过对融合后的多种传感器数据进行处理能够克服单一传感器的限制，提供更准确、鲁棒和全面的定位和地图构建结果。
附图说明
下面结合附图和实施例对本发明作进一步说明。
图1是本发明实施例所提供的一种子母式空地协同多传感器融合三维建图系统结构示意图。
图2是本发明另一实施例所提供的一种子母式空地协同多传感器融合三维建图系统结构示意图。
具体实施方式
在更加详细地讨论示例性实施例之前应当提到的是，一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各项操作描述成顺序的处理，但是其中的许多操作可以被并行地、并发地或者同时实施。此外，各项操作的顺序可以被重新安排。当其操作完成时所述处理可以被终止，但是还可以具有未包括在附图中的附加步骤。所述处理可以对应于方法、函数、规程、子例程、子程序等等。
应当理解的是，虽然在这里可能使用了术语“第一”、“第二”等等来描述各个单元，但是这些单元不应当受这些术语限制。使用这些术语仅仅是为了将一个单元与另一个单元进行区分。举例来说，在不背离示例性实施例的范围的情况下，第一单元可以被称为第二单元，并且类似地第二单元可以被称为第一单元。这里所使用的术语“和/或”包括其中一个或更多所列出的相关联项目的任意和所有组合。
现在结合附图对本发明作详细的说明。此图为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。
便于后续理解，对实施例中可能出现的专业术语进行解释：
SLAM系统：Simultaneous Localization and Mapping，同步定位与建图，机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现机器人的自主定位和导航。
IMU：Inertial Measurement Unit，惯性测量单元，典型的6轴IMU以较高频率(>100Hz)返回被测物体三个不同方向的角速度和角加速度。
残差：在数理统计中是指实际观察值与估计值(拟合值)之间的差。
回环检测：Loop Closure Detection，又称闭环检测，是指机器人识别曾到达某场景，使得地图闭环的能力。说的简单点，就是机器人在左转一下，右转一下建图的时候能意识到某个地方是“我”曾经来过的，然后把此刻生成的地图与刚刚生成的地图做匹配。
实施例1
请参阅图1-2，本发明实施例所提供的一种子母式空地协同多传感器融合三维建图系统结构示意图。
为了便于后续理解，在此处阐述本发明的整体发明构思：
该系统采用中心式架构，将系统分为两部分：子端和中心端。其中无人车平台为中心端，无人机平台为子端。中心端和子端均装备有SLAM系统，由激光雷达、IMU和摄像头等组成，通过融合多种传感器数据能够克服单一传感器的限制，提供更准确、鲁棒和全面的定位和地图构建结果。中心端和子端进行数据交互，接收子端传输的关键帧，地图点等数据，完成回环检测，地图融合，全局优化等非实时操作。
具体实施例如下：
作为示例，所述系统包括：中心端1和子端2，所述中心端1和子端2均装备有由激光雷达、IMU和摄像头组成的SLAM系统；数据获取模块3：用于分别获取子端2和中心端1的三维激光点云数据、IMU惯导数据以及图像数据；数据预处理模块4：用于对所述子端2和中心端1的三维激光点云数据、IMU惯导数据以及图像数据进行滤波、去噪以及平滑处理；特征提取模块5：用于对所述子端2和中心端1的三维激光点云数据、IMU惯导数据以及图像数据进行特征提取分别生成与之对应的观测数据；后端优化模块6：用于基于不同的观测数据进行多源数据的联合优化；关键帧选取模块7：用于对优化后的数据进行关键帧选取；局部地图构建模块8：用于分别对选取的子端2和中心端1的关键帧进行该帧局部地图的构建，并基于构建的该帧局部地图进行相邻帧局部地图的融合生成子端局部地图和中心端局部地图；全局地图构建模块9：用于对子端局部地图和中心端局部地图进行融合生成全局地图；输出模块10：用于对所述全局地图进行输出。
优选的，IMU惯导数据包括线速度、角速度、位姿等数据。通过对三维激光点云数据、IMU惯导数据以及图像数据进行预处理，以去除噪声影响、减小数据量，提高采集数据精度及系统运行效率。
优选的，所述中心端1包括无人车平台，所述子端2包括无人机平台；无人机平台与无人车平台的SLAM系统同步进行环境数据的采集分别生成第一三维激光点云数据、第一IMU惯导数据、第一图像数据、第二三维激光点云数据、第二IMU惯导数据以及第二图像数据。
优选的，所述特征提取模块5还用于：在子端侧，基于第一三维激光点云数据提取原始激光点云中的几何特征，利用IMU预报的位姿对载体运动过程中可能导致的点云畸变进行去除生成第一观测数据；基于第一IMU惯导数据，通过对两个相邻关键帧之间的IMU惯导数据进行预积分，生成第二观测数据；基于第一图像数据，进行视觉特征提取与跟踪生成第三观测数据；在中心端侧，基于第二三维激光点云数据提取原始激光点云中的几何特征，利用IMU预报的位姿对载体运动过程中可能导致的点云畸变进行去除生成第四观测数据；基于第二IMU惯导数据，通过对两个相邻关键帧之间的IMU惯导数据进行预积分，生成第五观测数据；基于第二图像数据，进行视觉特征提取与跟踪生成第六观测数据。具体地，对于三维激光点云数据，首先提取原始激光点云中的几何特征(角点和面点)，同时考虑到无人机/无人车在运动过程中，激光雷达数据会产生运动畸变，因此系统利用IMU预报的位姿对载体运动过程中可能导致的点云畸变进行去除；对于IMU惯导数据，通过对两个相邻关键帧之间的IMU数据进行预积分，构建新的预积分观测值用于后端优化；对于图像数据，进行视觉特征提取与跟踪，用于关键帧的选取。所述的对IMU惯导和图像数据的处理，使用了VINS-Mono算法，即采用基于紧耦合的非线性优化方法，融合预积分的IMU测量和特征观测，来获得高精度的视觉-惯性里程计数据，用于位姿优化和激光雷达畸变矫正。
优选的，由于得到的观测数据仍可能存在误差和精度不高等问题，因此系统又设计了一个后端优化模块6，所述后端优化模块6还用于：在子端侧，对第一观测数据、第二观测数据和第三观测数据构建残差项分别生成第一激光点云匹配残差、第一IMU预积分残差和第一视觉重投影残差；使用SLAM系统中的Ceres Solver库对所述第一激光点云匹配残差、第一IMU预积分残差和第一视觉重投影残差进行非线性最小二乘优化求解，完成多源数据的联合优化；在中心端侧，对第四观测数据、第五观测数据和第六观测数据构建残差项分别生成第二激光点云匹配残差、第二IMU预积分残差和第二视觉重投影残差；使用SLAM系统中的Ceres Solver库对所述第二激光点云匹配残差、第二IMU预积分残差和第二视觉重投影残差进行非线性最小二乘优化求解，完成多源数据的联合优化。也即，后端优化模块6通过分别对子端2和中心端1的不同传感器的观测数据构建残差项，并使用非线性最小二乘优化进行求解，完成对多源数据的联合优化。
优选的，在完成多源数据联合优化后，系统设计了一个关键帧选择模块7，所述关键帧选取模块7还用于：在子端2和中心端1分别基于连续跟踪的特征点判断当前帧与上一帧的平均视差,如果平均视差大于设定的阈值,则判断当前帧为关键帧，并将关键帧存储至中心端的关键帧数据库中；所述系统还包括回环检测模块11，用于对关键帧数据库进回环检测，回环检测使对曾经去过的位置能够准确识别,解决了位姿漂移问题。其中，关键帧数据库配置在中心端1侧。计算平均视差的公式为：
式中,N表示连续跟踪特征点的个数；k表示特征标识ID；duk＝ujk－uik；dvk＝vjk－vik；u和v表示像素坐标；i和j分别表示上一帧和当前帧序号。如果当前帧连续跟踪的特征点数量低于设定的阈值,则设定当前帧为关键帧,这个准则主要是为了保证跟踪的质量,防止出现完全跟踪失败的情况。
优选的，关键帧提取完成后，为进行无人机/无人车局部地图的构建和实时更新，系统设计了一个局部地图构建模块8，所述局部地图构建模块8包括多源数据融合建图模块801和局部地图融合模块802；所述多源数据融合建图模块801用于：在子端，通过对提取的第一三维激光点云数据和第一图像数据的特征点进行匹配，该匹配可使用ORB、SIFT和SURF等特征点匹配算法；根据特征点的匹配结果，计算激光雷达和摄像头之间的相对位姿，包括旋转和平移矩阵；采用非线性优化方法将第一三维激光点云和第一图像数据与第一IMU惯导数据进行融合，估计子端的位姿，其中非线性优化方法可以选用BundleAdjustment，BA；根据位姿估计结果，完成在子端该帧局部地图的构建。在中心端，通过对提取的第二三维激光点云数据和第二图像数据的特征点进行匹配；根据特征点的匹配结果，计算激光雷达和摄像头之间的相对位姿；采用非线性优化方法将第二三维激光点云和第二图像数据与第二IMU惯导数据进行融合，估计中心端的位姿；根据位姿估计结果，完成在中心端该帧局部地图的构建。所述局部地图融合模块802用于：采用基于字典机制的相似度检测算法分别对构建的该帧的子端局部地图和该帧的中心端局部地图进行重叠区域的检测，采用特征点匹配算法完成特征点的匹配；基于相邻两帧的IMU惯导数据分别对子端的无人机和中心端的无人车进行运动估计和位姿优化，进而完成子端和中心端的相邻两帧局部地图的融合分别生成子端局部地图和中心端局部地图；所述局部地图融合模块还用于分别对子端局部地图和中心端局部地图进行更新，并将更新后的子端局部地图存储在位于中心端的地图堆栈中，其中地图堆栈被配置在中心端1侧，便于后续与中心端局部地图进行融合。
优选的，所述全局地图构建模块9还用于：将子端局部地图和中心端局部地图的坐标系转换到统一的世界坐标系下；分别对子端局部地图和中心端局部地图进行特征点线的提取；基于所述提取的特征点，采用基于字典机制的相似度检测算法进行子端和中心端局部地图重叠区域的检测；根据重叠区域的检测结果，对子端和中心端的局部地图的位姿进行优化和更新；对优化更新后的子端和中心端的局部地图进行特征点的匹配以完成局部地图融合生成全局地图。具体地，由于无人机子端和无人车中心端所构建的局部地图的坐标系不同，因此需要进行坐标系的转换，均转换到统一的世界坐标系下。然后通过时间戳保证子端2和中心端1传感器数据在时间上的同步，进一步进行特征点、线的提取，作为地图匹配的依据。基于上述提取的特征点，系统采用基于字典机制的相似度检测算法进行子端2和中心端1局部地图重叠区域的检测，用以完成地图实时更新和回环检测，以提高定位和地图构建的精度。根据特征点的描述，经过粗配准和精配准两步对子端和中心端地图进行匹配以完成局部地图融合，系统采用融合了3D-HARRIS关键点检测算法与方向直方图签名(SHOT)特征描述子算法对局部地图进行特征描述，粗配准算法采用采样一致性算法，精配准使用ICP算法，最终完成子端和中心端局部地图的融合，形成全局地图。
优选的，为进一步提高全局地图的定位精度和准确度，系统还包括地图融合优化模块12，所述融合优化模块12置于中心端1，用于利用子端无人机和中心端无人车提供的IMU惯导数据，计算出两者的相对位姿关系，进行位置与位姿融合优化。具体地，通过融合IMU数据对子端和中心端的位姿进行优化，即通过采用分布式优化算法，将无人机和无人车的位姿优化过程联合在一起，并通过采用非线性优化算法求解出最优位姿，提高地图的准确性。
上述实施例中，该系统是一种子母式结构，由子端(无人机)和中心端(无人车)组成，利用空地平台协同进行多传感器数据的采集。其中无人车平台在限制较多的区域内执行任务并进行前期测量建图，无人机平台提供更高的视野和快速移动能力，两种平台数据互为补充，共同完成三维地图的构建。通过结合多个传感器的优势，进行融合优化建图，提高定位和地图构建的准确性、鲁棒性和稳定性。并且多传感器融合SLAM平台装备有激光雷达传感器，主动发射激光探测，不受光照条件限制，可以适应各种室内外环境。
以上所述的仅是本发明的实施例，方案中公知的具体结构及特性等常识在此未作过多描述，所属领域普通技术人员知晓申请日或者优先权日之前发明所属技术领域所有的普通技术知识，能够获知该领域中所有的现有技术，并且具有应用该日期之前常规实验手段的能力，所属领域普通技术人员可以在本申请给出的启示下，结合自身能力完善并实施本方案，一些典型的公知结构或者公知方法不应当成为所属领域普通技术人员实施本申请的障碍。应当指出，对于本领域的技术人员来说，在不脱离本发明结构的前提下，还可以做出若干变形和改进，这些也应该视为本发明的保护范围，这些都不会影响本发明实施的效果和专利的实用性。本申请要求的保护范围应当以其权利要求的内容为准，说明书中的具体实施方式等记载可以用于解释权利要求的内容。

Claims (10)

1.一种子母式空地协同多传感器融合三维建图系统，其特征在于，所述系统包括：
中心端和子端，所述中心端和子端均装备有由激光雷达、IMU和摄像头组成的SLAM系统；
数据获取模块：用于分别获取子端和中心端的三维激光点云数据、IMU惯导数据以及图像数据；
数据预处理模块：用于对所述子端和中心端的三维激光点云数据、IMU惯导数据以及图像数据进行滤波、去噪以及平滑处理；
特征提取模块：用于对所述子端和中心端的三维激光点云数据、IMU惯导数据以及图像数据进行特征提取分别生成与之对应的观测数据；
后端优化模块：用于基于不同的观测数据进行多源数据的联合优化；
关键帧选取模块：用于对优化后的数据进行关键帧选取；
局部地图构建模块：用于分别对选取的子端和中心端的关键帧进行该帧局部地图的构建，并基于构建的该帧局部地图进行相邻帧局部地图的融合生成子端局部地图和中心端局部地图；
全局地图构建模块：用于对子端局部地图和中心端局部地图进行融合生成全局地图；
输出模块：用于对所述全局地图进行输出。
2.根据权利要求1所述的子母式空地协同多传感器融合三维建图系统，其特征在于，所述中心端包括无人车平台，所述子端包括无人机平台；
无人机平台与无人车平台的SLAM系统同步进行环境数据的采集分别生成第一三维激光点云数据、第一IMU惯导数据、第一图像数据、第二三维激光点云数据、第二IMU惯导数据以及第二图像数据。
3.根据权利要求1所述的子母式空地协同多传感器融合三维建图系统，其特征在于，所述特征提取模块还用于：
在子端侧，基于第一三维激光点云数据提取原始激光点云中的几何特征，利用IMU预报的位姿对载体运动过程中可能导致的点云畸变进行去除生成第一观测数据；
基于第一IMU惯导数据，通过对两个相邻关键帧之间的IMU惯导数据进行预积分，生成第二观测数据；
基于第一图像数据，进行视觉特征提取与跟踪生成第三观测数据；
在中心端侧，基于第二三维激光点云数据提取原始激光点云中的几何特征，利用IMU预报的位姿对载体运动过程中可能导致的点云畸变进行去除生成第四观测数据；
基于第二IMU惯导数据，通过对两个相邻关键帧之间的IMU惯导数据进行预积分，生成第五观测数据；
基于第二图像数据，进行视觉特征提取与跟踪生成第六观测数据。
4.根据权利要求1所述的子母式空地协同多传感器融合三维建图系统，其特征在于，所述后端优化模块还用于：
在子端侧，对第一观测数据、第二观测数据和第三观测数据构建残差项分别生成第一激光点云匹配残差、第一IMU预积分残差和第一视觉重投影残差；
使用SLAM系统中的Ceres Solver库对所述第一激光点云匹配残差、第一IMU预积分残差和第一视觉重投影残差进行非线性最小二乘优化求解，完成多源数据的联合优化；
在中心端侧，对第四观测数据、第五观测数据和第六观测数据构建残差项分别生成第二激光点云匹配残差、第二IMU预积分残差和第二视觉重投影残差；
使用SLAM系统中的Ceres Solver库对所述第二激光点云匹配残差、第二IMU预积分残差和第二视觉重投影残差进行非线性最小二乘优化求解，完成多源数据的联合优化。
5.根据权利要求1所述的子母式空地协同多传感器融合三维建图系统，其特征在于，所述关键帧选取模块还用于：在子端和中心端分别基于连续跟踪的特征点判断当前帧与上一帧的平均视差,如果平均视差大于设定的阈值,则判断当前帧为关键帧，并将关键帧存储至中心端的关键帧数据库中；
所述系统还包括回环检测模块，用于对关键帧数据库进回环检测。
6.根据权利要求1所述的子母式空地协同多传感器融合三维建图系统，其特征在于，所述局部地图构建模块包括多源数据融合建图模块和局部地图融合模块；
所述多源数据融合建图模块用于：
在子端，通过对提取的第一三维激光点云数据和第一图像数据的特征点进行匹配；
根据特征点的匹配结果，计算激光雷达和摄像头之间的相对位姿；
采用非线性优化方法将第一三维激光点云和第一图像数据与第一IMU惯导数据进行融合，估计子端的位姿；
根据位姿估计结果，完成在子端该帧局部地图的构建；
在中心端，通过对提取的第二三维激光点云数据和第二图像数据的特征点进行匹配；
根据特征点的匹配结果，计算激光雷达和摄像头之间的相对位姿；
采用非线性优化方法将第二三维激光点云和第二图像数据与第二IMU惯导数据进行融合，估计中心端的位姿；
根据位姿估计结果，完成在中心端该帧局部地图的构建；
所述局部地图融合模块用于：
采用基于字典机制的相似度检测算法分别对构建的该帧的子端局部地图和该帧的中心端局部地图进行重叠区域的检测，采用特征点匹配算法完成特征点的匹配；
基于相邻两帧的IMU惯导数据分别对子端的无人机和中心端的无人车进行运动估计和位姿优化，进而完成子端和中心端的相邻两帧局部地图的融合分别生成子端局部地图和中心端局部地图；
所述局部地图融合模块还用于分别对子端局部地图和中心端局部地图进行更新，并将更新后的子端局部地图存储在位于中心端的地图堆栈中。
7.根据权利要求5所述的子母式空地协同多传感器融合三维建图系统，其特征在于，所述平均视差计算公式为：
式中,N表示连续跟踪特征点的个数；k表示特征标识ID；duk＝ujk－uik；dvk＝vjk－vik；u和v表示像素坐标；i和j分别表示上一帧和当前帧序号。
8.根据权利要求1所述的子母式空地协同多传感器融合三维建图系统，其特征在于，所述全局地图构建模块还用于：
将子端局部地图和中心端局部地图的坐标系转换到统一的世界坐标系下；
分别对子端局部地图和中心端局部地图进行特征点线的提取；
基于所述提取的特征点，采用基于字典机制的相似度检测算法进行子端和中心端局部地图重叠区域的检测；
根据重叠区域的检测结果，对子端和中心端的局部地图的位姿进行优化和更新；
对优化更新后的子端和中心端的局部地图进行特征点的匹配以完成局部地图融合生成全局地图。
9.根据权利要求8所述的子母式空地协同多传感器融合三维建图系统，其特征在于，所述对优化更新后的子端和中心端的局部地图进行特征点的匹配以完成局部地图融合生成全局地图包括：
通过融合了3D-HARRIS关键点检测算法与方向直方图签名特征描述子算法分别对优化更新后的子端和中心端局部地图进行特征描述；
通过采样一致性算法分别获得子端局部地图和中心端局部地图的初始位姿；
通过ICP算法分别获取子端局部地图和中心端局部地图精确的位姿变换结果进而完成对子端局部地图和中心端局部地图的融合生成全局地图。
10.根据权利要求1所述的子母式空地协同多传感器融合三维建图系统，其特征在于，所述系统还包括地图融合优化模块，所述融合优化模块置于中心端，用于利用子端无人机和中心端无人车提供的IMU惯导数据，计算出两者的相对位姿关系，进行位置与位姿融合优化。
CN202311438987.0A 2023-10-31 2023-10-31 一种子母式空地协同多传感器融合三维建图系统 Pending CN117470259A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311438987.0A CN117470259A (zh) 2023-10-31 2023-10-31 一种子母式空地协同多传感器融合三维建图系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311438987.0A CN117470259A (zh) 2023-10-31 2023-10-31 一种子母式空地协同多传感器融合三维建图系统

Publications (1)

Publication Number Publication Date
CN117470259A true CN117470259A (zh) 2024-01-30

Family

ID=89623362

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311438987.0A Pending CN117470259A (zh) 2023-10-31 2023-10-31 一种子母式空地协同多传感器融合三维建图系统

Country Status (1)

Country Link
CN (1) CN117470259A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117870652A (zh) * 2024-03-13 2024-04-12 北京航空航天大学 一种基于雷达、惯性、视觉融合的陆空在线协同定位方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117870652A (zh) * 2024-03-13 2024-04-12 北京航空航天大学 一种基于雷达、惯性、视觉融合的陆空在线协同定位方法
CN117870652B (zh) * 2024-03-13 2024-05-14 北京航空航天大学 一种基于雷达、惯性、视觉融合的陆空在线协同定位方法

Similar Documents

Publication Publication Date Title
CN112197770B (zh) 一种机器人的定位方法及其定位装置
CN111983639B (zh) 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
CN106840148B (zh) 室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法
Kanade et al. Real-time and 3D vision for autonomous small and micro air vehicles
CN106595659A (zh) 城市复杂环境下多无人机视觉slam的地图融合方法
Wan et al. Illumination-invariant image matching for autonomous UAV localisation based on optical sensing
Senlet et al. Satellite image based precise robot localization on sidewalks
CN105352509A (zh) 地理信息时空约束下的无人机运动目标跟踪与定位方法
CN104281148A (zh) 基于双目立体视觉的移动机器人自主导航方法
CN117470259A (zh) 一种子母式空地协同多传感器融合三维建图系统
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
Mi et al. A vision-based displacement measurement system for foundation pit
CN117152249A (zh) 基于语义一致性的多无人机协同建图与感知方法及系统
CN115371673A (zh) 一种未知环境中基于Bundle Adjustment的双目相机目标定位方法
Al-Rawabdeh et al. A robust registration algorithm for point clouds from UAV images for change detection
Veth et al. Two-dimensional stochastic projections for tight integration of optical and inertial sensors for navigation
CN112945233A (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN116957360A (zh) 一种基于无人机的空间观测与重建方法及系统
CN114459467B (zh) 一种未知救援环境中基于vi-slam的目标定位方法
Aggarwal Machine vision based SelfPosition estimation of mobile robots
CN113403942A (zh) 一种基于标签辅助的桥梁检测无人机视觉导航方法
Huntsberger et al. Sensory fusion for planetary surface robotic navigation, rendezvous, and manipulation operations
Mounier et al. High-Precision Positioning in GNSS-Challenged Environments: A LiDAR-Based Multi-Sensor Fusion Approach with 3D Digital Maps Registration
Shin et al. 3D LiDAR-based point cloud map registration: Using spatial location of visual features

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