一种基于轮式编码器的单目SLAM初始化方法及系统技术方案

本发明专利技术公开了一种基于轮式编码器的单目SLAM初始化方法及系统，在SLAM初始化图像序列中选取第一关键帧与第二关键帧，进行特征匹配，计算所有匹配点在第一关键帧的世界坐标系坐标，建立初始点云地图；并利用轮式编码器的位移信息确定第二关键帧的相机位姿，在优化时固定第一关键帧与第二关键帧的相机位姿，不对这两帧相机位姿进行局部捆集调整。本发明专利技术利用轮式编码器的位移信息与视觉传感器图像信息的点特征相结合，起到信息互补的作用，能够十分准确的确定第二关键帧的相机位姿，保留了真实世界的尺度信息，提高相机的定位精度，并且简化了运算，提高了响应速度。

An Initialization Method and System of Monocular SLAM Based on Wheel Encoder

The invention discloses a single SLAM initialization method and system based on wheel encoder. In the SLAM initialization image sequence, the first key frame and the second key frame are selected for feature matching, the coordinates of all matching points in the world coordinate system of the first key frame are calculated, the initial point cloud map is established, and the camera pose of the second key frame is determined by the displacement information of the wheel encoder. When optimizing, the camera pose of the first key frame and the second key frame is fixed, and the camera pose of the two frames is not adjusted by local bundling. The method combines the displacement information of the wheel encoder with the point feature of the image information of the visual sensor, plays the role of complementary information, can determine the camera position and pose of the second key frame very accurately, retains the scale information of the real world, improves the positioning accuracy of the camera, simplifies the operation and improves the response speed.

【技术实现步骤摘要】
一种基于轮式编码器的单目SLAM初始化方法及系统
本专利技术涉及同步定位与地图构建的初始化，尤其是一种基于轮式编码器的单目SLAM初始化方法及系统。
技术介绍
同时定位与地图构建(SimultaneousLocalizationandMapping，SLAM)其目标是在一个未知的环境中实时重建环境的三维结构并同时对机器人自身进行定位。视觉SLAM技术根据拍摄的视频信息推断出视觉传感器在未知环境中的方位，并同时构建环境地图，其基本原理为多视图几何原理。视觉SLAM的目标为同时恢复出每帧图像对应的相机运动参数C1…Cm，以及场景三维结构X1…Xn；其中每个相机运动参数Ci包含了相机的位置和姿态信息，通常表示为一个3×3的旋转矩阵Ri和一个三维位置向量Pi。Mur-Artal等人提出了ORB-SLAM，该方法公开了一种基于图像的单目SLAM初始化方法，提供了用以采集周围环境图像的摄像设备；利用图像的特征点来实现初始化过程；该方法包括如下内容：特征提取匹配：对输入的连续两帧图像分别提取ORB特征点，并进行匹配；场景模型的选择：考虑平面和非平面两种情况，根据计算的比例分值选择模型，平面对应单应矩阵，非平面对应基础矩阵，利用8点法求得矩阵；计算相机位姿：上一步能够得到一个矩阵(单应矩阵或基础矩阵)，分解该矩阵能够得到几组位姿解；恢复特征点深度：对上一步中的每一组相机位姿，利用三角化恢复出每个特征点的深度值；选择最好的位姿解：根据上一步每组求出的三维点云，选择结果最好的那一组位姿作为初始化第二帧的相机位姿。该方法可以有效地计算出第二帧的相机位姿和一系列的初始地图点，但同时也存在以下不足：1、计算本质矩阵会产生尺度不确定的问题，即计算值与真实值之间存在某个缩放因子，并且由于尺度的不确定，每次初始化的缩放因子都不相同，所以对于同一真实值的多次测量值可能产生较大偏差，这在某些情况下是很严重的问题，如扫地机器人的应用场景；2、测量值与真实值的误差较大。
技术实现思路
专利技术目的：针对上述现有技术存在的缺陷，本专利技术旨在提供一种实现尺度确定且定位精度高的基于轮式编码器的单目SLAM初始化方法及系统。技术方案：一种基于轮式编码器的单目SLAM初始化方法，包括如下步骤：在SLAM初始化图像序列中选取第一关键帧与第二关键帧，对第一关键帧和第二关键帧进行特征匹配，根据匹配得出的匹配点对的图像坐标信息、轮式编码器的位移信息以及相机内参计算所有匹配点在第一关键帧的世界坐标系坐标，建立初始点云地图；并利用轮式编码器的位移信息确定第二关键帧的相机位姿，在优化时固定第一关键帧与第二关键帧的相机位姿，不对这两帧相机位姿进行局部捆集调整。为了便于操作，进一步的，在初始化图像序列中选取第一关键帧与第二关键帧之前，先进行数据获取：在ROS平台上，令图像节点及轮式编码器节点向外发布信息；在SLAM节点中订阅这两个节点发布的信息；SLAM节点接收到图像信息和轮式编码器位移信息之后，将图像信息与位移信息对应关联。进一步的，在特征匹配前先进行特征提取，具体是对图像信息提取ORB特征，遍历图像的每一个像素点，判断是否满足FAST角点的条件，找出所有满足条件的关键点，再对每一个FAST角点计算其BRIEF描述子及主方向。为了提高匹配质量，进一步的，在初始化图像序列中选取第一关键帧与第二关键帧，对第一关键帧和第二关键帧进行特征匹配具有约束条件，包括：特征点数量约束：图像的特征点数量大于特征点阈值才可选取为第一关键帧或第二关键帧；位移间距约束：与第一关键帧的位移间距大于位移阈值才可选取为第二关键帧；匹配点对数量约束：与第一关键帧进行特征匹配的匹配点对数量大于匹配点对数量阈值才可选取为第二关键帧。为了点云地图更精确，进一步的，在所述特征匹配后还进行剔除误匹配点对操作。进一步的，所述剔除误匹配点对采用ACRANSAC算法或RANSAC算法。进一步的，所述剔除误匹配点对的具体方法为：剔除视差不大于视差阈值的点和深度值超出深度预设范围的点。进一步的，具体是先剔除视差过小的点，再计算匹配点在第一关键帧的世界坐标系坐标(x，y，z)中的z坐标，z值即为深度值，再剔除z值超出深度预设范围的点。进一步的，根据匹配点对的图像坐标信息、轮式编码器的位移信息以及相机内参计算匹配点在第一关键帧的世界坐标系坐标具体是采用小孔成像模型计算世界坐标系坐标(x，y，z)：其中，fx是x轴向的相机焦距，Δs＝s2-s1为两个轮式编码器位移之差，s1、s2分别为第一关键帧与第二关键帧对应的轮式编码器位移信息；(u1，v1)是第一帧图像特征点的像素坐标，(cx，cy)是基准点，(fx，fy)是相机焦距。进一步的，所述利用轮式编码器的位移信息确定第二关键帧的相机位姿具体是将第一关键帧的位姿T1设为4×4的单位矩阵，再计算第二关键帧的相机位姿T2：其中，(x1,y1)、(x2,y2)分别为轮式编码器在第一关键帧、第二关键帧的位移信息。进一步的，所述固定第一关键帧与第二关键帧的相机位姿的优化具体为：其中，X为地图点世界坐标，R为旋转矩阵，t为平移向量，KL为共视关键帧集合但不包含第一关键帧与第二关键帧，PL为这些关键帧中的可视点集合，KF为除KL之外的全部关键帧，Xk为PL中的点与关键帧k中的关键点的匹配点对集合，ρ为鲁棒Huber代价函数，xj为关键点，π为投影函数，∑为协方差矩阵。一种基于轮式编码器的单目SLAM初始化系统，包括：视觉传感器，用于拍摄SLAM初始化图像序列并传输至SLAM模块；轮式编码器，用于将运动信息转化为位移信息并传输至SLAM模块；SLAM模块，用于在SLAM初始化图像序列中选取第一关键帧与第二关键帧，对第一关键帧和第二关键帧进行特征匹配，根据匹配得出的匹配点对的图像坐标信息、轮式编码器的位移信息以及相机内参计算所有匹配点在第一关键帧的世界坐标系坐标，建立初始点云地图，并利用轮式编码器的位移信息确定第二关键帧的相机位姿，在优化时固定第一关键帧与第二关键帧的相机位姿，不对这两帧相机位姿进行局部捆集调整。有益效果：本专利技术利用轮式编码器的位移信息与视觉传感器图像信息的点特征相结合，起到信息互补的作用，从而计算得到相机的位姿，并构建三维的点云地图。利用轮式编码器的位移信息能够十分准确的确定第二关键帧的相机位姿，而优化时固定第一关键帧与第二关键帧的相机位姿能够保留了真实世界的尺度信息，提高相机的定位精度，并且简化了运算，提高了响应速度。附图说明图1是本专利技术的方法流程图。具体实施方式下面通过实施例结合附图对本技术方案进行详细说明。如图1所示，本实施例将一种基于轮式编码器的单目SLAM初始化方法及系统应用在室内环境的单目顶视扫地机器人上，实时获取相机拍摄的图像和轮式编码器的位移数据，对间距足够大的两帧图像提取并匹配ORB特征，根据已有的各种数据，利用相机小孔成像模型计算一系列的初始地图点，最终构建出初始点云地图，方便后续使用。基于轮式编码器的单目SLAM初始化系统包括视觉传感器、轮式编码器和SLAM模块，视觉传感器用于拍摄SLAM初始化图像序列并传输至SLAM模块；视觉传感器一般为彩色相机，也可结合3D深度摄像头，使用深度摄像组件；轮式编码器(wheelencoder)用于将运动信息转化为位移信本文档来自技高网...

【技术保护点】
1.一种基于轮式编码器的单目SLAM初始化方法，其特征在于，包括如下步骤：在SLAM初始化图像序列中选取第一关键帧与第二关键帧，对第一关键帧和第二关键帧进行特征匹配，根据匹配得出的匹配点对的图像坐标信息、轮式编码器的位移信息以及相机内参计算所有匹配点在第一关键帧的世界坐标系坐标，建立初始点云地图；并利用轮式编码器的位移信息确定第二关键帧的相机位姿，在优化时固定第一关键帧与第二关键帧的相机位姿，不对这两帧相机位姿进行局部捆集调整。

【技术特征摘要】
1.一种基于轮式编码器的单目SLAM初始化方法，其特征在于，包括如下步骤：在SLAM初始化图像序列中选取第一关键帧与第二关键帧，对第一关键帧和第二关键帧进行特征匹配，根据匹配得出的匹配点对的图像坐标信息、轮式编码器的位移信息以及相机内参计算所有匹配点在第一关键帧的世界坐标系坐标，建立初始点云地图；并利用轮式编码器的位移信息确定第二关键帧的相机位姿，在优化时固定第一关键帧与第二关键帧的相机位姿，不对这两帧相机位姿进行局部捆集调整。2.根据权利要求1所述的一种基于轮式编码器的单目SLAM初始化方法，其特征在于，在初始化图像序列中选取第一关键帧与第二关键帧之前，先进行数据获取：在ROS平台上，令图像节点及轮式编码器节点向外发布信息；在SLAM节点中订阅这两个节点发布的信息；SLAM节点接收到图像信息和轮式编码器位移信息之后，将图像信息与位移信息对应关联。3.根据权利要求1所述的一种基于轮式编码器的单目SLAM初始化方法，其特征在于，在所述特征匹配前先进行特征提取，具体是对图像信息提取ORB特征，遍历图像的每一个像素点，判断是否满足FAST角点的条件，找出所有满足条件的关键点，再对每一个FAST角点计算其BRIEF描述子及主方向。4.根据权利要求1所述的一种基于轮式编码器的单目SLAM初始化方法，其特征在于，所述在初始化图像序列中选取第一关键帧与第二关键帧，对第一关键帧和第二关键帧进行特征匹配具有约束条件，包括：特征点数量约束：图像的特征点数量大于特征点阈值才可选取为第一关键帧或第二关键帧；位移间距约束：与第一关键帧的位移间距大于位移阈值才可选取为第二关键帧；匹配点对数量约束：与第一关键帧进行特征匹配的匹配点对数量大于匹配点对数量阈值才可选取为第二关键帧。5.根据权利要求1所述的一种基于轮式编码器的单目SLAM初始化方法，其特征在于，在所述特征匹配后还进行剔除误匹配点对操作。6.根据权利要求5所述的一种基于轮式编码器的单目SLAM初始化方法，其特征在于，所述剔除误匹配点对采用ACRANSAC算法或RANSAC算法。7.根据权利要求5所述的一种基于轮式编码器的单目SLAM初始化方法，其特征在于，所述剔除误匹配点对的具体方法为：剔除视差不大于视差阈值的点和深度值超出深度预设范围的点。8.根据权利要求7所述的一种基于轮式编码器的单目SLAM初始化方法，其特征在于，具体是先剔除视差过小的点，再计算匹配点在第一关键帧的世界坐标系坐标(x，y，z)中的z坐标，z值即为深度值，再剔除z值超出深度预设范围的点。9.根据权利要求1所述的一种基于轮式编码器的单目SLAM初始化方法，其特征...

【专利技术属性】
技术研发人员：陈钊，黄骏，周晓军，杜逢博，李骊，王行，盛赞，李朔，杨淼，
申请(专利权)人：南京华捷艾米软件科技有限公司，
类型：发明
国别省市：江苏,32