[go: up one dir, main page]

CN113516692B - 一种多传感器融合的slam方法和装置 - Google Patents

一种多传感器融合的slam方法和装置 Download PDF

Info

Publication number
CN113516692B
CN113516692B CN202110541906.4A CN202110541906A CN113516692B CN 113516692 B CN113516692 B CN 113516692B CN 202110541906 A CN202110541906 A CN 202110541906A CN 113516692 B CN113516692 B CN 113516692B
Authority
CN
China
Prior art keywords
camera
coordinate system
map
key frame
slam
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
Application number
CN202110541906.4A
Other languages
English (en)
Other versions
CN113516692A (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.)
SAIC Motor Corp Ltd
Shanghai Automotive Industry Corp Group
Original Assignee
SAIC Motor Corp Ltd
Shanghai Automotive Industry Corp Group
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 SAIC Motor Corp Ltd, Shanghai Automotive Industry Corp Group filed Critical SAIC Motor Corp Ltd
Priority to CN202110541906.4A priority Critical patent/CN113516692B/zh
Publication of CN113516692A publication Critical patent/CN113516692A/zh
Application granted granted Critical
Publication of CN113516692B publication Critical patent/CN113516692B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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/292—Multi-camera tracking
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F9/00—Arrangements for program control, e.g. control units
    • G06F9/06—Arrangements for program control, e.g. control units using stored programs, i.e. using an internal store of processing equipment to receive or retain programs
    • G06F9/46—Multiprogramming arrangements
    • G06F9/50—Allocation of resources, e.g. of the central processing unit [CPU]
    • G06F9/5005—Allocation of resources, e.g. of the central processing unit [CPU] to service a request
    • G06F9/5027—Allocation of resources, e.g. of the central processing unit [CPU] to service a request the resource being a machine, e.g. CPUs, Servers, Terminals
    • 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/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments 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/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
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/77—Determining position or orientation of objects or cameras using statistical methods
    • 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
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2209/00—Indexing scheme relating to G06F9/00
    • G06F2209/50—Indexing scheme relating to G06F9/50
    • G06F2209/5018—Thread allocation
    • 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/10016—Video; Image sequence
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Multimedia (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Software Systems (AREA)
  • Automation & Control Theory (AREA)
  • Evolutionary Computation (AREA)
  • Probability & Statistics with Applications (AREA)
  • Artificial Intelligence (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明提供一种多传感器融合的SLAM方法和装置，方案为了扩大视觉传感器观测视野，避免因为遮挡、突发强光照和无明显纹理区域带来的特征点提取失效，本发明引入了多摄像机；同时，为了消除单目尺度不确定和快速旋转引起的单目退化，将IMU和轮速计的惯导观测与视觉观测紧耦合，以得到一个更加鲁棒的SLAM系统，以使得所述多传感器融合的SLAM装置具有更广的视野范围。

Description

一种多传感器融合的SLAM方法和装置
技术领域
本发明涉及技术领域，具体涉及一种基于多摄像机、IMU和轮速计的多传感器融合的SLAM方法和装置。
背景技术
即时定位与地图构建(Simultaneous Localization And Mapping，以下简称SLAM)技术是一种在未知的环境和位置下，运动体以自身传感器的观测作为输入，实时地输出自身的位置和姿态，同时根据位姿增量式构建地图的方法。SLAM技术目前在自动驾驶、高精地图、机器人、无人机和AR/VR等领域都有着重要应用。
SLAM按照主要传感器的不同可以分为激光雷达为主的激光SLAM和单目、双目、RGBD等相机为主的视觉SLAM。激光SLAM相对已经发展得比较成熟，但是考虑到大规模量产应用的成本，视觉SLAM仍是目前研究的主流。
对于视觉SLAM来说，双目相机测量范围受到基线和分辨率限制，且需要大量计算才能恢复像素深度，实时性较差；深度相机可以直接得到像素深度，但是价格昂贵、视野小、测量范围窄、光照敏感，目前主要用于室内。单目相机以其体积小、成本低廉、实时性好受到广泛关注，但缺点是需要平移才能确定深度，且本身存在尺度不确定性，导致重建轨迹和真实轨迹之间相差一个缩放因子。目前，单目视觉SLAM有众多优秀的开源框架，如ORB-SLAM系列、LSD-SLAM、DSO、SVO等。
IMU和轮速计不依赖外界环境，通过预积分能够给出运动体自身的速度和角速度，具有重建的绝对尺度；较高的输出频率可以在图像帧间给出预测结果，根据预测结果能够减小两帧间匹配的搜索范围，避免单目快速旋转时退化导致轨迹跟丢。但与此同时，IMU和轮速计的数据误差通过累积会不断放大，和视觉观测耦合优化可以减少由偏置带来的发散和累积误差，使得结果更加准确。目前，单目视觉融合IMU的SLAM开源框架主要有VINS、OKVINS、VINS-Fusion等，这些方案要求硬件对齐精确，同时初始化较为严格，需要给出尺度、偏置和重力方向。在本发明中，引入轮速计可以直接获得速度变量，简化初始化过程。
在实际应用中，单路相机观测往往视野较为局限，当视野范围出现遮挡、强光照或无明显纹理区域时会导致特征点跟丢，从而引发系统失效。多摄像机系统提供多个视角的观测，扩宽了视野范围，当其中某路摄像机失效时，也能通过其他摄像机继续追踪和建图，大大提高了系统的鲁棒性。Multicol-SLAM提出了基于鱼眼相机模型的多路纯视觉SLAM方法，直接在多个鱼眼相机图像上提取特征点和描述子，容易存在误匹配且实时性较差；同时去掉了鱼眼图像畸变较大的边缘，使得点、线特征的提取、匹配和优化都存在较大困难；它还要求相机视野必须存在视场重叠以确定尺度，使其应用具有局限性。Cubemap SLAM在将单路鱼眼图像用透视立方体模型校正，充分利用图像信息，但没有克服单路视野的局限性，同时单目无法恢复绝对尺度。
发明内容
有鉴于此，本发明实施例提供一种基于多摄像机、IMU和轮速计的多传感器融合的SLAM方法，以实现能给定绝对尺度、扩大观测视野的SLAM系统。
为实现上述目的，本发明实施例提供如下技术方案：
一种多传感器融合的SLAM方法，包括：
标定SLAM系统中相机的内参；
标定SLAM系统中相机、轮速计和IMU的外参；
基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型；
控制所述SLAM系统中多相机和轮速计松耦合初始化；
采用追踪线程获取初始化后的地图点和位姿，基于参考帧至当前关键帧获取到的图像数据、IMU数据和轮速计数据计算并优化当前关键帧位姿，将所述关键帧发送至局部建图线程；
采用局部建图线程获取追踪线程发送的关键帧，基于所述关键帧三角化生成新的地图点并融合，筛选并剔除冗余关键帧和冗余地图点；
采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正。
可选的，上述多传感器融合的SLAM方法中，当所述SLAM系统中相机为透视相机时，所述标定的SLAM系统中相机的内参，包括但不限于焦距、透视图像中心坐标、畸变系数中的一项或多项组合；当所述SLAM系统中相机为鱼眼相机时，所述标定的SLAM系统中相机的内参，包括但不限于正向映射系数，逆向映射系数，鱼眼图像中心坐标，仿射变换系数中的一项或多项的组合。
可选的，上述多传感器融合的SLAM方法中，基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型，包括：
基于所述内参以及外参，计算得到世界坐标系到本体坐标系的姿态变换矩阵、本体坐标系到相机坐标系的姿态变换矩阵、鱼眼相机的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵；
将公式作为相机投影模型，其中，所述p为世界坐标系下的地图点P(X，Y，Z)投影到相机ci的像素坐标系下得到的像素点p(x，y)，所述Kj为第j个虚拟透视相机的透视变换矩阵，所述为从鱼眼相机ci的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵，所述为本体坐标系到第i个相机ci相机坐标系的位姿变换矩阵，所述Tbw为从世界坐标系到本体坐标系的位姿变换矩阵，P为世界坐标系下的地图点P(X，Y，Z)。
可选的，上述多传感器融合的SLAM方法中，所述控制所述SLAM系统中多相机和轮速计松耦合初始化，包括：
通过对极约束分别计算每路相机的本质矩阵，选择内点最多的矩阵对应的相机作为初始化相机；
进行视觉初始化得到所述初始化相机的相机光心的平移距离，记为第一平移距离；
通过轮速计预积分得到相邻两帧之间的本体坐标系坐标点的平移距离；
基于所述本体坐标系坐标点的平移距离计算得到相邻两帧之间所述初始化相机的相机光心的平移距离，记为第二平移距离；
基于所述第一平移距离和第二平移距离计算尺度因子；
基于所述尺度因子恢复姿态和地图点的绝对尺度，并对多相机和轮速计进行初始化。
可选的，上述多传感器融合的SLAM方法中，采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正，包括：
将与当前关键帧相同的单词数超过阈值、相似度检测大于相邻关键帧最低分数的关键帧作为候选帧，再从所述候选帧中选出具有连续性的关键帧作为回环候选帧，所述相邻关键帧最低分数指的是：所述当前关键帧的相邻关键帧与所述前关键帧的最低相似度；
通过词袋向量加速描述子的匹配，利用RANSAC计算当前关键帧和闭环候选帧的Sim3变换，并根据初步求得的Sim3变换对地图点投影得到更多匹配的地图点；
将两个关键帧对应的Sim3变换和地图点作为顶点，遍历所有地图点的观测，添加正向和逆向投影的二元边，固定地图点的位姿，通过最小化重投影误差来优化两帧的Sim3变换；
根据优化后的Sim3，将闭环关键帧及其相连关键帧的地图点都投影到当前关键帧进行匹配，以得到更多的匹配点；
通过对优化后的Sim3求解，基于求解的Sim3，调整关键帧位姿和地图点位置。
一种多传感器融合的SLAM装置，包括：
标定单元，用于标定SLAM系统中相机的内参；标定SLAM系统中相机、轮速计和IMU的外参；
投影计算单元，用于基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型；
初始化单元，用于控制所述SLAM系统中多相机和轮速计松耦合初始化；
追踪线程单元，用于采用追踪线程获取初始化后的地图点和位姿，基于参考帧至当前关键帧获取到的图像数据、IMU数据和轮速计数据计算并优化当前关键帧位姿，将所述关键帧发送至局部建图线程；
局部建图线程单元，用于采用局部建图线程插入追踪线程发送的关键帧，基于所述关键帧三角化生成新的地图点并融合，筛选并剔除冗余关键帧和冗余地图点；
回环检测单元，用于采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正。
可选的，上述多传感器融合的SLAM装置中，当所述SLAM系统中相机为透视相机时，所述标定的SLAM系统中相机的内参，包括但不限于焦距、透视图像中心坐标、畸变系数中的一项或多项组合；当所述SLAM系统中相机为鱼眼相机时，所述标定的SLAM系统中相机的内参，包括但不限于正向映射系数，逆向映射系数，鱼眼图像中心坐标，仿射变换系数中的一项或多项的组合。
可选的，上述多传感器融合的SLAM装置中，所述投影计算单元在基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型时，具体用于：
基于所述内参以及外参，计算得到世界坐标系到本体坐标系的姿态变换矩阵、本体坐标系到相机坐标系的姿态变换矩阵、鱼眼相机的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵；
基于公式计算得到相机投影模型Kj，其中，所述p为世界坐标系下的地图点为P(X，Y，Z)投影到相机ci的像素坐标系下得到的像素点p(x，y)，所述Kj为第j个虚拟透视相机的透视变换矩阵，所述为从鱼眼相机ci的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵，所述为本体坐标系到第i个相机ci相机坐标系的位姿变换矩阵，所述Tbw为从世界坐标系到本体坐标系的位姿变换矩阵。
可选的，上述多传感器融合的SLAM装置中，所述初始化单元在控制所述SLAM系统中相机和轮速计松耦合初始化时，具体用于：
通过对极约束分别计算每路相机的本质矩阵，选择内点最多的矩阵对应的相机作初始化相机；
进行视觉初始化得到所述初始化相机的相机光心的平移距离，记为第一平移距离；
通过轮速计预积分得到相邻两帧之间的本体坐标系坐标点的平移距离；
基于所述本体坐标系坐标点的平移距离计算得到相邻两帧之间所述初始化相机的相机光心的平移距离，记为第二平移距离；
基于所述第一平移距离和第二平移距离计算尺度因子；
基于所述尺度因子恢复姿态和地图点的绝对尺度，并对所述IMU轮速计进行初始化。
可选的，上述多传感器融合的SLAM装置中，回环检测单元，采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正时，具体用于：
将与当前关键帧相同的单词数超过阈值、相似度检测大于相邻关键帧最低分数的关键帧作为候选帧，再从所述候选帧中选出具有连续性的关键帧作为回环候选帧，所述相邻关键帧最低分数指的是：所述当前关键帧的相邻关键帧与所述前关键帧的最低相似度；
通过词袋向量加速描述子的匹配，利用RANSAC计算当前关键帧和闭环候选帧的Sim3变换，并根据初步求得的Sim3变换对地图点投影得到更多匹配的地图点；
将两个关键帧对应的Sim3变换和地图点作为顶点，遍历所有地图点的观测，添加正向和逆向投影的二元边，固定地图点的位姿，通过最小化重投影误差来优化两帧的Sim3变换；
根据优化后的Sim3，将闭环关键帧及其相连关键帧的地图点都投影到当前关键帧进行匹配，以得到更多的匹配点；
通过对优化后的Sim3求解，基于求解的Sim3，调整关键帧位姿和地图点位置。
基于上述技术方案，本发明实施例提供的上述方案，包括：标定内参指的是标定SLAM系统中相机的内参，标定SLAM系统中相机、轮速计和IMU的外参，通过标定的内参和外参给定相机投影模型，对SLAM系统中的多摄像机和轮速计松耦合初始化，采用追踪线程根据新的图像、IMU和轮速计数据不断计算新的位姿、插入关键帧并且优化位姿，再采用局部建图线程对于追踪线程中送来的关键帧进行处理，三角化生成新的地图点并融合，检查剔除地图点和冗余关键帧，最后采用回环检测线程对局部建图线程送来的关键帧进行回环检测，克服了传统单路纯视觉SLAM系统中不能给定绝对尺度、单路视野受限的局限性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
图1为本申请实施例公开的多传感器融合的SLAM方法的流程示意图；
图2为本申请实施例公开的运动体t到t+Δt时刻近似圆弧运动示意图；
图3为本申请实施例提供的多传感器融合SLAM系统图优化结构示意图；
图4为本申请实施例公开的多传感器融合的SLAM装置的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本申请实施例公开了一种多传感器融合的SLAM方法，参见图1，该方法可以包括：
步骤S100：标定参数；
本步骤中的标定参数主要指的是标定内参和标定外参，其中，标定内参指的是标定SLAM系统中相机的内参。在本实例中，所述SLAM系统支持多摄像机系统。即，所述SLAM系统支持n(n≥1)个摄像机(所述摄像机可以为透视相机或者鱼眼相机)在运动体上以任意角度刚性布局，相机视场重叠与否不作要求，以二路鱼眼相机为例，鱼眼相机内参标定以Scaramuzza提出的基于泰勒级数的全向视觉模型为例，所述内参包括正向映射系数a0，a1，...，aN，逆向映射系数p0，p1，...，pN，鱼眼图像中心坐标u0和v0，仿射变换系数c、d、e，当然，本领域技术人员可以基于实际需求选择所需标定的相机内参。
所述标定外参，指的是标定SLAM系统中相机、轮速计和IMU的外参，其目的在于通过标定的外参给出相机、轮速计和IMU之间刚体变换的相对位姿。本例中，将SLAM系统中两个轮速计的中心点设置为运动体的本体坐标系(简称Body)坐标原点，Body坐标系的位姿可以看做运动体的绝对位姿。本领域技术人员可以基于实际需求选择本体坐标系位置和姿态。由于后续优化是紧耦合的，因此，将IMU、轮速计坐标系和Body坐标系重合，即运动体前进方向是IMU的x轴正方向，重力方向是IMU的z轴负方向。此时，该SLAM系统包含世界坐标系(World)、本体坐标系(Body)和多个相机坐标系(Camera)。外参标定需要将IMU轮速计参考系与Body坐标系对齐，并给出从Body坐标系到第i个相机ci相机坐标系的位姿变换关系
步骤S200：给定相机投影模型；
在本步骤中，基于内参和外参的标定结果给定相机投影模型。
具体的，根据内参和外参的标定结果计算投影模型的过程包括：
已知从世界坐标系到Body坐标系的位姿变换为Body坐标系到第i个相机ci相机坐标系的位姿变换为可以基于和求得从世界坐标系下的三维点到像素坐标系下二维点的变换。假设令世界坐标系下的地图点为P(X，Y，Z)，投影到相机ci的像素坐标系下得到像素点p(x，y)，则有如下关系：
其中，表示相机ci的投影模型。对于透视相机来说，可以用投影矩阵(Kn)和畸变系数(畸变系数k1、k2、p1、p2)来建模。对于鱼眼相机来说，可以用标定通用多项式模型来建模。
特别的，如果采用鱼眼相机作为视觉输入，虽然可以极大地扩展视野范围，但鱼眼图像存在较大畸变，后续特征提取和匹配存在一定问题。因此，本申请可以采用多针孔透视投影模型作为相机投影模型，将鱼眼相机的图像投影到针孔相机的成像平面上得到校正后的鱼眼图像。具体，该多针孔透视投影模型为N(N≥2)个光心与该鱼眼相机的光心重合的虚拟透视相机，各自的成像平面成一定的夹角，表示从鱼眼相机ci的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵，因此从世界坐标系下的三维点P(X，Y，Z)到第j个虚拟透视相机的像素坐标系下二维点p(x，y)的变换关系为：
所述即为相机投影模型，
其中Kj是第j个虚拟透视相机的透视变换矩阵。
步骤S300：多相机和轮速计松耦合初始化；
本步骤主要用于对多摄像机和轮速计进行松耦合初始化，在初始化开始时，SLAM系统传入多摄像机的初始帧和当前帧图像，每一帧均包含有来自多个摄像机的多张图像，这些图像的时间戳已经同步，即，这些图像具有相同的时间戳。在松耦合初始化过程中，SLAM系统的陀螺仪和轮速计数据对齐，假设轮速计是无偏的，可以由惯性观测的离散运动方程得到SLAM系统传入的相邻的两帧图像之间的预积分，从而基于所述预积分给出运动体的位置和姿态。所述多摄像机松耦合初始化即为视觉初始化，在视觉初始化的瞬间完成IMU的初始化，令陀螺仪的初始偏置为0，后续再对陀螺仪的偏置进行优化。视觉初始化对每一个相机的初始帧和当前帧图像进行特征提取和匹配，通过对极约束计算得到内点最多的相机像旋转R和平移t，把该相机作为Leading Camera(初始化相机)，以它的运动作为基准，根据外参关系得到运动体的位姿 三角化建立初始化地图。
由于初始化计算出的R和t具有单目的尺度不确定性，需要通过轮速计的观测恢复初始化地图和位姿的绝对尺度。所述SLAM系统中的视觉里程计所得的两帧间相机光心的移动距离为Svo，这个距离和世界坐标系下的真实距离相差一个预设的尺度因子。由于轮速计给出的是世界坐标系下目标物体的速度，因此可以给出绝对尺度。通过两帧之间轮速计的预积分可以由航位推算得到运动体在x轴方向的位移，同时考虑转动引起的位移，最终得两帧之间相机光心移动的距离Svio。因此真实的尺度为scale＝Svio/Svo，据此可以恢复位姿和地图点的绝对尺度，从而完成初始化。
具体的，在本方案中，
SLAM系统中的惯性传感器在t时刻的观测方程为：
其中为陀螺仪测出的角速度矢量在Body坐标系下的坐标，bg(t)为陀螺仪的偏置，ηg(t)为角速度噪声，是角速度在Body坐标系下的真实值；是轮速计测出的速度矢量在Body坐标系下的坐标，ηv(t)是速度噪声，是速度在Body坐标系下的真实值。t时刻到t+Δt时刻的离散运动方程为：
其中Rwb(t+Δt)表示从t+Δt时刻的Body坐标系到世界坐标系的旋转矩阵，Rwb(t)表示从t时刻的Body坐标系到世界坐标系的旋转矩阵。表示从t时刻到t+Δt时刻Body坐标系下的旋转矢量，可以看做一个李代数，经过指数映射得到李群表示从t+Δt时刻Body坐标系到t时刻的Body坐标系的旋转矩阵。假设运动体在两帧之间的运动满足恒定转弯率和速度幅度模型(CTRV)，即沿直线前进的同时还能以固定的转弯速率和恒定的速度运动，近似圆弧运动，如图2所示。假设这段时间内速度不变，表示从t时刻到t+Δt时刻Body坐标系下的位移矢量，表示从t+Δt时刻的Body坐标系到世界坐标系的位移矢量的旋转矩阵，二者相乘可得Δt时间内世界坐标系下的位移增量。
离散运动方程代入观测方程可得：
由于惯性观测的频率远高于视觉观测的频率，因此在第i帧和第j帧图像之间存在多个惯性观测数据，其预积分为：
可以通过预积分给出运动体的位置和姿态。在视觉初始化的瞬间完成IMU的初始化，令陀螺仪的初始偏置为0，后续再对偏置进行优化。
首先通过视觉初始化给出Body坐标系的位姿。对同一个相机的初始帧和当前帧图像进行特征提取和匹配，利用随机采样一致性算法(RANSAC)一共迭代X次，例如所述X可以为200，每次选取八组或其他组数的特征点通过对极约束计算本质矩阵E，计算所有特征点对应的极线在本质矩阵E下的误差，如果误差小于给定阈值则判定该特征点为内点，记录这些内点；同时记录该本质矩阵E下所有特征点对的累积得分，误差越大得分越低，记录得分最高的本质矩阵E作为相机ci初始化的本质矩阵Ei。对本质矩阵Ei进行奇异值分解(SVD)恢复出该相机的运动R和t，一共由四种组合。通过三角化恢复出该相机匹配的内点，去掉视场角外的点、深度为负的点和重投影误差过大的点，最终选择三角化点最多的R和t作为该相机的运动。令重建点数最多的相机作为Leading Camera(初始化相机)，以它的运动作为基准，根据外参关系得到运动体的位姿，即Body坐标系的位姿建立初始化地图。
由于纯视觉初始化计算出的R和t具有单目的尺度不确定性，需要通过轮速计的观测恢复初始化地图和位姿的绝对尺度。首先对初始帧和当前帧Body坐标系的位姿和地图点的位置做一次全局优化，使得地图点通过位姿投影到像素平面上的重投影误差最小。将初始帧的Leading Camera的相机坐标系作为世界坐标系，初始帧运动体的绝对位姿(即Body坐标系的位姿)为 其中表示旋转，表示平移(即初始帧相机光心在世界坐标系的坐标)。当前帧运动体的绝对位姿 其中表示旋转，表示平移(即当前帧相机光心在世界坐标系的坐标)。可得视觉里程计所得的两帧间相机光心的移动距离为 但这个距离的尺度是不准确的。
由于轮速计给出的是世界坐标系下的速度，因此可以给出绝对尺度。在这里可选地用初始帧和当前帧的惯性观测来计算，相机光心移动的距离可以分解成航位推算得到的x方向分量和旋转得到的y方向分量。首先通过两帧之间轮速计的预积分可以得到轮速计坐标系(即Body坐标系)坐标原点移动的距离Sodom。由于两帧之间间隔很短，因此Sodom近似看作运动体在x轴方向的平移量；同时由于两帧之间存在转动，将导致相机光心近似在y轴出现平移，现在计算转动引起的相机光心的平移量。已知Leading Camera和Body坐标系之间的外参在初始化过程中认为预先标定好的外参是不变的，即 是初始帧相机光心在初始帧Body坐标系下的坐标，经过转动之后，当前帧相机光心在初始帧Body坐标系下的坐标所以可得转动引起的相机光心移动的距离由轮速计和转动给出的两帧之间相机光心移动的距离因此真实的尺度为scale＝Svio/Svo，据此可以恢复位姿和地图点的绝对尺度，从而完成SLAM系统的初始化。
具体的，在本申请另一实施例公开的技术方案中，所述控制所述SLAM系统中多相机和轮速计松耦合初始化，包括：
通过对极约束分别计算每路相机的本质矩阵，选择内点最多的矩阵对应的相机作为初始化相机；
进行视觉初始化得到所述初始化相机的相机光心的平移距离，记为第一平移距离；
通过轮速计预积分得到相邻两帧之间的本体坐标系坐标点的平移距离；
基于所述本体坐标系坐标点的平移距离计算得到相邻两帧之间所述初始化相机的相机光心的平移距离，记为第二平移距离；
基于所述第一平移距离和第二平移距离计算尺度因子；
基于所述尺度因子恢复姿态和地图点的绝对尺度，并对多相机和轮速计进行初始化。
步骤S400：追踪线程(Tracking)。
获得初始化的地图点和位姿之后，随着新的图像、IMU和轮速计数据的传入，所述SLAM系统需要根据这些数据不断计算新的位姿、插入关键帧并且对关键帧位姿进行优化。在该过程中，追踪线程首先利用上一帧或上一关键帧的视觉和惯性观测值来计算两帧(上一帧或上一关键帧与当前帧)之间的位姿。
如果上次重定位的发生时间与当前时间之差不大于预设时长，则用多路视觉观测来解算位姿(Track Reference Key Frame)。对获取到的两帧图像的特征点进行匹配，具体的，匹配过程为：将特征点的描述子转化为词袋向量进行当前帧和参考帧之间的特征点匹配，通过计算距离阈值、比例阈值和角度投票剔除误匹配。对获取到的两帧图像的特征点进行匹配之后，令参考帧的位姿作为当前帧位姿的初始值，地图点、运动体位姿和多个相机的外参作为顶点，每个地图点的观测作为一条边，通过优化从世界坐标系下的三维点到像素坐标系的二维点的重投影误差来优化当前帧的运动体位姿。在本方案中总共可以优化四次，将每次优化后的观测分为outlier和inlier，其中，outlier不参与下一次优化，最后根据更新后的位姿剔除误匹配的地图点。可选地，更新完运动体位姿之后，可以将运动体位姿和地图点作为已知，每个相机的外参作为顶点，构造单元边的重投影误差来优化多相机的外参。
如果未触发重定位或者上次重定位发生时间与当前时间之差大于预设时长，则SLAM系统采用惯性观测来解算位姿(Track With Odom)。将参考帧的惯性观测的位姿和偏置作为当前帧的初始值，并计算从参考帧到当前帧惯性观测的预积分。将上一帧的地图点通过投影模型投影到当前帧，在小范围内根据描述子距离得到匹配点，通过角度投票剔除误匹配，将惯性观测得到的两帧的运动体位姿和惯性观测偏置、地图点和多相机外参作为顶点，通过两帧之间惯性观测的预积分，为两帧的位姿和偏置之间添加约束，同时将地图点的观测作为约束添加以当前位姿作为顶点的单元边，总共优化四次，将每次优化后的观测分为outlier和inlier，outlier不参与下一次优化，最后根据更新后的位姿剔除误匹配的地图点。可选地，可以通过最小化重投影误差继续优化多相机外参。
通过两帧之间的匹配和位姿优化得到当前帧的位姿后，对局部地图进行追踪以得到更多的匹配，继续优化当前帧位姿。具体来说，更新局部关键帧和局部地图点，并在局部地图中找到当前帧视野范围内的点，投影到当前帧进行匹配，以得到更多匹配的地图点。类似地，如果上次重定位的发生时间与当前时间之差不大于预设时长，则用多摄像机的视觉观测来解算位姿(Track Local Map)，位姿优化方法与Track Reference Key Frame中相同；如果未触发重定位或者上次重定位过去时间与当前时间之差不大于预设时长，则用视觉和惯性观测来解算位姿(Track Local Map With Odom)，位姿优化方法与Track WithOdom中相同。最后，通过更新后的当前帧运动体位姿来剔除误匹配的地图点。
如果两次追踪之后得到的匹配数量仍不足，将进入重定位模式。重定位过程首先将与当前关键帧相同的单词数超过阈值、相似度检测大于相邻关键帧最低分数的关键帧作为重定位候选帧，再用EPNP方法根据重定位候选帧计算当前帧的位姿和地图点。如果重定位失败，则整个SLAM系统重启。
上述优化过程重投影误差的代价函数为：
pij＝f(Ti，Pj)表示根据Body坐标系位姿将三维点Pj投影到像平面上的像素坐标。对于第m个鱼眼相机的第n个虚拟针孔相机来说，投影关系为：
pij＝KnRnTmTiPj
其中Tm表示从Body坐标系到第m个相机系的位姿变换，Rn表示第m个相机系到第n个虚拟针孔相机的位姿变换，Kn表示第n个虚拟针孔相机的投影矩阵。
上述优化过程图优化的结构如图3所示。表示t＝i时刻的位姿，也可以用惯性观测的位姿来表示，即同时，每一时刻的惯性观测还存在偏置Bi，该时刻惯性观测的位姿PRi与上一时刻的位姿PRi-1和偏置Bi-1之间存在预积分给定的约束边，同时两个时刻的偏置之间也存在约束边。在t＝1时刻相机c1看到地图点P0，相机c2看到地图点P0和P1。在t＝2时刻，相机c1看到地图点P1和P2，相机c2看到地图点P2和P3。表示t＝i时刻第m个相机的外参，则图优化可以表示成图3中所示结构，通过最小化边的观测误差可以对运动体位姿、多相机外参和地图点位置进行优化。
步骤S500：局部建图线程(Local Mapping)。
该线程对于追踪线程中送来的关键帧进行处理，检查剔除地图点，在两帧的多相机之间进行三角化恢复新的地图点并进行融合，联合插入的关键帧、相连的关键帧及所有地图点对位姿和地图点做局部优化(Local Bundle Adjustment)。最终通过不断加入新关键帧生成新地图点，剔除冗余关键帧和冗余地图点，为回环检测提供一个稳定的关键帧集合。
在本步骤中，当获取到的地图中的关键帧数量少于预设阈值时，采用滑动窗口的视觉观测来做局部优化。具体地，将共视图中所有相连关键帧的运动体位姿作为待优化的位姿顶点，将所有能被关键帧看到的地图点作为待优化的地图点顶点，将能看到这些地图点但不在共视图中的运动体位姿作为不参加优化、只提供约束关系的固定位姿顶点，将多个相机的外参作为固定的位姿顶点。遍历每个地图点的观测添加多元边，分别连接地图点、关键帧位姿顶点和外参顶点，通过最小化重投影误差来优化地图点位置和共视图中关键帧运动体位姿。可选地，如果需要优化每个相机的外参，遍历所有地图点的观测，添加以多个相机外参为顶点的单元边，通过最小化重投影误差优化相机外参。
当获取到的地图中的关键帧数量大于阈值时，采用滑动窗口的惯性观测来做局部优化。具体地，将共视图中所有相连关键帧的惯性观测位姿和偏置作为待优化的位姿顶点和偏置顶点，将所有能被关键帧看到的地图点作为待优化的地图点顶点，所有能看到局部地图点但不属于共视图的关键帧的惯性观测位姿和偏置作为不参加优化、只提供约束关系的固定位姿顶点和偏置顶点，将多个相机的外参作为固定的位姿顶点。通过当前关键帧和上一关键帧之间的预积分给出两帧之间位姿和偏置的约束关系作为观测边，遍历每个地图点的观测添加多元边，连接地图点、关键帧位姿顶点和外参顶点，通过最小化重投影误差来优化地图点位置和共视图中关键帧运动体位姿。可选地，如果需要优化每个相机的外参，遍历所有地图点的观测，添加以多个相机外参为顶点的单元边，通过最小化重投影误差优化相机外参。
步骤S400：回环检测线程。
该线程对局部建图线程送来的关键帧进行回环检测，根据回环帧调整地图内所有关键帧位姿和地图点位置，消除累积漂移误差，校正之前的轨迹和地图。
具体过程包括：
首先将与当前关键帧相同的单词数超过阈值、相似度检测大于相邻关键帧最低分数的关键帧作为候选帧，再从中选出具有连续性的关键帧作为回环候选帧。通过词袋向量加速描述子的匹配，利用RANSAC计算当前关键帧和闭环候选帧的Sim3变换，并根据初步求得的Sim3变换对地图点投影得到更多匹配的地图点。将两个关键帧的Sim3变换和地图点作为顶点，遍历所有地图点的观测，添加正向和逆向投影的二元边，固定地图点的位姿，通过最小化重投影误差来优化两帧的Sim3变换，得到更精确的Sim3变换。根据更新后的Sim3变换，将闭环关键帧及其相连关键帧的地图点都投影到当前关键帧进行匹配，以得到更多的匹配点。根据求解的Sim3变换，调整关键帧位姿和地图点位置，消除漂移误差，最后通过全局优化对所有的关键帧和地图点进行更新，校正之前的轨迹和地图。
相较于现有技术而言，本申请具有以下优点：
通过在SLAM系统中引入多摄像机，可以有效地扩展运动体观测视野，防止因相机遮挡、视野无明显特征和转弯引起视角变化剧烈等原因造成某路相机失效而引起整个系统的崩溃。在汽车上运行该系统的实验表明，当后方出现来车时，车灯的光照会使得后路摄像机图像无法提取特征点引起失效，如果是单路系统此时将发生重启，但多摄像机系统可以继续依靠其他摄像机进行追踪建图。
本所述SLAM系统支持透视摄像机和鱼眼摄像机，对于鱼眼摄像机提出多针孔透视投影模型。鱼眼摄像机视场角较大，但同时图像边缘存在较大畸变，现有的特征点和描述子不适用于大畸变的鱼眼图像，Multicol-SLAM中提出一种改进后的描述子但实时性较差，同时为了避免误匹配一般避开鱼眼图像边缘只利用图像中部的信息；线特征的提取、匹配和优化在鱼眼图像上难以直接进行。本发明利用多针孔透视投影模型将鱼眼图像校正成透视投影图像，实验表明，成熟的ORB特征实时性好、误匹配少，特征点的提取和匹配可以在整张图上进行，充分利用图像边缘信息，同时校正后的图像也适用于线特征。
并且，本发明不要求多摄像机之间存在视场重叠，通过利用轮速计给出绝对尺度，使得初始化过程具有较好的实时性和鲁棒性。由于轮速计直接给出速度观测，避免了初始化时IMU需要对齐重力方向；在视觉初始化的同时完成对IMU和轮速计的初始化，假设轮速计是无偏的，陀螺仪的初始偏置设置为0，将偏置加入到后续优化。因此大大简化了初始化时间和计算量，能实时给出一个较为鲁棒的初始化结果。
传统的单目SLAM系统框架如ORB-SLAM2中，将运动体的位姿和地图点作为优化顶点，引入多摄像机之后，相机外参也可选地可以作为待优化顶点；同时轮速计和IMU的预积分对两帧之间的惯性观测位姿惯性偏置加入了新的约束，通过最小化重投影误差的边可以对关键帧位姿、地图点位置和相机外参进行优化。结果表明，加入多摄像机和惯性观测的优化使得SLAM系统运行过程更加鲁棒。
本实施例中公开了一种多传感器融合的SLAM装置，装置中的各个单元的具体工作内容，请参见上述方法实施例的内容，下面对本发明实施例提供的多传感器融合的SLAM装置进行描述，下文描述的多传感器融合的SLAM装置与上文描述的多传感器融合的SLAM方法可相互对应参照。
参见图4，本申请实施例公开的多传感器融合的SLAM装置可以包括：
标定单元100，用于标定SLAM系统中相机的内参；标定SLAM系统中相机、轮速计和IMU的外参；
投影计算单元200，用于基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型；
初始化单元300，用于控制所述SLAM系统中多摄像机和轮速计松耦合初始化；
追踪线程单元400，用于采用追踪线程获取初始化后的地图点和位姿，基于参考帧至当前关键帧获取到的图像数据、IMU数据和轮速计数据计算并优化当前关键帧位姿，将所述关键帧发送至局部建图线程获；
局部建图线程单元500，用于采用局部建图线程插入追踪线程发送的关键帧，基于所述关键帧三角化生成新的地图点并融合，筛选并剔除冗余关键帧和冗余地图点；
回环检测单元600，用于采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正。
与上述方法相对应，所述标定单元标定的SLAM系统中相机的内参，对于透视相机，内参包括但不限于焦距、透视图像中心坐标、畸变系数中的一项或多项组合；对于鱼眼相机，内参包括但不限于正向映射系数，逆向映射系数，鱼眼图像中心坐标，仿射变换系数中的一项或多项的组合。
与上述方法相对应，所述投影计算单元在基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型时，具体用于：
基于所述内参以及外参，计算得到世界坐标系到本体坐标系的姿态变换矩阵、本体坐标系到相机坐标系的姿态变换矩阵、鱼眼相机的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵；
基于公式计算得到相机投影模型Kj，其中，所述p为世界坐标系下的地图点为P(X，Y，Z)投影到相机ci的像素坐标系下得到的像素点p(x，y)，所述Kj为第j个虚拟透视相机的透视变换矩阵，所述表示从鱼眼相机ci的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵，所述为本体坐标系到第i个相机ci相机坐标系的位姿变换矩阵，所述Tbw为从世界坐标系到本体坐标系的位姿变换矩阵。
与上述方法相对应，所述初始化单元在控制所述SLAM系统中多摄像机和轮速计松耦合初始化时，具体用于：
通过对极约束分别计算每路相机的本质矩阵，选择内点最多的矩阵对应的相机作为初始化相机；
进行视觉初始化得到所述初始化相机的相机光心的平移距离，记为第一平移距离；
通过轮速计预积分得到相邻两帧之间的本体坐标系坐标点的平移距离；
基于所述本体坐标系坐标点的平移距离计算得到相邻两帧之间所述初始化相机的相机光心的平移距离，记为第二平移距离；
基于所述第一平移距离和第二平移距离计算尺度因子；
基于所述尺度因子恢复姿态和地图点的绝对尺度，并对所述IMU轮速计进行初始化。
与上述方法相对应，回环检测单元，采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正时，具体用于：
将与当前关键帧相同的单词数超过阈值、相似度检测大于相邻关键帧最低分数的关键帧作为候选帧，再从所述候选帧中选出具有连续性的关键帧作为回环候选帧，所述相邻关键帧最低分数指的是：所述当前关键帧的相邻关键帧与所述前关键帧的最低相似度；
通过词袋向量加速描述子的匹配，利用RANSAC计算当前关键帧和闭环候选帧的Sim3变换，并根据初步求得的Sim3变换对地图点投影得到更多匹配的地图点；
将两个关键帧对应的Sim3变换和地图点作为顶点，遍历所有地图点的观测，添加正向和逆向投影的二元边，固定地图点的位姿，通过最小化重投影误差来优化两帧的Sim3变换；
根据优化后的Sim3，将闭环关键帧及其相连关键帧的地图点都投影到当前关键帧进行匹配，以得到更多的匹配点；
通过对优化后的Sim3求解，基于求解的Sim3，调整关键帧位姿和地图点位置。
为了描述的方便，描述以上系统时以功能分为各种模块分别描述。当然，在实施本发明时可以把各模块的功能在同一个或多个软件和/或硬件中实现。
本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统或系统实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。以上所描述的系统及系统实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (8)

1.一种多传感器融合的SLAM方法，其特征在于，包括：
标定SLAM系统中相机的内参；
标定SLAM系统中相机、轮速计和IMU的外参；
基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型；
控制所述SLAM系统中多相机和轮速计松耦合初始化；
采用追踪线程获取初始化后的地图点和位姿，基于获取到的图像数据、IMU数据和轮速计数据、基于所述图像数据插入关键帧并对所述位姿和地图点进行修正，将所述关键帧发送至局部建图线程；
采用局部建图线程获取追踪线程发送的关键帧，基于所述关键帧三角化生成新的地图点并融合地图点，筛选并剔除冗余关键帧和冗余地图点；
采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正；
所述控制所述SLAM系统中多相机和轮速计松耦合初始化，包括：
通过对极约束分别计算每路相机的本质矩阵，选择内点最多的矩阵对应的相机作为初始化相机；
进行视觉初始化得到所述初始化相机的相机光心的平移距离，记为第一平移距离；
通过轮速计预积分得到相邻两帧之间的本体坐标系坐标点的平移距离；
基于所述本体坐标系坐标点的平移距离计算得到相邻两帧之间所述初始化相机的相机光心的平移距离，记为第二平移距离；
基于所述第一平移距离和第二平移距离计算尺度因子；
基于所述尺度因子恢复姿态和地图点的绝对尺度，并对多相机和轮速计进行初始化。
2.根据权利要求1所述的多传感器融合的SLAM方法，其特征在于，当所述SLAM系统中相机为透视相机时，所述标定的SLAM系统中相机的内参，包括但不限于焦距、透视图像中心坐标、畸变系数中的一项或多项组合；当所述SLAM系统中相机为鱼眼相机时，所述标定的SLAM系统中相机的内参，包括但不限于正向映射系数，逆向映射系数，鱼眼图像中心坐标，仿射变换系数中的一项或多项的组合。
3.根据权利要求1所述的多传感器融合的SLAM方法，其特征在于，基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型，包括：
基于所述内参以及外参，计算得到世界坐标系到本体坐标系的姿态变换矩阵、本体坐标系到相机坐标系的姿态变换矩阵、鱼眼相机的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵；
将公式作为相机投影模型，其中，所述p为世界坐标系下的地图点P(X,Y,Z)投影到相机ci的像素坐标系下得到的像素点p(x,y)，所述Kj为第j个虚拟透视相机的透视变换矩阵，所述为从鱼眼相机ci的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵，所述为本体坐标系到第i个相机ci相机坐标系的位姿变换矩阵，所述Tbw为从世界坐标系到本体坐标系的位姿变换矩阵，P为世界坐标系下的地图点P(X,Y,Z)。
4.根据权利要求1所述的多传感器融合的SLAM方法，其特征在于，采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正，包括：
将与当前关键帧相同的单词数超过阈值、相似度检测大于相邻关键帧最低分数的关键帧作为候选帧，再从所述候选帧中选出具有连续性的关键帧作为回环候选帧，所述相邻关键帧最低分数指的是：所述当前关键帧的相邻关键帧与所述前关键帧的最低相似度；
通过词袋向量加速描述子的匹配，利用RANSAC计算当前关键帧和闭环候选帧的Sim3变换，并根据初步求得的Sim3变换对地图点投影得到更多匹配的地图点；
将两个关键帧对应的Sim3变换和地图点作为顶点，遍历所有地图点的观测，添加正向和逆向投影的二元边，固定地图点的位姿，通过最小化重投影误差来优化两帧的Sim3变换；
根据优化后的Sim3，将闭环关键帧及其相连关键帧的地图点都投影到当前关键帧进行匹配，以得到更多的匹配点；
通过对优化后的Sim3求解，基于求解的Sim3，调整关键帧位姿和地图点位置。
5.一种多传感器融合的SLAM装置，其特征在于，包括：
标定单元，用于标定SLAM系统中相机的内参；标定SLAM系统中相机、轮速计和IMU的外参；
投影计算单元，用于基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型；
初始化单元，用于控制所述SLAM系统中多相机和轮速计松耦合初始化；
追踪线程单元，用于采用追踪线程获取初始化后的地图点和位姿，基于参考帧至当前关键帧获取到的图像数据、IMU数据和轮速计数据计算并优化当前关键帧位姿，将所述关键帧发送至局部建图线程；
局部建图线程单元，用于采用局部建图线程插入追踪线程发送的关键帧，基于所述关键帧三角化生成新的地图点并融合，筛选并剔除冗余关键帧和冗余地图点；
回环检测单元，用于采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正；
所述初始化单元在控制所述SLAM系统中多相机和轮速计松耦合初始化时，具体用于：
通过对极约束分别计算每路相机的本质矩阵，选择内点最多的矩阵对应的相机作为初始化相机；
进行视觉初始化得到所述初始化相机的相机光心的平移距离，记为第一平移距离；
通过轮速计预积分得到相邻两帧之间的本体坐标系坐标点的平移距离；
基于所述本体坐标系坐标点的平移距离计算得到相邻两帧之间所述初始化相机的相机光心的平移距离，记为第二平移距离；
基于所述第一平移距离和第二平移距离计算尺度因子；
基于所述尺度因子恢复姿态和地图点的绝对尺度，并对多相机和轮速计进行初始化。
6.根据权利要求5所述的多传感器融合的SLAM装置，其特征在于，当所述SLAM系统中相机为透视相机时，所述标定的SLAM系统中相机的内参，包括但不限于焦距、透视图像中心坐标、畸变系数中的一项或多项组合；当所述SLAM系统中相机为鱼眼相机时，所述标定的SLAM系统中相机的内参，包括但不限于正向映射系数，逆向映射系数，鱼眼图像中心坐标，仿射变换系数中的一项或多项的组合。
7.根据权利要求5所述的多传感器融合的SLAM装置，其特征在于，所述投影计算单元在基于所述相机的内参以及相机、轮速计和IMU的外参的标定结果给定相机投影模型时，具体用于：
基于所述内参以及外参，计算得到世界坐标系到本体坐标系的姿态变换矩阵、本体坐标系到相机坐标系的姿态变换矩阵、鱼眼相机的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵；
基于公式计算得到相机投影模型Kj，其中，所述p为世界坐标系下的地图点为P(X,Y,Z)投影到相机ci的像素坐标系下得到的像素点p(x,y)，所述Kj为第j个虚拟透视相机的透视变换矩阵，所述为从鱼眼相机ci的相机坐标系到第j个虚拟透视相机的相机坐标系的变换矩阵，世界坐标系所述为本体坐标系到第i个相机ci相机坐标系的位姿变换矩阵，所述Tbw为从世界坐标系到本体坐标系的位姿变换矩阵，P为世界坐标系下的地图点P(X,Y,Z)。
8.根据权利要求5所述的多传感器融合的SLAM装置，其特征在于，回环检测单元，采用回环检测线程对所述局部建图线程筛选后的关键帧进行回环检测，根据回环帧调整对筛选后的关键帧位姿和地图点位置进行修正时，具体用于：
将与当前关键帧相同的单词数超过阈值、相似度检测大于相邻关键帧最低分数的关键帧作为候选帧，再从所述候选帧中选出具有连续性的关键帧作为回环候选帧，所述相邻关键帧最低分数指的是：所述当前关键帧的相邻关键帧与所述前关键帧的最低相似度；
通过词袋向量加速描述子的匹配，利用RANSAC计算当前关键帧和闭环候选帧的Sim3变换，并根据初步求得的Sim3变换对地图点投影得到更多匹配的地图点；
将两个关键帧对应的Sim3变换和地图点作为顶点，遍历所有地图点的观测，添加正向和逆向投影的二元边，固定地图点的位姿，通过最小化重投影误差来优化两帧的Sim3变换；
根据优化后的Sim3，将闭环关键帧及其相连关键帧的地图点都投影到当前关键帧进行匹配，以得到更多的匹配点；
通过对优化后的Sim3求解，基于求解的Sim3，调整关键帧位姿和地图点位置。
CN202110541906.4A 2021-05-18 2021-05-18 一种多传感器融合的slam方法和装置 Active CN113516692B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110541906.4A CN113516692B (zh) 2021-05-18 2021-05-18 一种多传感器融合的slam方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110541906.4A CN113516692B (zh) 2021-05-18 2021-05-18 一种多传感器融合的slam方法和装置

Publications (2)

Publication Number Publication Date
CN113516692A CN113516692A (zh) 2021-10-19
CN113516692B true CN113516692B (zh) 2024-07-19

Family

ID=78064620

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110541906.4A Active CN113516692B (zh) 2021-05-18 2021-05-18 一种多传感器融合的slam方法和装置

Country Status (1)

Country Link
CN (1) CN113516692B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114387352A (zh) * 2021-12-29 2022-04-22 东软睿驰汽车技术（沈阳）有限公司 一种外参标定方法、装置、设备及存储介质
CN114545348A (zh) * 2022-02-25 2022-05-27 中电科技扬州宝军电子有限公司 一种基于svd的雷达系统误差的标定方法
CN115164918B (zh) * 2022-09-06 2023-02-03 联友智连科技有限公司 语义点云地图构建方法、装置及电子设备
CN115272494B (zh) * 2022-09-29 2022-12-30 腾讯科技（深圳）有限公司 相机与惯性测量单元的标定方法、装置和计算机设备
CN117058430B (zh) * 2023-10-12 2023-12-22 北京万龙精益科技有限公司 用于视场匹配的方法、装置、电子设备和存储介质
CN117808882B (zh) * 2024-02-29 2024-05-17 上海几何伙伴智能驾驶有限公司 退化场景下基于多传感器融合的slam漂移检测与补偿方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111415375A (zh) * 2020-02-29 2020-07-14 华南理工大学 一种基于多鱼眼摄像机和双针孔投影模型的slam方法
CN112734841A (zh) * 2020-12-31 2021-04-30 华南理工大学 一种用轮式里程计-imu和单目相机实现定位的方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110462683B (zh) * 2018-03-06 2022-04-12 斯坦德机器人（深圳）有限公司 紧耦合视觉slam的方法、终端及计算机可读存储介质
CN109376785B (zh) * 2018-10-31 2021-09-24 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109887087B (zh) * 2019-02-22 2021-02-19 广州小鹏汽车科技有限公司 一种车辆的slam建图方法及系统
CN110411457B (zh) * 2019-08-27 2024-04-19 纵目科技（上海）股份有限公司 基于行程感知与视觉融合的定位方法、系统、终端和存储介质
CN112219087A (zh) * 2019-08-30 2021-01-12 深圳市大疆创新科技有限公司 位姿预测方法、地图构建方法、可移动平台及存储介质
CN111156984B (zh) * 2019-12-18 2022-12-09 东南大学 一种面向动态场景的单目视觉惯性slam方法
CN111462135B (zh) * 2020-03-31 2023-04-21 华东理工大学 基于视觉slam与二维语义分割的语义建图方法
CN111795686B (zh) * 2020-06-08 2024-02-02 南京大学 一种移动机器人定位与建图的方法
CN111986506B (zh) * 2020-07-20 2022-04-01 苏州易航远智智能科技有限公司 基于多视觉系统的机械式停车位泊车方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111415375A (zh) * 2020-02-29 2020-07-14 华南理工大学 一种基于多鱼眼摄像机和双针孔投影模型的slam方法
CN112734841A (zh) * 2020-12-31 2021-04-30 华南理工大学 一种用轮式里程计-imu和单目相机实现定位的方法

Also Published As

Publication number Publication date
CN113516692A (zh) 2021-10-19

Similar Documents

Publication Publication Date Title
CN113516692B (zh) 一种多传感器融合的slam方法和装置
US9171225B2 (en) Device, method, and recording medium for detecting and removing mistracked points in visual odometry systems
Kelly et al. A general framework for temporal calibration of multiple proprioceptive and exteroceptive sensors
JP5992184B2 (ja) 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム
Harmat et al. Multi-camera tracking and mapping for unmanned aerial vehicles in unstructured environments
CN111862673B (zh) 基于顶视图的停车场车辆自定位及地图构建方法
CN112649016A (zh) 一种基于点线初始化的视觉惯性里程计方法
US9959625B2 (en) Method for fast camera pose refinement for wide area motion imagery
CN110033489A (zh) 一种车辆定位准确性的评估方法、装置及设备
Honegger et al. Embedded real-time multi-baseline stereo
CN113551665A (zh) 一种用于运动载体的高动态运动状态感知系统及感知方法
Zhao et al. RTSfM: Real-time structure from motion for mosaicing and DSM mapping of sequential aerial images with low overlap
CN116380079B (zh) 一种融合前视声呐与orb-slam3的水下slam方法
CN114638897B (zh) 基于无重叠视域的多相机系统的初始化方法、系统及装置
CN113345032B (zh) 一种基于广角相机大畸变图的初始化建图方法及系统
Luo et al. Fast terrain mapping from low altitude digital imagery
CN116184430B (zh) 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法
Veth et al. Stochastic constraints for efficient image correspondence search
Beauvisage et al. Robust multispectral visual-inertial navigation with visual odometry failure recovery
CN113744308A (zh) 位姿优化方法、装置、电子设备、介质及程序产品
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
Gokhool et al. A dense map building approach from spherical RGBD images
CN117330052A (zh) 基于红外视觉、毫米波雷达和imu融合的定位与建图方法及系统
Kuo et al. Calibrating a wide-area camera network with non-overlapping views using mobile devices
KR102225321B1 (ko) 복수 영상 센서로부터 취득한 영상 정보와 위치 정보 간 연계를 통한 도로 공간 정보 구축을 위한 시스템 및 방법

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