[发明专利]一种基于RGB-D图像的SLAM方法

权利要求书

1.一种基于RGB-D图像的SLAM方法，其特征在于，该方法包括以下步骤：

步骤一、RGB-D图像的获取：

采用深度相机按照预设设定的采集时间对待拍摄空间进行实时采集，并将各个采样时刻采集到的RGB-D图像发送至计算机；其中，第i个采样时刻采集到的RGB-D图像包括第i帧彩色图像和第i帧彩色图像所对应的深度值，i为正整数，且1≤i≤M，M表示彩色图像的总帧数；

步骤二、RGB-D图像的特征点提取与匹配：

步骤201、计算机采用ORB算法对第i帧彩色图像进行特征点提取，得到第i帧彩色图像的特征点描述子集；其中，第i帧彩色图像中第a个特征点对应的特征点描述子记作第i帧彩色图像中第a个特征点描述子a为正整数，且1≤a≤A，A表示特征点的总数；

同时，计算机采用ORB算法对第i+1帧彩色图像进行特征点提取，得到第i+1帧彩色图像的特征点描述子集；其中，第i+1帧彩色图像中第a′个特征点对应的特征点描述子记作第i+1帧彩色图像中第a′个特征点描述子a′为正整数，1≤a′≤A；

步骤202、计算机采用K-means++算法对第i帧彩色图像的特征点描述子和第i+1帧彩色图像中的特征点描述子进行匹配，得到多组匹配点对；

步骤三、计算旋转矩阵和平移矩阵：

步骤301、计算机将每组匹配点对之间的汉明距离按照从小到大的顺序进行排序，并选取前N组匹配点对；

步骤302、计算机采用随机抽样一致RANSAC算法和PnP算法，对N组匹配点对进行姿态估计，得到第i+1帧彩色图像和第i帧彩色图像之间的姿态、特征匹配的内点数Inliers_i,i+1；其中，第i+1帧彩色图像和第i帧彩色图像之间的姿态包括第i+1帧彩色图像和第i帧彩色图像之间的旋转矩阵R_i+1,i和平移向量t_i+1,i；其中，旋转矩阵R_i+1,i为3×3矩阵，平移向量t_i+1,i为3×1向量；

步骤303、计算机对第i+1帧彩色图像和第i帧彩色图像之间的旋转矩阵R_i+1,i和平移向量t_i+1,i进行优化，得到第i+1帧彩色图像和第i帧彩色图像之间的优化后的旋转矩阵和优化后的平移向量

步骤四、RGB-D图像中关键帧的获取：

步骤401、选取第1帧彩色图像作为关键帧；

步骤402、选取下一帧彩色图像即第2帧彩色图像作为当前帧彩色图像；

步骤403、按照步骤二和步骤三所述的方法，得到第2帧彩色图像和第1帧彩色图像即当前帧彩色图像和上一个关键帧之间的特征匹配的内点数Inliers_c,s，计算机将Inliers_c,s和设定的最小内点数In_min进行判断，如果Inliers_c,sIn_min,执行步骤404和步骤405；否则，舍弃该当前帧彩色图像；

步骤404、根据公式D_c,s＝||t_c,s^*||+min(2π-||R_c,s^*||,||R_c,s^*||)，得到当前帧彩色图像和上一个关键帧之间的相对运动距离D_c,s；其中，R_c,s^*表示第2帧彩色图像和第1帧彩色图像之间即当前帧彩色图像和上一个关键帧之间的优化后的旋转矩阵，t_c,s^*表示第2帧彩色图像和第1帧彩色图像之间即当前帧彩色图像和上一个关键帧之间的优化后的平移向量，||·||表示向量的模；

步骤405、计算机将D_c,s与运动距离最小阈值D_min和运动距离最大阈值D_max进行判断，如果D_minD_c,sD_max成立，则第2帧彩色图像即当前帧彩色图像为关键帧；否则，舍弃该当前帧彩色图像；

步骤406、按照步骤401和步骤405所述的方法，对上一个关键帧和下一帧彩色图像之间进行判断，直至完成M-2帧彩色图像的判断，得到各个关键帧；

步骤五、关键帧的闭环判断：

步骤501、计算机将步骤406中得到的各个关键帧按照采样时间先后顺序存入数据库图像中；其中，关键帧的总数为B；

步骤502、计算机根据公式N_z＝B×A，得到数据库图像中特征点的总数N_z；

步骤503、计算机根据公式得到第b个关键帧中第t个特征点的词频-逆文档频率其中，N_z,t表示第b个关键帧中第t个特征点在数据库图像中出现的数量，N_b,t表示第b个关键帧中第t个特征点出现的数量，t为正整数，1≤t≤A，b为正整数，且1≤b≤B；

步骤504、计算机将剩余B-b个关键帧记作待判断关键帧，计算机根据公式得到第b′个待判断关键帧中第t′个特征点的词频-逆文档频率其中，N_z,t′表示第b′个待判断关键帧中第t′个特征点在数据库图像中出现的数量，N_b′,t′表示第b′个待判断关键帧中第t′个特征点出现的数量，b′和t′均为正整数，且b′的取值位于b+1～B，且b+1不大于B，1≤t′≤A；

步骤505、计算机根据，

得到第b个关键帧和第b′个待判断关键帧之间的相似度S(q^b,d^b′)；其中，q^b表示第b个关键帧中各个特征点的词频-逆文档频率向量，且d^b′表示第b′个待判断关键帧中各个特征点的词频-逆文档频率向量，且||·||表示向量的模；

步骤506、按照步骤505所述的方法，得到各个关键帧之间的相似度，获取相似度最大值并记作S_max，并设定相似度阈值系数为S_th；

步骤507、计算机将S(q^b,d^b′)和S_th×S_max进行判断，如果S(q^b,d^b′)大于S_th×S_max，则第b个关键帧和第b′个待判断关键帧作为候选闭环帧序列；

步骤508、按照步骤二和步骤三所述的方法，得到第b个关键帧和第b′个待判断关键帧之间优化后的旋转矩阵和优化后的平移向量以及第b个关键帧和第b′个待判断关键帧之间的特征匹配的内点数Inliers_b,b′(key)；

步骤509、计算机将第b个关键帧和第b′个待判断关键帧之间的特征匹配的内点数Inliers_b,b′(key)和特征匹配的内点数阈值进行判断，如果Inliers_b,b′(key)大于内点数阈值，则第b个关键帧和第b′个待判断关键帧作为一组闭环帧，

步骤5010、按照步骤502至步骤509所述的方法，对第b+1个关键帧和待判断关键帧进行判断，直至完成B-1个关键帧的判断，得到多组闭环帧；

步骤六、RGB-D图像的全局优化：

计算机对存在多组闭环帧的各个关键帧进行全局优化，得到优化后的位姿图；

步骤七、构建网格地图：

计算机根据优化后的位姿图和关键帧构建三角化网格地图；

步骤303中计算机对旋转矩阵R_i+1,i和平移向量t_i+1,i进行优化，得到第i+1帧彩色图像和第i帧彩色图像之间优化后的旋转矩阵和优化后的平移向量具体过程如下：

步骤3031、计算机将拍摄第i帧彩色图像时相机坐标系记作第i个相机坐标系，将拍摄第i+1帧彩色图像时相机坐标系记作第i+1个相机坐标系；设定第f组匹配点对应第f个空间点在第i个相机坐标系下的三维坐标记作在第i+1个相机坐标系下的三维坐标记作

步骤3032、计算机根据公式得到第f个空间点在第i+1帧彩色图像和第i帧彩色图像之间的姿态误差项其中，||·||²表示向量的模的平方；R′_i+1,i表示第i+1帧彩色图像和第i帧彩色图像之间的设定旋转矩阵，t′_i+1,i第i+1帧彩色图像和第i帧彩色图像之间的设定平移向量；

步骤3033、计算机建立姿态误差函数其中，1≤F≤N；

步骤3034、R′_i+1,i的初始值取R_i+1,i，t′_i+1,i的初始值取t_i+1,i，计算机采用最小二乘法对步骤3033中的姿态误差函数进行求解，得到姿态误差函数最小时的旋转矩阵R′_i,i+1和平移向量t′_i,i+1，并将姿态误差函数最小时的R′_i,i+1作为第i+1帧彩色图像和第i帧彩色图像之间优化后的旋转矩阵姿态误差函数最小时的t′_i+1,i作为第i+1帧彩色图像和第i帧彩色图像之间优化后的平移向量