[发明专利]一种RGB-D相机在动态场景下的SLAM方法

权利要求书

1.一种RGB-D相机在动态场景下的SLAM方法，其特征在于，所述方法包括以下步骤：

S1、在RGB-D相机采集的当前帧的RGB图像上进行ORB特征点提取和对所提取的ORB特征点进行描述子计算，将当前帧的RGB图像与上一帧的RGB图像进行描述子匹配，通过最小化重投影误差初步计算出当前帧的位姿；

S2、将当前帧的位姿与所有关键帧的位姿进行比较，找出位姿与当前帧相近并且时间上与当前帧相远的前m个关键帧，设为参考关键帧集，表示为：

其中，a,b,c,d为大于0的常数，id是关键帧的序号，id_c是当前帧的序号，R是关键帧的旋转矩阵，t是关键帧的平移矩阵，R_c是当前帧的旋转矩阵，t_c是当前帧的平移矩阵；

S3、将参考关键帧集中的每一帧的所有深度值大于0的特征点投影到当前帧上，找出投影后的深度值z_proj与当前帧投影点位置的实际深度值z之差的绝对值Δz大于阈值T_z的点，记为投影动态点，表示为：

Δz＝|z_proj-z|＞T_z；

S4、根据深度值对当前帧的特征点进行K-Means聚类，找出初始动态集；

S5、根据二维图像坐标(u，v)对当前帧的每个初始动态集中的特征点进行第二次K-Means聚类，得到最终动态集，最终动态集中的特征点记为动态点，当前帧除动态点外的所有特征点记为静态特征点；

所述步骤S5过程如下：

S51、根据二维图像坐标(u，v)对每个初始动态集的特征点进行K-Means聚类，并且计算出第i个初始动态集的第j个类的聚类中心c_ij，表示为：

其中，N_ij为属于第i个初始动态集的第j个类的特征点数量，u_ijk和v_ijk为属于第i个初始动态集的第j个类的第k个特征点的二维图像坐标；

进一步写成：

c_ij＝(u_ij,v_ij)

其中，(u_ij,v_ij)为聚类中心c_ij的二维坐标；

S52、将属于第i个初始动态集的投影动态点的二维图像坐标与第i个初始动态集的每个聚类中心c_ij进行比较，计算出与投影动态点p_ik的二维图像坐标的距离最小的类s(i，k)，表示为：

其中，(up_ik,vp_ik)为投影动态点p_ik的二维图像坐标；

S53、统计所有初始动态集的每个类的投影动态点的数量，表示如下：

其中，N_proj(i)为第i个初始动态集的投影动态点的数量，num_ij为第i个初始动态集的第j类的投影动态点的数量；

S54、统计属于每个类的投影动态点的数量，投影动态点的数量num_ij大于阈值T_uv的类为最终动态集；

S6、在时间维度上，计算当前帧每个特征点的静态观测数，得到每个特征点的静态观测权重；

S7、在几何维度上，计算当前帧每个特征点的静态概率，通过特征点的静态观测权重和静态概率得到该特征点的静态权重；

S8、对传统的RANSAC算法的特征点进行改进，剔除当前帧误匹配的特征点和处于动态对象上的特征点，再将剩下的特征点对应的地图点重投影到当前帧，对每个特征点进行静态权重加权，最小化加权重投影之和，求得相机当前帧的最终位姿。