CN110533722B - 一种基于视觉词典的机器人快速重定位方法及系统 - Google Patents

一种基于视觉词典的机器人快速重定位方法及系统 Download PDF

Info

Publication number
CN110533722B
CN110533722B CN201910826480.XA CN201910826480A CN110533722B CN 110533722 B CN110533722 B CN 110533722B CN 201910826480 A CN201910826480 A CN 201910826480A CN 110533722 B CN110533722 B CN 110533722B
Authority
CN
China
Prior art keywords
robot
pose
visual
image
laser
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
CN201910826480.XA
Other languages
English (en)
Other versions
CN110533722A (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.)
Dilu Technology Co Ltd
Original Assignee
Dilu Technology 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 Dilu Technology Co Ltd filed Critical Dilu Technology Co Ltd
Priority to CN201910826480.XA priority Critical patent/CN110533722B/zh
Publication of CN110533722A publication Critical patent/CN110533722A/zh
Application granted granted Critical
Publication of CN110533722B publication Critical patent/CN110533722B/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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/87—Combinations of systems using electromagnetic waves other than radio waves
    • G01S17/875—Combinations of systems using electromagnetic waves other than radio waves for determining attitude
    • 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
    • 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
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/42—Global feature extraction by analysis of the whole pattern, e.g. using frequency domain transformations or autocorrelation
    • G06V10/422—Global feature extraction by analysis of the whole pattern, e.g. using frequency domain transformations or autocorrelation for representing the structure of the pattern or shape of an object therefor
    • G06V10/424—Syntactic representation, e.g. by using alphabets or grammars
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/46—Descriptors for shape, contour or point-related descriptors, e.g. scale invariant feature transform [SIFT] or bags of words [BoW]; Salient regional features
    • G06V10/462—Salient features, e.g. scale invariant feature transforms [SIFT]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
    • G06V10/757—Matching configurations of points or features
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10004—Still image; Photographic image
    • 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/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • 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/30204—Marker
    • G06T2207/30208—Marker matrix

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Electromagnetism (AREA)
  • Multimedia (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computing Systems (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computational Linguistics (AREA)
  • Databases & Information Systems (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Manipulator (AREA)
  • Image Analysis (AREA)
  • Image Processing (AREA)

Abstract

本发明公开了一种基于视觉词典的机器人快速重定位方法及系统，包括以下步骤，使用图像采集模块获取当前图像帧，将其与视觉地图中保存的关键帧进行对比，找出最接近的相似关键帧；将当前图像帧与相似关键帧进行特征匹配，得出当前图像帧位姿相对于相似关键帧的位姿关系；根据相似关键帧在激光地图中的位姿以及当前图像帧相对于相似关键帧的位姿关系，得到当前机器人的位姿信息，完成重定位。本发明的有益效果：利用图像采集模块获取图像，当机器人在使用激光雷达传感器定位失败时进行重定位，是一种简单，鲁棒性强，无需人工干预的方法。

Description

一种基于视觉词典的机器人快速重定位方法及系统
技术领域
本发明涉及智能定位的技术领域，特别是，涉及一种基于视觉词典的机器人快速重定位方法及系统。
背景技术
SLAM技术的发展距今已有30余年的历史，涉及的技术领域众多。由于本身包含许多步骤，每一个步骤均可以使用不同算法实现，SLAM技术也是机器人和计算机视觉领域的热门研究方向。SLAM的英文全称是Simultaneous Localization and Mapping，中文称作“同时定位与地图创建”。SLAM试图解决这样的问题：一个机器人在未知的环境中运动，如何通过对环境的观测确定自身的运动轨迹，同时构建出环境的地图。
在机器人相关的技术领域中，精确定位是非常重要的一环。当前主流的机器人定位建图方案采用的传感器为激光雷达传感器，其具有方法简单、技术成熟的优点。然而，由于激光雷达传感器固有的缺点，当发生跟踪定位失败时，其不具有重新定位的能力，因此一旦发生跟踪定位失败，则需要将机器人重新置于原点位置，这给机器人的正常使用带来了极大的不便。
发明内容
本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。
鉴于上述和/或现有存在的问题，提出了本发明。
因此，本发明其中一个目的是提供一种基于视觉词典的机器人快速重定位方法，当机器人发生跟踪定位失败时，能够使用图像采集模块获取的图像进行机器人的重定位。
为解决上述技术问题，本发明提供如下技术方案：一种基于视觉词典的机器人快速重定位方法，包括以下步骤，使用图像采集模块获取当前图像帧，将其与视觉地图中保存的关键帧进行对比，找出最接近的相似关键帧；将当前图像帧与相似关键帧进行特征匹配，得出当前图像帧位姿相对于相似关键帧的位姿关系；根据相似关键帧在激光地图中的位姿以及当前图像帧相对于相似关键帧的位姿关系，得到当前机器人的位姿信息，完成重定位。
作为本发明所述的基于视觉词典的机器人快速重定位方法的一种优选方案，其中：还包括以下步骤，使用图像采集模块对机器人进行初始标定；分别基于激光SLAM算法和视觉SLAM算法建立激光地图和视觉地图，并对两个地图进行对齐；基于视觉SLAM算法，建立视觉词典。
作为本发明所述的基于视觉词典的机器人快速重定位方法的一种优选方案，其中：所述当前图像帧与关键帧的对比包括以下步骤，使用SURF特征提取算法对当前帧图像进行特征提取，并使用视觉词典DBow表示，与关键帧图像的词典表示进行汉明距离比对，找出最接近的相似关键帧图像；所述汉明距离对比阶段还包括，使用KD树方法进行对比，对该对比过程进行加速。
作为本发明所述的基于视觉词典的机器人快速重定位方法的一种优选方案，其中：所述得出当前帧位姿相对于相似关键帧的位姿关系阶段，包括以下步骤，找出当前的图像帧与相似关键帧中匹配度较高的前50组匹配特征点对；使用2D－2D对极几何算法计算出基础矩阵；使用RANSAC算法选取一个最优基础矩阵，得出当前图像帧位姿相对于相似关键帧的位姿关系。
作为本发明所述的基于视觉词典的机器人快速重定位方法的一种优选方案，其中：所述对机器人进行初始标定包括，使用棋盘格标定板及张氏标定法进行标定，得到相机内参系数及畸变系数；测量激光雷达传感器与图像采集模块之间的坐标系变换矩阵T，以机器人正向为y轴，垂直于y轴的右手侧为x轴，垂直于x轴和y轴的向上方向为z轴建立右手系坐标轴；调整激光雷达传感器与图像采集模块使其三轴分别与机器人坐标系一致；使用刻度尺测量出激光雷达传感器与图像采集模块间的三轴偏移距离，完成对机器人的初始标定。
作为本发明所述的基于视觉词典的机器人快速重定位方法的一种优选方案，其中：所述使用激光SLAM建立激光地图包括，基于ICP算法进行激光SLAM相邻位置间的位姿变换的匹配计算；基于gmapping算法进行激光SLAM的地图构建。
作为本发明所述的基于视觉词典的机器人快速重定位方法的一种优选方案，其中：所述使用视觉SLAM建立视觉地图包括，前端跟踪，所述前端跟踪基于PnP算法计算出两帧图像间的位姿变换；后端优化，所述后端优化基于光束平差算法对位姿和地图点进行优化的，得到最优的位姿估计；回环检测，所述回环检测通过判断图像之间的相似性完成，建立现在的图像和过去曾经见过图像的对应关系。
作为本发明所述的基于视觉词典的机器人快速重定位方法的一种优选方案，其中：所述对激光地图与视觉地图进行对齐包括，根据初始的机器人标定过程，利用激光雷达传感器与图像采集模块之间的坐标系变换矩阵T，使其起点位置相同，记录下视觉SLAM中每个关键帧对应于激光SLAM中的位姿，完成对两个地图进行对齐。
作为本发明所述的基于视觉词典的机器人快速重定位方法的一种优选方案，其中：所述建立视觉词典包括，从关键帧上提取图像特征，通过聚类的方法聚出若干类，将这些类作为视觉词典。
本发明解决的又一个技术问题是提供一种基于视觉词典的机器人快速重定位系统，能够将基于视觉词典的机器人快速重定位方法应用于机器人的重定位。
为解决上述技术问题，本发明提供如下技术方案：一种基于视觉词典的机器人快速重定位系统，包括，图像采集模块，所述图像采集模块用于采集机器人周围的图像信息；激光雷达传感器，所述激光雷达传感器用于采集机器人周围物体与机器人之间的距离信息；重定位模块，所述重定位模块能够接受所述图像采集模块采集到的图像，对机器人进行重新定位；建图模块，所述建图模块根据所述图像采集模块和所述激光雷达传感器采集的信息建立地图和视觉词典。
本发明的有益效果：本发明通过在机器人上安装图像采集模块，对环境地图建立视觉词典，以供机器人在使用激光雷达传感器发生定位失败、丢失机器人所在的位置时，使用图像采集模块获取的图像进行机器人的重定位。此方法简单，鲁棒性强，避免了定位失败时需要人工干预的缺陷。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。其中：
图1为本发明提供的第一种实施例中基于视觉词典的快速重定位方法的进行机器人重定位的流程图；
图2为本发明提供的第二种实施例中基于视觉词典的快速重定位方法的建立两种地图并对齐的流程图；
图3为本发明提供的第二种实施例中基于视觉词典的快速重定位方法的两种坐标系变换的示意图；
图4为本发明提供的第三种实施例中基于视觉词典的快速重定位系统的示意图。
具体实施方式
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明。
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。
其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。
实施例1
本发明第一个实施例中提供了一种基于视觉词典的快速重定位方法，能够在机器人使用激光雷达传感器200发生定位失败，丢失机器人所在的位置时，使用图像采集模块100获取的图像快速进行机器人的重定位。具体的，参照图1，包括以下步骤，
步骤1：使用图像采集模块100获取当前图像帧，将其与视觉地图中保存的关键帧进行对比，找出最接近的相似关键帧。该步骤中视觉地图基于视觉SLAM算法建立，图像采集模块100可以是摄像头或照相机，能够采集单帧图像信息，图像采集模块100设置于机器人上，且能够跟随机器人移动，当机器人发生定位失败时，图像采集模块100采集当前图像帧，将该当前图像帧与视觉地图中保存的关键帧进行对比，找出最接近的相似关键帧。
其中，当前图像帧与关键帧对比的具体步骤包括，首先使用SURF特征提取算法对当前帧图像进行特征提取，并使用视觉词典DBow表示。具体的，SURF特征提取算法是一种稳定的特征点提取算法，该算法实现的流程如下，
构建黑塞矩阵，生成所有的兴趣点，用于特征提取。黑塞矩阵是一个多元函数的二阶偏导数构成的方阵，描述了函数的局部曲率，每一个像素点都可以求出一个黑塞矩阵，具体如下：
黑塞矩阵的判别式为：
其中，函数值f(x，y)为图像像素l(x，y)，当黑塞矩阵的判别式取得局部极大值时，判定当前点是比周围邻域内其他点更亮或更暗的点，由此确定关键点的位置。为使特征点需要具备尺度无关性，因此在构造黑塞矩阵前，需要对其进行高斯滤波，滤波器为二阶标准高斯函数：
L(x，y)＝G(t)·I(x，t)
通过特定核间的卷积计算二阶偏导数，这样便能计算出H矩阵的三个矩阵元素L_xx,L_xy,L_yy，从而计算出H矩阵：
由于高斯核是服从正态分布的，从中心点往外，系数越来越低，因此算法中使用盒式滤波器来近似替代高斯滤波器从而提高运算速度。盒式滤波器对图像的滤波转化成计算图像上不同区域间像素和的加减运算问题，只需要简单几次查找积分图就可以完成。每个像素的黑塞矩阵行列式的近似值为：
det(H)＝Dxx*Dyy-(0.9*Dxy)2
构建尺度空间。尺度空间由O组L层组成，其中不同组间图像的尺寸一致，但不同组间使用的盒式滤波器的模板尺寸逐渐增大，而同一组间不同层间使用相同尺寸的滤波器，但是滤波器的模糊系数逐渐增大。
特征点定位。将经过黑塞矩阵处理的每个像素点与二维图像空间和尺度空间邻域内的26个点进行比较，初步定位出关键点，再经过滤除能量比较弱的关键点以及错误定位的关键点，筛选出最终的稳定的特征点。
特征点主方向分配。在SURF特征提取算法中，特征点方向分配采用的是统计特征点圆形邻域内的harr小波特征，具体的，在特征点的圆形邻域内，统计60度扇形内所有点的水平、垂直harr小波特征总和，然后扇形以一定间隔进行旋转并再次统计该区域内harr小波特征值之后，最后将值最大的那个扇形的方向作为该特征点的主方向。
生成特征点描述子。在SURF特征提取算法中，取特征点周围44个矩形区域块，方向沿特征点的主方向，每个子区域统计25个像素的水平方向和垂直方向的haar小波特征，这里的水平和垂直方向都是相对主方向而言的。该haar小波特征为水平方向值之后、垂直方向值之后、水平方向绝对值之后以及垂直方向绝对值之和4个方向，将这4个值作为每个子块区域的特征向量，一共有444＝64维向量作为SURF特征的描述子。
完成对当前帧图像进行特征提取后，将检测到的局部特征集合进行聚类，每个聚类中心对应一个视觉单词；构建表征图像的视觉词典DBow，动态调整视觉单词数量以评估视觉词典DBow的大小；最终当前帧图像由视觉词典DBow中的视觉单词权重向量表示。
将当前图像帧的词典表示与关键帧图像的词典表示进行汉明距离比对，找出与当前图像帧最接近的相似关键帧图像。具体的，汉明距离是指两个等长的字符串之间对应位置不同字符的个数，也就是将一个字符串变换为另一个字符串所需要替换的字符的个数。通过当前图像帧的词典与关键帧图像的词典的汉明距离比对，找出与当前图像帧最接近的相似关键帧图像。
具体的，在当前图像帧的词典表示与关键帧图像的词典表示进行汉明距离比对过程中，可以使用KD树方法进行对比，对该对比过程进行加速。KD树是对数据点在k为空间划分的一种二叉树数据结构，每个节点表示的一个数据空间范围。它的构建过程是一个循环迭代的过程，假设数据的维度是D维，每次选择D维数据中方差最大的维度，以中间的数据进行划分，划分为左子空间和右子空间，直到数据空间中只包含一个数据点。
在KD树中进行数据查询是特征匹配的重要环节，目的是检测在KD树中与查询点距离最近的数据点。查询过程可分为两步：二叉树搜索及回溯。其中，二叉树搜索是指顺着路径找到最近邻的近似点，也就是与待查询点处于同一子空间的叶子节点；回溯是指得到最邻近的近似点后，沿上述搜索路径反向考察是否有距离查询节点更近的数据点，直至回溯到根节点。
步骤2：将当前图像帧与相似关键帧进行特征匹配，得出当前图像帧位姿相对于相似关键帧的位姿关系。具体的，包括以下步骤，找出当前的图像帧与相似关键帧中匹配度较高的前50组匹配特征点对，通过对当前图像帧提取SURF特征点，然后与相似关键帧中的SURF特征点进行特征匹配，从而选出匹配度最高的前50组特征点对。其中，提取SURF特征点的算法可参考步骤1中的SURF特征点提取算法。
使用2D－2D对极几何算法计算出基础矩阵。具体的，对极几何描述的是两幅视图之间的内在射影关系，其与外部场景无关，只依赖于摄像机内参数和这两幅视图之间的的相对姿态，对极几何可以用以下等式描述：
其中，F是基本矩阵，能够表征两个相机的相对位置及相机内参数，p1和p2分别是两幅视图上的对应点的像素坐标。
使用RANSAC算法选取一个最优基础矩阵，从而得出当前图像帧位姿相对于相似关键帧的位姿关系。具体的，RANSAC算法的输入是一组观测数据(往往含有较大的噪声或无效点)，一个用于解释观测数据的参数化模型以及一些可信的参数。RANSAC算法通过反复选择数据中的一组随机子集来达成目标，被选取的子集被假设为局内点，并用以下方法进行验证：有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出；用该模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点；如果有足够多的点被归类为假设的局内点，那么认为该模型就足够合理；用所有假设的局内点去重新估计模型(譬如使用最小二乘法)，通过估计局内点与模型的错误率来评估模型；重复以上步骤直至一定的次数，每次产生的模型或因为局内点太少而被舍弃，或比现有的模型更好而被选用，最终能够选出一个最优模型。
通过选出的最优基础矩阵，计算当前图像帧位姿相对于相似关键帧的位姿关系。
步骤3：根据相似关键帧在激光地图中的位姿以及当前图像帧相对于相似关键帧的位姿关系，能够得到当前机器人在激光地图中的位姿信息，从而完成机器人的重定位。
实施例2
参照图2，为本发明的第二个实施例，该实施例与上一个实施例不同的是，还包括以下步骤，
步骤1：使用图像采集模块100对机器人进行初始标定。具体的，该步骤包括，使用棋盘格标定板及张氏标定法进行标定，得到相机内参系数及畸变系数。本专业相关人员可以理解的是，张氏标定法是一种基于单平面棋盘格的相机标定方法，该方法仅需要一张打印的棋盘格，标定简单，相机和标定板可以任意放置，且标定的精度高。
参照图3，测量激光雷达传感器200与图像采集模块100之间的坐标系变换矩阵T，变换矩阵T有如下关系：
其中，(x11，y11，z11，1)和(x21,y21，z21,1)分别是标定点在激光雷达传感器200坐标系和图像采集模块100坐标系下的广义坐标。
调整激光雷达传感器200与图像采集模块100使其x、y、z三轴分别与机器人的坐标系一致。
使用刻度尺测量出激光与图像采集模块100传感器间的三轴偏移距离，具体需要分别测量x,y,z三轴方向上的偏移距离，或者通过激光雷达与摄像头联合标定算法进行标定出三轴偏移完成对机器人的初始标定。
步骤2：分别基于激光SLAM算法和视觉SLAM算法建立激光地图和视觉地图，并对两个地图进行对齐。其中，基于激光SLAM算法建立激光地图包括以下步骤，
基于ICP算法进行激光SLAM相邻位置间的位姿变换的匹配计算。具体的，ICP算法是一种点云到点云的3D-3D配准方法，通过空间点云的配准，能够保证机器人定位的精度。ICP算法的基本原理如下：假设三维空间中存在两组点云，分别是固定不动的参考点云PL和需要调整的目标点云PR，
3D点匹配，在PL和PR中寻找对应坐标距离最相近的最近匹配点，其中，对于稀疏点云的微小运动，寻找欧拉空间最近点；对于密集点云或者较大运动，寻找描述子之间二进制汉明距离的最近点。
优化初值，通过初始配准的两个点集，计算各自点集质心的三维坐标L0与R0,通过这两个点的三维运动计算出相机运动初值.
迭代优化，由于初值匹配比较粗糙，通过初值变换获取的P′R与真实的PR点集之间存在误差，因此通过迭代减小此误差，直到小于阈值或者达到一定迭代次数。通过ICP算法能够获取机器人三维位姿变换的准确值。
基于gmapping算法进行激光SLAM的地图构建。其中，gmapping算法是一种基于粒子滤波的算法，具体的，其实现过程包括输入激光数据和里程计数据；根据t-1时刻的粒子位姿以及里程计数据，预测t时刻的粒子位姿，在初始值的基础上增加高斯采样的噪声点；对每个粒子执行扫描匹配算法，默认采用30个采样粒子，扫描匹配的作用是找到每个粒子在t时刻位姿的最佳坐标。为后面每个粒子权重更新做准备，如果扫描匹配失败，则粒子权重更新则采用默认的似然估计；混合运动模型和观测模型的建议分布，根据上一步扫描匹配获得的最佳坐标，围绕该坐标取若干位置样本(距离差值小于某阈值)计算均值与方差，使得当前粒子位置满足该均值方差的高斯分布；对各个粒子的权重进行更新，更新后进行归一化操作；使用Neff判断是否进行重采样，由于重采样频率越高，粒子退化越严重，粒子多样性降低，导致建图精确度降低，因此需要设定一个判定值改善该问题；维护地图，更新每个粒子维护的地图，每个粒子都维护自己的地图，即运动轨迹；在ros中进行地图更新。先得到最优的粒子，得到机器人最优轨迹，地图膨胀更新。
Gmapping算法能够实时构建地图，在构建小场景地图所需的计算量较小且精度较高，有效利用了车轮里程计信息，对激光雷达频率要求低、鲁棒性高。
基于视觉SLAM算法建立视觉地图，该算法主要包括前端跟踪、后端优化和回环检测三个步骤。具体的，前端跟踪是基于PnP算法计算出两帧图像间的位姿变换，该算法解决的问题是2D-3D位姿的变换，对其的求解有DLT、P3P、EPnP和UpnP，现阶段常用的做法是，先采用P3P得到初始解，然后构建重投影误差并使之最小化。
后端优化是基于光束平差算法对位姿和地图点进行优化的，通过后端优化能够得到最优的位姿估计。光束平差算法的目的是减少观测图像的点和参考图像的点之间位置投影变换误差。对场景中任意三维点P，由从每个视图所对应的的摄像机的光心发射出来并经过图像中P对应的像素后的光线，都将交于P这一点，对于所有三维点，则形成相当多的光束；实际过程中由于噪声等存在，每条光线几乎不可能汇聚于一点，因此在求解过程中，需要不断对待求信息进行调整，来使得最终光线能交于点P。对于m帧、每帧含N个特征点的目标函数如下：
针对具体的应用场景，光束平差法可以使用不同收敛方法，常用的方法有梯度下降法，牛顿法，高斯牛顿法，Levenber-Marquardt等方法。
回环检测通过判断图像之间的相似性完成，建立现在的图像和过去曾经见过图像的对应关系。在视觉SLAM问题中，位姿的估计是一个递推过程，即通过上一帧位姿来计算当前帧位姿，因此其中的误差便会逐帧累计起来，造成累积误差。为了减少累积误差，可以采用回环的方式令某一帧位姿不是与上一帧、而是与更之前的某一帧建立位姿约束关系，而距离较远的两帧不一定能够建立约束关系，因此需要通过回环检测找出可以建立这种位姿约束的历史帧。回环检测的主要问题是如果逐帧匹配则计算量太大、速度慢，对此可以采用成熟的词袋模型方法来帮助机器人完成闭环，即把帧与帧之间进行特征比配，具体的，从每幅图像中提取特征点和特征描述，特征描述一般是一个多维向量，因此可以计算两个特征描述之间的距离；将这些特征描述进行聚类，类别的个数就是词典的单词数，比如1000；也可以用Beyes、SVM等；将这些词典组织成树的形式，便于搜索。
完成激光地图和视觉地图建立后，需要对两个地图进行对齐。其中，激光地图和视觉地图的对齐是利用激光与图像采集模块100间的位姿变换T，根据起点位置相同，完成对两个地图进行对齐，
步骤3：基于视觉SLAM算法，建立视觉词典。包括从关键帧上提取图像特征，通过聚类的方法聚出若干类，将这些类作为视觉词典。具体的，视觉词典技术是采用视觉Bag-of-word模型(BOW模型)的技术。BOW模型最先是信息检索领域常用的文档表示方法，它假定对于一个文档，忽略它的单词顺序和语法、句法等要素，将其仅仅看作是若干个词汇的集合，文档中每个单词的出现都是独立的，不依赖于其它单词是否出现，在本实施例中，采用DboW模型，这是一个开源的C++库，用于给图像特征排序，并将图像转化成视觉词袋表示，它采用层级树状结构将相近的图像特征在物理存储上聚集在一起，创建一个视觉词典。视觉词典的生成包括以下步骤，
使用SURF特征提取算法从不同的图像中提取视觉词汇向量，这些向量代表的是图像中局部不变的特征点。
将所有特征点向量集合到一起，利用k均值聚类算法合并词义相近的视觉词汇，构造一个包含k个词汇的单词表；其中，k均值聚类算法是一种迭代求解的聚类分析算法，其步骤是随机选取K个对象作为初始的聚类中心，然后计算每个对象与各个种子聚类中心之间的距离，把每个对象分配给距离它最近的聚类中心。
统计单词表中每个单词在图像中出现的次数，从而将图像表示成为一个k维数值向量。
场景一：
通常测试方式有两种，一种是在开源标准测试数据集上进行测试，这种方式可以直接提供每个位置的真值，另一种是自己在真实环境中进行的测试，这种一般很难得到较精确的真实值，一个可行的测试重定位点的方法是可以让机器人运动一段时间后回到起初位置并计算位置差。
本实施例中可以选择干扰较多的走廊和汽车测试场作为测试区域，以上地点环境中一般白墙多，纹理少，不易提取特征，会对机器人的定位工作造成干扰，导致视觉跟踪发生失败；若选择在开源标准测试数据集上进行测试则选择SLAM标准测试数据集KITTI。
利用以上两种测试方法，分别采用本发明提供的基于视觉词典的机器人快速重定位方法和传统基于激光雷达传感器的重定位方法在定位失败时进行重新定位，并计算重定位的误差从而判断两种方法的重定位效果。具体的，机器人的位置可以通过高精GPS进行测量，测出来的位置作为实际值，判断重定位的精度可以通过误差进行，常用的误差度量包括以下两种：
(1)Relative Pose Error(RPE)。RPE表示的是在一个固定长度的间隔内，所估计到的机器人姿态到真实姿态的平均误差；
(2)Absolute Trajectory Error(ATE)。ATE表示的是估计出来的轨迹与真实轨迹之间的绝对距离。
测试结果如下：
表1：基于不同重定位方法下的误差对比
基于以上测试数据显然可以看出，分别在标准测试数据集和测试人员自己划定的测试区域中进行测试，对比两种误差类型指标得到的误差，本发明提供的基于视觉词典的机器人快速重定位方法的误差均小于传统基于激光雷达传感器重定位方法的误差，重定位效果更优。
实施例3
参照图4的示意，为本实施例提出的一种基于视觉词典的机器人快速重定位系统，以上实施例中的基于视觉词典的机器人快速重定位方法能够应用于该机器人快速重定位系统中。具体的，该系统可分为软件模块和硬件模块，其中硬件模块包括图像采集模块100和激光雷达传感器200，图像采集模块100用于采集机器人周围的图像信息，激光雷达传感器200则用于采集机器人周围物体与机器人之间的距离信息，图像采集模块100可以选用摄像头或相机，能够采购图像信息，激光雷达传感器200是利用激光技术进行测量的传感器，由激光器、激光检测器和测量电路组成，能够实现无接触远距离测量，速度快，精度高，量程大，抗光、电干扰能力强等。图像采集模块100和激光雷达传感器200均设置于机器人上，能够跟随机器人的移动而移动，从而采集到实时的信息。
软件模块包括重定位模块300和建图模块400，其中，建图模块400与图像采集模块100和激光雷达传感器200相连接，能够接收图像采集模块100和激光雷达传感器200采集到的信息，并根据这些信息建立激光地图和视觉地图，并将两个地图对齐；重定位模块300能够在机器人发生定位丢失时，接受图像采集模块100采集到的当前图像，使用基于视觉词典的机器人快速重定位方法对机器人进行重新定位。
在实际使用中，图像采集模块100和激光雷达传感器200分别采集机器人周围的图像、距离等信息，建图模块400根据图像采集模块100和激光雷达传感器200采集到的信息分别建立视觉地图和激光地图，并将两个地图对齐，当机器人发生定位失败时，图像采集模块100采集机器人当前所在环境的图像信息，重定位模块300根据此信息对机器人进行重新定位，避免了单一使用激光雷达传感器200时，定位一旦丢失需要进行人工干预的问题。
应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (2)

1.一种基于视觉词典的机器人快速重定位方法，其特征是：包括以下步骤，
使用图像采集模块(100)获取当前图像帧，将其与视觉地图中保存的关键帧进行对比，找出最接近的相似关键帧；
所述当前图像帧与关键帧的对比包括以下步骤，
使用SURF特征提取算法对当前帧图像进行特征提取，并使用视觉词典DBow表示，与关键帧图像的词典表示进行汉明距离比对，找出最接近的相似关键帧图像；
所述汉明距离对比阶段包括，使用KD树方法进行对比，使对比过程进行加速；
将当前图像帧与相似关键帧进行特征匹配，得出当前图像帧位姿相对于相似关键帧的位姿关系；
根据相似关键帧在激光地图中的位姿以及当前图像帧相对于相似关键帧的位姿关系，得到当前机器人的位姿信息，完成重定位；
使用图像采集模块(100)对机器人进行初始标定；
分别基于激光SLAM算法和视觉SLAM算法建立激光地图和视觉地图，并对两个地图进行对齐；
基于视觉SLAM算法，建立视觉词典；
得出当前帧位姿相对于相似关键帧的位姿关系阶段，包括以下步骤，
找出当前的图像帧与相似关键帧中匹配度较高的前50组匹配特征点对；
使用2D－2D对极几何算法计算出基础矩阵；
使用RANSAC算法选取一个最优基础矩阵，得出当前图像帧位姿相对于相似关键帧的位姿关系；
所述对机器人进行初始标定包括，
使用棋盘格标定板及张氏标定法进行标定，得到相机内参系数及畸变系数；
测量激光雷达传感器(200)与图像采集模块(100)之间的坐标系变换矩阵T，以机器人正向为y轴，垂直于y轴的右手侧为x轴，垂直于x轴和y轴的向上方向为z轴建立右手系坐标轴；
调整激光雷达传感器(200)与图像采集模块(100)使其三轴分别与机器人坐标系一致；
使用刻度尺测量出激光雷达传感器(200)与图像采集模块(100)间的三轴偏移距离，完成对机器人的初始标定；
使用激光SLAM建立激光地图包括，
基于ICP算法进行激光SLAM相邻位置间的位姿变换的匹配计算；
基于gmapping算法进行激光SLAM的地图构建；
使用视觉SLAM建立视觉地图包括，
前端跟踪，所述前端跟踪基于PnP算法计算出两帧图像间的位姿变换；
后端优化，所述后端优化基于光束平差算法对位姿和地图点进行优化的，得到最优的位姿估计；
回环检测，所述回环检测通过判断图像之间的相似性完成，建立现在的图像和过去曾经见过图像的对应关系；
对激光地图与视觉地图进行对齐包括，
根据初始的机器人标定过程，利用激光雷达传感器(200)与图像采集模块(100)之间的坐标系变换矩阵T，使其起点位置相同，记录下视觉SLAM中每个关键帧对应于激光SLAM中的位姿，完成对两个地图进行对齐；
所述建立视觉词典包括，从关键帧上提取图像特征，通过聚类的方法聚出若干类，将这些类作为视觉词典。
2.一种基于权利要求1所述的一种基于视觉词典的机器人快速重定位方法的系统，其特征在于：包括，图像采集模块(100)，所述图像采集模块(100)用于采集机器人周围的图像信息；
激光雷达传感器(200)，所述激光雷达传感器(200)用于采集机器人周围物体与机器人之间的距离信息；
重定位模块(300)，所述重定位模块(300)能够接受所述图像采集模块(100)采集到的图像，对机器人进行重新定位；
建图模块(400)，所述建图模块(400)根据所述图像采集模块(100)和所述激光雷达传感器(200)采集的信息建立地图和视觉词典。
CN201910826480.XA 2019-08-30 2019-08-30 一种基于视觉词典的机器人快速重定位方法及系统 Active CN110533722B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910826480.XA CN110533722B (zh) 2019-08-30 2019-08-30 一种基于视觉词典的机器人快速重定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910826480.XA CN110533722B (zh) 2019-08-30 2019-08-30 一种基于视觉词典的机器人快速重定位方法及系统

Publications (2)

Publication Number Publication Date
CN110533722A CN110533722A (zh) 2019-12-03
CN110533722B true CN110533722B (zh) 2024-01-12

Family

ID=68666386

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910826480.XA Active CN110533722B (zh) 2019-08-30 2019-08-30 一种基于视觉词典的机器人快速重定位方法及系统

Country Status (1)

Country Link
CN (1) CN110533722B (zh)

Families Citing this family (33)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111076733B (zh) * 2019-12-10 2022-06-14 亿嘉和科技股份有限公司 一种基于视觉与激光slam的机器人室内建图方法及系统
CN111077495B (zh) * 2019-12-10 2022-02-22 亿嘉和科技股份有限公司 一种基于三维激光的定位恢复方法
CN111105455B (zh) * 2019-12-13 2024-04-16 灵动科技（北京）有限公司 仓储系统与相关方法
CN111123279B (zh) * 2019-12-31 2022-05-27 芜湖哈特机器人产业技术研究院有限公司 一种融合nd和icp匹配的移动机器人重定位方法
CN111402331B (zh) * 2020-02-25 2023-03-28 华南理工大学 基于视觉词袋和激光匹配的机器人重定位方法
CN111337943B (zh) * 2020-02-26 2022-04-05 同济大学 一种基于视觉引导激光重定位的移动机器人定位方法
CN111340707B (zh) * 2020-02-27 2023-07-14 创新奇智(重庆)科技有限公司 基于关键帧的图像拼接系统及拼接方法
CN111402579A (zh) * 2020-02-29 2020-07-10 深圳壹账通智能科技有限公司 道路拥堵程度预测方法、电子装置及可读存储介质
CN114080625A (zh) * 2020-06-19 2022-02-22 深圳市大疆创新科技有限公司 绝对位姿确定方法、电子设备及可移动平台
CN111795704B (zh) * 2020-06-30 2022-06-03 杭州海康机器人技术有限公司 一种视觉点云地图的构建方法、装置
CN111931589A (zh) * 2020-07-15 2020-11-13 北京海益同展信息科技有限公司 机器人的重定位方法及装置、存储介质、电子设备
CN112101378A (zh) * 2020-08-20 2020-12-18 上海姜歌机器人有限公司 机器人重定位方法、装置及设备
CN112004196B (zh) * 2020-08-24 2021-10-29 唯羲科技有限公司 定位方法、装置、终端及计算机存储介质
CN112101145B (zh) * 2020-08-28 2022-05-17 西北工业大学 基于svm分类器的移动机器人位姿估计方法
CN112461228B (zh) * 2020-11-03 2023-05-09 南昌航空大学 一种相似环境下基于imu和视觉的二次回环检测定位方法
CN114529603B (zh) * 2020-11-23 2023-05-19 新疆大学 一种基于激光slam与单目视觉slam融合的里程计方法
CN112596064B (zh) * 2020-11-30 2024-03-08 中科院软件研究所南京软件技术研究院 激光与视觉融合的一体化室内机器人全局定位方法
CN114859370A (zh) * 2021-01-20 2022-08-05 京东科技信息技术有限公司 定位方法和装置、计算机装置和计算机可读存储介质
CN112950710A (zh) * 2021-02-24 2021-06-11 广州极飞科技股份有限公司 位姿确定方法、装置、电子设备和计算机可读存储介质
EP4057226A1 (en) * 2021-03-09 2022-09-14 Samsung Electronics Co., Ltd. Method and apparatus for estimating pose of device
CN112966616A (zh) * 2021-03-11 2021-06-15 深圳市无限动力发展有限公司 基于聚类的视觉重定位方法、装置、设备及介质
CN112990003B (zh) * 2021-03-11 2023-05-19 深圳市无限动力发展有限公司 图像序列重定位判断方法、装置和计算机设备
CN113008274B (zh) * 2021-03-19 2022-10-04 奥特酷智能科技(南京)有限公司 车辆初始化定位方法、系统及计算机可读介质
CN113223077A (zh) * 2021-05-21 2021-08-06 广州高新兴机器人有限公司 基于视觉辅助激光自动初始定位的方法及装置
CN113419249A (zh) * 2021-06-18 2021-09-21 珠海市一微半导体有限公司 一种重定位方法、芯片和移动机器人
CN113674409A (zh) * 2021-07-20 2021-11-19 中国科学技术大学先进技术研究院 基于视觉多机器人即时定位与同步建图方法、系统及介质
CN113609985B (zh) * 2021-08-05 2024-02-23 诺亚机器人科技(上海)有限公司 物体位姿检测方法、检测设备、机器人及可存储介质
CN113607160B (zh) * 2021-08-24 2023-10-31 湖南国科微电子股份有限公司 视觉定位恢复方法、装置、机器人和可读存储介质
CN113920187A (zh) * 2021-10-20 2022-01-11 上海微创医疗机器人（集团）股份有限公司 导管定位方法、介入手术系统、电子设备和存储介质
CN114445496A (zh) * 2022-01-27 2022-05-06 深圳市慧鲤科技有限公司 一种重定位模块的测试方法、装置、设备、系统和介质
CN114603555B (zh) * 2022-02-24 2023-12-08 江西省智能产业技术创新研究院 移动机器人初始位姿估计方法、系统、计算机及机器人
CN115267796B (zh) * 2022-08-17 2024-04-09 深圳市普渡科技有限公司 定位方法、装置、机器人和存储介质
CN117168469A (zh) * 2023-11-03 2023-12-05 普达迪泰(成都)智造研究院有限公司 一种自由切换导航方式的组合导航方法及系统

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置
CN110070580A (zh) * 2019-03-29 2019-07-30 南京华捷艾米软件科技有限公司 基于局部关键帧匹配的slam快速重定位方法及图像处理装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10593060B2 (en) * 2017-04-14 2020-03-17 TwoAntz, Inc. Visual positioning and navigation device and method thereof

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置
CN110070580A (zh) * 2019-03-29 2019-07-30 南京华捷艾米软件科技有限公司 基于局部关键帧匹配的slam快速重定位方法及图像处理装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
漫谈SLAM技术;Lemon;《HUAWEI》;20190723;1-13 *

Also Published As

Publication number Publication date
CN110533722A (zh) 2019-12-03

Similar Documents

Publication Publication Date Title
CN110533722B (zh) 一种基于视觉词典的机器人快速重定位方法及系统
CN109631855B (zh) 基于orb-slam的高精度车辆定位方法
CN113012212B (zh) 一种基于深度信息融合的室内场景三维点云重建方法和系统
Wahl et al. Surflet-pair-relation histograms: a statistical 3D-shape representation for rapid classification
CN108229416B (zh) 基于语义分割技术的机器人slam方法
US8274508B2 (en) Method for representing objects with concentric ring signature descriptors for detecting 3D objects in range images
Matei et al. Rapid object indexing using locality sensitive hashing and joint 3D-signature space estimation
CN110335319A (zh) 一种语义驱动的相机定位与地图重建方法和系统
CN108805906A (zh) 一种基于深度图的运动障碍物检测及定位方法
CN107169117B (zh) 一种基于自动编码器和dtw的手绘图人体运动检索方法
Yin et al. Dynam-SLAM: An accurate, robust stereo visual-inertial SLAM method in dynamic environments
CN112381890B (zh) 一种基于点线特征的rgb-d视觉slam方法
CN109272577B (zh) 一种基于Kinect的视觉SLAM方法
Etezadifar et al. A new sample consensus based on sparse coding for improved matching of SIFT features on remote sensing images
CN114299405A (zh) 一种无人机图像实时目标检测方法
Jiang et al. Learned local features for structure from motion of uav images: A comparative evaluation
CN108985375B (zh) 考虑粒子权值空间分布的多特征融合跟踪方法
Srivastava et al. Drought stress classification using 3D plant models
CN117036404A (zh) 一种单目热成像同时定位与建图方法和系统
CN116894876A (zh) 基于实时图像的6-dof的定位方法
Liu et al. Deep learning of directional truncated signed distance function for robust 3D object recognition
CN110070626B (zh) 一种基于多视角分类的三维物体检索方法
CN117197333A (zh) 基于多目视觉的空间目标重构与位姿估计方法及系统
CN115496859A (zh) 基于散乱点云交叉注意学习的三维场景运动趋势估计方法
CN111899284B (zh) 一种基于参数化esm网络的平面目标跟踪方法

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