[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN107301654A - 一种多传感器的高精度即时定位与建图方法 - Google Patents

一种多传感器的高精度即时定位与建图方法 Download PDF

Info

Publication number
CN107301654A
CN107301654A CN201710437709.1A CN201710437709A CN107301654A CN 107301654 A CN107301654 A CN 107301654A CN 201710437709 A CN201710437709 A CN 201710437709A CN 107301654 A CN107301654 A CN 107301654A
Authority
CN
China
Prior art keywords
lidar
frame
point cloud
point
image
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201710437709.1A
Other languages
English (en)
Other versions
CN107301654B (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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical 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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN201710437709.1A priority Critical patent/CN107301654B/zh
Publication of CN107301654A publication Critical patent/CN107301654A/zh
Application granted granted Critical
Publication of CN107301654B publication Critical patent/CN107301654B/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/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
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/10—Constructive solid geometry [CSG] using solid primitives, e.g. cylinders, cubes
    • 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
    • G06T2200/00—Indexing scheme for image data processing or generation, in general
    • G06T2200/08—Indexing scheme for image data processing or generation, in general involving all processing steps from image acquisition to 3D model generation
    • 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/10—Image acquisition modality
    • G06T2207/10024—Color image

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Geometry (AREA)
  • Multimedia (AREA)
  • Computer Graphics (AREA)
  • Software Systems (AREA)
  • Image Processing (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明提供了一种多传感器的高精度即时定位与建图方法，即使用彩色相机与激光雷达对周围环境进行高精度的实时三维构建，并实时测算出相机的位置与姿态。在快速的视觉SLAM的基础上，引入激光雷达的信息，并有选择的使用激光雷达信息，及时进行结果修正，再依靠修正后的结果构建三维地图，使定位与建图具有正确的尺度和较高的精度，同时使算法具有较低的复杂度。

Description

一种多传感器的高精度即时定位与建图方法
技术领域
本发明属机器视觉技术领域，具体涉及一种多传感器的高精度即时定位与建图方法。
背景技术
现阶段机器人的移动大多依靠人工路径规划，机器人的自主导航能力依赖于即时定位与建图技术(Simultaneous Localization and Mapping，以下简称为SLAM)。其核心任务是当机器人进入未知工作环境时，利用传感器信息，对周围环境进行高效率且准确地构建(Mapping)，同时得到设备在空间中的位置与姿态(Localization)。除了可以应用在机器人领域外，虚拟现实与增强现实设备的空间追踪以及自动驾驶也同样可以使用SLAM技术。
SLAM问题自提出以来已经过去了30年，这期间SLAM问题所用的传感器与计算方法都发生了巨大的变化。主流的SLAM技术多采用视觉传感器，包括单目传感器、多目传感器及彩色图像及深度信息(RGB-D)的传感器等。近年来，随着激光雷达技术的发展，激光雷达设备凭借其对环境度量的精准估计能力，以及对于光照变化不敏感等特性让激光雷达相关的SLAM方案在实际应用中有着广阔的发展空间。
现阶段主流的基于视觉的SLAM方案可根据优化方法分为两类，一类为使用滤波器的SLAM方法，另一类的是使用图优化的SLAM方法。
使用滤波器的SLAM方案模型构建比较简单，但误差会逐渐累计不可修复。Davison等人在文献“A.Davison,I.Reid,and N.Molton.MonoSLAM:Real-Time Single CameraSLAM.IEEE Transactions on Pattern Analysis and Machine Intelligence,pp.1052-1067,2007.”中提出了MonoSLAM方案，即是基于扩展卡尔曼滤波器(简称EKF)的单目传感器SLAM方案，通过在图像中构建路标点并使用EKF对SLAM进行模型构建而求解。
使用图优化方法的SLAM方案由于要构建姿态图往往运算量比较大。Artal等人在文献“R.Artal,J.Montiel,and J.Tardos.ORB-SLAM:AVersatile and AccurateMonocular SLAM System.IEEE Transactions on Robotics,vol.31,no.5,pp 1147-1163,2015.”中提出的ORB-SLAM是目前基于图优化方法的单目视觉SLAM方案，ORB-SLAM通过对图像提取ORB特征描述符(一种提取速度极快的特征描述符)，可以达到很高的速度，并且通过完善的图优化操作，得到的地图也具有很高的精确度。
单目视觉传感器的SLAM算法在实际应用中存在的问题有：单目视觉传感器难以估计环境的规模，即尺度；环境的深度是由三角化等数学计算求得，往往带有误差；视觉传感器难以处理光照剧烈，快速移动以及缺乏纹理等场景，会导致姿态估计精度下降；为了消除上述误差，需要引入大规模优化，求解困难且费时。
而单独使用其他的传感器如激光雷达传感器会有计算缓慢、信息不够丰富等问题，同样无法很好的解决SLAM任务。
发明内容
为了克服现有技术的不足，本发明提供一种多传感器的高精度即时定位与建图方法，即融合视觉传感器与激光雷达数据完成高精度即时定位与建图的方法。在快速的视觉SLAM基础上，通过引入激光雷达的信息，具有更高的定位与建图精度，且通过对激光雷达的数据进行有选择的使用，在保证精确度的情况下，让算法的复杂度较低。
一种多传感器的高精度即时定位与建图方法，其特征在于步骤如下：
步骤1：输入彩色图像并预处理：输入相机拍摄的彩色图像序列，将每一帧彩色图像转换为灰度图像；
步骤2：特征提取：利用ORB特征提取算法对每一帧灰度图像进行特征提取，得到每一帧图像的ORB特征点；
步骤3：特征匹配和运动估计：利用近似最近邻库对相邻两帧图像的特征点进行匹配，对匹配好的点对利用基于随机抽样一致法的PnP求解进行运动估计，得到相机的运动估计序列其中，为相机在拍摄第i帧和第i-1帧图像之间的运动估计，i＝2,…,N，N为图像帧数，RCam表示相机旋转矩阵，tCam表示相机位移矩阵；
步骤4：输入对应激光雷达数据并预处理：输入激光雷达数据，将距离大于激光雷达量程的0.8倍的点去掉，由剩余点构成激光雷达点云数据；这里，假定所述的激光雷达与步骤1中拍摄彩色图像的相机的位置关系固定、已完成联合标定，且二者视野相同、内部参数已知；
步骤5：激光雷达点云数据的特征匹配和运动估计：对激光雷达点云数据每隔x帧进行一次特征匹配和运动估计，即将每一帧激光雷达点云数据在水平方向上划分为四个等大区域，每个区域根据光滑度提取4个边缘点和4个平面点构成特征点，根据光滑度对第j帧和第j-x帧数据的特征点进行匹配，如果对匹配好的点对利用ICP算法进行运动估计，得到激光雷达的运动估计序列
其中，为激光雷达在扫描第j帧和第j-x帧数据之间的运动估计，j＝x+1,2x+1,…,M，M为激光雷达数据总帧数，x取值范围为[5,20]，RLidar表示激光雷达旋转矩阵，tLidar表示激光雷达位移矩阵；
所述的边缘点为区域内光滑度最小的点，所述的平面点为区域内光滑度最大的点，光滑度为其中，S代表第k次扫描中所有的点构成的集合，Lk,p与Lk,q代表p与q点的空间坐标，||·||表示欧式距离；所述的根据光滑度进行匹配是指如果光滑度的差值小于当前点的光滑度的5％，则匹配，否则，不匹配；
步骤6：误差修正并记录轨迹：用步骤5得到的激光雷达的运动估计替换对应时间间隔内相机的运动估计之和，得到误差修正后的相机运动估计序列S′，即相机的运动轨迹；
步骤7：建立三维点云地图：按照计算得到三维点云地图pclmap，其中，为第nj帧图像变换到第一帧图像视角坐标系下的三维点云地图，设第m帧激光雷达点云扫描的同一时刻相机拍摄的图像帧序号为nj，由nj构成的序列为N，m＝x+1,2x+1,…,M；为第nj帧图像的点云地图，L为步骤2中提取的第nj帧图像的特征点的个数，(xl,yl,zl)表示点的坐标，按以下公式计算：
xl＝(ul-cx)·dl/fx
yl＝(vl-cy)·dl/fy
zl＝dl
其中，l＝1,…,L，(ul,vl)为第l个特征点的图像像素坐标，(fx,fy,cx,cy)为给定的相机参数，dl为第l个特征点在图像拍摄时刻对应的激光雷达点云中检索到的深度值；
如果点云密度过大，则对点云地图数据进行降采样，即得到最终的三维点云地图；所述的点云密度过大是指一立方米空间中点的个数超过10000。
本发明的有益效果是：在快速的视觉SLAM的基础上，结合激光雷达的信息，使SLAM任务中的定位与建图都有了较高的精度与正确的尺度，同时克服了单激光雷达SLAM速度较慢、没有颜色信息等缺点；在保证精确度的前提下，算法有选择的使用激光雷达信息，保证了算法的实时性与低复杂度；本算法可以实时输出运动估计结果，并及时修正该结果，同时依靠修正后的结果构建三维地图，这样既保证了算法实用性，又保证了算法在较长时间内有着较高的精度。
附图说明
图1是本发明的一种多传感器的高精度即时定位与建图方法流程图
具体实施方式
下面结合附图和实施例对本发明进一步说明，本发明包括但不仅限于下述实施例。
本发明提供了一种多传感器的高精度即时定位与建图方法，使用彩色相机与激光雷达对周围环境进行高精度的实时三维构建，并实时测算出拍摄图像的相机的位置与姿态。本发明方法实现的前提是：用于拍摄的相机与激光雷达的位置关系是固定的，即二者在拍摄过程中有着相同的位移与偏转。并且，激光雷达已经完成与相机的联合标定，即图像中的每个像素可以在激光雷达三维点云中获取其深度值，且二者视野相同，内部参数已知。
如图1所示，本发明的一种多传感器的高精度即时定位与建图方法，其实现过程如下：
步骤1：输入彩色图像并预处理
本实施例使用PointGray Flea2彩色相机拍摄彩色图像。图像原始分辨率为1392×1040，帧率为17fps(帧每秒)，即一秒钟输入17帧的图像序列。首先，对图像进行预处理，即将彩色图像转化为灰度图像以进行后续运算。
步骤2：特征提取
对于上一步骤输入的灰度图像，首先逐帧提取ORB(Oriented FAST and RotatedBRIEF)特征点。此处的特征点也可以根据需求更换为其他类型的特征点，如SIFT、SURF等，但由于这些特征点的提取较为耗时，算法整体的运行速度可能会受到影响。
ORB(Oriented FAST and Rotated BRIEF)是一种快速特征点提取和描述的算法。这个算法是由Rublee等学者在2011年提出，“Rublee Ethan,et al."ORB:An efficientalternative to SIFT or SURF."IEEE International Conference on Computer VisionIEEE,2012:2564-2571.”。ORB算法分为两部分，分别是特征点提取和特征点描述。特征提取是由FAST(Features from Accelerated Segment Test)算法发展来的，特征点描述是根据BRIEF(Binary Robust Independent Elementary Features)特征描述算法改进的。ORB特征是将FAST特征点的检测方法与BRIEF特征描述子结合起来，并在它们原来的基础上做了改进与优化。经实验表明ORB特征的提取速度约为SIFT的100倍，是SURF的10倍。
本实施例直接使用计算机视觉库OPENCV中的ORB特征描述符提取函数对每一帧图像进行特征提取。
步骤3：特征匹配和运动估计
对提取特征点后相邻的两帧图像，使用近似最近邻库(Fast library forapproximate nearest neighbors，FLANN)进行特征匹配。FLANN库是目前最完整的近似最近邻匹配开源库，其不但实现了一系列查找算法，还包含了一种自动选取最快算法的机制。FLANN的基础算法由Muja等人在2009年提出，“M.Muja and D.Lowe,"Fast ApproximateNearest Neighbors with Automatic Algorithm Configuration",in InternationalConference on Computer Vision Theory and Applications(VISAPP'09),2009”。
对新输入的每一帧图像提取完特征点后，均使用计算机视觉库OPENCV中的FLANNbasedMatcher函数与上一帧图像进行特征匹配，得到两帧图像中若干对对应的特征点；然后对匹配结果基于随机抽样一致法(RANdom SAmple Consensus，RANSAC)的PnP求解对相机拍摄两帧图像之间的姿态变化进行估计。RANSAC是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。由Fischler和Bolles于1981年最先提出，“Fischler,M.A.and Bolles,R.C.Random Sample Consensus:A Paradigmfor Model Fitting with Applications to Image Analysis and AutomatedCartography.Communications of the ACM,24(6):381–395,1981.”。PnP求解的目的是利用已知匹配好的平面点对确定摄像头相对世界坐标系的位移和旋转，由Moreno-Noguer在2007年提出，“F.Moreno-Noguer,V.Lepetit and P.Fua,Accurate Non-Iterative O(n)Solution to the PnP Problem,IEEE International Conference on Computer Vision,Rio de Janeiro,Brazil,October 2007.”。本实施例直接使用计算机视觉库OPENCV中的solve PnPRansac函数来进行运算。
通过此步骤可以得到任意相邻两帧图像之间相机关于世界坐标系(第一帧图像的视角为主方向的坐标系)的运动估计(RCam,tCam)和相机的运动估计序列其中，为相机在拍摄第i帧和第i-1帧图像之间的运动估计，i＝2,…,N，N为图像帧数，RCam表示相机旋转矩阵，tCam表示相机位移矩阵。
由于特征点提取与匹配过程中有大量噪声存在，此步骤得到的姿态变化仍有一定误差，将利用步骤6进行修正。
步骤4：输入对应激光雷达数据并预处理
本实施例使用美国Velodyne公司生产的HDL-64E激光雷达，是一种小型多线全景激光雷达，拥有纵向26.9°，横向360°的视野。每秒钟会得到均匀分布在64条环境扫描线上的220000个点，来表示周围空间点距离激光雷达中心点的距离与位置关系。有效测量范围为120m。
实际使用中，为减小误差，将超出有效测量范围的0.8倍的点去掉，即去除测量距离大于80m的空间点，随后导入余下的所有点构成点云数据。
步骤5：激光雷达点云数据的特征匹配和运动估计
在本步骤中，本发明将根据输入的激光雷达点云数据，每隔x帧，提取特征并进行一次运动估计，以降低算法复杂度、提高计算效率，这样，可以得到激光雷达的运动估计序列其中，为激光雷达在扫描第j帧和第j-x帧数据之间的运动估计，j＝x+1,2x+1,…,M，M为激光雷达数据帧数，x取值范围为[5,20]，本实施例中x取值为5，RLidar表示激光雷达旋转矩阵，tLidar表示激光雷达位移矩阵。具体为：
由于点云是空间中离散的三维点，除了坐标之外没有其他信息，而同一个空间点坐标在移动的数据采集过程中是不断变化的，所以需在点云中定义可追踪的特征，为此定义一种基于光滑度的特征提取方式，即定义光滑度c为：
其中，S代表第k次扫描中所有的点构成的集合，Lk,p与Lk,q代表p与q点的空间坐标，||·||表示欧氏距离。
对于S中所有的点进行光滑度的计算，光滑度最大的点定义为平面点，光滑度最小的点则定义为边缘点。为了避免产生的特征点过多，首先将每次扫描的点云划分为四个等大区域，如取0°、90°、180°与270°四个主方向，每个方向左右各取±45°划分为一个区域。每个区域最多有4个边缘点和4个平面点构成特征点。
得到特征点后，根据光滑度对第j帧和第j-x帧数据的特征点进行匹配，光滑度的差值小于当前点的光滑度的5％的特征点点对认为是同一个空间点在两次扫描中的表达，即为匹配的点对。得到匹配的特征点后利用ICP算法(Iterative Closest Point)进行运动估计，即通过求解两组三维空间点得到激光雷达的运动估计(RLidar,tLidar)。ICP算法由Zhang等人提出，“Zhang,Zhengyou(1994)."Iterative point matching forregistration of free-form curves and surfaces".International Journal ofComputer Vision.Springer.13(12):119–152.”
步骤6：误差修正并记录轨迹
由于激光雷达点云数据精度高，数据可靠，在定位的结果上的误差极小，但是运算速度较慢；而基于彩色图像的算法在运算速度上较快，但精度较差。本发明针对两种传感器的姿态追踪的不同特性，采用激光雷达运动估计替换相机运动估计之和的方式来进行误差修正，提升整体算法的精确度，降低算法的整体时间复杂度。
即用步骤5得到的激光雷达的运动估计(扫描第j帧和第j-x帧数据之间的一次运动估计)替换对应时间间隔(扫描第j帧和第j-x帧数据的时间间隔)内相机的运动估计之和，得到误差修正后的相机运动估计序列，也就是相机的运动轨迹。
步骤7：建立三维点云地图
在最终建立高精度三维点云地图时，只使用误差较小的激光雷达的运动估计(RLidar,tLidar)。具体为：
按照计算得到三维点云地图pclmap，其中为第m帧激光雷达点云扫描的同一时刻相机拍摄的第nj帧图像(即设激光雷达扫描第m帧点云同一时刻相机拍摄的图像帧序号为nj，由nj构成的序列为N)变换到第一帧图像视角坐标系下的三维点云地图，为第nj帧图像的点云地图，L为步骤2中第nj帧图像提取得到的特征点个数，(xl,yl,zl)表示点的坐标，按以下公式计算：
xl＝(ul-cx)·dl/fx
yl＝(vl-cy)·dl/fy
zl＝dl
其中，l＝1,…,L，(ul,vl)为第l个特征点的图像坐标，(fx,fy,cx,cy)为给定的相机参数，dl为第l个特征点在图像拍摄时刻对应的激光雷达点云中检索到的深度值。
可以看出，这样构建的点云地图由多次扫描叠加而来，整体过于稠密。为了提高显示速度，降低点云密集程度，在点云的存储上设置空间点密度阈值，即当空间一立方米中的空间点个数超过10000个时，对点云数据进行降采样以保证空间点的数目在合理的范围内，得到最终的点云地图。当然，在扫描狭窄的空间时，空间点密度阈值可以相应的提高。
本实施例是在中央处理器为i5-4590 3.4GHz CPU、内存16G、Ubuntu14.04操作系统的电脑上进行的实验，实验使用第三方数据集在城市环境下相机运动轨迹以证明精确程度。
实验使用的序列13来自于KITTI数据集，该数据库由A.Geiger等人在文献“A.Geiger and Philip Lenz,Vision meets Robotics:The KITTI Dataset,International Journal of Robotics Research,2013”中提出，由PointGray Flea2color cameras相机拍摄彩色图像，由Velodyne HDL-64E激光雷达采集点云数据。
采用相机运动轨迹与真实值的偏移距离占地图大小的比例(％)及算法耗时为性能参数，将本发明方法与单目视觉中主流的ORB-SLAM算法进行对比，结果如表1所列。可以看出，本发明方法在引入激光雷达信息后，不但提高了整体的运算速度，而且在精度上超越了主流视觉SLAM方法。本发明还可以应用到自动驾驶、机器人自主导航等方面。
表1算法性能比较
方法 耗时(s) 偏移距离(％)
本发明方法 120.3 1.8
ORB-SLAM 126.0 3.1

Claims (1)

1.一种多传感器的高精度即时定位与建图方法，其特征在于步骤如下：
步骤1：输入彩色图像并预处理：输入相机拍摄的彩色图像序列，将每一帧彩色图像转换为灰度图像；
步骤2：特征提取：利用ORB特征提取算法对每一帧灰度图像进行特征提取，得到每一帧图像的ORB特征点；
步骤3：特征匹配和运动估计：利用近似最近邻库对相邻两帧图像的特征点进行匹配，对匹配好的点对利用基于随机抽样一致法的PnP求解进行运动估计，得到相机的运动估计序列其中，为相机在拍摄第i帧和第i-1帧图像之间的运动估计，i＝2,…,N，N为图像帧数，RCam表示相机旋转矩阵，tCam表示相机位移矩阵；
步骤4：输入对应激光雷达数据并预处理：输入激光雷达数据，将距离大于激光雷达量程的0.8倍的点去掉，由剩余点构成激光雷达点云数据；这里，假定所述的激光雷达与步骤1中拍摄彩色图像的相机的位置关系固定、已完成联合标定，且二者视野相同、内部参数已知；
步骤5：激光雷达点云数据的特征匹配和运动估计：对激光雷达点云数据每隔x帧进行一次特征匹配和运动估计，即将每一帧激光雷达点云数据在水平方向上划分为四个等大区域，每个区域根据光滑度提取4个边缘点和4个平面点构成特征点，根据光滑度对第j帧和第j-x帧数据的特征点进行匹配，如果对匹配好的点对利用ICP算法进行运动估计，得到激光雷达的运动估计序列
其中，为激光雷达在扫描第j帧和第j-x帧数据之间的运动估计，j＝x+1,2x+1,…,M，M为激光雷达数据总帧数，x取值范围为[5,20]，RLidar表示激光雷达旋转矩阵，tLidar表示激光雷达位移矩阵；
所述的边缘点为区域内光滑度最小的点，所述的平面点为区域内光滑度最大的点，光滑度为其中，S代表第k次扫描中所有的点构成的集合，Lk,p与Lk,q代表p与q点的空间坐标，||·||表示欧式距离；所述的根据光滑度进行匹配是指如果光滑度的差值小于当前点的光滑度的5％，则匹配，否则，不匹配；
步骤6：误差修正并记录轨迹：用步骤5得到的激光雷达的运动估计替换对应时间间隔内相机的运动估计之和，得到误差修正后的相机运动估计序列S′，即相机的运动轨迹；
步骤7：建立三维点云地图：按照计算得到三维点云地图pclmap，其中，为第nj帧图像变换到第一帧图像视角坐标系下的三维点云地图，设第m帧激光雷达点云扫描的同一时刻相机拍摄的图像帧序号为nj，由nj构成的序列为N，m＝x+1,2x+1,…,M；为第nj帧图像的点云地图，L为步骤2中提取的第nj帧图像的特征点的个数，(xl,yl,zl)表示点的坐标，按以下公式计算：
xl＝(ul-cx)·dl/fx
yl＝(vl-cy)·dl/fy
zl＝dl
其中，l＝1,…,L，(ul,vl)为第l个特征点的图像坐标，(fx,fy,cx,cy)为给定的相机参数，dl为第l个特征点在图像拍摄时刻对应的激光雷达点云中检索到的深度值；
如果点云密度过大，则对点云地图数据进行降采样，即得到最终的三维点云地图；所述的点云密度过大是指一立方米空间中点的个数超过10000。
CN201710437709.1A 2017-06-12 2017-06-12 一种多传感器的高精度即时定位与建图方法 Active CN107301654B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710437709.1A CN107301654B (zh) 2017-06-12 2017-06-12 一种多传感器的高精度即时定位与建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710437709.1A CN107301654B (zh) 2017-06-12 2017-06-12 一种多传感器的高精度即时定位与建图方法

Publications (2)

Publication Number Publication Date
CN107301654A true CN107301654A (zh) 2017-10-27
CN107301654B CN107301654B (zh) 2020-04-03

Family

ID=60135365

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710437709.1A Active CN107301654B (zh) 2017-06-12 2017-06-12 一种多传感器的高精度即时定位与建图方法

Country Status (1)

Country Link
CN (1) CN107301654B (zh)

Cited By (40)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108319976A (zh) * 2018-01-25 2018-07-24 北京三快在线科技有限公司 建图方法及装置
CN108399643A (zh) * 2018-03-15 2018-08-14 南京大学 一种激光雷达和相机间的外参标定系统和方法
CN108489496A (zh) * 2018-04-28 2018-09-04 北京空间飞行器总体设计部 基于多源信息融合的非合作目标相对导航运动估计方法及系统
CN108647646A (zh) * 2018-05-11 2018-10-12 北京理工大学 基于低线束雷达的低矮障碍物的优化检测方法及装置
CN108709560A (zh) * 2018-08-15 2018-10-26 苏州中研讯科智能科技有限公司 基于直线段特征的载运机器人高精度定位导航方法
CN108734654A (zh) * 2018-05-28 2018-11-02 深圳市易成自动驾驶技术有限公司 绘图与定位方法、系统及计算机可读存储介质
CN108759823A (zh) * 2018-05-28 2018-11-06 浙江大学 基于图像匹配的指定道路上低速自动驾驶车辆定位及纠偏方法
CN108896994A (zh) * 2018-05-11 2018-11-27 武汉环宇智行科技有限公司 一种无人驾驶车辆定位方法及设备
CN109031304A (zh) * 2018-06-06 2018-12-18 上海国际汽车城（集团）有限公司 基于视觉和毫米波雷达地图特征的隧道内车辆定位方法
CN109073398A (zh) * 2018-07-20 2018-12-21 深圳前海达闼云端智能科技有限公司 一种建立地图的方法、定位方法、装置、终端及存储介质
CN109166149A (zh) * 2018-08-13 2019-01-08 武汉大学 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN109326006A (zh) * 2018-09-30 2019-02-12 百度在线网络技术（北京）有限公司 地图融合方法和装置
CN109341706A (zh) * 2018-10-17 2019-02-15 张亮 一种面向无人驾驶汽车的多特征融合地图的制作方法
CN109407115A (zh) * 2018-12-25 2019-03-01 中山大学 一种基于激光雷达的路面提取系统及其提取方法
CN109579843A (zh) * 2018-11-29 2019-04-05 浙江工业大学 一种空地多视角下的多机器人协同定位及融合建图方法
CN109634279A (zh) * 2018-12-17 2019-04-16 武汉科技大学 基于激光雷达与单目视觉的物体定位方法
CN109974742A (zh) * 2017-12-28 2019-07-05 沈阳新松机器人自动化股份有限公司 一种激光里程计算方法和地图构建方法
CN110068824A (zh) * 2019-04-17 2019-07-30 北京地平线机器人技术研发有限公司 一种传感器位姿确定方法和装置
CN110163968A (zh) * 2019-05-28 2019-08-23 山东大学 Rgbd相机大型三维场景构建方法及系统
CN110398745A (zh) * 2019-08-05 2019-11-01 湖南海森格诺信息技术有限公司 基于激光雷达和视觉的叉车定位方法
CN110786585A (zh) * 2019-11-21 2020-02-14 朱利 一种多功能智能头盔
CN110823211A (zh) * 2019-10-29 2020-02-21 珠海市一微半导体有限公司 基于视觉slam的多传感器地图构建的方法、装置及芯片
WO2020097840A1 (en) * 2018-11-15 2020-05-22 Beijing Didi Infinity Technology And Development Co., Ltd. Systems and methods for correcting a high-definition map based on detection of obstructing objects
CN111207670A (zh) * 2020-02-27 2020-05-29 河海大学常州校区 一种线结构光标定装置及方法
CN111428578A (zh) * 2020-03-03 2020-07-17 深圳市镭神智能系统有限公司 一种自主体及其定位方法和装置
CN111445530A (zh) * 2020-03-24 2020-07-24 广东博智林机器人有限公司 一种轮椅床和轮椅的归位路径确定方法
CN111481109A (zh) * 2019-01-28 2020-08-04 北京奇虎科技有限公司 基于扫地机的地图噪点消除方法及装置
WO2020154966A1 (en) * 2019-01-30 2020-08-06 Baidu.Com Times Technology (Beijing) Co., Ltd. A rgb point clouds based map generation system for autonomous vehicles
CN111694903A (zh) * 2019-03-11 2020-09-22 北京地平线机器人技术研发有限公司 地图构建方法、装置、设备以及可读存储介质
CN111912431A (zh) * 2020-03-19 2020-11-10 中山大学 一种移动机器人导航系统定位精度测试方案
CN112132754A (zh) * 2020-11-25 2020-12-25 蘑菇车联信息科技有限公司 一种车辆移动轨迹修正方法及相关装置
CN112150507A (zh) * 2020-09-29 2020-12-29 厦门汇利伟业科技有限公司 一种物体姿态和位移的3d模型同步重现方法及系统
CN112230211A (zh) * 2020-10-15 2021-01-15 长城汽车股份有限公司 车辆定位方法、装置、存储介质及车辆
CN112484746A (zh) * 2020-11-26 2021-03-12 上海电力大学 一种基于地平面的单目视觉辅助激光雷达里程计方法
CN112955713A (zh) * 2018-10-29 2021-06-11 大疆科技股份有限公司 用于在可移动物体环境中进行实时地图构建的技术
EP3864484A2 (en) * 2018-10-12 2021-08-18 Boston Dynamics, Inc. Autonomous map traversal with waypoint matching
TWI772177B (zh) * 2021-09-10 2022-07-21 迪伸電子股份有限公司 自走裝置的移動控制方法及自走裝置
CN114863075A (zh) * 2022-07-05 2022-08-05 深圳市新天泽消防工程有限公司 基于多传感器的消防疏散路径规划方法、装置、设备
US12222723B2 (en) 2021-06-04 2025-02-11 Boston Dynamics, Inc. Directed exploration for navigation in dynamic environments
US12304082B2 (en) 2021-06-04 2025-05-20 Boston Dynamics, Inc. Alternate route finding for waypoint-based navigation maps

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040167716A1 (en) * 2002-12-17 2004-08-26 Goncalves Luis Filipe Domingues Systems and methods for controlling a density of visual landmarks in a visual simultaneous localization and mapping system
CN104359464A (zh) * 2014-11-02 2015-02-18 天津理工大学 基于立体视觉的移动机器人定位方法
CN104764457A (zh) * 2015-04-21 2015-07-08 北京理工大学 一种用于无人车的城市环境构图方法
CN104933708A (zh) * 2015-06-07 2015-09-23 浙江大学 一种基于多谱三维特征融合的植被环境中障碍物检测方法
US20150304634A1 (en) * 2011-08-04 2015-10-22 John George Karvounis Mapping and tracking system
CN105856230A (zh) * 2016-05-06 2016-08-17 简燕梅 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法
CN106052674A (zh) * 2016-05-20 2016-10-26 青岛克路德机器人有限公司 一种室内机器人的slam方法和系统
CN106127739A (zh) * 2016-06-16 2016-11-16 华东交通大学 一种结合单目视觉的rgb‑d slam方法
CN106153048A (zh) * 2016-08-11 2016-11-23 广东技术师范学院 一种基于多传感器的机器人室内定位及制图系统
CN106296812A (zh) * 2016-08-18 2017-01-04 宁波傲视智绘光电科技有限公司 同步定位与建图方法
CN106446815A (zh) * 2016-09-14 2017-02-22 浙江大学 一种同时定位与地图构建方法
CN106443687A (zh) * 2016-08-31 2017-02-22 欧思徕（北京）智能科技有限公司 一种基于激光雷达和全景相机的背负式移动测绘系统
CN106595659A (zh) * 2016-11-03 2017-04-26 南京航空航天大学 城市复杂环境下多无人机视觉slam的地图融合方法
CN106681330A (zh) * 2017-01-25 2017-05-17 北京航空航天大学 基于多传感器数据融合的机器人导航方法及装置

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040167716A1 (en) * 2002-12-17 2004-08-26 Goncalves Luis Filipe Domingues Systems and methods for controlling a density of visual landmarks in a visual simultaneous localization and mapping system
US20150304634A1 (en) * 2011-08-04 2015-10-22 John George Karvounis Mapping and tracking system
CN104359464A (zh) * 2014-11-02 2015-02-18 天津理工大学 基于立体视觉的移动机器人定位方法
CN104764457A (zh) * 2015-04-21 2015-07-08 北京理工大学 一种用于无人车的城市环境构图方法
CN104933708A (zh) * 2015-06-07 2015-09-23 浙江大学 一种基于多谱三维特征融合的植被环境中障碍物检测方法
CN105856230A (zh) * 2016-05-06 2016-08-17 简燕梅 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法
CN106052674A (zh) * 2016-05-20 2016-10-26 青岛克路德机器人有限公司 一种室内机器人的slam方法和系统
CN106127739A (zh) * 2016-06-16 2016-11-16 华东交通大学 一种结合单目视觉的rgb‑d slam方法
CN106153048A (zh) * 2016-08-11 2016-11-23 广东技术师范学院 一种基于多传感器的机器人室内定位及制图系统
CN106296812A (zh) * 2016-08-18 2017-01-04 宁波傲视智绘光电科技有限公司 同步定位与建图方法
CN106443687A (zh) * 2016-08-31 2017-02-22 欧思徕（北京）智能科技有限公司 一种基于激光雷达和全景相机的背负式移动测绘系统
CN106446815A (zh) * 2016-09-14 2017-02-22 浙江大学 一种同时定位与地图构建方法
CN106595659A (zh) * 2016-11-03 2017-04-26 南京航空航天大学 城市复杂环境下多无人机视觉slam的地图融合方法
CN106681330A (zh) * 2017-01-25 2017-05-17 北京航空航天大学 基于多传感器数据融合的机器人导航方法及装置

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
LV QIANG 等: "Absolute Scale Estimation of ORB-SLAM Algorithm based on Laser Ranging", 《PROCEEDINGS OF THE 35TH CHINESE CONTROL CONFERENCE》 *
RA´UL MUR-ARTAL 等: "ORB-SLAM: a Versatile and Accurate Monocular SLAM System", 《IEEE TRANSACTIONS ON ROBOTICS》 *
SATOSHIFUJIMOTO 等: "ORBSLAMMAPINITIALIZATIONIMPROVEMENTUSINGDEPTH", 《ICIP 2016》 *
侯荣波 等: "基于ORB-SLAM的室内机器人定位和三维稠密地图构建", 《计算机应用》 *
林连秀 等: "基于ORB-SLAM的移动机器人嵌入式实现与优化", 《微型机与应用》 *

Cited By (59)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109974742A (zh) * 2017-12-28 2019-07-05 沈阳新松机器人自动化股份有限公司 一种激光里程计算方法和地图构建方法
CN108319976A (zh) * 2018-01-25 2018-07-24 北京三快在线科技有限公司 建图方法及装置
CN108399643A (zh) * 2018-03-15 2018-08-14 南京大学 一种激光雷达和相机间的外参标定系统和方法
CN108489496A (zh) * 2018-04-28 2018-09-04 北京空间飞行器总体设计部 基于多源信息融合的非合作目标相对导航运动估计方法及系统
CN108647646A (zh) * 2018-05-11 2018-10-12 北京理工大学 基于低线束雷达的低矮障碍物的优化检测方法及装置
CN108896994A (zh) * 2018-05-11 2018-11-27 武汉环宇智行科技有限公司 一种无人驾驶车辆定位方法及设备
CN108647646B (zh) * 2018-05-11 2019-12-13 北京理工大学 基于低线束雷达的低矮障碍物的优化检测方法及装置
CN108759823B (zh) * 2018-05-28 2020-06-30 浙江大学 基于图像匹配的指定道路上低速自动驾驶车辆定位及纠偏方法
CN108734654A (zh) * 2018-05-28 2018-11-02 深圳市易成自动驾驶技术有限公司 绘图与定位方法、系统及计算机可读存储介质
CN108759823A (zh) * 2018-05-28 2018-11-06 浙江大学 基于图像匹配的指定道路上低速自动驾驶车辆定位及纠偏方法
CN109031304A (zh) * 2018-06-06 2018-12-18 上海国际汽车城（集团）有限公司 基于视觉和毫米波雷达地图特征的隧道内车辆定位方法
CN109073398B (zh) * 2018-07-20 2022-04-08 达闼机器人有限公司 一种建立地图的方法、定位方法、装置、终端及存储介质
CN109073398A (zh) * 2018-07-20 2018-12-21 深圳前海达闼云端智能科技有限公司 一种建立地图的方法、定位方法、装置、终端及存储介质
CN109166149A (zh) * 2018-08-13 2019-01-08 武汉大学 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN109166149B (zh) * 2018-08-13 2021-04-02 武汉大学 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN108709560A (zh) * 2018-08-15 2018-10-26 苏州中研讯科智能科技有限公司 基于直线段特征的载运机器人高精度定位导航方法
CN109326006A (zh) * 2018-09-30 2019-02-12 百度在线网络技术（北京）有限公司 地图融合方法和装置
EP3864484A2 (en) * 2018-10-12 2021-08-18 Boston Dynamics, Inc. Autonomous map traversal with waypoint matching
CN109341706A (zh) * 2018-10-17 2019-02-15 张亮 一种面向无人驾驶汽车的多特征融合地图的制作方法
CN109341706B (zh) * 2018-10-17 2020-07-03 张亮 一种面向无人驾驶汽车的多特征融合地图的制作方法
CN112955713A (zh) * 2018-10-29 2021-06-11 大疆科技股份有限公司 用于在可移动物体环境中进行实时地图构建的技术
US11035958B2 (en) 2018-11-15 2021-06-15 Bejing Didi Infinity Technology And Development Co., Ltd. Systems and methods for correcting a high-definition map based on detection of obstructing objects
WO2020097840A1 (en) * 2018-11-15 2020-05-22 Beijing Didi Infinity Technology And Development Co., Ltd. Systems and methods for correcting a high-definition map based on detection of obstructing objects
CN109579843A (zh) * 2018-11-29 2019-04-05 浙江工业大学 一种空地多视角下的多机器人协同定位及融合建图方法
CN109579843B (zh) * 2018-11-29 2020-10-27 浙江工业大学 一种空地多视角下的多机器人协同定位及融合建图方法
CN109634279A (zh) * 2018-12-17 2019-04-16 武汉科技大学 基于激光雷达与单目视觉的物体定位方法
CN109634279B (zh) * 2018-12-17 2022-08-12 瞿卫新 基于激光雷达与单目视觉的物体定位方法
CN109407115B (zh) * 2018-12-25 2022-12-27 中山大学 一种基于激光雷达的路面提取系统及其提取方法
CN109407115A (zh) * 2018-12-25 2019-03-01 中山大学 一种基于激光雷达的路面提取系统及其提取方法
CN111481109A (zh) * 2019-01-28 2020-08-04 北京奇虎科技有限公司 基于扫地机的地图噪点消除方法及装置
CN111481109B (zh) * 2019-01-28 2022-08-26 北京奇虎科技有限公司 基于扫地机的地图噪点消除方法及装置
CN112105890A (zh) * 2019-01-30 2020-12-18 百度时代网络技术（北京）有限公司 用于自动驾驶车辆的基于rgb点云的地图生成系统
WO2020154966A1 (en) * 2019-01-30 2020-08-06 Baidu.Com Times Technology (Beijing) Co., Ltd. A rgb point clouds based map generation system for autonomous vehicles
CN112105890B (zh) * 2019-01-30 2023-11-17 百度时代网络技术（北京）有限公司 用于自动驾驶车辆的基于rgb点云的地图生成系统
CN111694903A (zh) * 2019-03-11 2020-09-22 北京地平线机器人技术研发有限公司 地图构建方法、装置、设备以及可读存储介质
CN111694903B (zh) * 2019-03-11 2023-09-12 北京地平线机器人技术研发有限公司 地图构建方法、装置、设备以及可读存储介质
CN110068824A (zh) * 2019-04-17 2019-07-30 北京地平线机器人技术研发有限公司 一种传感器位姿确定方法和装置
CN110068824B (zh) * 2019-04-17 2021-07-23 北京地平线机器人技术研发有限公司 一种传感器位姿确定方法和装置
CN110163968B (zh) * 2019-05-28 2020-08-25 山东大学 Rgbd相机大型三维场景构建方法及系统
CN110163968A (zh) * 2019-05-28 2019-08-23 山东大学 Rgbd相机大型三维场景构建方法及系统
CN110398745A (zh) * 2019-08-05 2019-11-01 湖南海森格诺信息技术有限公司 基于激光雷达和视觉的叉车定位方法
CN110823211A (zh) * 2019-10-29 2020-02-21 珠海市一微半导体有限公司 基于视觉slam的多传感器地图构建的方法、装置及芯片
CN110786585A (zh) * 2019-11-21 2020-02-14 朱利 一种多功能智能头盔
CN111207670A (zh) * 2020-02-27 2020-05-29 河海大学常州校区 一种线结构光标定装置及方法
CN111428578A (zh) * 2020-03-03 2020-07-17 深圳市镭神智能系统有限公司 一种自主体及其定位方法和装置
CN111912431A (zh) * 2020-03-19 2020-11-10 中山大学 一种移动机器人导航系统定位精度测试方案
CN111445530A (zh) * 2020-03-24 2020-07-24 广东博智林机器人有限公司 一种轮椅床和轮椅的归位路径确定方法
CN112150507B (zh) * 2020-09-29 2024-02-02 厦门汇利伟业科技有限公司 一种物体姿态和位移的3d模型同步重现方法及系统
CN112150507A (zh) * 2020-09-29 2020-12-29 厦门汇利伟业科技有限公司 一种物体姿态和位移的3d模型同步重现方法及系统
CN112230211A (zh) * 2020-10-15 2021-01-15 长城汽车股份有限公司 车辆定位方法、装置、存储介质及车辆
CN112132754A (zh) * 2020-11-25 2020-12-25 蘑菇车联信息科技有限公司 一种车辆移动轨迹修正方法及相关装置
CN112132754B (zh) * 2020-11-25 2021-06-04 蘑菇车联信息科技有限公司 一种车辆移动轨迹修正方法及相关装置
CN112484746B (zh) * 2020-11-26 2023-04-28 上海电力大学 一种基于地平面的单目视觉辅助激光雷达里程计方法
CN112484746A (zh) * 2020-11-26 2021-03-12 上海电力大学 一种基于地平面的单目视觉辅助激光雷达里程计方法
US12222723B2 (en) 2021-06-04 2025-02-11 Boston Dynamics, Inc. Directed exploration for navigation in dynamic environments
US12304082B2 (en) 2021-06-04 2025-05-20 Boston Dynamics, Inc. Alternate route finding for waypoint-based navigation maps
TWI772177B (zh) * 2021-09-10 2022-07-21 迪伸電子股份有限公司 自走裝置的移動控制方法及自走裝置
CN114863075B (zh) * 2022-07-05 2022-10-14 深圳市新天泽消防工程有限公司 基于多传感器的消防疏散路径规划方法、装置、设备
CN114863075A (zh) * 2022-07-05 2022-08-05 深圳市新天泽消防工程有限公司 基于多传感器的消防疏散路径规划方法、装置、设备

Also Published As

Publication number Publication date
CN107301654B (zh) 2020-04-03

Similar Documents

Publication Publication Date Title
CN107301654B (zh) 一种多传感器的高精度即时定位与建图方法
CN111462200B (zh) 一种跨视频行人定位追踪方法、系统及设备
CN110223348B (zh) 基于rgb-d相机的机器人场景自适应位姿估计方法
CN110070615B (zh) 一种基于多相机协同的全景视觉slam方法
CN111076733B (zh) 一种基于视觉与激光slam的机器人室内建图方法及系统
CN107392964B (zh) 基于室内特征点和结构线结合的室内slam方法
CN104484648B (zh) 基于轮廓识别的机器人可变视角障碍物检测方法
CN110146099B (zh) 一种基于深度学习的同步定位与地图构建方法
CN109186606B (zh) 一种基于slam和图像信息的机器人构图及导航方法
Tian et al. Research on multi-sensor fusion SLAM algorithm based on improved gmapping
CN112785702A (zh) 一种基于2d激光雷达和双目相机紧耦合的slam方法
CN110686677A (zh) 一种基于几何信息的全局定位方法
CN108460779A (zh) 一种动态环境下的移动机器人图像视觉定位方法
CN107680133A (zh) 一种基于改进闭环检测算法的移动机器人视觉slam方法
CN112001926A (zh) 基于多维语义映射rgbd多相机标定方法、系统及应用
CN104732518A (zh) 一种基于智能机器人地面特征的ptam改进方法
CN114004900A (zh) 一种基于点线面特征的室内双目视觉里程计方法
CN112419497A (zh) 基于单目视觉的特征法与直接法相融合的slam方法
CN118274849B (zh) 一种基于多特征融合的智能搬运机器人定位方法及系统
CN102693543A (zh) 室外环境下变焦云台摄像机全自动标定方法
CN110648362A (zh) 一种双目立体视觉的羽毛球定位识别与姿态计算方法
CN116468786B (zh) 一种面向动态环境的基于点线联合的语义slam方法
Wang et al. Temporal and spatial online integrated calibration for camera and LiDAR
Bai et al. Colmap-pcd: An open-source tool for fine image-to-point cloud registration
CN114972539B (zh) 机房相机平面在线标定方法、系统、计算机设备和介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant