CN113625271B - 基于毫米波雷达和双目相机的同时定位与建图方法 - Google Patents

基于毫米波雷达和双目相机的同时定位与建图方法 Download PDF

Info

Publication number
CN113625271B
CN113625271B CN202110865159.XA CN202110865159A CN113625271B CN 113625271 B CN113625271 B CN 113625271B CN 202110865159 A CN202110865159 A CN 202110865159A CN 113625271 B CN113625271 B CN 113625271B
Authority
CN
China
Prior art keywords
image
frame
millimeter wave
wave radar
binocular camera
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
CN202110865159.XA
Other languages
English (en)
Other versions
CN113625271A (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.)
China Automotive Innovation Corp
Original Assignee
China Automotive Innovation Corp
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 China Automotive Innovation Corp filed Critical China Automotive Innovation Corp
Priority to CN202110865159.XA priority Critical patent/CN113625271B/zh
Publication of CN113625271A publication Critical patent/CN113625271A/zh
Application granted granted Critical
Publication of CN113625271B publication Critical patent/CN113625271B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/89—Radar or analogous systems specially adapted for specific applications for mapping or imaging
    • 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
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
    • G01S11/12—Systems for determining distance or velocity not using reflection or reradiation using electromagnetic waves other than radio waves
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • G01S13/865—Combination of radar systems with lidar systems
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Radar Systems Or Details Thereof (AREA)
  • Traffic Control Systems (AREA)

Abstract

本文属于物同时定位和建图技术领域，具体涉及一种基于毫米波雷达和双目相机的同时定位与建图方法，包括：对设备行进过程中双目相机采集到的图像数据进行特征提取，获得多个特征点；根据毫米波雷达采集到的距离数据，确定出各个特征点对应的第一深度参数；根据图像数据中前一帧图像中各个特征点对应的第一深度参数，利用视觉里程计计算后一帧图像中各个特征点对应的第二深度参数；根据每一帧图像中各个特征点对应的第一深度参数和第二深度参数，确定出每一帧图像中各个特征点对应的目标深度参数；基于目标深度参数完成同时定位与建图。本申请的实施能够在图像采集的过程中实现同时定位和建图，提高建图精度。

Description

基于毫米波雷达和双目相机的同时定位与建图方法
技术领域
本发明涉及同时定位与建图方法技术领域，特别涉及一种基于毫米波雷达和双目相机的同时定位与建图方法。
背景技术
随着人工智能概念的兴起以及计算机视觉相关技术的成熟，无人设备和机器人技术受到越来越多的关注。其中，同时定位与地图构建(Simultaneous Localization andMapping，SLAM)是机器人相关技术中一个不可缺少的模块。根据使用的传感器不同，SLAM技术主要可分为基于相机的视觉SLAM以及基于激光雷达(Light Detection and Ranging，LiDAR)的激光SLAM，其中视觉SLAM可细分为单目SLAM、双目SLAM以及RGBD-SLAM，激光SLAM又可分为基于单线激光雷达的2D激光SLAM以及基于多线激光雷达的3D激光SLAM。
现有的同时定位与地图构建依赖的硬件有GNSS(全球导航卫星系统)、惯性导航、激光雷达、相机等。激光雷达一般安装在车顶，360旋转采集数据，但是激光雷达在雨雾或者沙层暴天不能工作且激光雷达的价格高，目前不适宜用于普通的乘用车。相机虽然能提供丰富的纹理信息，但是相机的信息维度是二维的，缺少深度信息，需要通过复杂的算法来计算，并且和真实值之间存在误差，这就对所用的计算机硬件性能提出了更高的要求。
因此，如何利用毫米波雷达和双目相机完成同时定位与建图，提升建图的深度参数，仍是一个值得研究的问题。
发明内容
为了解决上述技术问题，本文的具体技术方案如下：
一方面，本文提供一种基于毫米波雷达和双目相机的同时定位与建图方法，所述方法应用于设置有毫米波雷达和双目相机的设备中，所述方法包括：
对所述设备行进过程中所述双目相机采集到的图像数据进行特征提取，获得所述图像数据中每一帧图像的特征点；所述图像数据包括连续拍摄的图像帧；
根据所述毫米波雷达采集到的距离数据，确定出所述每一帧图像中各个特征点对应的第一深度参数；
根据所述图像数据中前一帧图像中各个特征点对应的第一深度参数，利用视觉里程计计算后一帧图像中各个特征点对应的第二深度参数；
根据每一帧图像中各个特征点对应的第一深度参数和第二深度参数，对每一帧图像中各个特征点对应的深度参数进行优化，获得每一帧图像中各个特征点对应的目标深度参数；
基于所述目标深度参数对所述图像数据中相邻两帧图像进行运动估算，完成所述前一帧图像至所述后一帧图像的同时定位与建图
进一步地，所述对所述设备行进过程中所述双目相机采集到的图像数据进行特征提取，获得所述图像数据中每一帧图像的特征点；所述图像数据包括连续拍摄的图像帧，之前还包括：
对所述双目相机和所述毫米波雷达的内参标定。
进一步地，所述根据所述毫米波雷达采集到的距离数据，确定出所述每一帧图像中各个特征点对应的第一深度参数，包括：
利用所述双目相机和所述毫米波雷达的内参标定结果，将所述距离数据投影至对应图像数据的特征点，转化为每个特征点的第一深度参数。
进一步地，所述利用所述双目相机和所述毫米波雷达的内参标定结果，将所述距离数据投影至对应图像数据的特征点，转化为每个特征点的第一深度参数，包括：
利用所述双目相机和所述毫米波雷达的内参标定结果，将所述距离数据投影至对应图像数据的特征点；
滤除没有所述距离数据投影的图像数据特征点；
将所述距离数据转化为对应图像数据的特征点的第一深度参数。
进一步地，所述根据所述图像数据中前一帧图像中各个特征点对应的第一深度参数，利用视觉里程计计算后一帧图像中各个特征点对应的第二深度参数，包括：
将所述前一帧图像中各个特征点与所述后一帧图像中各个特征点进行比对，确定出所述前一帧图像和所述后一帧图像中相同的特征点；
利用视觉里程计根据所述相同的特征点计算后一帧图像中各个特征点对应的第二深度参数。
进一步地，所述根据每一帧图像中各个特征点对应的第一深度参数和第二深度参数，对每一帧图像中各个特征点对应的深度参数进行优化，获得每一帧图像中各个特征点对应的目标深度参数，根据如下公式：
Xf～N(μf，σf 2)＝Xp*Xm＝N(μp，σp 2)*N(μm，σm 2)
其中，Xf为目标深度参数，N代表正太分布，μf、μp和μm均为均值，σf、σp和σm均为标准差，Xp为第一深度参数，Xm为第二深度参数。
另一方面，本发明提供一种基于毫米波雷达和双目相机的同时定位与建图装置，所述装置包括：
特征提取模块，用于对设备行进过程中所述双目相机采集到的图像数据进行特征提取，获得所述图像数据中每一帧图像的特征点；所述图像数据包括连续拍摄的图像帧；
第一深度参数确定模块，用于根据所述毫米波雷达采集到的距离数据，确定出所述每一帧图像中各个特征点对应的第一深度参数；
第二深度参数确定模块，用于根据所述图像数据中前一帧图像中各个特征点对应的第一深度参数，利用视觉里程计计算后一帧图像中各个特征点对应的第二深度参数；
目标深度参数确定模块，用于根据每一帧图像中各个特征点对应的第一深度参数和第二深度参数，对每一帧图像中各个特征点对应的深度参数进行优化，获得每一帧图像中各个特征点对应的目标深度参数；
同时定位与建图模块，用于基于所述目标深度参数对所述图像数据中相邻两帧图像进行运动估算，完成所述前一帧图像至所述后一帧图像的同时定位与建图。
另一方面，本发明提供一种计算机可读存储介质，所述计算机可读存储介质中存储有至少一条指令或至少一段程序，所述至少一条指令或至少一段程序由处理器加载并执行以实现如上述所述基于毫米波雷达和双目相机的同时定位与建图方法。
再一方面，本发明提供一种电子设备，包括至少一个处理器，以及与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述至少一个处理器通过执行所述存储器存储的指令实现如上述所述基于毫米波雷达和双目相机的同时定位与建图方法。
再一方面，本发明提供一种车辆，所述车辆设置有自动驾驶系统，所述自动驾驶系统包括基于毫米波雷达和双目相机的同时定位与建图装置；其中，所述基于毫米波雷达和双目相机的同时定位与建图装置包括特征提取模块、第一深度参数确定模块、第二深度参数确定模块、目标深度参数确定模块和同时定位与建图模块。
采用上述技术方案，本文所述的一种基于毫米波雷达和双目相机的同时定位与建图方法，在设备行进过程中通过双目相机采集设备周围环境的图像数据，利用毫米波雷达采集设备周围环境的障碍物的距离数据，在对图像数据特征提取后将每个特征点对应的距离数据投影到图像数据中的到对应的第二深度参数，根据前一帧的第一深度参数利用视觉里程计计算后一帧图像中各个特征点对应的第一深度参数，对第一深度参数和第二深度参数进行优化，计算出目标深度参数，并利用目标深度参数对图像数据中相邻两帧图像进行运动估算，完成前一帧图像至所述后一帧图像的同时定位与建图。本文的实施能够提升同时定位与建图的效率，同时定位与建图的算法相对现有技术的算法更加简单，减少同时定位与建图过程中的数据冗余。
为让本文的上述和其他目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附图式，做详细说明如下。
附图说明
为了更清楚地说明本文实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本文的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1示出了本文实施例中提供的一种基于毫米波雷达和双目相机的同时定位与建图方法的步骤示意图；
图2示出了本文实施例中提供的另一种基于毫米波雷达和双目相机的同时定位与建图方法的步骤示意图；
图3示出了本文实施例中提供的又一种基于毫米波雷达和双目相机的同时定位与建图方法的步骤示意图；
图4示出了本文实施例中提供的再一种基于毫米波雷达和双目相机的同时定位与建图方法的步骤示意图；
图5示出了本文实施例中一种基于毫米波雷达和双目相机的同时定位与建图装置的结构示意图；
图6示出了本文实施例提供的一种基于毫米波雷达和双目相机的同时定位与建图的电子设备的结构示意图。
具体实施方式
下面将结合本文实施例中的附图，对本文实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本文一部分实施例，而不是全部的实施例。基于本文中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本文保护的范围。
需要说明的是，本文的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本文的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、装置、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
为了更好的解释本申请，现对激光雷达和毫米波雷达进行对比：
激光雷达特点：精度高，稳定性强。
但是激光雷达通过发射光束进行探测因此探测范围窄，光束受遮挡后就无法正常使用，因此在雨雪雾霾天，沙尘暴等恶劣天气不能开启，受环境影响大。并且没有穿透能力，探头必须完全外露才能达到探测效果，对于安装车辆来说影响车辆外形美观。因此，激光雷达防撞器在使用过程中局限性较大。
毫米波雷达特点：精准度高，抗干扰能力强探测距离远，呈广角探测，探测范围广，作用时速可达到120码以上，全天候工作，雨雪雾霾沙尘暴等恶劣天气，均能开启正常使用。穿透能力强，安装也可以完全隐蔽，不影响车辆整体外观。因此毫米波雷达技术更适用于汽车防撞领域。
目前主流的毫米波雷达频段为24G毫米波雷达，少数采用60GHz频段。车元素最新开发了77G毫米波雷达相对于24G毫米波雷达拥有更多的优势，也将是未来实现无人驾驶不可或缺的部分。
为了解决上述问题，本文实施例提供了基于毫米波雷达和双目相机的同时定位与建图方法，能够降低同时定位与建图方法的成本，提升同时定位与建图的效率。图1示出了本文实施例中提供的一种基于毫米波雷达和双目相机的同时定位与建图方法的步骤示意图，图2示出了本文实施例中提供的另一种基于毫米波雷达和双目相机的同时定位与建图方法的步骤示意图，如图1-图2所示，所述方法应用于设置有毫米波雷达和双目相机的设备中，所述方法包括以下步骤：
步骤S102、对所述设备行进过程中所述双目相机采集到的图像数据进行特征提取，获得所述图像数据中每一帧图像的特征点；所述图像数据包括连续拍摄的图像帧。
在具体的实施过程中，设备可以是车辆、移动机器人、无人机等能够移动的工具。设备中设置有多个双目相机和多个毫米波雷达，在设备行进过程中，双目相机可以采集设备周围环境的图像数据，图像数据是以所在的双目相机的坐标点采集的，在采集图像数据后提取图像数据中的特征点，每个特征点表征一个障碍物，不同的特征点可以通过不同标签的形式标记出不同的类别，本申请采用的双目相机可以通过坐标转换的形式将采集的图像数据转换至设备的坐标系下。图像数据可以是通过左右目匹配的方式得到在左右相机中都可以看到的在单帧中相对稳定的特征点。
可以理解的是，图像数据中包括多个连续的图像帧，相邻的图像帧中具有多个相同的特征点(即障碍物)，每个图像帧可以携带有不同的时间标签。
其中，特征点可以采用特征向量的形式体现。
步骤S104、根据所述毫米波雷达采集到的距离数据，确定出所述每一帧图像中各个特征点对应的第一深度参数。
在具体的实施过程中，距离数据表征设备周围障碍物的距离特征，距离数据可以与采集到的特征点相对应，即距离数据的数量可以小于全部特征点。毫米波雷达采集的距离数据可以是设备坐标系下的向量数据。同样的图像数据也是转换到设备坐标系下的数据。其中，第一深度参数可以是经过处理的距离数据，第一深度参数可以表征的是对应特征点相对与设备坐标系原点的向量数据。
步骤S106、根据所述图像数据中前一帧图像中各个特征点对应的第一深度参数，利用视觉里程计计算后一帧图像中各个特征点对应的第二深度参数。
在一些可能的实施例中，所述根据所述图像数据中前一帧图像中各个特征点对应的第一深度参数，利用视觉里程计计算后一帧图像中各个特征点对应的第二深度参数，包括：
将所述前一帧图像中各个特征点与所述后一帧图像中各个特征点进行比对，确定出所述前一帧图像和所述后一帧图像中相同的特征点。
在具体的实施过程中，前一帧图像中各个特征点的描述子进行比对，当特征点所在前一帧图像和后一帧图像的轮廓大致相同、标定轮廓的颜色相同且特征点所在第一图像帧的位置与所在第一图像帧的位置与其他特征点所在第一图像帧的位置和所在第二图像帧的相对位置相同时，这两个特征点的描述子的向量距离差最小，则可以确定两个该特征点为相同特征点。
利用视觉里程计根据所述相同的特征点计算后一帧图像中各个特征点对应的第二深度参数。
在具体的实施过程中，视觉里程计的原理如下：
输入可以是左右相机图像序列(图像数据)，时刻t和t+1时采集的图像表示为It和It+1，相机内参数据已提前标定好。
输出可以是针对不同帧图像对，需要估算出两帧图像之间的旋转矩阵R和平移向量T。具体的算法可以是捕捉图像对It，It+1，即前一帧图像和后一帧图像；并对以上图像进行畸变校准；计算时刻t和t+1的视差图；使用FAST算法检测图像It中的特征，并与图像It+1中的特征匹配；利用深度图，计算上一步中匹配特征点的3D坐标值，此时，获得两组点云数据；选取点云数据中的一部分子集，使得所有的匹配都是互相兼容的；估算上一步检测中的第二深度参数。其中，在计算深度图之前，为了补偿镜头带来的畸变，通过相机标定以及校准等过程，可以得到去畸变的图像对，图像对通过校准后，同一目标在左右图像中的水平位置是对齐的。
可以理解的是，前一帧图像和后一帧图像具有相同的特征点。在设备移动过程中，相同的特征点在前一帧图像和后一帧图像的深度参数是不同的，由于前一帧图像中各个特征点对应的第一深度参数已经确定，因此，可以利用利用视觉里程计计算出后一帧图像中各个特征点对应的第二深度参数。第一深度参数和第二深度参数均表征为对应时刻设备所在位置与特征点的距离。
需要说明的是，第一深度参数为毫米波雷达采集的经过处理后的距离数据，第二深度参数是根据第一深度参数计算出。
S108、根据每一帧图像中各个特征点对应的第一深度参数和第二深度参数，对每一帧图像中各个特征点对应的深度参数进行优化，获得每一帧图像中各个特征点对应的目标深度参数。
在具体的实施过程中，前一帧图像(特征点已有第一深度值)和后一帧图像(特征点未有深度值)可以通过2D-3D PnP匹配可以得到运动估计的初值，此时后一帧图像的特征点可以通过此初值得到一个第二深度参数，毫米波雷达提供后一帧图像的第一深度参数，结合滤波处理可以得到一个更为精确的值，具体的滤波的公式可以如下：说明如下所示：
Xf～N(μf，σf 2)＝Xp*Xm＝N(μp，σp 2)*N(μm，σm 2)
其中，Xf为目标深度参数，N代表正太分布(Normal distribution)，μf、μp和μm均为均值，σf、σp和σm均为标准差，Xp为第一深度参数，Xm为第二深度参数。
S110、基于所述目标深度参数对所述图像数据中相邻两帧图像进行运动估算，完成所述前一帧图像至所述后一帧图像的同时定位与建图。
在具体的实施过程中，视觉运动信息作为初始位姿，通过局部视觉特征点地图对图像数据进行配准，构建出环境地图，并利用关键帧不断进行回环检测与重定位，对关键帧的位姿和地图点进行优化，即可以根据目标深度参数，按照RGBD_SLAM算法即可完成前一帧图像至所述后一帧图像的同时定位与建图。需要说明的是，在完成所述前一帧图像至所述后一帧图像的同时定位与建图后还可以重复执行S102-110(即图2中的后端优化)，直至完成特定区域的建图。上述特定区域可以是室内或室外，如室内停车场、室外停车场、商场等。
可以理解的是，为了保证同时定位与建图的准确向可以对采集的图像数据的帧图像进行限定，即只有在图像数据的帧图像满足同时定位与建图要求后才能够对图像数据进行同时定位与建图。
本说明书实施例提供的基于毫米波雷达和双目相机的同时定位与建图方法，可以仅通过毫米波雷达和双目相机在采集的图像数据满足预设要求时，且在设备行进过程中通过双目相机采集设备周围环境的图像数据，利用毫米波雷达采集设备周围环境的障碍物的距离数据，在对图像数据特征提取后将每个特征点对应的距离数据投影到图像数据中的到对应的第二深度参数，根据前一帧的第一深度参数利用视觉里程计计算后一帧图像中各个特征点对应的第一深度参数，对第一深度参数和第二深度参数进行优化，计算出目标深度参数，并利用目标深度参数对图像数据中相邻两帧图像进行运动估算，完成前一帧图像至所述后一帧图像的同时定位与建图。本文的实施能够提升同时定位与建图的效率，同时定位与建图的算法相对现有技术的算法更加简单，减少同时定位与建图过程中的数据冗余。
在上述实施例基础上，本说明书一个实施例中，图3示出了本文实施例中提供的又一种基于毫米波雷达和双目相机的同时定位与建图方法的步骤示意图，如图3所示，所述对所述设备行进过程中所述双目相机采集到的图像数据进行特征提取，获得所述图像数据中每一帧图像的特征点；所述图像数据包括连续拍摄的图像帧，之前还包括：
S202、对所述双目相机和所述毫米波雷达的内参标定。
在具体的实施过程中，对双目相机的标定的内参可以包括一下一种或多种：摄像机内参数矩阵，畸变系数矩阵，本征矩阵，基础矩阵，旋转矩阵以及平移矩阵。
毫米波雷达标定的内参为毫米波雷达与设备坐标系之间的旋转矩阵以及平移矩阵，可以理解的是，本申请中使用多个毫米波雷达，因此每个毫米波雷达均需要与设备设备坐标系之间的旋转矩阵以及平移矩阵标定。
双目相机和毫米波雷达之间标定的内参为双目相机和毫米波之间的旋转矩阵以及平移矩阵；
通过上述内参的标定就可以得到设备与双目相机、双目相机与毫米波雷达、毫米波雷达与设备两两之间的旋转矩阵以及平移矩阵及每个单独的双目相机或毫米波雷达与设备坐标系之间的旋转矩阵以及平移矩阵。
在上述实施例基础上，本说明书一个实施例中，所述根据所述毫米波雷达采集到的距离数据，确定出所述每一帧图像中各个特征点对应的第一深度参数，包括：
利用所述双目相机和所述毫米波雷达的内参标定结果，将所述距离数据投影至对应图像数据的特征点，转化为每个特征点的第一深度参数。
在具体的实施过程中，示例地，多毫米波雷达的数据结合标定统一映射到设备坐标系下，可以得到设备周边障碍物信息的三维坐标值，公式如下：
其中代表从毫米波雷达到设备坐标系的旋转矩阵，/>代表从毫米波雷达到车设备坐标系的平移矩阵，/>代表在毫米波雷达的测量值(距离数据)，/>代表在设备坐标系下的转换值(第一深度参数)。
再将这上述转换值通过车身和相机之间的标定投影到相机平面得到其中为图像的像素坐标值。
在上述实施例基础上，本说明书一个实施例中，图4示出了本文实施例中提供的再一种基于毫米波雷达和双目相机的同时定位与建图方法的步骤示意图，如图4所示，所述利用所述双目相机和所述毫米波雷达的内参标定结果，将所述距离数据投影至对应图像数据的特征点，转化为每个特征点的第一深度参数，包括：
S402、利用所述双目相机和所述毫米波雷达的内参标定结果，将所述距离数据投影至对应图像数据的特征点；
S404、滤除没有所述距离数据投影的图像数据特征点；
S406、将所述距离数据转化为对应图像数据的特征点的第一深度参数。
在具体的实施过程中，图像数据中采集的特征点的数据大于毫米波雷达采集的距离数据，为避免数据处理量过大，可以按照距离数据将图像帧中未投影到距离数据的特征点滤除，即为投影到距离数据的特征点可以使用图像处理的方式标定对应的深度参数。
另一方面，本发明提供一种基于毫米波雷达和双目相机的同时定位与建图装置，图5示出了本文实施例中一种基于毫米波雷达和双目相机的同时定位与建图装置的结构示意图，如图5所示，所述装置可以包括：
特征提取模块11，用于对设备行进过程中所述双目相机采集到的图像数据进行特征提取，获得所述图像数据中每一帧图像的特征点；所述图像数据包括连续拍摄的图像帧；
第一深度参数确定模块12，用于根据所述毫米波雷达采集到的距离数据，确定出所述每一帧图像中各个特征点对应的第一深度参数；
第二深度参数确定模块13，用于根据所述图像数据中前一帧图像中各个特征点对应的第一深度参数，利用视觉里程计计算后一帧图像中各个特征点对应的第二深度参数；
目标深度参数确定模块14，用于根据每一帧图像中各个特征点对应的第一深度参数和第二深度参数，对每一帧图像中各个特征点对应的深度参数进行优化，获得每一帧图像中各个特征点对应的目标深度参数；
同时定位与建图模块15，用于基于所述目标深度参数对所述图像数据中相邻两帧图像进行运动估算，完成所述前一帧图像至所述后一帧图像的同时定位与建图。
另一方面，本说明书实施例一种计算机可读存储介质，所述存储介质中存储有至少一条指令或者至少一段程序，所述至少一条指令或者至少一段程序由处理器加载并执行以实现如上述所述的基于毫米波雷达和双目相机的同时定位与建图方法。
再一方面，本说明书实施例提供一种多租户场景下的应用程序更新的电子设备，图6示出了本文实施例提供的一种基于毫米波雷达和双目相机的同时定位与建图的电子设备的结构示意，如图6所示，所述设备包括处理器和存储器，所述存储器中存储有至少一条指令或者至少一段程序，所述至少一条指令或者至少一段程序由所述处理器加载并执行以实现如上述任一所述的基于毫米波雷达和双目相机的同时定位与建图方法。
由于基于毫米波雷达和双目相机的同时定位与建图装置、计算机可读存储介质及基于毫米波雷达和双目相机的同时定位与建图设备与基于毫米波雷达和双目相机的同时定位与建图方法的技术效果相同，在此不在赘述。
另一方面，本申请提供一种车辆，所述车辆设置有自动驾驶系统，所述自动驾驶系统包括基于毫米波雷达和双目相机的同时定位与建图装置；其中，所述基于毫米波雷达和双目相机的同时定位与建图装置包括特征提取模块、第一深度参数确定模块、第二深度参数确定模块、目标深度参数确定模块和同时定位与建图模块。
其中，特征提取模块，用于对设备行进过程中所述双目相机采集到的图像数据进行特征提取，获得所述图像数据中每一帧图像的特征点；所述图像数据包括连续拍摄的图像帧；
第一深度参数确定模块，用于根据所述毫米波雷达采集到的距离数据，确定出所述每一帧图像中各个特征点对应的第一深度参数；
第二深度参数确定模块，用于根据所述图像数据中前一帧图像中各个特征点对应的第一深度参数，利用视觉里程计计算后一帧图像中各个特征点对应的第二深度参数；
目标深度参数确定模块，用于根据每一帧图像中各个特征点对应的第一深度参数和第二深度参数，对每一帧图像中各个特征点对应的深度参数进行优化，获得每一帧图像中各个特征点对应的目标深度参数；
同时定位与建图模块，用于基于所述目标深度参数对所述图像数据中相邻两帧图像进行运动估算，完成所述前一帧图像至所述后一帧图像的同时定位与建图。
需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。本发明实施例所提供测试方法，其实现原理及产生的技术效果和前述系统实施例相同，为简要描述，方法实施例部分未提及之处，可参考前述系统实施例中相应内容。
应理解，在本文的各种实施例中，上述各过程的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本文实施例的实施过程构成任何限定。
还应理解，在本文实施例中，术语“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系。例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本文的范围。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本文所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口、装置或单元的间接耦合或通信连接，也可以是电的，机械的或其它的形式连接。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本文实施例方案的目的。
另外，在本文各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本文的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本文各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
本文中应用了具体实施例对本文的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本文的方法及其核心思想；同时，对于本领域的一般技术人员，依据本文的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本文的限制。

Claims (10)

1.一种基于毫米波雷达和双目相机的同时定位与建图方法，其特征在于，所述方法应用于设置有毫米波雷达和双目相机的设备中，所述方法包括：
对所述设备行进过程中所述双目相机采集到的图像数据进行特征提取，获得所述图像数据中每一帧图像的特征点；所述图像数据包括连续拍摄的图像帧；
根据所述毫米波雷达采集到的距离数据，确定出所述每一帧图像中各个特征点对应的第一深度参数；
根据所述图像数据中前一帧图像中各个特征点对应的第一深度参数，利用视觉里程计计算后一帧图像中各个特征点对应的第二深度参数；
根据每一帧图像中各个特征点对应的第一深度参数和第二深度参数，对每一帧图像中各个特征点对应的深度参数进行优化，获得每一帧图像中各个特征点对应的目标深度参数；
基于所述目标深度参数对所述图像数据中相邻两帧图像进行运动估算，完成所述前一帧图像至所述后一帧图像的同时定位与建图。
2.根据权利要求1所述基于毫米波雷达和双目相机的同时定位与建图方法，其特征在于，所述对所述设备行进过程中所述双目相机采集到的图像数据进行特征提取，获得所述图像数据中每一帧图像的特征点；所述图像数据包括连续拍摄的图像帧，之前还包括：
对所述双目相机和所述毫米波雷达的内参标定。
3.根据权利要求2所述基于毫米波雷达和双目相机的同时定位与建图方法，其特征在于，所述根据所述毫米波雷达采集到的距离数据，确定出所述每一帧图像中各个特征点对应的第一深度参数，包括：
利用所述双目相机和所述毫米波雷达的内参标定结果，将所述距离数据投影至对应图像数据的特征点，转化为每个特征点的第一深度参数。
4.根据权利要求3所述基于毫米波雷达和双目相机的同时定位与建图方法，其特征在于，所述利用所述双目相机和所述毫米波雷达的内参标定结果，将所述距离数据投影至对应图像数据的特征点，转化为每个特征点的第一深度参数，包括：
利用所述双目相机和所述毫米波雷达的内参标定结果，将所述距离数据投影至对应图像数据的特征点；
滤除没有所述距离数据投影的图像数据特征点；
将所述距离数据转化为对应图像数据的特征点的第一深度参数。
5.根据权利要求3所述基于毫米波雷达和双目相机的同时定位与建图方法，其特征在于，所述根据所述图像数据中前一帧图像中各个特征点对应的第一深度参数，利用视觉里程计计算后一帧图像中各个特征点对应的第二深度参数，包括：
将所述前一帧图像中各个特征点与所述后一帧图像中各个特征点进行比对，确定出所述前一帧图像和所述后一帧图像中相同的特征点；
利用视觉里程计根据所述相同的特征点计算后一帧图像中各个特征点对应的第二深度参数。
6.根据权利要求1所述基于毫米波雷达和双目相机的同时定位与建图方法，其特征在于，所述根据每一帧图像中各个特征点对应的第一深度参数和第二深度参数，对每一帧图像中各个特征点对应的深度参数进行优化，获得每一帧图像中各个特征点对应的目标深度参数，根据如下公式：
Xf～N(μf,σf 2)＝Xp*Xm＝N(μp,σp 2)*N(μm,σm 2)
其中，Xf为目标深度参数，N代表正太分布，μf、μp和μm均为均值，σf、σp和σm均为标准差，Xp为第一深度参数，Xm为第二深度参数。
7.一种基于毫米波雷达和双目相机的同时定位与建图装置，其特征在于，所述装置包括：
特征提取模块，用于对设备行进过程中所述双目相机采集到的图像数据进行特征提取，获得所述图像数据中每一帧图像的特征点；所述图像数据包括连续拍摄的图像帧；
第一深度参数确定模块，用于根据所述毫米波雷达采集到的距离数据，确定出所述每一帧图像中各个特征点对应的第一深度参数；
第二深度参数确定模块，用于根据所述图像数据中前一帧图像中各个特征点对应的第一深度参数，利用视觉里程计计算后一帧图像中各个特征点对应的第二深度参数；
目标深度参数确定模块，用于根据每一帧图像中各个特征点对应的第一深度参数和第二深度参数，对每一帧图像中各个特征点对应的深度参数进行优化，获得每一帧图像中各个特征点对应的目标深度参数；
同时定位与建图模块，用于基于所述目标深度参数对所述图像数据中相邻两帧图像进行运动估算，完成所述前一帧图像至所述后一帧图像的同时定位与建图。
8.一种计算机可读存储介质，所述计算机可读存储介质中存储有至少一条指令或至少一段程序，所述至少一条指令或至少一段程序由处理器加载并执行以实现如权利要求1-6中任一项所述基于毫米波雷达和双目相机的同时定位与建图方法。
9.一种电子设备，包括至少一个处理器，以及与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述至少一个处理器通过执行所述存储器存储的指令实现如权利要求1-6中任一项所述基于毫米波雷达和双目相机的同时定位与建图方法。
10.一种车辆，所述车辆设置有自动驾驶系统，所述自动驾驶系统包括如权利要求7所述基于毫米波雷达和双目相机的同时定位与建图装置；其中，所述基于毫米波雷达和双目相机的同时定位与建图装置包括特征提取模块、第一深度参数确定模块、第二深度参数确定模块、目标深度参数确定模块和同时定位与建图模块。
CN202110865159.XA 2021-07-29 2021-07-29 基于毫米波雷达和双目相机的同时定位与建图方法 Active CN113625271B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110865159.XA CN113625271B (zh) 2021-07-29 2021-07-29 基于毫米波雷达和双目相机的同时定位与建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110865159.XA CN113625271B (zh) 2021-07-29 2021-07-29 基于毫米波雷达和双目相机的同时定位与建图方法

Publications (2)

Publication Number Publication Date
CN113625271A CN113625271A (zh) 2021-11-09
CN113625271B true CN113625271B (zh) 2023-10-27

Family

ID=78381818

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110865159.XA Active CN113625271B (zh) 2021-07-29 2021-07-29 基于毫米波雷达和双目相机的同时定位与建图方法

Country Status (1)

Country Link
CN (1) CN113625271B (zh)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110517303A (zh) * 2019-08-30 2019-11-29 的卢技术有限公司 一种基于双目相机和毫米波雷达的融合slam方法及系统
CN110570449A (zh) * 2019-09-16 2019-12-13 电子科技大学 一种基于毫米波雷达与视觉slam的定位与建图方法
CN110599546A (zh) * 2019-08-28 2019-12-20 贝壳技术有限公司 一种获取三维空间数据的方法、系统、装置和存储介质
CN111538029A (zh) * 2020-04-24 2020-08-14 江苏盛海智能科技有限公司 一种视觉与雷达融合的测量方法及终端
CN111899277A (zh) * 2020-07-09 2020-11-06 浙江大华技术股份有限公司 运动目标检测方法及装置、存储介质、电子装置
CN112241978A (zh) * 2020-10-21 2021-01-19 广州小鹏自动驾驶科技有限公司 一种数据处理的方法和装置
CN112258600A (zh) * 2020-10-19 2021-01-22 浙江大学 一种基于视觉与激光雷达的同时定位与地图构建方法
CN112348869A (zh) * 2020-11-17 2021-02-09 的卢技术有限公司 通过检测和标定恢复单目slam尺度的方法
CN113095154A (zh) * 2021-03-19 2021-07-09 西安交通大学 基于毫米波雷达与单目相机的三维目标检测系统及方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107747941B (zh) * 2017-09-29 2020-05-15 歌尔股份有限公司 一种双目视觉定位方法、装置及系统
US10739784B2 (en) * 2017-11-29 2020-08-11 Qualcomm Incorporated Radar aided visual inertial odometry initialization
CN112446909B (zh) * 2019-08-30 2022-02-01 上海商汤临港智能科技有限公司 一种深度图像补全方法及装置、计算机可读存储介质

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110599546A (zh) * 2019-08-28 2019-12-20 贝壳技术有限公司 一种获取三维空间数据的方法、系统、装置和存储介质
CN110517303A (zh) * 2019-08-30 2019-11-29 的卢技术有限公司 一种基于双目相机和毫米波雷达的融合slam方法及系统
CN110570449A (zh) * 2019-09-16 2019-12-13 电子科技大学 一种基于毫米波雷达与视觉slam的定位与建图方法
CN111538029A (zh) * 2020-04-24 2020-08-14 江苏盛海智能科技有限公司 一种视觉与雷达融合的测量方法及终端
CN111899277A (zh) * 2020-07-09 2020-11-06 浙江大华技术股份有限公司 运动目标检测方法及装置、存储介质、电子装置
CN112258600A (zh) * 2020-10-19 2021-01-22 浙江大学 一种基于视觉与激光雷达的同时定位与地图构建方法
CN112241978A (zh) * 2020-10-21 2021-01-19 广州小鹏自动驾驶科技有限公司 一种数据处理的方法和装置
CN112348869A (zh) * 2020-11-17 2021-02-09 的卢技术有限公司 通过检测和标定恢复单目slam尺度的方法
CN113095154A (zh) * 2021-03-19 2021-07-09 西安交通大学 基于毫米波雷达与单目相机的三维目标检测系统及方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Autonomous Obstacle Avoidance for UAV based on Fusion of Radar and Monocular Camera;Hang Yu 等;《2020 IEEE/RSJ International Conference on Intelligent Robots and Systems》;第23卷(第05期);5954-5961 *
单目相机-3维激光雷达的外参标定及融合里程计研究;肖军浩 等;《机器人》;第43卷(第01期);17-28 *

Also Published As

Publication number Publication date
CN113625271A (zh) 2021-11-09

Similar Documents

Publication Publication Date Title
CN110988912B (zh) 自动驾驶车辆的道路目标与距离检测方法、系统、装置
JP7073315B2 (ja) 乗物、乗物測位システム、及び乗物測位方法
CA3028653C (en) Methods and systems for color point cloud generation
CN110832275B (zh) 基于双目图像更新高分辨率地图的系统和方法
US10445928B2 (en) Method and system for generating multidimensional maps of a scene using a plurality of sensors of various types
CN110032949B (zh) 一种基于轻量化卷积神经网络的目标检测与定位方法
CN110859044B (zh) 自然场景中的集成传感器校准
WO2022007198A1 (en) Method and system for generating bird's eye view bounding box associated with object
CN112074875B (zh) 构造3d特征图的组优化深度信息方法和系统
JP2021508815A (ja) 妨害物体の検出に基づいて高精細度マップを補正するためのシステムおよび方法
CN111699410B (zh) 点云的处理方法、设备和计算机可读存储介质
CN112805766B (zh) 用于更新详细地图的装置和方法
CN109993060A (zh) 深度摄像头的车辆全向障碍物检测方法
CN116978009A (zh) 基于4d毫米波雷达的动态物体滤除方法
CN114459467B (zh) 一种未知救援环境中基于vi-slam的目标定位方法
CN113624223B (zh) 一种室内停车场地图构建方法及装置
CN113625271B (zh) 基于毫米波雷达和双目相机的同时定位与建图方法
Pfeiffer et al. Ground truth evaluation of the Stixel representation using laser scanners
US11138448B2 (en) Identifying a curb based on 3-D sensor data
Velat et al. Vision based vehicle localization for autonomous navigation
EP4148375A1 (en) Ranging method and apparatus
AU2018102199A4 (en) Methods and systems for color point cloud generation
WO2020232709A1 (en) Method and system for evaluating quality of a point cloud map
Jaspers et al. Fast and robust b-spline terrain estimation for off-road navigation with stereo vision
John et al. Sensor fusion and registration of lidar and stereo camera without calibration objects

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