CN115880364B - 基于激光点云和视觉slam的机器人位姿估计方法 - Google Patents

Abstract

本发明公开了基于激光点云和视觉SLAM的机器人位姿估计方法，使用摄像机采集图像，再使用视觉里程计提取图像中的ORB特征点，判断关键帧并与ORB特征点进行特征匹配获得匹配点对，进行位姿估计获得摄像机下的机器人位姿；使用激光雷达采集激光点云数据，使用激光雷达里程计提取激光点云数据的特征，与关键帧进行特征匹配并进行位姿估计求得激光雷达下的机器人位姿；根据摄像机的频率，对激光雷达下的机器人位姿进行三次样条插值处理对齐时间戳；进行双线程回环检测得到回环检测结果；将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中融合得到机器人位姿。本发明补全了与视觉特征点对应的点云缺失，提高了视觉SLAM建图及回环检测精度。

Description

基于激光点云和视觉SLAM的机器人位姿估计方法

技术领域

本发明属于机器人定位的技术领域，具体涉及一种基于激光点云和视觉SLAM的机器人位姿估计方法。

背景技术

随着无人驾驶、5G通信被人们提出和高度关注，SLAM（即时定位与建图）迎来了很大的挑战。SLAM技术是机器人领域的关键技术，其依靠传感器完成自主导航、建图、以及路径规划等任务。传统SLAM系统一般是依靠单一的传感器进行工作，但单一传感器的缺陷也较为明显，其对于环境因素的要求高导致其工作性能不稳定；而多传感器SLAM系统存在着不同传感器的测量原理不同以及其运行频率不同的问题，导致SLAM系统建图的误差较大；针对多传感器SLAM系统的问题，传统解决方法是将频率较快的视觉传感器的位姿估计作为激光传感器位姿估计的先验数据放入卡尔曼滤波器中，然后进行SLAM系统的位姿估计输出，但由于多传感器的数据并没有对齐在同一个时间戳内，可能存在着无法满足多传感器SLAM系统的建图要求。

近几年，深度学习十分热门，图像处理技术的进步也带动了视觉SLAM的发展，一方面由于摄像机的价格相对便宜，可以在立体空间中简单快速地获取重要的角或边信息；另一方面在于摄像机具有较高的采样率，从而被广泛青睐。但是摄像机也有自己的缺点，在大范围旋转时，很容易失去正常功能，造成较大的漂移误差；同时摄像机还需要运行在稳定的光源环境才能提取到较多且质量高的特征点。激光雷达可以测量的范围和距离非常广，并且是主动传感器，所以激光雷达在黑暗中仍然具有极好的鲁棒性；但激光雷达的缺点在于价格昂贵，同时由于二维激光传感器只有深度信息，并且只扫描一个平面，当机器人在走廊中运动时，传感器返回的深度信息不随时间变化。因此通过不同传感器的相互协同降低位姿估计的误差，成为提高SLAM系统定位建图的鲁棒性的重要手段。

发明内容

本发明的主要目的在于克服现有技术的缺点与不足，提供一种基于激光点云和视觉SLAM的机器人位姿估计方法，利用三次样条函数对激光雷达的位姿估计进行样条插值处理，同时引入视觉-激光雷达双线程回环检测模块，提高了SLAM系统建图精度，提高了回环检测的精度，防止出现“假阳性”的回环现象。

为了达到上述目的，本发明采用以下技术方案：

基于激光点云和视觉SLAM的机器人位姿估计方法，所述方法包括下述步骤：

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

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

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

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

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

作为优选的技术方案，所述ORB特征点包括FAST角点和BRIEF描述子；

所述使用视觉里程计提取图像中的ORB特征点，判断关键帧并与ORB特征点进行特征匹配获得匹配点对，具体为：

将摄像机采集的图像输入视觉里程计中，通过对图像上的像素点进行阈值比较，提取FAST角点，同时引入图像金字塔对每一层金字塔图像进行FAST角点提取；

比对FAST角点附近的两个随机像素，构成描述子向量得到BRIEF描述子；

根据摄像机的移动距离是否超过阈值距离来确定图像是否为关键帧；

对关键帧的ORB特征点进行距离计算，通过暴力匹配法进行特征匹配得到匹配点对。

作为优选的技术方案，所述视觉里程计通过在图像上选取像素点 p，计算该像素点的亮度 I _p ，以像素点 p为中心点，选取半径为3的圆上的像素点，若该圆上12个连续的像素点亮度与中心点 p亮度的差值大于阈值，则认定像素点 p为FAST角点；

引入图像金字塔对图像进行不同层次的下采样，得到多层不同分辨率的图像，并对每一层图像进行FAST角点提取；

对于每一FAST角点，随机选取FAST角点附近128组像素点对 p、 q，比较每一组像素点 p和像素点 q的大小并编码为二进制描述向量，若 p大于 q则取1，否则取0，得到的128个描述向量构成该FAST角点的BRIEF描述子；

所述确定图像是否为关键帧，具体为：

获取图像的摄像机移动距离，并与阈值距离进行比较；

若图像的摄像机移动距离大于阈值距离，则判定该图像为关键帧。

作为优选的技术方案，所述通过暴力匹配法进行特征匹配得到匹配点对，具体为：

设图像中第 t帧提取到的第 m个ORB特征点表示为 x _t,m ， m=1,2,…, M， M表示第 t帧中提取到的ORB特征点的个数，第 t+1帧提取到的第 n个ORB特征点表示为 x _{t+1 ,n} ， n= 1,2,…, N， N表示第 t+1帧中提取到的ORB特征点的个数；

采用欧氏距离测量第 t帧中ORB特征点的BRIEF描述子与第 t+1帧中ORB特征点的BRIEF描述子之间的距离，对齐排序，选取距离最近的一个作为匹配点对，计算公式为：

，

其中， n表示ORB特征点的BRIEF描述子的长度， x _t,i,k 表示图像中第 t帧提取到的第 i个ORB特征点的BRIEF描述子中的第 k个元素， x _{t+1 ,j,k} 表示图像中第 t+1帧提取到的第 j个ORB特征点的BRIEF描述子中的第 k个元素。

作为优选的技术方案，根据获得的匹配点对，使用PnP法对摄像机进行位姿估计，并使用最小化投影求解，获得摄像机下的机器人位姿，具体为：

设图像在投影空间中某点的坐标为 P _i = [ X _i ,Y _i ,Z _i ] ^T ，该点投影的像素坐标为 U _i =[ u _i ,v _i ] ^T ，将投影关系转换为齐次坐标系下表示，转换式为：

，

其中， Z _i 表示图像中的特征点在世界坐标系下的深度值， K表示摄像机的内参矩阵， T _C,W 表示世界坐标系与相机坐标系之间的转换矩阵；

通过转换式求得变换矩阵 T _C,W ，构建位姿估计中第 i个点的误差项，表示为：

，

把位姿估计的误差项进行求和，构建最小二乘问题，寻找误差最小时摄像机下的机器人位姿变换矩阵，公式为：

，

其中， n表示图像中提取出的ORB特征点数量， P _i 为第 i个ORB特征点在世界坐标系下的坐标，表示通过变换矩阵得到的第 i个ORB特征点在世界坐标系下的估计坐标；

根据机器人位姿变换矩阵求得第 k帧时摄像机下的机器人位姿估计值，公式为：

，

其中，表示视觉里程计第 k帧时输出的在世界坐标系下的机器人位姿估计值。

作为优选的技术方案，所述对激光雷达进行位姿估计求得激光雷达下的机器人位姿，具体为：

将激光雷达采集的图像的激光点云数据输入激光雷达里程计中，计算每一帧激光点云数据中每个点的曲率，并对每个点的曲率进行排序，选择曲率最大的2个点作为边缘点，曲率最小的4个点作为平面点；

对激光点云数据进行异常值排除；

使用基于特征的Scan-to-Scan匹配方法将异常值排除后的激光点云数据进行特征点匹配，获取边缘线和平面；

计算边缘点到边缘线的点线残差和边缘点到平面的点面残差；

计算残差总和，通过对残差总和构建最小二乘法求得极值，使用迭代法求解使得残差总和最小，并根据激光点云数据进行位姿估计，求得激光雷达下的机器人位姿。

作为优选的技术方案，所述曲率的计算公式为：

，

其中， C表示曲率，表示激光雷达坐标系下第 k帧激光点云数据中第 i个点的坐标， S表示激光点云数据的集合；

所述异常值包括激光点云数据中平行于激光扫描平面的点和遮挡区域边界的点；

所述获取边缘线和平面，具体为：

设第 k-1帧的激光点云数据为，第 k帧的激光点云数据为；

在中找到与中边缘点 i最近的边缘点 j，再在另外的激光线束上找到距离边缘点 j最近的边缘点 l，连接边缘点 j和边缘点 l得到直线 jl构成边缘线，并在中找到三个与中面点 i最近的面点 j，在同一激光线束上找到距离面点 j最近的面点 l，再在另外的激光线束上找到距离面点 j最近的面点 m，构成平面 jlm；

记中边缘点 i到中直线 jl的距离为点线残差 d _ε ，中面点 i到中 jlm平面的距离为点面残差 d _ω ，则点线残差 d _ε 的计算公式为：

，

点面残差 d _ω 的计算公式为：

，

其中，表示激光雷达坐标系下第 k-1帧激光点云数据中第 j个点的坐标；表示激光雷达坐标系下第 k-1帧激光点云数据中 l个点的坐标；表示激光雷达坐标系下第 k-1帧激光点云数据中 m个点的坐标；

所述残差总和 d= d _ε + d _ω ；

使用点线残差和点面残差构建与激光雷达坐标系下两帧之间位姿变换矩阵 T ^L 的方程，表示为：

，

其中， f _ε ()表示点线残差的计算式； f _ω ()表示点面残差的计算式；表示激光雷达坐标系下第 k-1帧与第 k帧之间机器人的位姿变换矩阵 T ^L ；

使用残差总和构建最小二乘问题，表示为：

，

其中， F( x)表示最小二乘问题的目标函数；

使用迭代法对最小二乘问题求极值：给定初始值 x ₀的坐标，对于第 k-1次迭代，寻找增量，使得达到极小值；若足够小则停止，得到极小值 F( x)下的变换矩阵；否则令，继续下一次迭代；其中，表示激光雷达坐标系下第 k-1帧激光点云数据中第 i个点的坐标，表示在取得极小值 F( x)下的变换矩阵；

通过极小值下的变换矩阵求得第 k帧时激光雷达下的机器人位姿估计值，公式为：

，

其中，表示激光雷达里程计第 k帧时输出的在世界坐标系下的机器人位姿估计值。

作为优选的技术方案，所述根据摄像机的频率，对雷达位姿进行三次样条插值处理，利用 n-1个三次函数线段拟合出 n个时间帧的激光雷达输出，通过插值对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳，具体为：

根据样条插值假设条件，利用已知的历史激光雷达的机器人位姿数据构建出 n-1段样条函数 S _i ( x)，表示为：

S _i ( x) = a _i + b _i ( x- x _i ) + c _i ( x- x _i )²+ d _i ( x- x _i )³ ,i= 1 ,2 ,…,n-1，

根据样条函数性质，列出四个等式约束，令，，求得样条函数 S _i ( x)的曲线参数 a _i 、 b _i 、 c _i 、 d _i ，等式约束表示为：

，

其中， x表示样条函数自变量为时间， x _i 表示第 i帧激光点云数据对应的时间， n表示激光点云数据的个数或者长度， h _i = x _i+1- x _i ， y _i 表示第 i帧时激光雷达下的机器人位姿估计值；

假设摄像机的采样频率为 f _c ，激光雷达的采样频率为 f _L ，根据已知的历史激光雷达的机器人位姿数据，利用三次样条函数来拟合机器人运动的轨迹关系，再使用摄像机的采样频率对该拟合函数进行插值处理，即利用样本函数 S _i ( x)分别插值得到时间点为、、…、下的函数值， i=1 ,2 ,… ,l，其中 l为同一段时间内摄像机获得的帧数；插值后得到对应时刻的激光雷达的机器人位姿，即表示相机第 i帧时对应的激光雷达的机器人位姿，实现摄像机和激光雷达数据的同步生成。

作为优选的技术方案，所述双线程回环检测包括视觉回环检测、激光雷达回环检测和过滤器；

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

采用机器学习的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列的元素值；

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

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

作为优选的技术方案，所述根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，融合摄像机下的机器人位姿及激光雷达下的机器人位姿得到机器人位姿，具体为：

已知SLAM中的机器人状态方程为：

，

其中 A为状态转移矩阵， B为控制矩阵， H为观测矩阵， W∼ N(0, Q)是过程噪声， V∼ N(0, R)是观测噪声， X _k 为 k时刻的机器人位姿， Z _k 为 k时刻的机器人位姿估计值， U _k-1为 k-1时刻的控制输入， W _k 为 k时刻的过程噪声， V _k 为 k时刻的观测噪声值；

假设机器人初始状态以及每一时刻的噪声是互相独立的；

根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，对第一帧进行判断，若第一帧是第一次进行检测，则使用第一帧时摄像机下的机器人位姿与激光雷达下的机器人位姿的平均值对卡尔曼滤波器进行初始化，初始值表示为且；为摄像机下的机器人位姿与激光雷达下的机器人位姿的平均值；

根据上一时刻的机器人位姿估计值，预测当前时刻机器人位姿的先验估计值，公式为：

，

计算先验估计值的协方差：

，

由于先验估计与实际值存在误差，通过更新卡尔曼增益系数 K _k ，使得误差最小：

，

根据卡尔曼增益系数计算出当前时刻机器人位姿的后验估计值以及协方差矩阵 P _k ，公式为：

，

其中， P _k-1为 k-1帧的协方差矩阵， A ^T 为状态转移矩阵的转置， Q为过程噪声协方差， R为测量噪声协方差， H ^T 为观测矩阵的转置，表示当前时刻机器人位姿的后验估计值，即当前时刻误差最小的机器人位姿；

将计算结果的后验估计值与协方差矩阵保存，以便于下次的迭代计算。

本发明与现有技术相比，具有如下优点和有益效果：

1、多传感器SLAM系统存在着不同传感器的测量原理不同以及其运行频率不同导致整个SLAM系统建图的误差较大，通过补全了与视觉特征点对应的点云缺失，来解决建图时多传感器数据不一致的问题，利用视觉里程计的位姿估计和经过插值得到的激光雷达里程计的位姿估计这两者的均值，通过输入两者的位姿估计均值即同一时间戳的机器人位姿估计到卡尔曼滤波器中，通过不断更新卡尔曼增益，从而输出机器人的位姿估计，得到更好的SLAM系统的建图效果。

2、引入视觉-雷达双线程回环检测，将视觉回环检测与激光雷达的回环检测的结果输入过滤器，只有都找到了当前帧的回环帧，才认定当前帧发生了回环现象，有效避免不是回环帧的“过阳性”情况发生，对于未检测到的回环现象，通过多次检测也可以判断出该回环现象，提高回环检测的鲁棒性以及准确率。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例基于激光点云和视觉SLAM的机器人位姿定位方法的流程图；

图2为本发明实施例获得相机位姿的流程图；

图3为本发明实施例求得雷达位姿的流程图；

图4为本发明实施例激光点云数据中异常值排除的示意图；

图5为本发明实施例进行双线程回环检测的流程图；

图6为本发明实施例词袋模型的结构示意图；

图7为本发明实施例卡尔曼滤波器的流程步骤图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

在本申请中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本申请所描述的实施例可以与其它实施例相结合。

如图1所示，本实施例提供一种基于激光点云和视觉SLAM的机器人位姿估计方法，包括下述步骤：

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

具体的，如图2所示，视觉里程计提取ORB特征点的步骤分为FAST角点提取及BRIEF描述子计算，判断关键帧后进行特征匹配，再对摄像机进行位姿估计获得摄像机下的机器人位姿，具体步骤为：

S101、将摄像机采集的图像输入视觉里程计中，通过对图像上的像素点进行阈值比较，提取FAST角点，同时引入图像金字塔对每一层金字塔图像进行FAST角点提取；

本实施例中，视觉里程计通过在图像上选取像素点 p，计算该像素点的亮度 I _p ，以像素点 p为中心点，选取半径为3的圆上的像素点，若该圆上12个连续的像素点亮度与中心点 p亮度的差值大于阈值，则认定像素点 p为FAST角点；同时引入图像金字塔对图像进行不同层次的下采样，得到多层不同分辨率的图像，并对每一层图像进行FAST角点提取，实现不同层金字塔之间的尺度不变性。

S102、比对FAST角点附近的两个随机像素，构成描述子向量得到BRIEF描述子；

本实施例中，对于每一FAST角点，随机选取FAST角点附近128组像素点对 p、 q，比较每一组像素点 p和像素点 q的大小并编码为二进制描述向量，若 p大于 q则取1，否则取0，得到的128个描述向量构成该FAST角点的BRIEF描述子。

S103、根据摄像机的移动距离是否超过阈值距离来确定图像是否为关键帧；

本实施例中，通过获取图像的摄像机移动距离与阈值距离进行比较；若图像的摄像机移动距离大于阈值距离，则判定该图像为关键帧；否则该图像不为关键帧且不进行特征匹配。

S104、对关键帧的ORB特征点进行距离计算，通过暴力匹配法进行特征匹配得到匹配点对；

本实施例中，通过暴力匹配法进行特征匹配得到匹配点对，具体为：

设图像中第 t帧提取到的第 m个ORB特征点表示为 x _t,m ， m= 1,2,…, M， M表示第 t帧中提取到的ORB特征点的个数，第 t+1帧提取到的第 n个ORB特征点表示为 x _{t+1 ,n} ， n= 1,2,…, N， N表示第 t+1帧中提取到的ORB特征点的个数；

采用欧氏距离测量第 t帧中ORB特征点的BRIEF描述子与第 t+1帧中ORB特征点的BRIEF描述子之间的距离，对齐排序，选取距离最近的一个作为匹配点对，计算公式为：

，

其中， n表示ORB特征点的BRIEF描述子的长度， x _t,i,k 表示图像中第 t帧提取到的第 i个ORB特征点的BRIEF描述子中的第 k个元素， x _{t+1 ,j,k} 表示图像中第 t+1帧提取到的第 j个ORB特征点的BRIEF描述子中的第 k个元素。

S105、根据获得的匹配点对，使用PnP法对摄像机进行位姿估计，并使用最小化投影求解，获得摄像机下的机器人位姿；

PnP是一类问题，通过n个3D空间点及其投影，估计摄像机的机器人位姿；PnP问题求解方法有很多种，本实施例使用非线性优化的方式，构建最小二乘问题并进行迭代求解，最终得到的结果是误差最小下的相机变换矩阵，根据这个变换矩阵及像素坐标，估算出机器人的世界坐标值，即摄像机下的机器人位姿，具体为：

设图像在投影空间中某点的坐标为 P _i = [ X _i ,Y _i ,Z _i ] ^T ，该点投影的像素坐标为 U _i =[ u _i ,v _i ] ^T ，将投影关系转换为齐次坐标系下表示，转换式为：

，

其中， Z _i 表示图像中的特征点在世界坐标系下的深度值， K表示摄像机的内参矩阵， T _C,W 表示世界坐标系与相机坐标系之间的转换矩阵；

通过转换式求得变换矩阵 T _C,W ，构建位姿估计中第 i个点的误差项，表示为：

，

把位姿估计的误差项进行求和，构建最小二乘问题，寻找误差最小时摄像机下的机器人位姿变换矩阵，公式为：

，

其中， n表示图像中提取出的ORB特征点数量， P _i 为第 i个ORB特征点在世界坐标系下的坐标，表示通过变换矩阵得到的第 i个ORB特征点在世界坐标系下的估计坐标；

根据机器人位姿变换矩阵求得第 k帧时摄像机下的机器人位姿估计值，公式为：

，

其中，表示视觉里程计第 k帧时输出的在世界坐标系下的机器人位姿估计值。

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

具体的，如图3所示，激光雷达里程计的操作步骤具体为：

S201、将激光雷达采集的图像的激光点云数据输入激光雷达里程计中，计算每一帧激光点云数据中每个点的曲率，并对每个点的曲率进行排序，选择曲率最大的2个点作为边缘点，曲率最小的4个点作为平面点，曲率的计算公式为：

，

其中， C表示曲率，表示激光雷达坐标系下第 k帧激光点云数据中第 i个点的坐标， S表示激光点云数据的集合，取模是求点云的数量；

S202、对激光点云数据进行异常值排除；

本实施例中，如图4所示，异常值包括平行于激光扫描平面的点和遮挡区域边界的点，即排除平行于激光扫描平面的点时，当前帧激光点云数据中的点及其附近点集合拟合的平面与激光线束的夹角不能过小；排除遮挡区域边界的点时，当前帧激光点云数据中点附近的点集中没有在激光束方向上间距过大的点。

如在图4（a）中点A位于与激光线束(虚线段)有一定角度的线上；点B位于与激光线束(虚线段)大致平行的线上；由于激光点云数据中的点及其附近点集合拟合的平面与激光线束的夹角不能过小，故认为B是一个不可靠的激光返回点，不选择它作为特征点因此将其排除。

又如在图4（b）中，实线段是激光可观测的物体，点A在被遮挡区域的边界上(虚线段)可以被检测到的边缘点；然而，如果从不同的角度观察，被遮挡的区域可以改变并变得可观察到，因此不把A作为显著边点，也不选择A作为特征点，将其排除。

S203、使用基于特征的Scan-to-Scan匹配方法将异常值排除后的激光点云数据进行特征点匹配，获取边缘线和平面；

本实施例中，设第 k-1帧的激光点云数据为，第 k帧的激光点云数据为；基于这两个激光点云数据来寻找它们之间的对应关系；

在中找到与中边缘点 i最近的边缘点 j，再在另外的激光线束上找到距离边缘点 j最近的边缘点 l，连接边缘点 j和边缘点 l得到直线 jl构成边缘线，并在中找到三个与中面点 i最近的面点 j，在同一激光线束上找到距离面点 j最近的面点 l，再在另外的激光线束上找到距离面点 j最近的面点 m，构成平面 jlm。

S204、计算边缘点到边缘线的点线残差和边缘点到平面的点面残差；

本实施例中，记中边缘点 i到中直线 jl的距离为点线残差 d _ε ，中面点 i到中 jlm平面的距离为点面残差 d _ω ，则点线残差 d _ε 的计算公式为：

，

点面残差 d _ω 的计算公式为：

，

其中，表示激光雷达坐标系下第 k-1帧激光点云数据中第 j个点的坐标；表示激光雷达坐标系下第 k-1帧激光点云数据中 l个点的坐标；表示激光雷达坐标系下第 k-1帧激光点云数据中 m个点的坐标；

S205、计算残差总和，通过对残差总和构建最小二乘法求得极值，使用迭代法求解使得残差总和最小，并根据激光点云数据进行位姿估计，求得激光雷达下的机器人位姿。

本实施例中，残差总和 d= d _ε + d _ω ；使用点线残差和点面残差构建与激光雷达坐标系下两帧之间位姿变换矩阵 T ^L 的方程，表示为：

，

其中， f _ε ()表示点线残差的计算式； f _ω ()表示点面残差的计算式；表示激光雷达坐标系下第 k-1帧与第 k帧之间机器人的位姿变换矩阵 T ^L ；

使用残差总和构建最小二乘问题，表示为：

，

其中， F( x)表示最小二乘问题的目标函数；

使用迭代法对最小二乘问题求极值：给定初始值 x ₀的坐标，对于第 k-1次迭代，寻找增量，使得达到极小值；若足够小则停止，得到极小值 F( x)下的变换矩阵；否则令，继续下一次迭代；其中，表示激光雷达坐标系下第 k-1帧激光点云数据中第 i个点的坐标，表示在取得极小值 F( x)下的变换矩阵；

通过极小值下的变换矩阵求得第 k帧时激光雷达下的机器人位姿估计值，公式为：

，

其中，表示激光雷达里程计第 k帧时输出的在世界坐标系下的机器人位姿估计值。

S3、根据摄像机的频率，对激光雷达下的机器人位姿进行三次样条插值处理，利用 n-1个三次函数线段拟合出 n个时间帧的激光雷达输出，对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳；

本实施例中，假设获得的激光雷达下的机器人位姿有 n个时间点的状态，共 n-1个段，插入3次多项式，以光滑的线段连接，并使其二阶导数连续的方法称为三次样条插值法；则对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳，具体为：

S301、根据样条插值假设条件，构建样条函数方程组；

样条函数是用于拟合出机器人的位姿曲线，通过得到位姿估计曲线，然后运用插值方法得到与摄像机同一时间戳下的机器人位姿估计值，样条函数有以下四个假设条件：

条件1：样条函数穿过所有已知节点；

条件2：在所有节点(除了第一节点和最后一个节点)处0阶连续(保证数据不间断，无跳变)，即前一段方程在节点处的函数值和后一段方程在相同节点处的函数值相等；

条件3：在所有节点（除了第一节点和最后一个节点）处1阶连续（保证节点处有相同的斜率，原函数曲线上没有剧烈的跳变）；

条件4：在所有节点（除了第一节点和最后一个节点）处2阶连续（保证节点处有相同的曲率，即相同的弯曲程度）。

根据上述四个假设条件，利用已知的历史激光雷达的机器人位姿数据可以构建出 n-1段样条函数 S _i ( x)，表示为：

S _i ( x) = a _i + b _i ( x- x _i ) + c _i ( x- x _i )²+ d _i ( x- x _i )³ ,i= 1 ,2 ,…,n-1，

根据样条函数性质，列出四个等式约束，令，，求得样条函数 S _i ( x)的曲线参数 a _i 、 b _i 、 c _i 、 d _i ，等式约束表示为：

，

其中， x表示样条函数自变量为时间， x _i 表示第 i帧激光点云数据对应的时间， n表示激光点云数据的个数或者长度， h _i = x _i+1- x _i ， y _i 表示第 i帧时激光雷达下的机器人位姿估计值；

S302、根据已知的历史激光雷达的机器人位姿数据，对等式约束进行求解得到所有参数，利用三次样条函数来拟合机器人运动的轨迹关系；

假设摄像机的采样频率为 f _c ，激光雷达的采样频率为 f _L 由于激光雷达的采样第 k帧与摄像机采样的第 k帧对应的时间不同，所以不能直接合并利用，因此

S303、使用摄像机的采样频率对该拟合函数进行插值处理，插值出对应时刻的激光雷达的机器人位姿，实现摄像机和激光雷达数据的同步生成，对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳，具体为：

利用样本函数 S _i ( x)分别插值得到时间点为、、…、下的函数值， i=1 ,2 ,… ,l，其中 l为同一段时间内摄像机获得的帧数；

插值后得到对应时刻的激光雷达的机器人位姿，即表示相机第 i帧时对应的激光雷达的机器人位姿。

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

本实施例中的双线程回环检测包括视觉回环检测和激光雷达回环检测，其中视觉回环检测采用词袋模型（已训练好）进行检测；激光雷达回环检测采用Scan-Context描述子以及相似距离来进行检测；如图5所示，双线回环检测的具体步骤为：

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

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

，

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

S403、当两帧图像的特征矩阵完全相同时，则输出1；完全不同时则输出0；将当前帧的特征矩阵与每层中间节点的聚类中心比较 d次，即可找到最后对应的帧数，保证了对数级别的查找效率；在根据当前帧与历史帧的相似度和设定阈值进行比较，判断当前帧图像是否发生回环现象；本实施例中当前帧与历史帧的相似度大于0.7这个阈值时，则认为当前帧发生了回环现象。

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

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

，

其中， I ^q 为当前帧激光点云数据， I ^c 为历史帧激光点云数据，为当前帧激光点云数据的Scan-Context描述子矩阵中第 j列的元素值，为历史帧激光点云数据的Scan-Context描述子矩阵中第 j列的元素值；本实施例中的历史帧是指执行本方法后得到的所有关键帧，会存储下来用于进行回环检测。

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

S407、将视觉回环检测与激光雷达回环检测的结果输入过滤器中，当检测结果均为当前帧的回环帧时，认定当前帧发生回环现象，有效避免不是回环帧判定情况的发生，克服SLAM系统建图不准确的问题；同时对于未检测到的回环现象，可通过多次检测判断出该回环现象。

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

具体的，本实施例中已知SLAM中的机器人状态方程为：

，

其中 A为状态转移矩阵， B为控制矩阵， H为观测矩阵， W∼ N(0, Q)是过程噪声， V∼ N(0, R)是观测噪声， X _k 为 k时刻的机器人位姿， Z _k 为 k时刻的机器人位姿估计值， U _k-1为 k-1时刻的控制输入， W _k 为 k时刻的过程噪声， V _k 为 k时刻的观测噪声值；同时假设机器人初始状态以及每一时刻的噪声是互相独立的，且帧数以摄像机采样频率取得的帧数为准；则如图7所示卡尔曼滤波器的流程具体为：

S501、根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，对第一帧进行判断，若第一帧是第一次进行检测，则使用时摄像机下的机器人位姿与激光雷达下的机器人位姿的平均值对卡尔曼滤波器进行初始化，初始值表示为且；为摄像机下的机器人位姿与激光雷达下的机器人位姿的平均值；

S502、根据上一时刻的机器人位姿估计值，预测当前时刻机器人位姿的先验估计值，公式为：

，

S503、计算先验估计值的协方差：

，

S504、由于先验估计与实际值存在误差，通过更新卡尔曼增益系数 K _k ，使得误差最小：

，

S505、根据卡尔曼增益系数计算出当前时刻机器人位姿的后验估计值以及协方差矩阵 P _k ，公式为：

，

其中， P _k-1为 k-1帧的协方差矩阵， A ^T 为状态转移矩阵的转置， Q为过程噪声协方差， R为测量噪声协方差， H ^T 为观测矩阵的转置；

本发明中将视觉里程计输出的摄像机下的位姿结果与激光雷达里程计输出的激光雷达下的机器人后结果进行融合使用，将上一帧的位姿估计平均值作为上一帧机器人的位姿估计值，入到卡尔曼滤波器中，同时利用噪声相互独立的条件以及噪声的协方差Q、R，通过机器人的状态方程粗略计算当前时刻的先验估计值，然后更新卡尔曼增益系数 K _k ，计算出当前时刻机器人位姿的后验估计值以及协方差矩阵 P _k ，得到当前时刻误差最小的机器人位姿；最后将计算结果的后验估计值与协方差矩阵保存，用于下次的迭代计算，降低了不同传感器的相互协同位姿估计的误差，提高了SLAM系统定位建图的鲁棒性。

需要说明的是，对于前述的各方法实施例，为了简便描述，将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其它顺序或者同时进行。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (9)

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列的元素值；

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

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

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

2.根据权利要求1所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述ORB特征点包括FAST角点和BRIEF描述子；

所述使用视觉里程计提取图像中的ORB特征点，判断关键帧并与ORB特征点进行特征匹配获得匹配点对，具体为：

将摄像机采集的图像输入视觉里程计中，通过对图像上的像素点进行阈值比较，提取FAST角点，同时引入图像金字塔对每一层金字塔图像进行FAST角点提取；

比对FAST角点附近的两个随机像素，构成描述子向量得到BRIEF描述子；

根据摄像机的移动距离是否超过阈值距离来确定图像是否为关键帧；

对关键帧的ORB特征点进行距离计算，通过暴力匹配法进行特征匹配得到匹配点对。

3.根据权利要求2所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述视觉里程计通过在图像上选取像素点p，计算该像素点的亮度I _p ，以像素点p为中心点，选取半径为3的圆上的像素点，若该圆上12个连续的像素点亮度与中心点p亮度的差值大于阈值，则认定像素点p为FAST角点；

引入图像金字塔对图像进行不同层次的下采样，得到多层不同分辨率的图像，并对每一层图像进行FAST角点提取；

对于每一FAST角点，随机选取FAST角点附近128组像素点对p、q，比较每一组像素点p和像素点q的大小并编码为二进制描述向量，若p大于q则取1，否则取0，得到的128个描述向量构成该FAST角点的BRIEF描述子；

所述确定图像是否为关键帧，具体为：

获取图像的摄像机移动距离，并与阈值距离进行比较；

若图像的摄像机移动距离大于阈值距离，则判定该图像为关键帧。

4.根据权利要求3所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述通过暴力匹配法进行特征匹配得到匹配点对，具体为：

设图像中第t帧提取到的第m个ORB特征点表示为x _t,m ，m=1,2,…,M，M表示第t帧中提取到的ORB特征点的个数，第t+1帧提取到的第n个ORB特征点表示为x _t+1,n ，n= 1,2,…,N，N表示第t+1帧中提取到的ORB特征点的个数；

采用欧氏距离测量第t帧中ORB特征点的BRIEF描述子与第t+1帧中ORB特征点的BRIEF描述子之间的距离，对齐排序，选取距离最近的一个作为匹配点对，计算公式为：

，

其中，n表示ORB特征点的BRIEF描述子的长度，x _t,i,k 表示图像中第t帧提取到的第i个ORB特征点的BRIEF描述子中的第k个元素，x _t+1,j,k 表示图像中第t+1帧提取到的第j个ORB特征点的BRIEF描述子中的第k个元素。

5.根据权利要求4所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，根据获得的匹配点对，使用PnP法对摄像机进行位姿估计，并使用最小化投影求解，获得摄像机下的机器人位姿，具体为：

设图像在投影空间中某点的坐标为P _i  = [X _i ,Y _i ,Z _i ] ^T ，该点投影的像素坐标为U _i  = [u _i , v _i ] ^T ，将投影关系转换为齐次坐标系下表示，转换式为：

，

其中，Z _i 表示图像中的特征点在世界坐标系下的深度值，K表示摄像机的内参矩阵，T _C,W 表示世界坐标系与相机坐标系之间的转换矩阵；

通过转换式求得变换矩阵T _C,W ，构建位姿估计中第i个点的误差项，表示为：

，

把位姿估计的误差项进行求和，构建最小二乘问题，寻找误差最小时摄像机下的机器人位姿变换矩阵，公式为：

，

其中，n表示图像中提取出的ORB特征点数量，P _i 为第i个ORB特征点在世界坐标系下的坐标， 表示通过变换矩阵得到的第i个ORB特征点在世界坐标系下的估计坐标；

根据机器人位姿变换矩阵求得第k帧时摄像机下的机器人位姿估计值，公式为：

，

其中，表示视觉里程计第k帧时输出的在世界坐标系下的机器人位姿估计值。

6.根据权利要求5所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述对激光雷达进行位姿估计求得激光雷达下的机器人位姿，具体为：

将激光雷达采集的图像的激光点云数据输入激光雷达里程计中，计算每一帧激光点云数据中每个点的曲率，并对每个点的曲率进行排序，选择曲率最大的2个点作为边缘点，曲率最小的4个点作为平面点；

对激光点云数据进行异常值排除；

使用基于特征的Scan-to-Scan匹配方法将异常值排除后的激光点云数据进行特征点匹配，获取边缘线和平面；

计算边缘点到边缘线的点线残差和边缘点到平面的点面残差；

计算残差总和，通过对残差总和构建最小二乘法求得极值，使用迭代法求解使得残差总和最小，并根据激光点云数据进行位姿估计，求得激光雷达下的机器人位姿。

7.根据权利要求6所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述曲率的计算公式为：

，

其中，C表示曲率，表示激光雷达坐标系下第k帧激光点云数据中第i个点的坐标，S表示激光点云数据的集合；

所述异常值包括激光点云数据中平行于激光扫描平面的点和遮挡区域边界的点；

所述获取边缘线和平面，具体为：

设第k-1帧的激光点云数据为，第k帧的激光点云数据为；

在中找到与中边缘点i最近的边缘点j，再在另外的激光线束上找到距离边缘点j最近的边缘点l，连接边缘点j和边缘点l得到直线jl构成边缘线，并在中找到三个与中面点i最近的面点j，在同一激光线束上找到距离面点j最近的面点l，再在另外的激光线束上找到距离面点j最近的面点m，构成平面jlm；

记中边缘点i到中直线jl的距离为点线残差d _ε ，中面点i到中jlm平面的距离为点面残差d _ω ，则点线残差d _ε 的计算公式为：

，

点面残差d _ω 的计算公式为：

，

其中，表示激光雷达坐标系下第k-1帧激光点云数据中第j个点的坐标；表示激光雷达坐标系下第k-1帧激光点云数据中l个点的坐标；表示激光雷达坐标系下第k-1帧激光点云数据中m个点的坐标；

所述残差总和d = d _ε  + d _ω ；

使用点线残差和点面残差构建与激光雷达坐标系下两帧之间位姿变换矩阵T ^L 的方程，表示为：

，

其中，f _ε ()表示点线残差的计算式；f _ω ()表示点面残差的计算式；表示激光雷达坐标系下第k-1帧与第k帧之间机器人的位姿变换矩阵T ^L ；

使用残差总和构建最小二乘问题，表示为：

，

其中，F(x)表示最小二乘问题的目标函数；

使用迭代法对最小二乘问题求极值：给定初始值x ₀的坐标，对于第k-1次迭代，寻找增量，使得达到极小值；若足够小则停止，得到极小值F(x)下的变换矩阵；否则令，继续下一次迭代；其中，表示激光雷达坐标系下第k-1帧激光点云数据中第i个点的坐标，表示在取得极小值F(x)下的变换矩阵；

通过极小值下的变换矩阵求得第k帧时激光雷达下的机器人位姿估计值，公式为：

，

其中，表示激光雷达里程计第k帧时输出的在世界坐标系下的机器人位姿估计值。

8.根据权利要求7所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述根据摄像机的频率，对激光雷达下的机器人位姿进行三次样条插值处理，利用n-1个三次函数线段拟合出n个时间帧的激光雷达输出，通过插值对齐摄像机下的机器人位姿与激光雷达下的机器人位姿的时间戳，具体为：

根据样条插值假设条件，利用已知的历史激光雷达的机器人位姿数据构建出n-1段样条函数S _i (x)，表示为：

S _i (x) = a _i  + b _i (x - x _i ) + c _i (x - x _i )² + d _i (x - x _i )³ ,i = 1,2,…,n-1，

根据样条函数性质，列出四个等式约束，令，，求得样条函数S _i (x)的曲线参数a _i 、b _i 、c _i 、d _i ，等式约束表示为：

，

其中，x表示样条函数自变量为时间，x _i 表示第i帧激光点云数据对应的时间，n表示激光点云数据的个数或者长度，h _i = x _i+1 - x _i ，y _i 表示第i帧时激光雷达下的机器人位姿估计值；

假设摄像机的采样频率为f _c ，激光雷达的采样频率为f _L ，根据已知的历史激光雷达的机器人位姿数据，利用三次样条函数来拟合机器人运动的轨迹关系，再使用摄像机的采样频率对该拟合机器人运动的轨迹关系进行插值处理，即利用样本函数S _i (x)分别插值得到时间点为、、…、下的函数值，i=1,2,…,l，其中l为同一段时间内摄像机获得的帧数；插值后得到对应时刻的激光雷达的机器人位姿，即表示相机第i帧时对应的激光雷达的机器人位姿，实现摄像机和激光雷达数据的同步生成。

9.根据权利要求8所述的基于激光点云和视觉SLAM的机器人位姿估计方法，其特征在于，所述根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，融合摄像机下的机器人位姿及激光雷达下的机器人位姿得到机器人位姿，具体为：

已知SLAM中的机器人状态方程为：

，

其中A为状态转移矩阵，B为控制矩阵，H为观测矩阵，W∼N(0,Q)是过程噪声，V∼N(0,R)是观测噪声，X _k 为k时刻的机器人位姿，Z _k 为k时刻的机器人位姿估计值，U _k-1为k-1时刻的控制输入，W _k 为k时刻的过程噪声，V _k 为k时刻的观测噪声值；

假设机器人初始状态以及每一时刻的噪声是互相独立的；

根据回环检测结果，将摄像机下的机器人位姿与激光雷达下的机器人位姿输入卡尔曼滤波器中，对第一帧进行判断，若第一帧是第一次进行检测，则使用第一帧时摄像机下的机器人位姿与激光雷达下的机器人位姿的平均值对卡尔曼滤波器进行初始化，初始值表示为且；为摄像机下的机器人位姿与激光雷达下的机器人位姿的平均值；

根据上一时刻的机器人位姿估计值，预测当前时刻机器人位姿的先验估计值，公式为：

，

计算先验估计值的协方差：

，

由于先验估计与实际值存在误差，通过更新卡尔曼增益系数K _k ，使得误差最小：

，

根据卡尔曼增益系数计算出当前时刻机器人位姿的后验估计值以及协方差矩阵P _k ，公式为：

，

其中，P _k-1为k-1帧的协方差矩阵，A ^T 为状态转移矩阵的转置，Q为过程噪声协方差，R为测量噪声协方差，H ^T 为观测矩阵的转置，表示当前时刻机器人位姿的后验估计值，即当前时刻误差最小的机器人位姿；

将计算结果的后验估计值与协方差矩阵保存，以便于下次的迭代计算。

Images