CN108090958B - 一种机器人同步定位和地图构建方法和系统 - Google Patents

一种机器人同步定位和地图构建方法和系统 Download PDF

Info

Publication number
CN108090958B
CN108090958B CN201711274095.6A CN201711274095A CN108090958B CN 108090958 B CN108090958 B CN 108090958B CN 201711274095 A CN201711274095 A CN 201711274095A CN 108090958 B CN108090958 B CN 108090958B
Authority
CN
China
Prior art keywords
line segment
point
intersection
image
blanking
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
CN201711274095.6A
Other languages
English (en)
Other versions
CN108090958A (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.)
Shanghai Readsense Network Technology Co ltd
Original Assignee
Shanghai Readsense Network 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 Shanghai Readsense Network Technology Co ltd filed Critical Shanghai Readsense Network Technology Co ltd
Priority to CN201711274095.6A priority Critical patent/CN108090958B/zh
Publication of CN108090958A publication Critical patent/CN108090958A/zh
Application granted granted Critical
Publication of CN108090958B publication Critical patent/CN108090958B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/05—Geographic models
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/536—Depth or shape recovery from perspective effects, e.g. by using vanishing points
    • 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
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Abstract

本发明公开了一种基于线段特征和线段消隐点的机器人同步定位和地图构建方法，应用于机器人在未知环境中的定位和地图构建。该方法包括从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；在地图中插入关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；基于交叉点进行闭环检测和全局的地图优化。本发明还公开了一种同步定位和地图构建系统。本发明可以用于机器人在未知环境中的定位和地图构建，特别适用于结构化或者半结构化的场景中，例如室内环境、室外有建筑的环境等。

Description

一种机器人同步定位和地图构建方法和系统
技术领域
本发明涉及智能机器人的导航技术领域，更具体的涉及同步定位与地图构建(simultaneous localization and mapping,SLAM)技术，涉及一种基于线段特征和线段消隐点建立地图，并根据所建立的地图进行定位的方法和系统。
背景技术
在各类自主移动机器人系统中，同步定位和地图构建(SLAM)系统起着重要的作用。在不需要对环境进行改造的方式中，目前的SLAM主要有基于激光雷达的方案和基于视觉传感器的方案。所述的视觉传感器通常包括单目摄像头、双目摄像头、多个摄像头的组合等。
基于激光雷达的SLAM方案相对比较成熟，其具有精度高、容易实现、不易受光照等外界光源干扰等优点。但是，性能好的激光雷达的成本较高；而低成本的激光雷达通常使用电机进行360度扫描。激光雷达的SLAM方案需要电机按照一定速率旋转，一般使用皮带及滑环的方式，这种低成本的激光雷达具有能耗高、噪声大、使用寿命有限等问题。近年来，随着计算机视觉技术的日渐成熟，开始有机器人产品使用基于视觉传感器的SLAM方案。基于视觉传感器的SLAM方案具有价格低、功耗低等优点，但是算法复杂，实现难度较大。
在机器人的SLAM系统中，地图的表示方式起着非常重要的作用。基于激光雷达的SLAM方案通常使用栅格地图；基于视觉传感器的SLAM方案通常使用基于点特征的特征地图。
除了点特征以外，线段特征也是一类比较重要的特征，但是在SLAM领域，关于线段特征的研究比较少。然而，在室内环境或者室外有建筑物的环境中，存在丰富的线段特征，如果能够充分利用这些线段特征构建地图，那么地图将拥有更强的几何表达能力，从而更能表现环境的几何结构。此外，线段特征对光照变化比较鲁棒，并且可以建立半稠密的环境地图，还可以方便地提取平面结构。因此，本申请的发明人认为：在线段特征丰富或者点特征比较稀少的环境下，基于线段特征的SLAM方法具有优势。
发明内容
本申请针对基于视觉传感器的同步定位和地图构建，提出一种基于线段特征和消隐点的同步定位和地图构建方法和系统，以实现在室内环境和室外有建筑环境下的定位和地图构建。
本申请公开了一种同步定位和地图构建SLAM方法，应用于机器人在未知环境中的定位和地图构建，包括：
A、从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；
B、在地图中插入关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；
C、基于交叉点进行闭环检测和全局的地图优化。
较佳的，所述A中从图像中提取线段特征、交叉点和消隐点包括：
A1、采集当前图像，从当前图像中提取线段特征；
A2、提取线段的交叉点；
A3、提取交叉点的描述子；
A4、根据描述子建立交叉点的初始匹配，利用对极几何和非刚性匹配消除错误的匹配；
A5、在线段特征的基础上提取消隐点。
较佳的，所述A2包括：
如果两条线段满足以下三个条件，则判定所述两条线段构成一个候选的交叉点：
条件1：两条线段的长度均大于一设定阈值；
条件2：交叉点到两条线段的距离均小于一设定阈值；
条件3：如果两条线段延长之后交叉，交叉点须位于图像平面内。
较佳的，所述A3包括：
定义交叉点的方向和尺度，其中，将交叉点的方向定义为构成该交叉点的两条线段的中分线，并采用至少两个尺度的策略；
将交叉点视为特征点，在交叉点周围提取描述子。
较佳的，所述A4中根据描述子建立交叉点的初始匹配包括：
对于双目图像和非双目图像采用不同的策略进行匹配，其中：
对于双目摄像头获取的双目图像，根据所述双目图像中的一幅图像中给定的交叉点，在另一幅图像的极线附近寻找匹配的交叉点；如果在极线附近存在匹配的交叉点，则两个交叉点之间的描述子距离；如果描述子距离小于设定的阈值，则判定所述两个交叉点构成一对匹配；
对于非双目图像，根据所述非双目图像中的一幅图像中给定的交叉点，在另一幅图像中寻找描述子距离最小、以及距离次小的交叉点，并计算最小距离和次小距离的比值，如果所述比值小于设定的阈值，则判定所述一幅图像中给定的交叉点与所述另一幅图像中距离最小的交叉点构成一对匹配。
较佳的，所述B包括：
在局部地图的集束调整中，建立消除线段的重投影误差和消隐点的重投影误差的误差函数，其中：
线段的重投影误差定义为平面上检测的线段与投影线段的两个端点的距离之和；
消隐点的重投影误差的定义为投影到单位球面上的消隐点坐标和实际检测的消隐点坐标之间的距离；
如果线段的重投影误差超过设定阈值，则剔除该线段，如果消隐点的重投影误差超过阈值，则剔除该消隐点。
较佳的，所述误差函数为：
Figure BDA0001496157320000031
其中，ρ(·)是鲁棒的柯西函数；
el是一个四维向量，表示线段的重投影误差；
ev是一个三维向量，表示消隐点的重投影误差；
Figure BDA0001496157320000032
和
Figure BDA0001496157320000033
分别是4×4和3×3的对角阵，分别表示线段和消隐点的信息矩阵；
i是图像的索引；
k是线段的索引；
j是属于同一组平行线的所有直线的索引。
较佳的，所述C包括：
C1、根据当前图像的交叉点描述子，构建所述描述子的词袋特征，并利用当前图像的词袋特征，从关键帧中挑选相似度大于设定阈值的帧作为候选关键帧，然后将当前图像与候选关键帧之间进行几何一致性验证；如果一候选关键帧通过了几何一致性验证，则认为检测到闭环；
C2、当检测到闭环后，对关键帧建立消除线段的重投影误差和消隐点的重投影误差的误差函数，进行全局地图的优化。
本申请还公开了一种同步定位和地图构建SLAM系统，应用于机器人在未知环境中的定位和地图构建，包括：
运动估计模块，用于从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；
局部地图更新模块，用于在地图中插入关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；
闭环优化模块，用于基于交叉点进行闭环检测和全局的地图优化。
本发明提供的基于线段特征和消隐点的同步定位和地图构建方法和系统，可以用于机器人在未知环境中的定位和地图构建，特别适用于结构化或者半结构化的场景中，例如室内环境、室外有建筑的环境等。
附图说明
图1为本发明一较佳实施例中SLAM系统的示意图；
图2为本发明一较佳实施例中线段交叉点提取规则的示意图；
图3为本发明一较佳实施例中交叉点描述子提取方法的示意图；
图4为本发明一较佳实施例中线段提取与匹配流程的示意图；
图5为本发明一较佳实施例中线段三维重建方法的示意图；
图6为本发明一较佳实施例中线段重投影误差的示意图；
图7为本发明一较佳实施例中消隐点重投影误差的示意图；
图8为本发明一较佳实施例中SLAM实验结果的示意图。
具体实施方式
为使本申请的目的、技术方案及优点更加清楚明白，以下参照附图并举实施例，对本申请作进一步详细说明。
本发明提出一种基于线段特征和线段消隐点的同步定位和地图构建方法。本发明提及的同步定位和地图构建(simultaneous localization and mapping)，简称SLAM，是指：机器人从未知环境的未知地点出发，在运动过程中通过不同视角下多次观测到的地图特征(比如，点特征、线段特征等)对自身进行定位，再根据自身位置增量式地构建地图，从而达到同步定位和地图构建的目的。本发明中所述的消隐点(vanishing point)，也称作消失点、灭点，指的是一组平行线在图像平面上的交汇点。
本发明主要用于自主移动机器人在室内环境以及室外有建筑物的环境中进行定位和地图构建，使用的传感器包括视觉传感器，可选地，可以使用辅助定位传感器。在本发明较佳实施例中，采用的视觉传感器为双目摄像头。需要说明的是，本发明同样适用于其他的视觉传感器，如多目摄像头。本发明也可以增加辅助定位传感器，包括码盘和惯性测量单元(inertial measurement unit,IMU)。
本发明提出的同步定位和地图构建方法，应用于机器人在未知环境中的定位和地图构建，主要包括以下步骤：
A、从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；
B、在地图中插入关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；
C、基于交叉点进行闭环检测和全局的地图优化。
本发明方法可以用软件实现，该软件系统的模块划分如图1所示，主要包括三个部分：运动估计模块、局部地图更新模块、闭环优化模块，其中：
运动估计模块，用于从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；
局部地图更新模块，用于在地图中插入关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；
闭环优化模块，用于基于交叉点进行闭环检测和全局的地图优化。
作为一个实施例，本发明将这三个部分各用一个线程来实现。其中：
运动估计部分主要包括：(1)提取图像的线段特征、交叉点和消隐点；(2)根据交叉点建立线段的匹配；(3)进行位姿估计，选取新关键帧。
局部地图更新部分包括：在地图中插入关键帧、计算新出现线段的三维坐标、局部集束调整、剔除野点观测。局部集束调整的目标是最小化一个误差函数，使得相机位姿的估计更加准确。该误差函数包含两类误差：线段的重投影误差(reprojection error)和消隐点的重投影误差。
闭环优化部分包括：基于交叉点的闭环检测、全局相机位姿优化。
接下来分别介绍这三个部分。
一、运动估计部分
1.1提取线段特征、交叉点和消隐点的方法
线段的特征提取可以采用现有的线段特征提取方法，如线段特征提取器(LSD,line segment detector)方法、边缘画线(EDLine,Edge Drawing Line)方法、道格拉斯-普克(Douglas-Peucker)算法等。作为一个实施例，本发明采用道格拉斯-普克算法提取线段特征。
得到线段特征之后，我们提取候选的线段交叉点(junction)。本发明定义线段交叉点如下：
如果两条线段满足以下三个条件，则判定这两条线段可以构成一个候选的线段交叉点(junction)：
条件1：两条线段的长度均大于一设定阈值。
条件2：交叉点到两条线段的距离均小于一设定阈值。
条件3：如果两条线段延长之后交叉，交叉点须位于图像平面内。值得一提的是，两条线段可以不直接相交，只要这两条线段的延长线相交即可。
图2为上述三个条件的图示。其中，图2中的(a)满足以上三个条件，因而构成一个交叉点；图2中的(b)不满足条件1，不构成交叉点；图2中的(c)不满足条件2，不构成交叉点；图2中的(d)不满足条件3，不构成交叉点。
消隐点的提取可以采用已有的算法，本发明并不依赖于某个特定的消隐点提取算法。作为一个实施例，本发明采用基于随机采样一致性(RANdom SAmple Consensus,RANSAC)算法的消隐点提取方法。
1.2根据交叉点建立线段的匹配
这部分包括：提取交叉点的描述子、进行交叉点的初始匹配、消除交叉点的错误匹配、根据交叉点的匹配建立线段匹配。接下来详细介绍这些过程。
线段交叉点(junction)的描述子提取方法如图3所示。图3的中部示出两条共面的线段，这两条共面的线段被两个不同视角的摄像头分别观测到，如图像1和图像2所示，这两条共面的线段形成了交叉点。图中，xi和x′i分别是图像1和图像2中交叉点的图像坐标，θi和
Figure BDA0001496157320000071
是图像1中构成交叉点的两条线段的角度，θ′i和
Figure BDA0001496157320000072
是图像2中构成交叉点的两条线段的角度。
首先，定义交叉点的方向和尺度。交叉点的方向为构成它的两条线段的中分线，即：图像1中交叉点的方向为
Figure BDA0001496157320000073
图像2中交叉点的方向为
Figure BDA0001496157320000074
交叉点的尺度采用多尺度的策略，在交叉点定义多个尺度，本实施例采用3个尺度。例如：图像1中交叉点xi周围的虚线矩形框为一个尺度的举例。
其次，在交叉点周围提取描述子。给定交叉点的位置、方向和尺度，可以将交叉点当作常规的特征点，因此可以采用特征点描述子提取方法在交叉点周围提取描述子。作为一个较佳实施例，本发明采用带有旋转的BRIEF(Binary Robust Independent ElementaryFeatures，二值的鲁棒独立基本特征)描述子。
提取了交叉点的描述子之后，就可以建立描述子之间的初始匹配。本实施例采用汉明距离作为BRIEF描述子之间的距离度量。对于双目图像和非双目图像采用不同的策略进行匹配：
(1)对于双目摄像头获取的双目图像，图像通过校正之后，所有的极线都是平行于图像水平方向的。给定左目图像中的某个交叉点，我们在右目图像的极线附近寻找匹配的交叉点；如果在极线附近存在交叉点，则计算左目图像与右目图像的交叉点之间的描述子距离；如果描述子距离小于设定的阈值，则认为左目图像的交叉点与右目图像的交叉点构成一对匹配。
(2)对于非双目图像，两幅图像之间的相对位姿未知，因此极线也是未知的。给定一幅图像中的交叉点，首先，在另一幅图像中寻找描述子距离最小、以及距离次小的交叉点；其次，计算最小距离和次小距离的比值；最后，如果该比值小于设定的阈值，则认为左目图像的交叉点和右图距离最小的交叉点构成一对匹配。
由于描述子只表达了交叉点周围的局部信息，因此交叉点的匹配不可避免地含有错误匹配(亦称作野点、离群点)，需要进一步消除交叉点的错误匹配。本实施例采用随机采样一致性方法来消除野值点。如果一对匹配的交叉点是三维空间中的两条共面直线投影得到的，那么它们满足对极几何。将对极几何中的五点法结合到随机采样一致性算法中，可以消除错误匹配。优选地，还可以结合非刚性匹配做进一步的筛选，进一步消除错误匹配。本实施例采用向量场一致性算法作为非刚性匹配算法。
当找到两幅图像中的交叉点匹配后，根据构成交叉点的两条线段的逆时针角度介于0和180度之间这一准则，可以方便地建立两条线段的匹配。具体地，假设第一幅图像中线段1到线段2的角度介于0和180度之间，第二幅图像中的线段3到线段4的角度介于0和180度之间，那么线段1和线段3构成一个匹配、线段2和线段4构成一个匹配，从而可以建立两条线段的匹配。线段匹配的整体流程如图4。
1.3进行位姿估计，选取新关键帧
对于每一幅新获取的双目图像，通过上述的线段提取和匹配方法可以得到线段之间的匹配，通过消隐点检测算法可以检测左目和右目图像中的所有消隐点。所有检测到的线段和消隐点将作为观测量，用于局部地图的更新。
根据计算机视觉的理论，只要在两个不同时刻的双目图像中寻找到3条匹配线段，就可以根据三焦张量进行运动估计。将该求解方法和随机采样一致性(RANSAC)框架结合起来，可以鲁棒地进行位姿估计。
在视觉SLAM领域，当摄像机的位移或旋转超过阈值时，通常把当前图像的主要信息保存下来，作为关键帧。本发明也采用了提取关键帧的策略。当SLAM算法初始化成功后，把当前图像作为关键帧。在此之后，如果当前图像与最后时刻的关键帧之间的相对旋转角度大于设定的阈值、或者相对平移的距离大于设定的距离，那么把当前图像选作关键帧。
二、局部地图更新
这部分包括：在地图中插入关键帧、计算新出现线段的三维坐标、局部集束调整、剔除野点观测。
在运动估计部分，会判断当前图像是否需要加入到关键帧。随着关键帧的加入，系统维护一个位姿图。其中，位姿图的节点代表关键帧，位姿图的边代表关键帧之间的相对位姿。当一个新的关键帧插入到位姿图中时，首先计算该关键帧中线段的三维坐标，如图5。图中，双目图像的线段构成了一个匹配。
首先，计算关键帧中新出现的线段的三维坐标，计算过程如下：
(1)根据双目图像之间的极线是水平的这一准则，可以找到两条线段之间的对应端点。如图5所示，左目图像中的端点(u1,v1)对应右目图像中的端点(u′1,v′1)，左目图像中的端点(u2,v2)对应右目图像中的端点(u′2,v′2)。
(2)根据两个对应的端点、相机的内参数和双目相机的基线距离b，计算两个对应端点的三维坐标。此处相机的内参数包括相机的主点(principle point)和焦距。
(3)根据两个对应端点的三维坐标计算线段端点的三维坐标。将端点(u1,v1)对应的三维坐标记做P1，端点(u2,v2)对应的三维坐标记做P2，则端点三维坐标的计算公式为：
Figure BDA0001496157320000091
Figure BDA0001496157320000092
其次，将当前的关键帧以及最新的N个关键帧作为活跃的关键帧，使用集束调整来优化相机姿态以及活跃关键帧中出现的线段特征的三维坐标。
本发明使用基于线段特征的集束调整来优化相机的姿态和线段的三维坐标。优化的过程利用三维线段在图像平面上的两种观测：图像平面上的线段、平行线的消隐点。具体地，定义如下的代价函数：
Figure BDA0001496157320000101
该代价函数同时考虑了线段和消隐点的观测。
其中，ρ(·)是鲁棒的柯西函数；
el是一个四维向量，表示线段的重投影误差，具体的定义在下文阐述；
ev是一个三维向量，表示消隐点的重投影误差，具体的定义在下文阐述；
Figure BDA0001496157320000102
和
Figure BDA0001496157320000103
分别是4×4和3×3的对角阵，分别表示线段和消隐点的信息矩阵(协方差矩阵的逆)；本实施例将这两个信息矩阵设为单位矩阵；
i是图像的索引；
在求和的第一项中，k是线段的索引；
在求和的第二项中，j是属于同一组平行线的所有直线的索引。
接下来定义线段和消隐点的重投影误差。
对于线段特征，重投影误差定义为平面上检测的线段与投影线段的两个端点的距离之和，如图6所示。投影线段是将线段的三维坐标按照相机位姿投影到图像平面得到。图中，d1和d2分别是线段投影的两个端点到检测线段的距离。本实施例采用双目相机，对于左目和右目图像可以分别求取线段的重投影误差，因此线段的重投影误差是三维向量。
对于消隐点特征，我们把消隐点映射到单位球面上。消隐点的重投影误差定义为投影过来的消隐点坐标和实际检测的消隐点坐标之间的距离，如图7所示。图中L1和L2是一组平行线，它们在图像平面I上的投影线段分别为l1和l2，消隐点为v。如果投影线段l2有噪声，变成了l′2，那么消隐点变成了vn。消隐点的重投影误差定义为v和vn在图像平面上的坐标差异。本实施例中，我们可以把v和vn进一步投影到单位球面上，图中v在单位球上的投影点是vs，然后用v和vn在球面上的坐标差异作为重投影误差。
最后，当姿态修正完毕，剔除不好的线段匹配和消隐点。具体地，如果线段的重投影误差超过阈值，则剔除该线段特征。类似地，如果消隐点的观测位置和投影位置超过阈值，则剔除该消隐点。
三、闭环优化
闭环优化部分包括：基于交叉点进行闭环检测，进行全局相机位姿优化。
我们将前文所述的交叉点作为特征点，利用交叉点的描述子进行闭环检测。本发明采用词袋(bag-of-words)模型，用一个离线聚类得到的词汇树(vocabulary tree)模型来对特征点进行组织。闭环检测部分将当前的图像与之前的t个关键帧进行比较。当前图像作为查询图像，关键帧集合中的各关键帧与查询图像进行比较，排除掉相似度小于指定阈值的图像。若构成一个候选的闭环，需要连续3帧得到的闭环候选是相同的。得到这些候选的闭环后，进一步进行几何验证。几何验证采用五点法和随机采样一致性(RANSAC)框架。如果随机采样一致性算法保留的正确匹配数量大于指定的阈值，认为检测到了一个闭环。
当检测到闭环之后，进行全局地图的优化。全局地图优化时，误差函数的定义与局部地图更新中定义的误差函数基本相同，区别在于将闭环检测所涉及关键帧的线段参与到优化中。
本发明的典型实验结果如图8所示。
以上所述仅为本申请的较佳实施例而已，并不用以限制本申请，凡在本申请的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本申请保护的范围之内。

Claims (8)

1.一种同步定位和地图构建SLAM方法，应用于机器人在未知环境中的定位和地图构建，其特征在于，包括：
A、从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；其中，消隐点是指一组平行线在图像平面上的交汇点；
B、在地图中插入关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；
C、基于交叉点进行闭环检测和全局的地图优化；
其中，所述B包括：
在局部地图的集束调整中，建立消除线段的重投影误差和消隐点的重投影误差的误差函数，其中：
线段的重投影误差定义为平面上检测的线段与投影线段的两个端点的距离之和；
消隐点的重投影误差的定义为投影到单位球面上的消隐点坐标和实际检测的消隐点坐标之间的距离；
如果线段的重投影误差超过设定阈值，则剔除该线段，如果消隐点的重投影误差超过阈值，则剔除该消隐点。
2.根据权利要求1所述的方法，其特征在于，所述A中从图像中提取线段特征、交叉点和消隐点包括：
A1、采集当前图像，从当前图像中提取线段特征；
A2、提取线段的交叉点；
A3、提取交叉点的描述子；
A4、根据描述子建立交叉点的初始匹配，利用对极几何和非刚性匹配消除错误的匹配；
A5、在线段特征的基础上提取消隐点。
3.根据权利要求2所述的方法，其特征在于，所述A2包括：
如果两条线段满足以下三个条件，则判定所述两条线段构成一个候选的交叉点：
条件1：两条线段的长度均大于一设定阈值；
条件2：交叉点到两条线段的距离均小于一设定阈值；
条件3：如果两条线段延长之后交叉，交叉点须位于图像平面内。
4.根据权利要求2所述的方法，其特征在于，所述A3包括：
定义交叉点的方向和尺度，其中，将交叉点的方向定义为构成该交叉点的两条线段的中分线，并采用至少两个尺度的策略；
将交叉点视为特征点，在交叉点周围提取描述子。
5.根据权利要求2所述的方法，其特征在于，所述A4中根据描述子建立交叉点的初始匹配包括：
对于双目图像和非双目图像采用不同的策略进行匹配，其中：
对于双目摄像头获取的双目图像，根据所述双目图像中的一幅图像中给定的交叉点，在另一幅图像的极线附近寻找匹配的交叉点；如果在极线附近存在匹配的交叉点，则计算两个交叉点之间的描述子距离；如果描述子距离小于设定的阈值，则判定所述两个交叉点构成一对匹配；
对于非双目图像，根据所述非双目图像中的一幅图像中给定的交叉点，在另一幅图像中寻找描述子距离最小、以及距离次小的交叉点，并计算最小距离和次小距离的比值，如果所述比值小于设定的阈值，则判定所述一幅图像中给定的交叉点与所述另一幅图像中距离最小的交叉点构成一对匹配。
6.根据权利要求1至5任一项所述的方法，其特征在于，所述误差函数为：
Figure FDA0002988209660000021
其中，ρ(·)是鲁棒的柯西函数；
el是一个四维向量，表示线段的重投影误差；
ev是一个三维向量，表示消隐点的重投影误差；
Figure FDA0002988209660000031
和
Figure FDA0002988209660000032
分别是4×4和3×3的对角阵，分别表示线段和消隐点的信息矩阵；
i是图像的索引；
k是线段的索引；
j是属于同一组平行线的所有直线的索引。
7.根据权利要求1所述的方法，其特征在于，所述C包括：
C1、根据当前图像的交叉点描述子，构建所述描述子的词袋特征，并利用当前图像的词袋特征，从关键帧中挑选相似度大于设定阈值的帧作为候选关键帧，然后将当前图像与候选关键帧之间进行几何一致性验证；如果一候选关键帧通过了几何一致性验证，则认为检测到闭环；
C2、当检测到闭环后，对关键帧建立消除线段的重投影误差和消隐点的重投影误差的误差函数，进行全局地图的优化。
8.一种同步定位和地图构建SLAM系统，应用于机器人在未知环境中的定位和地图构建，其特征在于，包括：
运动估计模块，用于从图像中提取线段特征、交叉点和消隐点，根据交叉点在图像之间建立线段的匹配，根据线段匹配的结果进行相机位姿估计，选取新关键帧；其中，消隐点是指一组平行线在图像平面上的交汇点；
局部地图更新模块，用于在地图中插入关键帧，计算新加入的线段特征的三维坐标，并进行局部地图的集束调整，剔除野点观测；所述局部地图更新模块具体用于：在局部地图的集束调整中，建立消除线段的重投影误差和消隐点的重投影误差的误差函数，其中：
线段的重投影误差定义为平面上检测的线段与投影线段的两个端点的距离之和；
消隐点的重投影误差的定义为投影到单位球面上的消隐点坐标和实际检测的消隐点坐标之间的距离；
如果线段的重投影误差超过设定阈值，则剔除该线段，如果消隐点的重投影误差超过阈值，则剔除该消隐点；
闭环优化模块，用于基于交叉点进行闭环检测和全局的地图优化。
CN201711274095.6A 2017-12-06 2017-12-06 一种机器人同步定位和地图构建方法和系统 Active CN108090958B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711274095.6A CN108090958B (zh) 2017-12-06 2017-12-06 一种机器人同步定位和地图构建方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711274095.6A CN108090958B (zh) 2017-12-06 2017-12-06 一种机器人同步定位和地图构建方法和系统

Publications (2)

Publication Number Publication Date
CN108090958A CN108090958A (zh) 2018-05-29
CN108090958B true CN108090958B (zh) 2021-08-27

Family

ID=62173919

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711274095.6A Active CN108090958B (zh) 2017-12-06 2017-12-06 一种机器人同步定位和地图构建方法和系统

Country Status (1)

Country Link
CN (1) CN108090958B (zh)

Families Citing this family (24)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108776976B (zh) * 2018-06-07 2020-11-20 驭势科技（北京）有限公司 一种同时定位与建图的方法、系统及存储介质
CN110631554B (zh) * 2018-06-22 2021-11-30 北京京东乾石科技有限公司 机器人位姿的确定方法、装置、机器人和可读存储介质
WO2020014924A1 (zh) * 2018-07-19 2020-01-23 驭势科技（北京）有限公司 视觉定位地图存储及加载方法、装置、系统及存储介质
CN108709560A (zh) * 2018-08-15 2018-10-26 苏州中研讯科智能科技有限公司 基于直线段特征的载运机器人高精度定位导航方法
CN111060101B (zh) * 2018-10-16 2022-06-28 深圳市优必选科技有限公司 视觉辅助的距离slam方法及装置、机器人
CN109509230B (zh) * 2018-11-13 2020-06-23 武汉大学 一种应用于多镜头组合式全景相机的slam方法
CN109887003B (zh) * 2019-01-23 2021-11-19 亮风台（上海）信息科技有限公司 一种用于进行三维跟踪初始化的方法与设备
JP7220591B2 (ja) * 2019-03-07 2023-02-10 三菱重工業株式会社 自己位置推定装置、自己位置推定方法及びプログラム
CN110070578B (zh) * 2019-03-15 2023-07-18 西安电子科技大学 一种回环检测方法
CN110163915B (zh) * 2019-04-09 2021-07-13 深圳大学 一种多rgb-d传感器的空间三维扫描方法及装置
CN110018688B (zh) * 2019-04-11 2022-03-29 清华大学深圳研究生院 基于视觉的自动引导车定位方法
CN110070615B (zh) * 2019-04-12 2021-04-27 北京理工大学 一种基于多相机协同的全景视觉slam方法
CN110132280B (zh) * 2019-05-20 2021-07-13 广州小鹏自动驾驶科技有限公司 室内场景下的车辆定位方法、车辆定位装置和车辆
CN110276826A (zh) * 2019-05-23 2019-09-24 全球能源互联网研究院有限公司 一种电网作业环境地图的构建方法及系统
CN110472585B (zh) * 2019-08-16 2020-08-04 中南大学 一种基于惯导姿态轨迹信息辅助的vi-slam闭环检测方法
CN110926405B (zh) * 2019-12-04 2022-02-22 中科新松有限公司 一种基于单目视觉灭点检测的arv姿态测量方法
CN111161412B (zh) * 2019-12-06 2024-02-09 苏州艾吉威机器人有限公司 三维激光建图方法及系统
CN111680685B (zh) * 2020-04-14 2023-06-06 上海高仙自动化科技发展有限公司 一种基于图像的定位方法、装置、电子设备及存储介质
CN111928861B (zh) * 2020-08-07 2022-08-09 杭州海康威视数字技术股份有限公司 地图构建方法及装置
CN112596063B (zh) * 2020-11-27 2024-04-02 北京迈格威科技有限公司 点云描述子构建方法及装置，闭环检测方法及装置
CN112734839B (zh) * 2020-12-31 2022-07-08 浙江大学 一种提高鲁棒性的单目视觉slam初始化方法
CN113450411B (zh) * 2021-06-30 2023-02-28 电子科技大学 一种基于方差分量估计理论的实时自身位姿计算方法
CN114399547B (zh) * 2021-12-09 2024-01-02 中国人民解放军国防科技大学 一种基于多帧的单目slam鲁棒初始化方法
CN114993293B (zh) * 2022-07-28 2022-12-06 南京航空航天大学 室内弱纹理环境下移动无人系统同步定位与建图方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104732518A (zh) * 2015-01-19 2015-06-24 北京工业大学 一种基于智能机器人地面特征的ptam改进方法
US9495761B2 (en) * 2013-11-04 2016-11-15 The Regents Of The University Of California Environment mapping with automatic motion model selection
CN106127739A (zh) * 2016-06-16 2016-11-16 华东交通大学 一种结合单目视觉的rgb‑d slam方法
US20170161546A1 (en) * 2015-12-08 2017-06-08 Mitsubishi Electric Research Laboratories, Inc. Method and System for Detecting and Tracking Objects and SLAM with Hierarchical Feature Grouping
CN106897666A (zh) * 2017-01-17 2017-06-27 上海交通大学 一种室内场景识别的闭环检测方法
CN107392964A (zh) * 2017-07-07 2017-11-24 武汉大学 基于室内特征点和结构线结合的室内slam方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9495761B2 (en) * 2013-11-04 2016-11-15 The Regents Of The University Of California Environment mapping with automatic motion model selection
CN104732518A (zh) * 2015-01-19 2015-06-24 北京工业大学 一种基于智能机器人地面特征的ptam改进方法
US20170161546A1 (en) * 2015-12-08 2017-06-08 Mitsubishi Electric Research Laboratories, Inc. Method and System for Detecting and Tracking Objects and SLAM with Hierarchical Feature Grouping
CN106127739A (zh) * 2016-06-16 2016-11-16 华东交通大学 一种结合单目视觉的rgb‑d slam方法
CN106897666A (zh) * 2017-01-17 2017-06-27 上海交通大学 一种室内场景识别的闭环检测方法
CN107392964A (zh) * 2017-07-07 2017-11-24 武汉大学 基于室内特征点和结构线结合的室内slam方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
《基于点线综合特征的双目视觉SLAM方法》;谢晓佳;《中国优秀硕士学位论文全文数据库信息科技辑》;20170815(第8期);摘要，正文第21、37、41-54页 *

Also Published As

Publication number Publication date
CN108090958A (zh) 2018-05-29

Similar Documents

Publication Publication Date Title
CN108090958B (zh) 一种机器人同步定位和地图构建方法和系统
US20230260151A1 (en) Simultaneous Localization and Mapping Method, Device, System and Storage Medium
Feng et al. 2d3d-matchnet: Learning to match keypoints across 2d image and 3d point cloud
Mur-Artal et al. Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras
CN107025668B (zh) 一种基于深度相机的视觉里程计的设计方法
US11270148B2 (en) Visual SLAM method and apparatus based on point and line features
WO2023184968A1 (zh) 一种基于点线面特征的结构化场景视觉slam方法
US9613420B2 (en) Method for locating a camera and for 3D reconstruction in a partially known environment
Geppert et al. Efficient 2d-3d matching for multi-camera visual localization
Cui et al. Efficient large-scale structure from motion by fusing auxiliary imaging information
CN103646391A (zh) 一种针对动态变化场景的实时摄像机跟踪方法
Lim et al. Avoiding degeneracy for monocular visual SLAM with point and line features
CN112484746B (zh) 一种基于地平面的单目视觉辅助激光雷达里程计方法
CN110570474B (zh) 一种深度相机的位姿估计方法及系统
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
US11568598B2 (en) Method and device for determining an environment map by a server using motion and orientation data
AU2019233778A1 (en) Urban environment labelling
CN111145255A (zh) 一种结合深度学习和几何优化的位姿计算方法和系统
Chen et al. EIL‐SLAM: Depth‐enhanced edge‐based infrared‐LiDAR SLAM
CN113658337A (zh) 一种基于车辙线的多模态里程计方法
Liu et al. Stereo visual-inertial SLAM with points and lines
Yin et al. Stabilize an unsupervised feature learning for LiDAR-based place recognition
CN116222543A (zh) 用于机器人环境感知的多传感器融合地图构建方法及系统
CN114140527A (zh) 一种基于语义分割的动态环境双目视觉slam方法
Hao et al. LMVI-SLAM: Robust low-light monocular visual-inertial simultaneous localization and mapping

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