CN112115874B - 一种融合云端的视觉slam系统及方法 - Google Patents

一种融合云端的视觉slam系统及方法 Download PDF

Info

Publication number
CN112115874B
CN112115874B CN202010994717.8A CN202010994717A CN112115874B CN 112115874 B CN112115874 B CN 112115874B CN 202010994717 A CN202010994717 A CN 202010994717A CN 112115874 B CN112115874 B CN 112115874B
Authority
CN
China
Prior art keywords
map
local
cloud
frame
pose
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
CN202010994717.8A
Other languages
English (en)
Other versions
CN112115874A (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.)
Wuhan University WHU
Original Assignee
Wuhan University WHU
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 Wuhan University WHU filed Critical Wuhan University WHU
Priority to CN202010994717.8A priority Critical patent/CN112115874B/zh
Publication of CN112115874A publication Critical patent/CN112115874A/zh
Application granted granted Critical
Publication of CN112115874B publication Critical patent/CN112115874B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/10—Terrestrial scenes
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • 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/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
    • G06V10/443—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components by matching or filtering
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/40—Scenes; Scene-specific elements in video content
    • G06V20/41—Higher-level, semantic clustering, classification or understanding of video scenes, e.g. detection, labelling or Markovian modelling of sport events or news items
    • G06V20/42—Higher-level, semantic clustering, classification or understanding of video scenes, e.g. detection, labelling or Markovian modelling of sport events or news items of sport video content
    • 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

Abstract

本发明公开了一种融合云端的视觉SLAM系统及方法，该SLAM系统包括：本地端和云端；本地端包括追踪模块、局部地图维护模块；云端包括局部建图模块、闭环检测模块。该方法包括：步骤1，完成云端的地图初始化，本地端获取局部地图；步骤2，进入追踪定位与地图创建模式；步骤3，本地端局部地图的更新维护；步骤4，系统重定位、更新维护云端地图并进行闭环检测，开始全局优化消除累计误差，优化位姿和云端地图；步骤5，输出优化后的位姿结果。本发明通过对SLAM系统功能模块进行分离，利用云端对资源占用较大，时间成本较高的模块处理，本地端处理要求实时性的模块并通过BRPC实现云端和本地端的数据传输，缓解了本地端运行SLAM的计算、存储和实时性的压力。

Description

一种融合云端的视觉SLAM系统及方法
技术领域
本发明涉及视觉空间定位技术领域，尤其涉及一种融合云端的视觉SLAM系统及方法。
背景技术
同时定位与成图(SLAM)技术是指移动服务机器人在未知环境进行工作时，需要运用搭载的传感器获取周围环境的信息，对获取的信息进行处理，进行定位和增量式构图，进而指导移动机器人的工作。其中视觉SLAM采用视觉传感器获取外界信息具备诸多优点，成本低，自身重量轻，能够获取丰富的高分辨率图像信息，逐渐在机器人定位和构图领域越发受到重视。
SLAM系统计算密集，构建一致性的地图更需要处理能力强的设备，当前的SLAM的搭载设备往往是嵌入式设备，其计算能力和存储空间特别有限，难以提供实时的地图构建功能，这就给设备的体积、资源负担和成本带来压力。因此，SLAM的实际应用和大面积推广还较为缓慢。
为了缓解上述SLAM的任务需求和设备能力之间的矛盾，并且云机器人的概念被提出来后，就引起很多研究者的兴趣，并慢慢引入到SLAM领域中,云SLAM与传统的SLAM有明显不同的特点，云端具备有良好的计算能力和存储空间，可以把传统的SLAM系统搭载平台无法处理的复杂计算任务部署到云端，缓解本地处理的压力。云SLAM的模式利用云计算、高性能并行计算、大数据等技术，可以打破SLAM运行在成本、存储等方面的钳制。因此，SLAM任务的计算复杂性和云端的特性，云SLAM也给SLAM领域提供了一种解决方案。
传统的SLAM系统计算量复杂、耗时高，即使是经典的ORB-SLAM系统在闭环检测和局部构图方面仍然需要较好的CPU，在嵌入式设备或者低配置的机器人上，其计算能力和存储空间都极其有限。本发明将ORB-SLAM系统中耗时长，计算量繁重的任务部署到云端，采用brpc框架在云端和搭载端进行通信，研究云SLAM方案与单机SLAM方案的优势。降低SLAM对搭载平台的硬件要求，为SLAM系统的实地移动定位构图提供一种低成本的解决方案。
传统SLAM系统架构中比较耗时的环节有：特征提取、地图三维点的创建、闭环检测以及重定位候选帧的选取。但是，特征提取和跟踪里程计紧密相连的，至于地图三维点的创建、闭环检测等对实时性要求不高，可见可以将耗时量大、实时性不高、计算量大的任务从本地机器人中脱离出来。
相对于传统的单机SLAM系统，本发明提出的方法可以有效的解决任务需求和设备能力之间的问题。
发明内容
本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种融合云端的视觉SLAM系统及方法。
本发明解决其技术问题所采用的技术方案是：
本发明提供一种融合云端的视觉SLAM系统，该SLAM系统包括：本地端和云端；本地端包括追踪模块、局部地图维护模块；云端包括局部建图模块、闭环检测模块；本地端和云端之间通过BRPC通讯模块进行数据通讯；其中：
追踪模块，用于完成传感器测量值之间、传感器测量与地图特征之间的匹配关系，用以确定环境中真实存在的物理信息，并用于提取图像特征信息进行位姿估计，完成相机在运动过程中的位置轨迹计算与定位；
局部地图维护模块，用于本地地图信息的维护，获取云端的更新的地图信息对地图中包含局部关键帧集合和局部地图点集合同步更新，完成本地端添加关键帧和地图点的操作来更新地图信息，同时为追踪模块提供追踪的地图信息；
局部建图模块，用于在接受到本地端的关键帧信息后，利用云端关键帧数据进行特征匹配，进而三角化恢复特征点对应三维空间点的信息，并存储关键帧和地图点的信息，同时在地图的基础上进行位姿优化得到更加精确的地图信息和运动信息；
闭环检测模块，处理局部地图中的关键帧，试图检测闭环并纠正闭环，利用在闭环检测出的回环关键帧与当前帧之间的相似变换矫正当前关键帧及相邻的关键帧集合的位姿；另一方面，系统将回环关键帧及其相邻关键帧集合中可视的地图点投影到当前帧中进行地图点数据的融合，并构建基本姿态图，进行位姿图的优化。
进一步地，本发明的本地端为具有单目摄像头的机器人。
一种融合云端的视觉SLAM方法，该方法包括以下步骤：
步骤1，系统初始化，本地端获取图像数据，并判断本地端是否完成初始化；若未完成初始化，则对本地端进行系统初始化，并与云端交互，完成云端的地图初始化，本地端获取局部地图；若已经完成初始化，则操作直接跳转至步骤2；
步骤2，系统完成初始化开始进入追踪定位与地图创建模式，本地端在完成初始化后采用帧间追踪获取初始位姿，帧间追踪成功后开始局部地图的追踪和关键帧的选取；
步骤3，本地端局部地图的更新维护，将本地端选取的关键帧存入本地端关键帧库，并利用BRPC通讯接收云端数据，实现对本地端局部地图的更新；
步骤4，云端接收本地端发送的关键帧，将其存入云端关键帧库，系统重定位、更新维护云端地图并进行闭环检测，在云端判断是否需要重定位，若要重定位，则通过BRPC通讯向本地端发送指令，跳转至步骤2执行局部地图追踪；若不要重定位，则在云端进行局部地图更新，并通过BRPC通讯将更新后的局部地图发送给本地端，云端进行闭环检测；将当前接收的关键帧与云端关键帧库中存储的关键帧进行相似度比较，若相似度满足闭环检测条件，则开始全局优化消除累计误差，优化位姿和云端地图；
步骤5，输出优化后的位姿结果。
进一步地，本发明的步骤1的具体方法为：
步骤1.1，将图像数据输入本地端，图像提取的特征数目大于阈值100时的该图像可作为初始帧F0并同标准对之后输入的图像帧处理得到初始帧F1，将F0与F1输入系统本地端中，开始进行初始化操作，首先对初始帧进行特征提取和匹配得到F1与F0任意一对对应的匹配点对{pc,pr}，依据该匹配点计算得到对应归一化平面的坐标{xc,xr}，之后通过对极几何等原理计算得到初始位姿T0；
步骤1.2，将步骤1.1中的初始帧F0作为初始关键帧KF0与对应的地图点MapPoints0添加进入局部地图中，将关键帧与地图点存储在关键帧数据库CDB；
步骤1.3，将初始关键帧KF0和KF1和地图点进行封装利用BRPC通讯框架上传到云端；云端接收封装的数据之后将关键帧存储在关键帧数据库SDB中并进行数据共视，至此完成系统初始化。
进一步地，本发明的步骤1.1的具体方法如下：
依据归一化平面坐标{xc,xr}，之后分别计算本次运动的单应矩阵H和基础矩阵F，依据匹配点计算得到归一化平面的坐标，对于单应矩阵H建立直接线性变换矩阵：
xc＝Hxr
基于RANSAC使用归一化4点法算法求解出单应矩阵H；对于基础矩阵F，依据对极几何约束建立方程：
Figure BDA0002692156750000042
基于RANSAC，使用归一化8点算法求解基础矩阵；其中RANSAC算法在一定程度上可以剔除外点，同时针对单应矩阵H和基础矩阵F计算矩阵得分：
Figure BDA0002692156750000041
其中SH和SF是采用H和F恢复运动的精度打分，如果阈值RH>0.45，使用H矩阵通过SVD分解计算相机位姿，否则使用F矩阵计算本质矩阵E，然后通过SVD分解本质矩阵E计算位姿；最后对位姿进行检查，求出最优解得到初始位姿T0；
得到初始位姿之后利用初始位姿对该图像的特征信息三角化得到世界坐标系下的地图点集合MapPoints0，依据归一化平面的坐标对{xc,xr}，存在的几何关系为：
Figure BDA0002692156750000051
其中zc,zr为对应的相机坐标系下的Z轴坐标，即深度信息，kc,kr为当前帧和参考帧的相机内参，Pw为对应的3D点坐标，Tcw和Trw表示世界坐标系与相机坐标系下的位姿变换，在初始化阶段F0与F1位姿T可分别设置为单位阵和T0；
对以上公式叉乘对应的归一化平面的点可得：
Figure BDA0002692156750000052
整理得：
Figure BDA0002692156750000053
此时对公式进行SVD求解最终得到最终的3D点坐标Pw，对{pc,pr}的匹配点对完成以上的三角化的操作后最终得到地图点集合MapPoints0，完成初试化操作。
进一步地，本发明的步骤2的具体方法为：
步骤2.1，系统初始化后开始进行追踪，如果系统处理完毕所有图像数据则直接跳转步骤5，否则将输入图像信息记作当前帧Fc开始进行帧间追踪，通过帧间追踪获取当前帧Fc的位姿。
步骤2.1.1首先系统进行匀速模型追踪，利用前一帧的位姿变换作为当前帧Fc的位姿T，并将前一帧中的地图点投影至当前帧完成3D-2D数据关联，建立重投影误差方程进行位姿优化，建立相关关系如下：
Figure BDA0002692156750000054
T表示相机位姿，ui为观测点像素坐标，si为尺度信息，K为内参矩阵，Pi为空间点3维坐标，T*为需要优化的相机位姿；之后进行BA优化最小化重投影误差得到优化后的位姿；
采用ORB-SLAM系统中的3D-2D匹配点对检验方法进行过滤剔除得到最终地图点与特征点的匹配数目，如果匹配数目大于或等于10，则追踪成功直接跳转至步骤2.3，否则进入步骤2.1.2,进行参考帧追踪；
步骤2.1.2，参考帧模型追踪，采用参考帧模型进行追踪，参考帧的选取方式是：在本地端CDB中选取最近的关键帧作为参考帧；通过当前帧和参考关键帧的特征匹配，然后将参考关键帧中特征点对应的地图点关联到当前帧的匹配特征点上，形成3D-2D的数据关联，最后通过BA优化最小化重投影误差求解位姿；通过这种参考帧模式最终地图点与特征点的匹配数目，如果匹配数目大于或等于10，则追踪成功，得到当前帧位姿Tc直接进入步骤2.3；如果参考帧模型失败系统就进入步骤2.2进行重定位；
步骤2.2，系统重定位模式；
步骤2.2.1，首本地端依据当前信息和本地的局部地图实现重定位，如果定位失败则云端发送重定位申请，将当前帧Fc的信息封装利用BRPC通讯发送到云端，此处跳转至步骤4，系统在云端开展重定位，重定位成功系统会从步骤4跳转进入2.2.2；
步骤2.2.2，此步骤作为系统重定位的后续，发送信息至云端后接收云端重定位成功的位姿结果即当前帧位姿Tc，完成重定位操作之后进入局部地图追踪；
步骤2.3，该步骤进行局部地图追踪，利用当前帧位姿Tc和局部地图信息开始局部地图的追踪，对于关键帧和地图信息进行更新；更新完成后，依据关键帧位姿和局部地图的关键帧、地图点信息建立重投影误差方程进行BA位姿优化；
步骤2.4，进行关键帧的选取和判定后创建关键帧并上传至云端进行地图更新。
进一步地，本发明的步骤2.4中选取关键帧的方法具体为：
步骤2.4.1，选取关键帧的三个条件包含：(1)距离上次插入关键帧最少间隔8帧；(2)局部建图线程中关键帧队列的关键帧数量不超过3个；(3)当前帧跟踪到的特征点数目和参考帧总特征点数的比值小于一定的阈值，避免信息冗余；满足以上的条件后创建关键帧，否则跳转至步骤2.1；
步骤2.4.2，创建关键帧并上传至云端，发送云端更新维护地图的请求，此处跳转至步骤4。
进一步地，本发明的步骤3的具体方法为：
步骤3.1，接收到云端更新的关键帧、地图点和关键帧之间的共视关系等信息，对本地端的地图信息进行更新，针对本地端关键帧数据库中添加更新的关键帧、地图点数据同时更新共视关系，使本地端和云端局部地图信息保持一致。
进一步地，本发明的步骤4的具体方法为：
步骤4.1，云端收到重定位的请求后并接收当前帧Fc的信息，遍历关键帧数据库SDB中的关键帧采用词袋模型检测出和当前帧Fc相似性最高的关键帧KFr并检测出Fc的2D特征点信息和KFr对应地图点3D信息的匹配对，2D-3D之间的几何关系如下：
suc＝KTcwPw
其中s为尺度因子，uc为当前帧Fc的特征点坐标，K为相机内参，Pw为特征点匹配的地图点坐标，Tcw为世界坐标系至相机坐标系的位姿变换；当匹配特征点大于30时，结合EPNP和RANSAC算法估计当前帧的位姿Tc，之后使用BA优化算法获得当前帧在局部地图上对应的地图点；如果当前帧的地图点数恢复大于50，则重定位成功；此处将Tc传输至本地端，本地端进入步骤2.2.2，云端任务暂时完成；
步骤4.2，云端接收到本地端发送的当前关键帧KFN和对应地图点信息并存储在关键帧数据库SDB中，对SDB中的关键帧和地图点之间进行数据更新，删除地图外点并依据当前关键帧KFN和地图中邻近关键帧KFL之间依据几何关系创建更多的地图点；完成数据更新后，采用局部优化来优化地图信息；之后将更新的关键帧和地图信息封装传输至本地端，本地端进入步骤3，云端进行闭环检测；
步骤4.3，新的关键帧的插入后云端开始闭环检测，对于新的关键帧，使用BoW计算关键帧数据库SDB中所有关键帧的共视关系，并将具有共视关系但没有直接相连的关键帧作为候选关键帧，对候选关键帧和当前关键帧进行特征匹配，并结合ICP和RANSAC算法来求解和优化位姿得到闭环关键帧。之后进行全局优化，对系统信息可建立如下如下目标函数：
eij＝zij-h(Ti,pj)
Figure BDA0002692156750000081
其中zij表示在位姿Ti处观测到pj产生的图像坐标数据，eij表示投影误差，Fij为目标函数关于位姿的偏导数，Eij是目标函数对于地图点的偏导，求解该目标函数时，使用高斯-牛顿法Gauss-Newton或者列文伯格-马夸尔特Levenberg-Marquadt方法利用图优化的方式进行优化，消除累计误差，优化位姿和地图信息；此时将优化结果传输至本地端，本地端进入步骤3。
本发明产生的有益效果是：本发明的融合云端的视觉SLAM系统及方法，通过对SLAM系统功能模块进行分离，利用云端对资源占用较大，时间成本较高的模块处理，本地端处理要求实时性的模块并通过BRPC实现云端和本地端的数据传输，缓解了本地端运行SLAM的计算、存储和实时性的压力。
附图说明
下面将结合附图及实施例对本发明作进一步说明，附图中：
图1是本发明实施例的流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
本发明实施例的融合云端的视觉SLAM系统，该SLAM系统包括：本地端和云端；本地端包括追踪模块、局部地图维护模块；云端包括局部建图模块、闭环检测模块；本地端和云端之间通过BRPC通讯模块进行数据通讯；本地端为具有单目摄像头的机器人；其中：
追踪模块，用于完成传感器测量值之间、传感器测量与地图特征之间的匹配关系，用以确定环境中真实存在的物理信息，并用于提取图像特征信息进行位姿估计，完成相机在运动过程中的位置轨迹计算与定位；
局部地图维护模块，用于本地地图信息的维护，获取云端的更新的地图信息对地图中包含局部关键帧集合和局部地图点集合同步更新，完成本地端添加关键帧和地图点的操作来更新地图信息，同时为追踪模块提供追踪的地图信息；
局部建图模块，用于在接受到本地端的关键帧信息后，利用云端关键帧数据进行特征匹配，进而三角化恢复特征点对应三维空间点的信息，并存储关键帧和地图点的信息，同时在地图的基础上进行位姿优化得到更加精确的地图信息和运动信息；
闭环检测模块，处理局部地图中的关键帧，试图检测闭环并纠正闭环，利用在闭环检测出的回环关键帧与当前帧之间的相似变换矫正当前关键帧及相邻的关键帧集合的位姿；另一方面，系统将回环关键帧及其相邻关键帧集合中可视的地图点投影到当前帧中进行地图点数据的融合，并构建基本姿态图，进行位姿图的优化。
本发明实施例的融合云端的视觉SLAM方法，该方法包括以下步骤：
步骤1，系统初始化，本地端获取图像数据，并判断本地端是否完成初始化；若未完成初始化，则对本地端进行系统初始化，并与云端交互，完成云端的地图初始化，本地端获取局部地图；若已经完成初始化，则操作直接跳转至步骤2；
步骤1的具体方法为：
步骤1.1，将图像数据输入本地端，图像提取的特征数目大于阈值100时的该图像可作为初始帧F0并同标准对之后输入的图像帧处理得到初始帧F1，将F0与F1输入系统本地端中，开始进行初始化操作，首先对初始帧进行特征提取和匹配得到F1与F0任意一对对应的匹配点对{pc,pr}，依据该匹配点计算得到对应归一化平面的坐标{xc,xr}，之后通过对极几何等原理计算得到初始位姿T0；
步骤1.1的具体方法如下：
依据归一化平面坐标{xc,xr}，之后分别计算本次运动的单应矩阵H和基础矩阵F，依据匹配点计算得到归一化平面的坐标，对于单应矩阵H建立直接线性变换矩阵：
xc＝Hxr
基于RANSAC使用归一化4点法算法求解出单应矩阵H；对于基础矩阵F，依据对极几何约束建立方程：
Figure BDA0002692156750000101
基于RANSAC，使用归一化8点算法求解基础矩阵；其中RANSAC算法在一定程度上可以剔除外点，同时针对单应矩阵H和基础矩阵F计算矩阵得分：
Figure BDA0002692156750000102
其中SH和SF是采用H和F恢复运动的精度打分，如果阈值RH>0.45，使用H矩阵通过SVD分解计算相机位姿，否则使用F矩阵计算本质矩阵E，然后通过SVD分解本质矩阵E计算位姿；最后对位姿进行检查，求出最优解得到初始位姿T0；
得到初始位姿之后利用初始位姿对该图像的特征信息三角化得到世界坐标系下的地图点集合MapPoints0，依据归一化平面的坐标对{xc,xr}，存在的几何关系为：
Figure BDA0002692156750000103
其中zc,zr为对应的相机坐标系下的Z轴坐标，即深度信息，kc,kr为当前帧和参考帧的相机内参，Pw为对应的3D点坐标，Tcw和Trw表示世界坐标系与相机坐标系下的位姿变换，在初始化阶段F0与F1位姿T可分别设置为单位阵和T0；
对以上公式叉乘对应的归一化平面的点可得：
Figure BDA0002692156750000111
整理得：
Figure BDA0002692156750000112
此时对公式进行SVD求解最终得到最终的3D点坐标Pw，对{pc,pr}的匹配点对完成以上的三角化的操作后最终得到地图点集合MapPoints0，完成初试化操作。
步骤1.2，将步骤1.1中的初始帧F0作为初始关键帧KF0与对应的地图点MapPoints0添加进入局部地图中，将关键帧与地图点存储在关键帧数据库CDB；
步骤1.3，将初始关键帧KF0和KF1和地图点进行封装利用BRPC通讯框架上传到云端；云端接收封装的数据之后将关键帧存储在关键帧数据库SDB中并进行数据共视，至此完成系统初始化。
步骤2，系统完成初始化开始进入追踪定位与地图创建模式，本地端在完成初始化后采用帧间追踪获取初始位姿，帧间追踪成功后开始局部地图的追踪和关键帧的选取；
步骤2的具体方法为：
步骤2.1，系统初始化后开始进行追踪，如果系统处理完毕所有图像数据则直接跳转步骤5，否则将输入图像信息记作当前帧Fc开始进行帧间追踪，通过帧间追踪获取当前帧Fc的位姿。
步骤2.1.1首先系统进行匀速模型追踪，利用前一帧的位姿变换作为当前帧Fc的位姿T，并将前一帧中的地图点投影至当前帧完成3D-2D数据关联，建立重投影误差方程进行位姿优化，建立相关关系如下：
Figure BDA0002692156750000113
T表示相机位姿，ui为观测点像素坐标，si为尺度信息，K为内参矩阵，Pi为空间点3维坐标，T*为需要优化的相机位姿；之后进行BA优化最小化重投影误差得到优化后的位姿；
采用ORB-SLAM系统中的3D-2D匹配点对检验方法进行过滤剔除得到最终地图点与特征点的匹配数目，如果匹配数目大于或等于10，则追踪成功直接跳转至步骤2.3，否则进入步骤2.1.2,进行参考帧追踪；
步骤2.1.2，参考帧模型追踪，采用参考帧模型进行追踪，参考帧的选取方式是：在本地端CDB中选取最近的关键帧作为参考帧；通过当前帧和参考关键帧的特征匹配，然后将参考关键帧中特征点对应的地图点关联到当前帧的匹配特征点上，形成3D-2D的数据关联，最后通过BA优化最小化重投影误差求解位姿；通过这种参考帧模式最终地图点与特征点的匹配数目，如果匹配数目大于或等于10，则追踪成功，得到当前帧位姿Tc直接进入步骤2.3；如果参考帧模型失败系统就进入步骤2.2进行重定位；
步骤2.2，系统重定位模式；
步骤2.2.1，首本地端依据当前信息和本地的局部地图实现重定位，如果定位失败则云端发送重定位申请，将当前帧Fc的信息封装利用BRPC通讯发送到云端，此处跳转至步骤4，系统在云端开展重定位，重定位成功系统会从步骤4跳转进入2.2.2；
步骤2.2.2，此步骤作为系统重定位的后续，发送信息至云端后接收云端重定位成功的位姿结果即当前帧位姿Tc，完成重定位操作之后进入局部地图追踪；
步骤2.3，该步骤进行局部地图追踪，利用当前帧位姿Tc和局部地图信息开始局部地图的追踪，对于关键帧和地图信息进行更新；更新完成后，依据关键帧位姿和局部地图的关键帧、地图点信息建立重投影误差方程进行BA位姿优化；
步骤2.4，进行关键帧的选取和判定后创建关键帧并上传至云端进行地图更新。
步骤2.4中选取关键帧的方法具体为：
步骤2.4.1，选取关键帧的三个条件包含：(1)距离上次插入关键帧最少间隔8帧；(2)局部建图线程中关键帧队列的关键帧数量不超过3个；(3)当前帧跟踪到的特征点数目和参考帧总特征点数的比值小于一定的阈值，避免信息冗余；满足以上的条件后创建关键帧，否则跳转至步骤2.1；
步骤2.4.2，创建关键帧并上传至云端，发送云端更新维护地图的请求，此处跳转至步骤4。
步骤3，本地端局部地图的更新维护，将本地端选取的关键帧存入本地端关键帧库，并利用BRPC通讯接收云端数据，实现对本地端局部地图的更新；
步骤3的具体方法为：
步骤3.1，接收到云端更新的关键帧、地图点和关键帧之间的共视关系等信息，对本地端的地图信息进行更新，针对本地端关键帧数据库中添加更新的关键帧、地图点数据同时更新共视关系，使本地端和云端局部地图信息保持一致。
步骤4，云端接收本地端发送的关键帧，将其存入云端关键帧库，系统重定位、更新维护云端地图并进行闭环检测，在云端判断是否需要重定位，若要重定位，则通过BRPC通讯向本地端发送指令，跳转至步骤2执行局部地图追踪；若不要重定位，则在云端进行局部地图更新，并通过BRPC通讯将更新后的局部地图发送给本地端，云端进行闭环检测；将当前接收的关键帧与云端关键帧库中存储的关键帧进行相似度比较，若相似度满足闭环检测条件，则开始全局优化消除累计误差，优化位姿和云端地图；
步骤4的具体方法为：
步骤4.1，云端收到重定位的请求后并接收当前帧Fc的信息，遍历关键帧数据库SDB中的关键帧采用词袋模型检测出和当前帧Fc相似性最高的关键帧KFr并检测出Fc的2D特征点信息和KFr对应地图点3D信息的匹配对，2D-3D之间的几何关系如下：
suc＝KTcwPw
其中s为尺度因子，uc为当前帧Fc的特征点坐标，K为相机内参，Pw为特征点匹配的地图点坐标，Tcw为世界坐标系至相机坐标系的位姿变换；当匹配特征点大于30时，结合EPNP和RANSAC算法估计当前帧的位姿Tc，之后使用BA优化算法获得当前帧在局部地图上对应的地图点；如果当前帧的地图点数恢复大于50，则重定位成功；此处将Tc传输至本地端，本地端进入步骤2.2.2，云端任务暂时完成；
步骤4.2，云端接收到本地端发送的当前关键帧KFN和对应地图点信息并存储在关键帧数据库SDB中，对SDB中的关键帧和地图点之间进行数据更新，删除地图外点并依据当前关键帧KFN和地图中邻近关键帧KFL之间依据几何关系创建更多的地图点；完成数据更新后，采用局部优化来优化地图信息；之后将更新的关键帧和地图信息封装传输至本地端，本地端进入步骤3，云端进行闭环检测；
步骤4.3，新的关键帧的插入后云端开始闭环检测，对于新的关键帧，使用BoW计算关键帧数据库SDB中所有关键帧的共视关系，并将具有共视关系但没有直接相连的关键帧作为候选关键帧，对候选关键帧和当前关键帧进行特征匹配，并结合ICP和RANSAC算法来求解和优化位姿得到闭环关键帧。之后进行全局优化，对系统信息可建立如下目标函数：
eij＝zij-h(Ti,pj)
Figure BDA0002692156750000141
其中zij表示在位姿Ti处观测到pj产生的图像坐标数据，eij表示投影误差，Fij为目标函数关于位姿的偏导数，Eij是目标函数对于地图点的偏导，求解该目标函数时，使用高斯-牛顿法Gauss-Newton或者列文伯格-马夸尔特Levenberg-Marquadt方法利用图优化的方式进行优化，消除累计误差，优化位姿和地图信息；此时将优化结果传输至本地端，本地端进入步骤3。
步骤5，输出优化后的位姿结果。
应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (6)

1.一种融合云端的视觉SLAM方法，其特征在于，通过融合云端的视觉SLAM系统实现，该SLAM系统包括：本地端和云端；本地端包括追踪模块、局部地图维护模块；云端包括局部建图模块、闭环检测模块；本地端和云端之间通过BRPC通讯模块进行数据通讯；其中：
追踪模块，用于完成传感器测量值之间、传感器测量与地图特征之间的匹配关系，用以确定环境中真实存在的物理信息，并用于提取图像特征信息进行位姿估计，完成相机在运动过程中的位置轨迹计算与定位；
局部地图维护模块，用于本地地图信息的维护，获取云端的更新的地图信息对地图中包含局部关键帧集合和局部地图点集合同步更新，完成本地端添加关键帧和地图点的操作来更新地图信息，同时为追踪模块提供追踪的地图信息；
局部建图模块，用于在接收到本地端的关键帧信息后，利用云端关键帧数据进行特征匹配，进而三角化恢复特征点对应三维空间点的信息，并存储关键帧和地图点的信息，同时在地图的基础上进行位姿优化得到更加精确的地图信息和运动信息；
闭环检测模块，处理局部地图中的关键帧，试图检测闭环并纠正闭环，利用在闭环检测出的回环关键帧与当前帧之间的相似变换矫正当前关键帧及相邻的关键帧集合的位姿；另一方面，系统将回环关键帧及其相邻关键帧集合中可视的地图点投影到当前帧中进行地图点数据的融合，并构建基本姿态图，进行位姿图的优化；
该方法包括以下步骤：
步骤1，系统初始化，本地端获取图像数据，并判断本地端是否完成初始化；若未完成初始化，则对本地端进行系统初始化，并与云端交互，完成云端的地图初始化，本地端获取局部地图；若已经完成初始化，则操作直接跳转至步骤2；
步骤2，系统完成初始化开始进入追踪定位与地图创建模式，本地端在完成初始化后采用帧间追踪获取初始位姿，帧间追踪成功后开始局部地图的追踪和关键帧的选取；
步骤3，本地端局部地图的更新维护，将本地端选取的关键帧存入本地端关键帧库，并利用BRPC通讯接收云端数据，实现对本地端局部地图的更新；
步骤4，云端接收本地端发送的关键帧，将其存入云端关键帧库，系统重定位、更新维护云端地图并进行闭环检测，在云端判断是否需要重定位，若要重定位，则通过BRPC通讯向本地端发送指令，跳转至步骤2执行局部地图追踪；若不要重定位，则在云端进行局部地图更新，并通过BRPC通讯将更新后的局部地图发送给本地端，云端进行闭环检测；将当前接收的关键帧与云端关键帧库中存储的关键帧进行相似度比较，若相似度满足闭环检测条件，则开始全局优化消除累计误差，优化位姿和云端地图；
步骤5，输出优化后的位姿结果；
步骤1的具体方法为：
步骤1.1，将图像数据输入本地端，图像提取的特征数目大于阈值100时的该图像可作为初始帧F0并同标准对之后输入的图像帧处理得到初始帧F1，将F0与F1输入系统本地端中，开始进行初始化操作，首先对初始帧进行特征提取和匹配得到F1与F0任意一对对应的匹配点对{pc，pr}，依据该匹配点计算得到对应归一化平面的坐标{xc，xr}，之后通过对极几何原理计算得到初始位姿T0；
步骤1.2，将步骤1.1中的初始帧F0作为初始关键帧KF0与对应的地图点MapPoints0添加进入局部地图中，将关键帧与地图点存储在关键帧数据库CDB；
步骤1.3，将初始关键帧KF0和KF1和地图点进行封装利用BRPC通讯框架上传到云端；云端接收封装的数据之后将关键帧存储在关键帧数据库SDB中并进行数据共视，至此完成系统初始化；
步骤1.1的具体方法如下：
依据归一化平面坐标{xc，xr}，之后分别计算本次运动的单应矩阵H和基础矩阵F，依据匹配点计算得到归一化平面的坐标，对于单应矩阵H建立直接线性变换矩阵：
xc＝Hxr
基于RANSAC使用归一化4点法算法求解出单应矩阵H；对于基础矩阵F，依据对极几何约束建立方程：
Figure FDA0003631247250000031
基于RANSAC，使用归一化8点算法求解基础矩阵；其中RANSAC算法在一定程度上可以剔除外点，同时针对单应矩阵H和基础矩阵F计算矩阵得分：
Figure FDA0003631247250000032
其中SH和SF是采用H和F恢复运动的精度打分，如果阈值RH＞0.45，使用H矩阵通过SVD分解计算相机位姿，否则使用F矩阵计算本质矩阵E，然后通过SVD分解本质矩阵E计算位姿；最后对位姿进行检查，求出最优解得到初始位姿T0；
得到初始位姿之后利用初始位姿对该图像的特征信息三角化得到世界坐标系下的地图点集合MapPoints0，依据归一化平面的坐标对{xc，xr}，存在的几何关系为：
Figure FDA0003631247250000033
其中zc，zr为对应的相机坐标系下的Z轴坐标，即深度信息，kc，kr为当前帧和参考帧的相机内参，Pw为对应的3D点坐标，Tcw和Trw表示世界坐标系与相机坐标系下的位姿变换，在初始化阶段F0与F1位姿T分别设置为单位阵和T0；
对以上公式叉乘对应的归一化平面的点可得：
Figure FDA0003631247250000034
整理得：
Figure FDA0003631247250000035
此时对公式进行SVD求解最终得到最终的3D点坐标Pw，对{pc，pr}的匹配点对完成以上的三角化的操作后最终得到地图点集合MapPoints0，完成初试化操作。
2.根据权利要求1所述的融合云端的视觉SLAM方法，其特征在于，本地端为具有单目摄像头的机器人。
3.根据权利要求1所述的融合云端的视觉SLAM方法，其特征在于，步骤2的具体方法为：
步骤2.1，系统初始化后开始进行追踪，如果系统处理完毕所有图像数据则直接跳转步骤5，否则将输入图像信息记作当前帧Fc开始进行帧间追踪，通过帧间追踪获取当前帧Fc的位姿；
步骤2.1.1首先系统进行匀速模型追踪，利用前一帧的位姿变换作为当前帧Fc的位姿T，并将前一帧中的地图点投影至当前帧完成3D-2D数据关联，建立重投影误差方程进行位姿优化，建立相关关系如下：
Figure FDA0003631247250000041
T表示相机位姿，ui为观测点像素坐标，si为尺度信息，K为内参矩阵，Pi为空间点3维坐标，T*为需要优化的相机位姿；之后进行BA优化最小化重投影误差得到优化后的位姿；
采用ORB-SLAM系统中的3D-2D匹配点对检验方法进行过滤剔除得到最终地图点与特征点的匹配数目，如果匹配数目大于或等于10，则追踪成功直接跳转至步骤2.3，否则进入步骤2.1.2，进行参考帧追踪；
步骤2.1.2，参考帧模型追踪，采用参考帧模型进行追踪，参考帧的选取方式是：在本地端CDB中选取最近的关键帧作为参考帧；通过当前帧和参考关键帧的特征匹配，然后将参考关键帧中特征点对应的地图点关联到当前帧的匹配特征点上，形成3D-2D的数据关联，最后通过BA优化最小化重投影误差求解位姿；通过这种参考帧模式最终地图点与特征点的匹配数目，如果匹配数目大于或等于10，则追踪成功，得到当前帧位姿Tc直接进入步骤2.3；如果参考帧模型失败系统就进入步骤2.2进行重定位；
步骤2.2，系统重定位模式；
步骤2.2.1，本地端依据当前信息和本地的局部地图实现重定位，如果定位失败则云端发送重定位申请，将当前帧Fc的信息封装利用BRPC通讯发送到云端，此处跳转至步骤4，系统在云端开展重定位，重定位成功系统会从步骤4跳转进入2.2.2；
步骤2.2.2，此步骤作为系统重定位的后续，发送信息至云端后接收云端重定位成功的位姿结果即当前帧位姿Tc，完成重定位操作之后进入局部地图追踪；
步骤2.3，该步骤进行局部地图追踪，利用当前帧位姿Tc和局部地图信息开始局部地图的追踪，对于关键帧和地图信息进行更新；更新完成后，依据关键帧位姿和局部地图的关键帧、地图点信息建立重投影误差方程进行BA位姿优化；
步骤2.4，进行关键帧的选取和判定后创建关键帧并上传至云端进行地图更新。
4.根据权利要求3所述的融合云端的视觉SLAM方法，其特征在于，步骤2.4中选取关键帧的方法具体为：
步骤2.4.1，选取关键帧的三个条件包含：(1)距离上次插入关键帧最少间隔8帧；(2)局部建图线程中关键帧队列的关键帧数量不超过3个；(3)当前帧跟踪到的特征点数目和参考帧总特征点数的比值小于一定的阈值，避免信息冗余；满足以上的条件后创建关键帧，否则跳转至步骤2.1；
步骤2.4.2，创建关键帧并上传至云端，发送云端更新维护地图的请求，此处跳转至步骤4。
5.根据权利要求1所述的融合云端的视觉SLAM方法，其特征在于，步骤3的具体方法为：
步骤3.1，接收到云端更新的关键帧、地图点和关键帧之间的共视关系信息，对本地端的地图信息进行更新，针对本地端关键帧数据库中添加更新的关键帧、地图点数据同时更新共视关系，使本地端和云端局部地图信息保持一致。
6.根据权利要求3所述的融合云端的视觉SLAM方法，其特征在于，步骤4的具体方法为：
步骤4.1，云端收到重定位的请求后并接收当前帧Fc的信息，遍历关键帧数据库SDB中的关键帧采用词袋模型检测出和当前帧Fc相似性最高的关键帧KFr并检测出Fc的2D特征点信息和KFr对应地图点3D信息的匹配对，2D-3D之间的几何关系如下：
suc＝KTcwPw
其中s为尺度因子，uc为当前帧Fc的特征点坐标，K为相机内参，Pw为特征点匹配的地图点坐标，Tcw为世界坐标系至相机坐标系的位姿变换；当匹配特征点大于30时，结合EPNP和RANSAC算法估计当前帧的位姿Tc，之后使用BA优化算法获得当前帧在局部地图上对应的地图点；如果当前帧的地图点数恢复大于50，则重定位成功；此处将Tc传输至本地端，本地端进入步骤2.2.2，云端任务暂时完成；
步骤4.2，云端接收到本地端发送的当前关键帧KFN和对应地图点信息并存储在关键帧数据库SDB中，对SDB中的关键帧和地图点之间进行数据更新，删除地图外点并依据当前关键帧KFN和地图中邻近关键帧KFL之间依据几何关系创建更多的地图点；完成数据更新后，采用局部优化来优化地图信息；之后将更新的关键帧和地图信息封装传输至本地端，本地端进入步骤3，云端进行闭环检测；
步骤4.3，新的关键帧的插入后云端开始闭环检测，对于新的关键帧，使用BoW计算关键帧数据库SDB中所有关键帧的共视关系，并将具有共视关系但没有直接相连的关键帧作为候选关键帧，对候选关键帧和当前关键帧进行特征匹配，并结合ICP和RANSAC算法来求解和优化位姿得到闭环关键帧；之后进行全局优化，对系统信息可建立如下目标函数：
eij＝zij-h(Ti，pj)
Figure FDA0003631247250000061
其中zij表示在位姿Ti处观测到pj产生的图像坐标数据，eij表示投影误差，Fij为目标函数关于位姿的偏导数，Eij是目标函数对于地图点的偏导数，求解该目标函数时，使用高斯-牛顿法Gauss-Newton或者列文伯格-马夸尔特Levenberg-Marquadt方法利用图优化的方式进行优化，消除累计误差，优化位姿和地图信息；此时将优化结果传输至本地端，本地端进入步骤3。
CN202010994717.8A 2020-09-21 2020-09-21 一种融合云端的视觉slam系统及方法 Active CN112115874B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010994717.8A CN112115874B (zh) 2020-09-21 2020-09-21 一种融合云端的视觉slam系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010994717.8A CN112115874B (zh) 2020-09-21 2020-09-21 一种融合云端的视觉slam系统及方法

Publications (2)

Publication Number Publication Date
CN112115874A CN112115874A (zh) 2020-12-22
CN112115874B true CN112115874B (zh) 2022-07-15

Family

ID=73801037

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010994717.8A Active CN112115874B (zh) 2020-09-21 2020-09-21 一种融合云端的视觉slam系统及方法

Country Status (1)

Country Link
CN (1) CN112115874B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112859130B (zh) * 2021-01-14 2022-08-16 山东瑞联博地理信息有限公司 一种野外导航巡查的高精度电子地图位置匹配方法
CN113835099A (zh) * 2021-02-01 2021-12-24 贵州京邦达供应链科技有限公司 点云地图更新方法及装置、存储介质、电子设备
CN113295159B (zh) * 2021-05-14 2023-03-03 浙江商汤科技开发有限公司 端云融合的定位方法、装置和计算机可读存储介质
CN113701760B (zh) * 2021-09-01 2024-02-27 火种源码(中山)科技有限公司 基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置
CN113984068A (zh) * 2021-11-16 2022-01-28 浙江商汤科技开发有限公司 定位方法、定位装置和计算机可读存储介质
CN114332360A (zh) * 2021-12-10 2022-04-12 深圳先进技术研究院 一种协同三维建图方法及系统
CN116958267B (zh) * 2023-09-21 2024-01-12 腾讯科技（深圳）有限公司 位姿处理方法、装置、电子设备及存储介质

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111462207A (zh) * 2020-03-30 2020-07-28 重庆邮电大学 一种融合直接法与特征法的rgb-d同时定位与地图创建方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180005015A1 (en) * 2016-07-01 2018-01-04 Vangogh Imaging, Inc. Sparse simultaneous localization and matching with unified tracking
CN109544636B (zh) * 2018-10-10 2022-03-15 广州大学 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法
CN109559277B (zh) * 2018-11-28 2023-02-28 中国人民解放军国防科技大学 一种面向数据共享的多无人机协同地图构建方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111462207A (zh) * 2020-03-30 2020-07-28 重庆邮电大学 一种融合直接法与特征法的rgb-d同时定位与地图创建方法

Also Published As

Publication number Publication date
CN112115874A (zh) 2020-12-22

Similar Documents

Publication Publication Date Title
CN112115874B (zh) 一种融合云端的视觉slam系统及方法
CN110125928B (zh) 一种基于前后帧进行特征匹配的双目惯导slam系统
Dubé et al. An online multi-robot SLAM system for 3D LiDARs
US10268201B2 (en) Vehicle automated parking system and method
CN108051002B (zh) 基于惯性测量辅助视觉的运输车空间定位方法及系统
CN111445526B (zh) 一种图像帧之间位姿的估计方法、估计装置和存储介质
CN109523589B (zh) 一种更鲁棒的视觉里程计的设计方法
CN109461208B (zh) 三维地图处理方法、装置、介质和计算设备
Lim et al. Online environment mapping
JP2020067439A (ja) 移動体位置推定システムおよび移動体位置推定方法
CN107193279A (zh) 基于单目视觉和imu信息的机器人定位与地图构建系统
CN112634451A (zh) 一种融合多传感器的室外大场景三维建图方法
CN110570453B (zh) 一种基于双目视觉的闭环式跟踪特征的视觉里程计方法
CN110717927A (zh) 基于深度学习和视惯融合的室内机器人运动估计方法
CN110553648A (zh) 一种用于室内导航的方法和系统
Yin et al. Dynam-SLAM: An accurate, robust stereo visual-inertial SLAM method in dynamic environments
US10991105B2 (en) Image processing device
CN112734841A (zh) 一种用轮式里程计-imu和单目相机实现定位的方法
CN110751123B (zh) 一种单目视觉惯性里程计系统及方法
CN116468786B (zh) 一种面向动态环境的基于点线联合的语义slam方法
CN114708293A (zh) 基于深度学习点线特征和imu紧耦合的机器人运动估计方法
CN114494150A (zh) 一种基于半直接法的单目视觉里程计的设计方法
Ge et al. Vipose: Real-time visual-inertial 6d object pose tracking
CN113570716A (zh) 云端三维地图构建方法、系统及设备
Zhu et al. PairCon-SLAM: Distributed, online, and real-time RGBD-SLAM in large scenarios

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