[发明专利]基于激光点云和视觉SLAM的机器人位姿估计方法

权利要求书

1.基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述方法包括：

使用摄像机采集机器人行进过程中的图像，再使用视觉里程计提取图像中的ORB特征点，判断关键帧，并使用关键帧的ORB特征点进行特征匹配获得匹配点对，对摄像机进行位姿估计获得摄像机下的机器人位姿；

使用激光雷达采集图像的激光点云数据，再使用激光雷达里程计提取激光点云数据的特征，与关键帧进行特征匹配，并对激光雷达进行位姿估计求得激光雷达下的机器人位姿；

根据摄像机的频率，对激光雷达下的机器人位姿进行三次样条插值处理，对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳；

对采集图像和激光点云数据进行双线程回环检测，得到回环检测结果；

所述双线程回环检测包括视觉回环检测、激光雷达回环检测和过滤器；

所述视觉回环检测采用词袋模型进行检测，具体为：

采用机器学习的K-means++算法对词袋模型中的根节点进行k次聚类，形成第一层节点，对第一层节点进行聚类处理，得到第二层节点，依次聚类d次，形成一个深度为d，枝叶数为K^d的KD-树；

利用KD-树构造字典的单词，通过输入的图像，提取图像的特征矩阵，利用公式计算图像相似度，其中A_K表示第k帧图像的特征矩阵，A_K-1表示第k-1帧图像的特征矩阵，则图像相似度计算公式为：

，

其中，M表示特征矩阵的长度，m1表示特征矩阵的m1范数；

当两帧图像的特征矩阵完全相同时，则输出1；完全不同时则输出0；

将当前帧的特征矩阵与每层中间节点的聚类中心比较d次，根据当前帧与历史帧的相似度和设定阈值进行比较，判断当前帧图像是否发生回环现象；

所述激光雷达回环检测采用Scan-Context描述子以及相似距离来进行检测，具体为：

计算激光点云数据的描述子，将每一帧的激光点云数据按照径向与环向进行划分，划分为不同的扇形栅格，其中径向与环向数量分别为Nr与Ns，将激光点云数据转换为一个(Nr,Ns)的Scan-Context描述子矩阵，该描述子矩阵的元素值为所在扇形栅格中高度的最大值；

Scan-Context描述子矩阵中每一列向量表示一个描述子，通过计算相似距离描述两个描述子的相似程度，公式为：

，

其中，I^q为当前帧激光点云数据，I^c为历史帧激光点云数据，为当前帧激光点云数据的Scan-Context描述子矩阵中第j列的元素值，为历史帧激光点云数据的Scan-Context描述子矩阵中第j列的元素值；

对历史中的关键帧进行相似距离比对，找到与当前帧最相似的描述子，最终找到对应的回环帧，判断当前帧是否发生回环现象；

将视觉回环检测与激光雷达回环检测的结果输入过滤器中，当检测结果均为当前帧的回环帧时，认定当前帧发生回环现象；

根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，融合摄像机下的机器人位姿及激光雷达下的机器人位姿得到机器人位姿。