CN108830191A - 基于改进emm及orb算法的移动机器人slam方法 - Google Patents

Abstract

本发明涉及一种基于改进EMM及ORB算法的移动机器人SLAM方法，包括以下步骤：S1、通过3D相机采集彩色图像与深度图像，使用改进ORB算法提取彩色图像的特征点并计算描述子，同时对深度图像进行滤波；S2、将彩色图像的像素点转换为空间坐标系下的3D点，使用ICP算法进行位姿计算，去除误匹配点并进行位姿提纯；S3、通过Dijkstra算法与随机采样方法进行回环检测，得到优化后的位姿信息；S4、根据优化后的位姿信息，通过改进环境测量模型的观测似然性计算方法建立稀疏点云图。与现有技术相比，本发明相比激光雷达定位成本低，与其他视觉方法相比有着鲁棒性强、误差小、无需训练等优点。

Description

基于改进EMM及ORB算法的移动机器人SLAM方法

技术领域

本发明涉及智能巡检机器人***的定位导航技术，尤其是涉及一种基于改进EMM及ORB算法的移动机器人SLAM方法。

背景技术

从行业发展趋势可以看出，巡检机器人将在未来的各个领域占有较大的市场，其中在变电站、校园、工厂、军工、船舶等场所的应用尤为广泛。针对在研发过程中激光雷达价格高和GPS模块精度不足等条件，使用深度摄像头进行局部辅助定位。

视觉定位***主要分为前端和后端，以及回环检测，前端用于提取图像中的特征，用于建立路标点，而后端用于优化摄像头自身误差而产生的位姿累计误差，使得轨迹跟踪更加精确。前端主要的技术有光流法、特征点提取法、基于灰度不变假设的直接法。回环检测的技术目前主要使用的是基于K均值分类的词袋模型法，需要前期进行训练。目前SLAM***算法的鲁棒性较低，误差较大。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于改进EMM及ORB算法的移动机器人SLAM方法。

本发明的目的可以通过以下技术方案来实现：

一种基于改进EMM及ORB算法的移动机器人SLAM方法，包括以下步骤：

S1、通过3D相机采集彩色图像与深度图像，使用改进ORB算法提取彩色图像的特征点并计算描述子，同时对深度图像进行滤波；

S2、将彩色图像的像素点转换为空间坐标系下的3D点，使用ICP算法进行位姿计算，去除误匹配点并进行位姿提纯；

S3、通过Dijkstra最短路径搜索算法与随机采样算法选择回环候选帧进行回环检测，得到优化后的位姿信息；

S4、根据优化后的位姿信息，通过改进环境测量模型的观测似然性计算方法建立稀疏点云图。

优选的，所述步骤S1中使用改进ORB算法提取彩色图像的特征点的过程具体包括：

对彩色图像构建金字塔，计算每一层图像的边缘；将每层金字塔的图像根据其分辨率分成N个a×a的栅格窗口，a表示根据图像像素设定的栅格窗口边长，在每层的每个窗口区域中使用FAST算法提取角点作为特征点，之后构造四叉树进行节点剔除，实现对特征点的提取。

优选的，所述步骤S1中对深度图像进行滤波的过程具体包括：

计算深度图像中每个像素点像素位置分别在垂直和水平方向的方差σ_z、σ_L，对每个边缘像素点进行填充；遍历深度图像中每个像素点，以各像素点为中心，3像素为长度，建立3×3区域，对于该区域中每个像素点u_k，计算其深度值D_i(u_k)，以及u_k分别与中心像素点坐标u_i及u_i的深度值D_i(u_i)的2范数Δu、Δz，判断Δz是否大于0，若是，则计算像素点u_k的2范数权重；根据每个像素点的2范数权重，重新计算深度图像中每个像素点的深度值。

优选的，所述步骤S2中位姿提纯的过程具体包括：

(1)计算彩色图像中的像素位置的横向标准差σ_x和纵向标准差σ_y，根据σ_x和σ_y得到特征点在某一相机坐标系下的协方差矩阵，计算同一特征点的协方差矩阵在不同坐标系下的估计值的相邻帧之和；

(2)计算同一特征点的3D坐标在不同相机坐标系下的估计值，根据步骤(1)得到的相邻帧之和，通过马氏距离的平方计算特征点的测量值和估计值的误差；

(3)判断特征点的测量值和估计值的误差是否小于第一设定阈值，若是，则该特征点则被选为inlier；

(4)将筛选出的inlier集合作为下一次迭代计算的特征点，并计算出提纯后的位姿T_n-1,n′，通过反复迭代设定次数，得到位姿提纯结果T_n-1,n。

优选的，所述步骤S4中改进环境测量模型的观测似然性计算方法包括：

对第n帧的像素点p_j，记其深度值为z_j，利用计算得到的位姿T_ij把像素点p_j投影到第n-1帧中的相机坐标系下，得到像素点p_i，并在第n-1帧中建立以p_i为中心，3个像素为半径的像素连通区域，遍历区域中的像素点，根据高斯分布分别计算以z_j为均值、σ_ij为方差的概率分布，并计算区域中每个像素点深度的概率值，判断所述概率值是否在第二阈值范围内，若是，则设定对应像素点为正确投影点，若概率值低于第二阈值，则设定对应像素点为遮蔽点，若概率值高于第二阈值，则设定对应像素点为错误投影点。

优选的，所述第二阈值为：

P(p_i|p_j)＝η(p_i；p_j,σ_ij)

其中， 和分别表示像素点p_i和像素点p_j的深度值的方差。

与现有技术相比，本发明使用改良过后鲁棒性更强的ORB特征点提取算法来进行前期的特征点计算，基于随机搜索以及dijkstra算法进行回环检测，相比激光雷达定位成本低，与其他视觉方法相比有着鲁棒性强、误差小、无需训练等优点，可用于巡检机器人工业和生活中涉及安全的各个领域的机器人的智能巡检。

附图说明

图1为本发明移动机器人SLAM方法的流程示意图；

图2为本发明SLAM方法运行数据包FR1/xyz误差对比图；

图3为本发明SLAM方法运行数据包FR2/xyz误差对比图；

图4为本发明SLAM方法运行数据包FR1/desk误差对比图；

图5为本发明SLAM方法运行数据包FR1/plant误差对比图；

图6为本发明SLAM方法运行数据包FR1/rpy误差对比图；

图7为本发明SLAM方法运行数据包FR1/desk建立的点云图；

图8为本发明SLAM方法运行数据包FR2/desk建立的点云图；

图9为本发明SLAM方法运行数据包FR2/desk时产生的环境特征点；

图10为本发明SLAM方法在对实验室环境建立点云图的结果；

图11为本发明SLAM方法在实验室环境下计算机器人运动的轨迹。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例

本申请提出的基于改进的环境测量模型(EMM)与ORB算法的移动机器人SLAM方法，用于巡检机器人工业和生活中涉及安全的各个领域的机器人的智能巡检，主要包括特征点提取与匹配算法，特征点马氏距离计算，环境测量模型的观测似然性计算，回环检测及后端优化。如图1所示，本方法具体包括以下步骤：

S1、通过3D相机采集彩色图像与深度图像，本实施例中，3D相机采用Kinect，图片像素数为640×480，使用改进ORB算法提取彩色图像的特征点并计算描述子，同时对深度图像进行滤波。

使用改进ORB算法提取彩色图像的特征点的过程具体包括：

对彩色图像构建金字塔，计算每一层图像的边缘；将每层金字塔的图像根据其分辨率分成N个a×a的栅格窗口，a表示根据图像像素设定的栅格窗口边长，本实施例中a取31，在每层的每个窗口区域中使用FAST算法提取角点作为特征点，之后构造四叉树进行节点剔除，即首先在图像中构造N个彼此相邻的节点，每个节点保存着一个图像正方形区域四个角的像素坐标以及此区域内的所有特征点，当构造完节点之后，再次将每个节点划分成四个子节点区域，直到每个子节点区域只包含一个特征点时，实现对特征点的提取。

对深度图像进行滤波的过程具体包括：

假定相机光心所在光轴与物体夹角为θ＝30°，计算深度图像中每个像素点像素位置分别在垂直和水平方向的方差σ_z、σ_L，对每个边缘像素点进行填充。遍历深度图像中每个像素点，以各像素点为中心，3像素为长度，建立3×3区域，对于该区域中每个像素点u_k，计算其深度值D_i(u_k)，以及u_k分别与中心像素点坐标u_i及u_i的深度值D_i(u_i)的2范数Δu、Δz，判断Δz是否大于0，若是，则计算像素点u_k的2范数权重：

根据每个像素点的2范数权重，重新计算深度图像中每个像素点的深度值：

其中，u_k表示第k个像素点，D_i(u_k)表示像素点u_k的深度值，w_k表示像素点u_k的权重。

S2、将彩色图像的像素点转换为空间坐标系下的3D点，使用ICP算法进行位姿计算，去除误匹配点并进行位姿提纯。

由于Kinect第一代的有效深度值为1.2m～3.5m，在水平方向的视角为57.5°，垂直方向的视角为43.5°，分辨率为640×480。

位姿提纯的过程具体包括：

(1)计算彩色图像中的像素位置的横向标准差σ_x和纵向标准差σ_y：

σ_x＝σ_d·tan(θ_x/m_x)

σ_y＝σ_d·tan(θ_y/m_y)

两式中θ_x、θ_y分别表示相机在水平和竖直方向的视角，m_x和m_y分别表示相机在水平和竖直方向的图像分辨率，而σ_d为滤波之后的深度值的标准差，经过深度滤波之后，得到平滑之后的深度值，计算σ_d：

式中m为红外相机和RGB相机的视差，b为基线，f为3D相机的焦距，Z为滤波之后得到的深度值；根据σ_x和σ_y得到特征点在某一相机坐标系下的协方差矩阵：

计算同一特征点的协方差矩阵在不同坐标系下的估计值：

式中R_n-1,n表示相机从第n-1帧到n帧的旋转矩阵，而Σ_xy ⁽ⁿ⁾表示在第n帧中特征点的协方差矩阵，Σ_xy ^(n-1)表示同一特征点在第n-1帧中协方差矩阵，为了将相邻帧的情况考虑进来，定义特征点的相邻帧协方差矩阵Σ_xy ^(n-1)与Σ_xy ⁽ⁿ⁾之和为：

Σ_C＝Σ_xy ^(n-1)+Σ_xy ⁽ⁿ⁾；

(2)计算同一特征点的3D坐标在不同相机坐标系下的估计值：

其中表示特征点在第n-1帧中的3D坐标的测量值，为同一特征点在第n帧中的3D点坐标的估计值，T_n-1,n表示相机的位姿变换矩阵的估计值，并计算该特征点测量值与估计值的误差：

通过马氏距离的平方计算特征点的测量值和估计值的误差：

其中，表示特征点在第n帧中的估计值，表示特征点在第n帧中的测量值；

(3)判断特征点的测量值和估计值的误差是否小于第一设定阈值，若是，则该特征点则被选为inlier；

(4)将筛选出的inlier集合作为下一次迭代计算的特征点，并计算出提纯后的位姿T_n-1,n′，通过反复迭代设定次数，本实施例中迭代15次，得到位姿提纯结果T_n-1,n。

S3、通过Dijkstra最短路径搜索算法与随机采样算法选择回环候选帧进行回环检测，得到优化后的位姿信息。

S4、根据优化后的位姿信息，通过改进环境测量模型的观测似然性计算方法建立稀疏点云图，具体包括：

对第n帧的像素点p_j，记其深度值为z_j，利用计算得到的位姿T_ij把像素点p_j投影到第n-1帧中的相机坐标系下，得到像素点p_i，并在第n-1帧中建立以p_i为中心，3个像素为半径的像素连通区域，遍历区域中的像素点，根据高斯分布分别计算以z_j为均值，σ_ij为方差的分布中每个像素点深度的概率值，判断概率值是否在第二阈值范围内，若是，则设定对应像素点为正确投影点，若概率值低于第二阈值，则设定对应像素点为遮蔽点，若概率值高于第二阈值，则设定对应像素点为错误投影点，并且分别统计三类点的数量。

其中第二阈值为：

P(p_i|p_j)＝η(p_i；p_j,σ_ij)

其中， 和分别表示像素点p_i和p_j的深度值的方差。

表1为本发明方法与原始方法的误差与耗时对比结果。原始方法是指不采用EMM与回环检测，只使用原始ORB匹配特征点以及ICP算法计算每一帧的相机位姿的方法。从表中可知，本方法相比于原始方法耗时更少，误差更低。

表1方法误差耗时评估

本实施例中，使用ROS节点管理SLAM***运行，并通过WiFi模块来进行工控机到远程PC端的数据交换，机器人端通过发布深度相机节点来传输相机的图片数据，而PC端将采用SLAM方法的***作为另一个节点来接收Kinect的图片数据，计算完机器人的运动轨迹之后，再进行离线建图。

实验中，为了检测机器人回环检测的能力，控制移动机器人低速环绕电脑桌沿长方形轨迹移动，由于起点靠门处有一处障碍，使得机器人轨迹中产生了一个小圆弧突角。图11为***估计得出的机器人运动的最终轨迹，从图中可以看出机器人运动轨迹较为平滑，效果较好，而图10为通过机器人轨迹建立的实验室3D点云环境。

图2～图6为实验数据集估计的误差对比图，图的来源为慕尼黑工业大学的公开数据集，由八个高速摄像机捕捉机器人运动，捕捉到的机器人轨迹如图实线所示，可近似为机器人的实际运动轨迹，虚线为本发明所计算出的机器人轨迹值，其中图5的旋转较为剧烈，产生的偏差比较大，而剩余图都存在回环的情况，匹配程度较好。

图7和图8为数据集中通过本方法计算的机器人位姿得到的3D点云环境图，而图9为特征点的环境分布图。

Claims (6)

1.一种基于改进EMM及ORB算法的移动机器人SLAM方法，其特征在于，包括以下步骤：

S1、通过3D相机采集彩色图像与深度图像，使用改进ORB算法提取彩色图像的特征点并计算描述子，同时对深度图像进行滤波；

S2、将彩色图像的像素点转换为空间坐标系下的3D点，使用ICP算法进行位姿计算，去除误匹配点并进行位姿提纯；

S3、通过Dijkstra最短路径搜索算法与随机采样算法选择回环候选帧进行回环检测，得到优化后的位姿信息；

S4、根据优化后的位姿信息，通过改进环境测量模型的观测似然性计算方法建立稀疏点云图。

2.根据权利要求1所述的基于改进EMM及ORB算法的移动机器人SLAM方法，其特征在于，所述步骤S1中使用改进ORB算法提取彩色图像的特征点的过程具体包括：

对彩色图像构建金字塔，计算每一层图像的边缘；将每层金字塔的图像根据其分辨率分成N个a×a的栅格窗口，a表示根据图像像素设定的栅格窗口边长，在每层的每个窗口区域中使用FAST算法提取角点作为特征点，之后构造四叉树进行节点剔除，实现对特征点的提取。

3.根据权利要求1所述的基于改进EMM及ORB算法的移动机器人SLAM方法，其特征在于，所述步骤S1中对深度图像进行滤波的过程具体包括：

计算深度图像中每个像素点像素位置分别在垂直和水平方向的方差σ_z、σ_L，对每个边缘像素点进行填充；遍历深度图像中每个像素点，以各像素点为中心，3像素为长度，建立3×3区域，对于该区域中每个像素点u_k，计算其深度值D_i(u_k)，以及u_k分别与中心像素点坐标u_i及u_i的深度值D_i(u_i)的2范数Δu、Δz，判断Δz是否大于0，若是，则计算像素点u_k的2范数权重；根据每个像素点的2范数权重，重新计算深度图像中每个像素点的深度值。

4.根据权利要求1所述的基于改进EMM及ORB算法的移动机器人SLAM方法，其特征在于，所述步骤S2中位姿提纯的过程具体包括：

(1)计算彩色图像中的像素位置的横向标准差σ_x和纵向标准差σ_y，根据σ_x和σ_y得到特征点在某一相机坐标系下的协方差矩阵，计算同一特征点的协方差矩阵在不同坐标系下的估计值的相邻帧之和；

(2)计算同一特征点的3D坐标在不同相机坐标系下的估计值，根据步骤(1)得到的相邻帧之和，通过马氏距离的平方计算特征点的测量值和估计值的误差；

(3)判断特征点的测量值和估计值的误差是否小于第一设定阈值，若是，则该特征点则被选为inlier；

(4)将筛选出的inlier集合作为下一次迭代计算的特征点，并计算出提纯后的位姿T_n-1,n′，通过反复迭代设定次数，得到位姿提纯结果T_n-1,n。

5.根据权利要求1所述的基于改进EMM及ORB算法的移动机器人SLAM方法，其特征在于，所述步骤S4中改进环境测量模型的观测似然性计算方法包括：

对第n帧的像素点p_j，记其深度值为z_j，利用计算得到的位姿T_ij把像素点p_j投影到第n-1帧中的相机坐标系下，得到像素点p_i，并在第n-1帧中建立以p_i为中心，3个像素为半径的像素连通区域，遍历区域中的像素点，根据高斯分布分别计算以z_j为均值、σ_ij为方差的概率分布，并计算区域中每个像素点深度的概率值，判断所述概率值是否在第二阈值范围内，若是，则设定对应像素点为正确投影点，若概率值低于第二阈值，则设定对应像素点为遮蔽点，若概率值高于第二阈值，则设定对应像素点为错误投影点。

6.根据权利要求5所述的基于改进EMM及ORB算法的移动机器人SLAM方法，其特征在于，所述第二阈值为：

P(p_i|p_j)＝η(p_i；p_j,σ_ij)

其中， 和分别表示像素点p_i和像素点p_j的深度值的方差。