CN112305576A - 一种多传感器融合的slam算法及其系统 - Google Patents

一种多传感器融合的slam算法及其系统 Download PDF

Info

Publication number
CN112305576A
CN112305576A CN202011197500.0A CN202011197500A CN112305576A CN 112305576 A CN112305576 A CN 112305576A CN 202011197500 A CN202011197500 A CN 202011197500A CN 112305576 A CN112305576 A CN 112305576A
Authority
CN
China
Prior art keywords
camera
module
camera module
image
coordinate system
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
CN202011197500.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.)
Dilu Technology Co Ltd
Original Assignee
Central Manpu Technology Nanjing Co ltd
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 Central Manpu Technology Nanjing Co ltd filed Critical Central Manpu Technology Nanjing Co ltd
Priority to CN202011197500.0A priority Critical patent/CN112305576A/zh
Publication of CN112305576A publication Critical patent/CN112305576A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Theoretical Computer Science (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Studio Devices (AREA)

Abstract

本发明公开了一种多传感器融合的SLAM算法及系统，包括以下步骤，在车身前侧分别安装多类型的摄像机模块；对所述摄像机模块进行相机标定；所述摄像机模块采集图像信息并对其采集的图像进行矫正和ORB特征点的抽取、深度图的计算以及将采集的彩色图与深度图对齐；所述硬件同步模块将所述摄像机模块采集的不同类型图像信息在时间戳上同步。本发明的有益效果：考虑了多种场景，融合多种数据，定位的结果可靠，能够解决单个传感器的限制。

Description

一种多传感器融合的SLAM算法及其系统
技术领域
本发明涉及同时定位与建图技术的技术领域，尤其涉及一种多传感器融合的SLAM算法及融合系统。
背景技术
在车辆高级驾驶辅助系统中，定位和建图是不可缺少的技术，比如在弱GPS或者无GPS时车辆需要依赖SLAM技术导航和路径规划。高级驾驶辅助系统是利用安装在车上的各式各样传感器，如毫米波雷达、激光雷达、单\双目摄像头以及卫星导航，在汽车行驶过程中随时来感应周围的环境，收集数据，进行静态、动态物体的辨识、侦测与追踪，并结合导航仪地图数据，进行系统的运算与分析，从而预先让驾驶者察觉到可能发生的危险，有效增加汽车驾驶的舒适性和安全性。
目前在车辆ADAS系统中，定位是极其重要的一环，在缺乏地图的前提下，SLAM技术提供了一个很好的解决方案，目前主流技术中，有基于视觉的，有基于激光的，但是纯视觉或者纯激光的方案或多或少都存在问题，如纯视觉方案中，需要场景中有丰富的纹理。在激光的解决方案中，没有激光的回环检测方法。
发明内容
本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。
鉴于上述现有存在的问题，提出了本发明。
因此，本发明解决的技术问题是：解决单种类相机的应用限制，以及提高整个系统的鲁棒性。
为解决上述技术问题，本发明提供如下技术方案：一种多传感器融合的SLAM算法，包括以下步骤，在车身前侧分别安装多类型的摄像机模块；对所述摄像机模块进行相机标定；所述摄像机模块采集图像信息并对其采集的图像进行矫正和ORB特征点的抽取、深度图的计算以及将采集的彩色图与深度图对齐；所述硬件同步模块将所述摄像机模块采集的不同类型图像信息在时间戳上同步；SLAM融合模块将所述摄像机模块采集的不同类型图像进行图像融合进行车载显示。
作为本发明所述的多传感器融合的SLAM算法的一种优选方案，其中：所述摄像机模块还包括单目相机模块、双目相机模块、RGB-D相机模块。
作为本发明所述的多传感器融合的SLAM算法的一种优选方案，其中：所述单目相机模块采集图像信息并对其采集的图像进行矫正和ORB特征点的抽取，同时所述双目相机模块对其采集的图像进行深度图的计算，同时所述RGB-D相机模块将其采集的彩色图与深度图对齐。
作为本发明所述的多传感器融合的SLAM算法的一种优选方案，其中：所述相机标定包括同时标定相机的内外参和相机相对于车载坐标系的变换矩阵，多相机之前的变换关系通过相机对车载坐标系的变换矩阵推算得出。
作为本发明所述的多传感器融合的SLAM算法的一种优选方案，其中：所述相机相对于车载坐标系的变换矩阵获取包括如下步骤，标定摄像机模块并将所述摄像机模块设置于车身的预设目标位置上；在预设拍摄位置上放置原始标定图案；利用所述摄像机模块拍摄所述原始标定图案得到所述拍摄图像；对所述拍摄图像进行预处理，获取所述拍摄图像的像素坐标与参照坐标系间的坐标变换关系。
作为本发明所述的多传感器融合的SLAM算法的一种优选方案，其中：定义所述参照坐标系包括以下步骤，选取所述车身上的任一位置为原点O；根据所述原点O定义车身坐标系O1为所述参照坐标系。
作为本发明所述的多传感器融合的SLAM算法的一种优选方案，其中：所述变换矩阵推算包括以下步骤，标定所述摄像机模块并获取其标定数据；所述拍摄图像根据所述标定数据进行畸变校正，获得消除畸变图像并求出畸变校正的图像变换映射E1；利用所述消除畸变图像中标定图案的特征点获取像素坐标；利用所述像素坐标和所述原始标定图案的位置坐标求出透视变换矩阵E2，且所述位置坐标相对于所述车身坐标系O1；定义所述拍摄图像的像素坐标与所述车身坐标系O1的变换关系为E＝E2·E1。
作为本发明所述的多传感器融合的SLAM算法的一种优选方案，其中：所述硬件同步模块包括单片机模块、GPS时间接收机和RTC芯片；所述单片机模块时间信息接收模块接收所述GPS时间接收机的时间信息和时间脉冲；所述RTC芯片与所述单片机模块内的RTC模块连接；相机反馈信号连接所述单片机模块的反馈信号检测模块。
作为本发明所述的多传感器融合的SLAM算法的一种优选方案，其中：所述SLAM融合包括以下步骤，根据实时所处的场景的环境因数，决策单元选择合适的相机作为当前场景下的主使用相机；将其它相机的特征点通过变换矩阵计算出在当前主使用相机的图像坐标点，并以所述图像坐标点为坐标原点；固定半径范围内搜索得出距离最短的特征点，滤出超出一定距离阈值的外点，输出更加可靠的特征点。
本发明解决的另一个技术问题是：提供一种基于多类型相机的SLAM融合系统，上述方法依托于本系统实现，能够解决单种类相机的应用限制，以及提高整个系统的鲁棒性。
为解决上述技术问题，本发明提供如下技术方案：一种基于多类型相机的SLAM融合系统，其特征在于：包括摄像机模块、硬件同步模块和SLAM融合模块；所述摄像机模块设置于车身且与水平面平行的多类型相机，还包括单目相机模块、双目相机模块、RGB-D相机模块；所述硬件同步模块用于所述单目相机模块、所述双目相机模块和所述RGB-D相机模块在时间戳上的同步；所述SLAM融合模块将所述摄像机模块采集的多类型图像信息进行融合并与车载显示器进行显示。
本发明的有益效果：在接入差分RTK的情况下，定位的精度能够达到厘米级的定位。双目相机+IMU在弱GPS和丰富纹理的情况下能够提供定位，如果纹理不够丰富，视觉不能够提供可靠的信息，在短时间中可以依赖IMU定位，考虑了多种场景，融合多种数据，定位的结果可靠，能够解决单个传感器的限制。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。其中：
图1为本发明第一个实施例所述多传感器融合的SLAM算法的整体流程结构示意图；
图2为本发明第一个实施例所述世界坐标系、相机坐标系和图像坐标系的示意图；
图3为本发明第一个实施例所述坐标系的示意图；
图4为本发明第一个实施例所述相机成像原理的示意图；
图5为本发明第一个实施例所述相机成像畸变原理的示意图；
图6为本发明第一个实施例所述相机成像径向畸变原理的示意图；
图7为本发明第二个实施例所述硬件同步模块的硬件电路连接示意图；
图8为本发明第二个实施例所述相机同步曝光的流程示意图；
图9为本发明第二个实施例所述时间校正流程的示意图；
图10为本发明第三个实施例所述基于多类型相机的SLAM融合系统应用于车身的示意图；
图11为本发明第三个实施例所述基于多类型相机的SLAM融合系统流程示意图；
图12为本发明第三个实施例所述基于多类型相机的SLAM融合系统的整体原理结构示意图。
具体实施方式
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明，显然所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护的范围。
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。
其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。
本发明结合示意图进行详细描述，在详述本发明实施例时，为便于说明，表示器件结构的剖面图会不依一般比例作局部放大，而且所述示意图只是示例，其在此不应限制本发明保护的范围。此外，在实际制作中应包含长度、宽度及深度的三维空间尺寸。
同时在本发明的描述中，需要说明的是，术语中的“上、下、内和外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一、第二或第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。
本发明中除非另有明确的规定和限定，术语“安装、相连、连接”应做广义理解，例如：可以是固定连接、可拆卸连接或一体式连接；同样可以是机械连接、电连接或直接连接，也可以通过中间媒介间接相连，也可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
实施例1
参照图1的示意，为本实施例中提出的一种基于多类型相机的SLAM融合算法，解决单种类相机的使用瓶颈方案硬件成本低；并且只需要三组相机，即可满足多场景的要求，解决单种类相机的应用限制，满足车辆高级驾驶辅助系统的要求。GPS+IMU组合成高精度惯性导航。在接入差分RTK的情况下，定位的精度能够达到厘米级的定位。双目相机+IMU在弱GPS和丰富纹理的情况下能够提供定位，如果纹理不够丰富，视觉不能够提供可靠的信息，在短时间中可以依赖IMU定位，考虑了多种场景，融合多种数据，定位的结果可靠，能够解决单个传感器的限制。
具体的，包括以下步骤，
在车身前侧分别安装多类型的摄像机模块100；对摄像机模块100进行相机标定；摄像机模块100采集图像信息并对其采集的图像进行矫正和ORB特征点的抽取、深度图的计算以及将采集的彩色图与深度图对齐；硬件同步模块200将摄像机模块100采集的不同类型图像信息在时间戳上同步；SLAM融合模块300将摄像机模块100采集的不同类型图像进行图像融合进行车载显示。
其中摄像机模块100还包括单目相机模块101、双目相机模块102、RGB-D相机模块103。单目相机模块101采集图像信息并对其采集的图像进行矫正和ORB特征点的抽取，同时双目相机模块102对其采集的图像进行深度图的计算，同时RGB-D相机模块103将其采集的彩色图与深度图对齐。
相机标定包括同时标定相机的内外参和相机相对于车载坐标系的变换矩阵，多相机之前的变换关系通过相机对车载坐标系的变换矩阵推算得出。
本实施例中相机相对于车载坐标系的变换矩阵获取包括如下步骤，标定摄像机模块100并将摄像机模块100设置于车身400的预设目标位置上；在预设拍摄位置上放置原始标定图案500；利用摄像机模块100拍摄原始标定图案500得到拍摄图像600；对拍摄图像600进行预处理，获取拍摄图像600的像素坐标与参照坐标系间的坐标变换关系。还包括定义参照坐标系，其包括选取车身400上的任一位置为原点O；根据原点O定义车身坐标系O1为参照坐标系。变换矩阵推算包括以下步骤，标定摄像机模块100并获取其标定数据；拍摄图像600根据标定数据进行畸变校正，获得消除畸变图像并求出畸变校正的图像变换映射E1；利用消除畸变图像中标定图案的特征点获取像素坐标；利用像素坐标和原始标定图案500的位置坐标求出透视变换矩阵E2，且位置坐标相对于车身坐标系O1；定义拍摄图像600的像素坐标与车身坐标系O1的变换关系为E＝E2·E1。
硬件同步模块200包括单片机模块201、GPS时间接收机202和RTC芯片；单片机模块201时间信息接收模块接收GPS时间接收机202的时间信息和时间脉冲；RTC芯片与单片机模块201内的RTC模块连接；相机反馈信号连接单片机模块201的反馈信号检测模块。
SLAM融合还包括以下步骤，根据实时所处的场景的环境因数，决策单元选择合适的相机作为当前场景下的主使用相机；将其它相机的特征点通过变换矩阵计算出在当前主使用相机的图像坐标点，并以图像坐标点为坐标原点；固定半径范围内搜索得出距离最短的特征点，滤出超出一定距离阈值的外点，输出更加可靠的特征点。通俗的说，即根据当时所处的场景，比如说室内、室外，阳光是否强烈等等，选择一个合适的相机作为主使用相机，为了减少数据计算，将其他相机的特征点通过变换矩阵计算出在当前主相机的图像坐标点，并以此为坐标原点，固定半径范围内搜索得出距离最短的特征点，滤出超出一定距离阈值的外点，由此就可以得出更加可靠的特征点，增强了系统的可靠性和鲁棒性。
本实施例中通过在车身前侧分别安装单目、双目以及RGB-D相机以及硬件同步模块。相机搭载全局曝光技术，有效降低图像畸变，适应高速环境捕捉，同时辅以近红外探测光，增强弱纹理对象识别精度。硬件同步模块200主要用来实现相机的纳秒级同步，保证三个相机采集的图像在时间戳上的同步。SLAM融合算法模块300用于决策在当前的场景中使用哪个或者多个相机，并决策以哪个相机为主或者为辅，将辅助用的相机的信息投射到主用的相机中，从而能提高整个系统的鲁棒性，解决单种类相机的使用瓶颈。
相机标定：同时标定相机的内外参和相机相对于车载坐标系的变换矩阵，多相机之前的变换关系可通过相机对车载坐标系的变换矩阵推算得出。设备标定：三个相机安照金字塔的方式，单目相机在最上方，双目相机在中间，深度相机在最下方。相机自身标定：采用常规相机标定手法，利用相机拍摄若干张标定板的图片，利用多组图片中标定图案的标记点坐标计算出相机的内参数，畸变方程系数、投影矩阵等。相机与车身标定：首先相机安装时注意与水平面平行，以车辆后轴的中心点为车身坐标系的坐标原点，分别丈量与此点的偏航、俯仰、横滚的距离建立坐标变换矩阵。
进一步的具体的，具体自身标定参照图2～4的示意，确定世界坐标系，也称为测量坐标系，是一个三维直角坐标系，以其为基准可以描述相机和待测物体的空间位置。世界坐标系的位置可以根据实际情况自由确定。相机坐标系，也是一个三维直角坐标系，原点位于镜头光心处，x、y轴分别与相面的两边平行，z轴为镜头光轴，与像平面垂直。将世界坐标系转换为相机坐标系：
Figure BDA0002754448450000071
其中R为3*3的旋转矩阵，t为3*1的平移矢量，等号左侧公式为相机坐标系的齐次坐标，右侧公式为世界坐标系的齐次坐标，
像素坐标系不利于坐标变换，原点是相机光轴与相面的交点(称为主点)，即图像的中心点。故两个坐标系实际是平移关系，即可以通过平移就可得到图像坐标系转换为像素坐标系：
Figure BDA0002754448450000081
u0，v0为主点(图像原点)坐标。
参照图2示意为空间任意一点P与其图像点p之间的关系，P与相机光心o的连线为oP，oP与像面的交点p即为空间点P在图像平面上的投影。该过程为透视投影，如下矩阵表示：
Figure BDA0002754448450000082
其中，s为比例因子(s不为0)，f为有效焦距(光心到图像平面的距离)，右式是空间点P在相机坐标系中的齐次坐标，左式是像点p在图像坐标系中的齐次坐标。
故将世界坐标系转换为像素坐标系：
Figure BDA0002754448450000083
根据上述对像素坐标的说明，此步骤中为计算外参，设三维世界坐标的点为M＝[X,Y,Z,1]T，二维相机平面像素坐标为m＝[u,v,1]T，因此标定用的棋盘格平面到图像平面的单应性关系为：sm＝A[R,t]M，其中s为世界坐标系到图像坐标系的尺度因子，A为相机的内参矩阵，(u0，v0)为像主点坐标，α，β为焦距与像素横纵比的融合，γ为径向畸变参数，R为旋转矩阵，t为平移向量，如下公式：
Figure BDA0002754448450000084
设棋盘格位于Z＝0，定义旋转矩阵R的第i列为ri,则有
Figure BDA0002754448450000091
令H＝[h1 h2 h3]＝λA[r1 r2 t]，于是空间到图像的映射可改为：sm＝HM，其中H是描述矩阵，H是一个齐次矩阵，所以有8个未知数，至少需要8个方程，每对对应点能提供两个方程，所以至少需要四个对应点，就可以算出世界平面到图像平面的单应性矩阵H。
外部参数可通过上述矩阵求解，由上式H＝[h1 h2 h3]＝λA[r1 r2 t]可得：
r1＝λA-1h1 r2＝λA-1h2 r3＝r1×r2 t＝λA-1h3
λ＝1/||A-1h1||＝1/||A-1h2||
外参的具体计算公式。注意：R3是t，求解出的R＝[r1 r2 t]不会满足正交与归一的标准，在实际操作中，R可以通过分解实现规范化。
进一步的，计算内参，包括以下步骤，
由r1和r2正交，且r1和r2的模相等，可以得到如下约束：正交
Figure BDA0002754448450000092
模相等：
Figure BDA0002754448450000093
定义：
Figure BDA0002754448450000094
B中的未知量可表示为6D向量b，可以表示为：
b＝[B11 B12 B22 B13 B23 B33]T
设H中的第i列为hi，表示为：
hi＝[hi1 hi2 hi3]T
根据b的定义，可以推导出如下公式：
Figure BDA0002754448450000101
Figure BDA0002754448450000102
Figure BDA0002754448450000103
Figure BDA0002754448450000104
可以得出下式：
Figure BDA0002754448450000105
根据推到的结果可知如果有n组观察图像，则V是2nx6的矩阵，根据最小二乘定义，Vb＝0的解是最小特征值对应的特征向量。因此，可以直接估算出b，后续可以通过b求解内参，因为B中的未知量为6个，所以当观测平面n≥3时，可以得到b的唯一解。当n＝2时，一般可令畸变参数γ＝0；当n＝1时，仅能估算出α与β，此时一般可假定像主点坐标u0与v0为0。
内部参数可通过如下公式计算分解：
Figure BDA0002754448450000106
Figure BDA0002754448450000107
Figure BDA0002754448450000108
Figure BDA0002754448450000109
γ＝-B12α2β/λ
u0＝γv0/α-B13α2/λ
本实施例中上述的推导结果是基于理想情况下的解，但由于可能存在误差，所以需要进行估计优化。设我们采集了n副包含棋盘格的图像进行定标，每个图像里有棋盘格角点m个。令第i副图像上的角点Mj在上述计算得到的摄像机矩阵下图像上的投影点为：
Figure BDA00027544484500001010
其中K为相机内参矩阵A；Ri和ti是第i副图对应的旋转矩阵和平移向量，K是内参数矩阵。则mij的函数为：
Figure BDA00027544484500001011
由上式可得：
Figure BDA0002754448450000111
让L取得最大值，即让下面式子最小，本实施例使用的是多参数非线性系统优化问题的算法进行迭代求最优解。
经过上述过程可获得摄像机模块100的标定数据，即内外参数。
参照图5～7的示意，本实施例标定还需要获得摄像机模块100的畸变参数，畸变是对直线投影的一种偏移，畸变简单来说就是一条直线投影到图片上不能保持为一条直线了，这是一种光学畸变，可能由于摄像机镜头的原因。畸变一般可以分为：径向畸变、切向畸变。径向畸变来自于透镜形状，切向畸变来自于整个摄像机的组装过程，畸变还有其他类型的畸变，但是没有径向畸变、切向畸变显著。具体的，
切向畸变模型：
δxd＝2p1xy+p2(r2+2x2)+K
δyd＝2p1(r2+2y2)+2p2xy+K
理想坐标与真实坐标：
x′＝x+δxr+δxd
y′＝y+δyr+δyd
也即：
Figure BDA0002754448450000112
将上述坐标变换为像素坐标(u0，v0)，可获得本步骤中对摄像机模块100的标定数据，即相机内外参数、镜头畸变系数。
进一步的，标定摄像机模块100将放置于目标位置，在预设位置上放置原始标定图案，该预设位置为摄像机模块100的视野范围、待测量区域范围以及根据设置的参照坐标系计算得出的位置坐标。本实施例中目标位置为根据坐标变换方法需要去操作的位置，由于单个摄像机具有视野范围的限制，因此想要对指定的位置处进行坐标变换，就必须选择将摄像机放置到能够拍摄的目标物体的位置上，也就是上述的目标位置。且当摄像机模块100放置完成后，其可以拍摄的视野范围也随之被确定，因此预设位置即摄像机模块100的视野范围等。
利用摄像机模块100拍摄原始标定图案500获得拍摄图像600；对拍摄图像600进行预处理，获取拍摄图像600的像素坐标与参照坐标系间的坐标变换关系。
结合上述对采用本实施例的标定法标定摄像机模块100并获取其标定数据；相机标定是三维场景重构中必不可少的步骤，目的是提取出二维图像中包含的计量信息。将拍摄图像600根据标定数据进行畸变校正，获得消除畸变图像并求出畸变校正的图像变换映射E1；利用消除畸变图像中标定图案的特征点获取像素坐标；利用像素坐标和原始标定图案500的位置坐标求出透视变换矩阵E2，且位置坐标相对于参照坐标系；该参照坐标系可根据实际过程中进行选择，最终定义拍摄图像的像素坐标与参照坐标系的变换关系E＝E2·E1。
实际使用中将车开到室外，此时卫星惯性导航将起主导作用，如果此时再将车开到室内停车场，此时双目+IMU将起主导作用，如果再将车开到白墙附件，此时IMU将起主导作用。在实际场景中，如果能够接收到GPS信号，则提高卫星惯性导航的定位结果在最终输出中的权重；如果接收不到GPS信号且场景中有丰富的视觉纹理，则提高VIO的定位结果在最终输出中的权重；如果接收不到GPS信号且场景中没有丰富的视觉纹理信息，则提高IMU的定位结果在最终输出中的权重。
实施例2
参照图7～9的示意，本实施例进一步的需要说明的是，硬件同步模块200采用同步曝光模块硬件电路和GPS时间校正模块实现，单片机的时间信息接收模块接收GPS时间接收机的时间信息和时间脉冲，RTC(实时时钟)芯片连接单片机的RTC模块，单片机曝光信号控制模块控制4路相机的曝光部分，4路相机反馈信号连接单片机的反馈信号检测模块，不难发现，本实施例为3路相机的反馈信号。
具体的流程为单片机接收到外部请求曝光信号时，3路管脚立即动作并延时，由于相机曝光的动作有机械延时，只有当单片机4路管脚延时的时间大于相机的机械动作时间时3路相机才发生曝光动作。如果相机未发生曝光动作，程序立即返回并重新请求曝光。当相机发生曝光动作，单片机捕捉相机曝光后的反馈信号，通过逻辑运算将多路反馈信号反映到反馈信号标志(CAM＿MARK)，如果3路相机全部曝光并且曝光的最快反馈信号与最慢反馈信号的时差很短，则认为达到预期效果并结束循环；如果3路相机全部曝光，但是捕捉到的最快反馈信号与最慢反馈信号时差很长，则认为3路相机曝光不同步，返回循环并重新补拍；如果未完全捕捉到4路相机的反馈信号，则认为3路相机未完全曝光，返回循环并重新补拍。
GPS时间校正具体包括，相机同步曝光模块接收GPS时间接收机的分信号(GPS时间信息和GPS时间脉冲)，GPS时间信息接入单片机时间接收模块的串口部分，当时间信息到来时通过串口中断提取时间信息。GPS时间脉冲接入单片机时间接收模块的外部中断部分，当时间脉冲到来时单片机立即进入外部中断服务程序中，此时判断时间信息标志(CAM＿MARK)，如果为真，则表示实时时间相差1s(设置实时时间和GPS时间信息时差1s)，补偿后结束循环。如果时间信息标志为假，则返回并等待下一次时间信息。
同时本实施例中需要说明的是，单目相机模块101、双目相机模块102、RGB-D相机模块103分别进行采集的图像的矫正和ORB特征点的抽取、深度图的计算以及将采集的彩色图与深度图对齐。具体的，ORB特征点的抽取，实际为图像特征的检测和提取是计算机视觉最重要的研究领域之一，ORB算法是目前最快速稳定的特征点检测和提取算法，包括计算特征点的主方向，坐标，灰度值和梯度等信息。本实施例可以采用FAST算法来检测特征点。
深度图的计算以及将采集的彩色图与深度图对齐，深度指的是场景中某个点到相机中心所在的XY平面的距离，可以利用多视图立体法、光度立体视觉法、色度成形法或散焦推断法等实现。
进一步的，SLAM融合模块300包括如下的数据融合步骤，
预备阶段，包括传感器的选型和各种标定，包括3个线程，前端线程、后端优化线程、闭环检测线程。前端线程主要涉及到：特征的提取、特征的匹配；多视图几何的知识，包括对极几何、PnP、刚体运动、李代数等。后端优化线程涉及到非线性最小二乘优化，属于数值优化的内容。闭环检测线程涉及到地点识别，本质上是图像检索问题。
在使用深度相机获取深度数据之前，通常需要对这些输人数据进行预处理，这包括去噪声影响、数据转换两大块。设备对实体场景进行多视角扫描过程中，通过配准的方法实现相机位置的追踪，使得不同视点获取的影像位于统一坐标系下，为下一步数据融合提供标准的数据结构。基于隐函数法将物体多个方向的表面数据合并，定义函数D(x)来表示采样每个点，函数中每一个点x都带有权重信息和距离信息，当D(x)＝0时数据融合标准。
随着数据不断进入，对同一区域采集其深度数据时需要不断的更新，其产生的误差不同，因此数据融合的时候应对其加权求平均。此时函数D(x)的值通过{di(x)|i＝0，…，n}和对应的权重信息，{wi(x)|i＝0，…，n}通过下式计算：
Figure BDA0002754448450000141
通过特征提取，每一张影像上都提取出一系列特征点，而由于照片之间有重叠.以及扫描时可能同一区域经过了多次重复扫描，因此一组影像中有许多重复的特征，故可通过特征归并减少数据的冗余，提高后续特征匹配的效率。特征归并的过程其实就是邻近点聚类的过程：
给定一个多维空间RK中的一个向量是一个样本点，这些样本点的有限集合称为样本集，给定样本集E和一个样本点s’，样本点最近邻就是任意样本点s∈E满足Nearest(E，s′，s)，其定义如下：
Figure BDA0002754448450000142
上面的公式中距离度量是欧式距离，即：
Figure BDA0002754448450000143
其中：si是向量s的第i个维度。
当两幅图像的SIFT特征向量生成后，下一步采用关键点特征向量的欧式距离作为两幅图像中关键点的相似性判定度量。当相似度可由相似函数求出，其满足阈值时，则认为匹配成功，即完成图像的数据融合。
实施例3
参照图10～11的示意，本实施例中提出一种基于多类型相机的SLAM融合系统，上述实施例的方法能够依托于本系统进行实现，其系统框架包括图像处理：单目主要负责图像矫正、ORB特征点的抽取，对于双目相机，还包含深度图的计算，深度相机则包含彩色图和深度图的对齐；决策单元：主要根据场景的不用选择合适的相机组合；数据融合单元：多相机的图像融合。具体的，该系统包括摄像机模块100、硬件同步模块200和SLAM融合模块300；其中摄像机模块100设置于车身400且与水平面平行的多类型相机，还包括单目相机模块101、双目相机模块102、RGB-D相机模块103；硬件同步模块200用于单目相机模块101、双目相机模块102和RGB-D相机模块103在时间戳上的同步；SLAM融合模块300将摄像机模块100采集的多类型图像信息进行融合并与车载显示器进行显示。例如本系统使用场景在当车辆在室外时，将以双目相机为主，单目相机为辅；当车辆处于室内时，将以深度相机为主，单目相机为辅。
需要说明的是，本实施例中硬件同步模块200和SLAM融合模块300为与摄像机模块100连接的芯片硬件，例如在本实施例中硬件同步模块200的单片机可以采用型号STC12C5A60S2-35I-P的时钟单片机，GPS时间接收机202型号为RTK，以Trimble BD970主板的接收机，而RTC芯片采用型号为PCF8563T实时时钟芯片实现。SLAM融合模块300为植入上述实施例算法的嵌入式开发芯片，通过电路集成的方式形成电路板的硬件设备。与车载显示器连接，不难理解的是，该SLAM融合模块300可以与车载控制电脑的主板就行电路集成连接，设置于车载电脑内通过其显示器将融合的数据进行显示供驾驶者查看，解决，解决单种类相机的使用瓶颈。
应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (10)

1.一种多传感器融合的SLAM算法，其特征在于：包括以下步骤，
在车身前侧分别安装多类型的摄像机模块(100)；
对所述摄像机模块(100)进行相机标定；
所述摄像机模块(100)采集图像信息并对其采集的图像进行矫正和ORB特征点的抽取、深度图的计算以及将采集的彩色图与深度图对齐；
所述硬件同步模块(200)将所述摄像机模块(100)采集的不同类型图像信息在时间戳上同步；
车身上安装GPS+IMU组合成高精度惯性导航，并标定摄像机模块(100)、GPS和IMU三者之间的变换关系；
SLAM融合模块(300)将所述摄像机模块(100)、GPS和IMU采集的不同类型图像进行图像融合进行车载显示。
2.如权利要求1所述的多传感器融合的SLAM算法，其特征在于：所述摄像机模块(100)还包括单目相机模块(101)、双目相机模块(102)或者RGB-D相机模块(103)，
在实际场景中，如果能够接收到GPS信号，则提高卫星惯性导航的定位结果在最终输出中的权重；
如果接收不到GPS信号且场景中有丰富的视觉纹理，则提高VIO的定位结果在最终输出中的权重；
如果接收不到GPS信号且场景中没有丰富的视觉纹理信息，则提高IMU的定位结果在最终输出中的权重。
3.如权利要求2所述的多传感器融合的SLAM算法，其特征在于：所述单目相机模块(101)采集图像信息并对其采集的图像进行矫正和ORB特征点的抽取，同时所述双目相机模块(102)对其采集的图像进行深度图的计算，同时所述RGB-D相机模块(103)将其采集的彩色图与深度图对齐。
4.如权利要求1～3任一所述的多传感器融合的SLAM算法，其特征在于：所述相机标定包括同时标定相机的内外参和相机相对于车载坐标系的变换矩阵，多相机之前的变换关系通过相机对车载坐标系的变换矩阵推算得出。
5.如权利要求4所述的多传感器融合的SLAM算法，其特征在于：所述相机相对于车载坐标系的变换矩阵获取包括如下步骤，
标定摄像机模块(100)并将所述摄像机模块(100)设置于车身(400)的预设目标位置上；
在预设拍摄位置上放置原始标定图案(500)；
利用所述摄像机模块(100)拍摄所述原始标定图案(500)得到所述拍摄图像(600)；
对所述拍摄图像(600)进行预处理，获取所述拍摄图像(600)的像素坐标与参照坐标系间的坐标变换关系。
6.如权利要求5所述的多传感器融合的SLAM算法，其特征在于：定义所述参照坐标系包括以下步骤，
选取所述车身(400)上的任一位置为原点O；
根据所述原点O定义车身坐标系O1为所述参照坐标系。
7.如权利要求6所述的多传感器融合的SLAM算法，其特征在于：所述变换矩阵推算包括以下步骤，
标定所述摄像机模块(100)并获取其标定数据；
所述拍摄图像(600)根据所述标定数据进行畸变校正，获得消除畸变图像并求出畸变校正的图像变换映射E1；
利用所述消除畸变图像中标定图案的特征点获取像素坐标；
利用所述像素坐标和所述原始标定图案(500)的位置坐标求出透视变换矩阵E2，且所述位置坐标相对于所述车身坐标系O1；
定义所述拍摄图像(600)的像素坐标与所述车身坐标系O1的变换关系为E＝E2·E1。
8.如权利要求4～7任一所述的多传感器融合的SLAM算法，其特征在于：所述硬件同步模块(200)包括单片机模块(201)、GPS时间接收机(202)和RTC芯片；
所述单片机模块(201)时间信息接收模块接收所述GPS时间接收机(202)的时间信息和时间脉冲；所述RTC芯片与所述单片机模块(201)内的RTC模块连接；相机反馈信号连接所述单片机模块(201)的反馈信号检测模块。
9.如权利要求8所述的多传感器融合的SLAM算法，其特征在于：所述SLAM融合包括以下步骤，
根据实时所处的场景的环境因数，决策单元选择合适的相机作为当前场景下的主使用相机；
将其它相机的特征点通过变换矩阵计算出在当前主使用相机的图像坐标点，并以所述图像坐标点为坐标原点；
固定半径范围内搜索得出距离最短的特征点，滤出超出一定距离阈值的外点，输出更加可靠的特征点。
10.一种多传感器融合的SLAM系统，其特征在于：包括摄像机模块(100)、硬件同步模块(200)和SLAM融合模块(300)；
所述摄像机模块(100)设置于车身(400)且与水平面平行的多类型相机，还包括单目相机模块(101)、双目相机模块(102)、RGB-D相机模块(103)；
所述硬件同步模块(200)用于所述单目相机模块(101)、所述双目相机模块(102)和所述RGB-D相机模块(103)在时间戳上的同步；
所述SLAM融合模块(300)将所述摄像机模块(100)采集的多类型图像信息进行融合并与车载显示器进行显示。
CN202011197500.0A 2020-10-31 2020-10-31 一种多传感器融合的slam算法及其系统 Pending CN112305576A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011197500.0A CN112305576A (zh) 2020-10-31 2020-10-31 一种多传感器融合的slam算法及其系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011197500.0A CN112305576A (zh) 2020-10-31 2020-10-31 一种多传感器融合的slam算法及其系统

Publications (1)

Publication Number Publication Date
CN112305576A true CN112305576A (zh) 2021-02-02

Family

ID=74334090

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011197500.0A Pending CN112305576A (zh) 2020-10-31 2020-10-31 一种多传感器融合的slam算法及其系统

Country Status (1)

Country Link
CN (1) CN112305576A (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113311452A (zh) * 2021-05-26 2021-08-27 上海新纪元机器人有限公司 一种基于多传感器的定位方法及其系统
CN113624228A (zh) * 2021-07-26 2021-11-09 中国科学院上海微系统与信息技术研究所 一种图像传感器和加速度计的同步标定装置及方法
CN113932814A (zh) * 2021-09-30 2022-01-14 杭州电子科技大学 一种基于多模态地图的协同定位方法
CN114199235A (zh) * 2021-11-29 2022-03-18 珠海一微半导体股份有限公司 一种基于扇面深度相机的定位系统及定位方法
TWI772208B (zh) * 2021-10-22 2022-07-21 元智大學 基於毫米波雷達的人數偵測方法
US20220236069A1 (en) * 2021-09-30 2022-07-28 Beijing Baidu Netcom Science Technology Co., Ltd. Method and apparatus for route navigation, electronic device, computer readable medium
WO2022179197A1 (zh) * 2021-02-27 2022-09-01 华为技术有限公司 一种信息处理方法及相关设备

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107656545A (zh) * 2017-09-12 2018-02-02 武汉大学 一种面向无人机野外搜救的自主避障与导航方法
CN108235809A (zh) * 2017-12-25 2018-06-29 深圳前海达闼云端智能科技有限公司 端云结合定位方法、装置及电子设备、计算机程序产品
CN108235735A (zh) * 2017-12-20 2018-06-29 深圳前海达闼云端智能科技有限公司 一种定位方法、装置及电子设备、计算机程序产品
CN109374008A (zh) * 2018-11-21 2019-02-22 深动科技（北京）有限公司 一种基于三目摄像头的图像采集系统及方法
CN110517216A (zh) * 2019-08-30 2019-11-29 的卢技术有限公司 一种基于多类型相机的slam融合方法及其系统
CN110706279A (zh) * 2019-09-27 2020-01-17 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111121767A (zh) * 2019-12-18 2020-05-08 南京理工大学 一种融合gps的机器人视觉惯导组合定位方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107656545A (zh) * 2017-09-12 2018-02-02 武汉大学 一种面向无人机野外搜救的自主避障与导航方法
CN108235735A (zh) * 2017-12-20 2018-06-29 深圳前海达闼云端智能科技有限公司 一种定位方法、装置及电子设备、计算机程序产品
CN108235809A (zh) * 2017-12-25 2018-06-29 深圳前海达闼云端智能科技有限公司 端云结合定位方法、装置及电子设备、计算机程序产品
CN109374008A (zh) * 2018-11-21 2019-02-22 深动科技（北京）有限公司 一种基于三目摄像头的图像采集系统及方法
CN110517216A (zh) * 2019-08-30 2019-11-29 的卢技术有限公司 一种基于多类型相机的slam融合方法及其系统
CN110706279A (zh) * 2019-09-27 2020-01-17 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111121767A (zh) * 2019-12-18 2020-05-08 南京理工大学 一种融合gps的机器人视觉惯导组合定位方法

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022179197A1 (zh) * 2021-02-27 2022-09-01 华为技术有限公司 一种信息处理方法及相关设备
CN113311452A (zh) * 2021-05-26 2021-08-27 上海新纪元机器人有限公司 一种基于多传感器的定位方法及其系统
CN113624228A (zh) * 2021-07-26 2021-11-09 中国科学院上海微系统与信息技术研究所 一种图像传感器和加速度计的同步标定装置及方法
CN113624228B (zh) * 2021-07-26 2024-01-26 中国科学院上海微系统与信息技术研究所 一种图像传感器和加速度计的同步标定装置及方法
CN113932814A (zh) * 2021-09-30 2022-01-14 杭州电子科技大学 一种基于多模态地图的协同定位方法
US20220236069A1 (en) * 2021-09-30 2022-07-28 Beijing Baidu Netcom Science Technology Co., Ltd. Method and apparatus for route navigation, electronic device, computer readable medium
CN113932814B (zh) * 2021-09-30 2024-04-02 杭州电子科技大学 一种基于多模态地图的协同定位方法
TWI772208B (zh) * 2021-10-22 2022-07-21 元智大學 基於毫米波雷達的人數偵測方法
CN114199235A (zh) * 2021-11-29 2022-03-18 珠海一微半导体股份有限公司 一种基于扇面深度相机的定位系统及定位方法
CN114199235B (zh) * 2021-11-29 2023-11-03 珠海一微半导体股份有限公司 一种基于扇面深度相机的定位系统及定位方法

Similar Documents

Publication Publication Date Title
CN110517216B (zh) 一种基于多类型相机的slam融合方法及其系统
CN112305576A (zh) 一种多传感器融合的slam算法及其系统
CN112894832B (zh) 三维建模方法、装置、电子设备和存储介质
CN110033489B (zh) 一种车辆定位准确性的评估方法、装置及设备
Teller et al. Calibrated, registered images of an extended urban area
CN112444242B (zh) 一种位姿优化方法及装置
CN107635129B (zh) 一种三维三目摄像装置及深度融合方法
KR100513055B1 (ko) 변이지도 및 깊이지도의 융합을 통한 3차원 장면 모델생성 장치 및 그 방법
US10909395B2 (en) Object detection apparatus
CN103093459B (zh) 利用机载LiDAR点云数据辅助影像匹配的方法
CN101408422B (zh) 基于双目立体全方位视觉的交通事故现场测绘仪
CN110874854B (zh) 一种基于小基线条件下的相机双目摄影测量方法
CN112799096B (zh) 基于低成本车载二维激光雷达的地图构建方法
CN109920009B (zh) 基于二维码标识的控制点检测与管理方法及装置
CN109859269B (zh) 岸基视频辅助定位无人机大范围流场测量方法及装置
CN109341668A (zh) 基于折射投影模型和光束追踪法的多相机测量方法
CN109596121A (zh) 一种机动站自动目标检测与空间定位方法
KR101255461B1 (ko) 도로시설물 자동 위치측정 방법
CN103411587A (zh) 定位定姿方法及系统
CN114037762A (zh) 基于图像与高精度地图配准的实时高精度定位方法
CN106500729A (zh) 一种无需控制信息的智能手机自检校方法
Mi et al. A vision-based displacement measurement system for foundation pit
CN117036300A (zh) 基于点云-rgb异源图像多级配准映射的路面裂缝识别方法
Zhao et al. Alignment of continuous video onto 3D point clouds
CN109712200B (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
TA01 Transfer of patent application right

Effective date of registration: 20210728

Address after: Floor 11, building A1, Huizhi science and Technology Park, 8 Hengtai Road, Nanjing Economic and Technological Development Zone, Jiangsu Province 210000

Applicant after: DILU TECHNOLOGY Co.,Ltd.

Address before: 211106 block a, Mingjia science and technology building, 99 Shengli Road, Jiangning District, Nanjing City, Jiangsu Province

Applicant before: Central Manpu Technology (Nanjing) Co.,Ltd.

TA01 Transfer of patent application right
RJ01 Rejection of invention patent application after publication

Application publication date: 20210202

RJ01 Rejection of invention patent application after publication