[发明专利]一种基于激光SLAM与单目视觉SLAM融合的里程计方法

权利要求书

1.一种基于激光SLAM与单目视觉SLAM融合的里程计方法，其特征在于，包括以下步骤：

步骤1，视觉数据与激光数据的采集

机器人安装单目相机以及激光雷达传感器，通过单目相机获取RGB图像数据；三维激光雷达获取点云数据，工控机实时读取上述传感器数据；

步骤2，通过激光点云赋予特征点深度

对单目相机采集的RGB图像数据进行特征提取，在特征提取后进行特征匹配，同时将激光雷达采集的点云投影在RGB图上，在特征点周围选取方形区域，提取区域内各个点云的深度值，通过对各个点云深度值进行插值计算，从而得到该特征点的深度；

步骤3，通过判断特征点深度情况估计先验位姿

在估计帧与帧之间的先验位姿时，先对前一帧的中特征点的深度信息进行判断：①若前一帧中的特征点全部具有深度信息，则与当前帧中全部的特征点构建PNP(Perspective-n-point)问题从而计算帧间位姿；②若前一帧中存在不具备深度信息的特征点，采用前一帧中有深度特征点与当前帧中对应的特征点构建PNP(Perspective-n-point)问题，提取该位姿中的平移向量作为两帧之间估计位姿的位移向量；同时，对两帧中所有的特征点构建对极几何问题，提取该位姿中的旋转矩阵作为两帧之间估计位姿的旋转矩阵；③当前一帧中的特征点均没有深度信息时，对两帧中所有的特征点构建对极几何问题，提取该位姿中的旋转矩阵作为两帧之间估计位姿的旋转矩阵；并采用对极几何的方式计算位移向量作为两帧之间的估计位姿的位移向量；

步骤4，后端BundleAdjustment优化

在得到初始先验位姿后，通过后端的BundleAdjustment优化得到最终的优化后位姿。

2.根据权利要求1所述的基于激光SLAM与单目视觉SLAM融合里程计方法，其特征在于：所述步骤1中，单目相机获取RGB图像数据，激光雷达通过扫描获得具有坐标数值、角度、距离信息的激光点云数据。

3.根据权利要求1所述的基于激光SLAM与单目视觉SLAM融合里程计方法，其特征在于：所述步骤2中，在本方法中，利用图像特征点作为路标；提取ORB特征点，并通过描述子的差异来完成特征匹配；从单目相机采集的RGB图像中提取的特征点并不具有深度信息，此时将激光点云投影到RGB图像上，在特征点周围选取方形区域并提取区域内点云的深度值，通过插值计算得到特征点的深度信息。

4.根据权利要求1所述的基于激光SLAM与单目视觉SLAM融合里程计方法，其特征在于：所述步骤3中，对于前一帧中特征点的深度信息进行判断后；

①若前一帧中的特征点全部具有深度信息，则与当前帧中的特征点构建PNP(Perspective-n-point)问题从而计算估计位姿T₁；

②若前一帧中特征点不全具有深度信息时，则采用具有深度信息的特征点和当前图像帧中对应的特征点构建PNP问题方式求解位姿T₂′，取T₂′中的位移部分t₂作为估计位姿T₂中的位移向量，并对所有特征点构建对极几何问题求解位姿T₂′′，取T₂′′中的旋转部分R₂作为估计位姿T₂中的旋转矩阵，t₂与R₂构成实际估计位姿T₂；

0＝X_iK^-Tt^RK^-1X_i-1

式中X_i-1为前一帧中所有特征点，X_i为当前帧中所有特征点；

③若前一帧中的特征点均不具有深度信息时，则采用对两帧中所有特征点进行构建对极几何问题的方式求解位姿T₃′，取T₃′中的旋转部分R₃作为估计位姿T₃中的旋转矩阵，T₃中的位移向量为：

式中t为T₃′中的位移向量，d为两帧中特征点的实际距离，取t₃和R₃构成实际估计位姿T₃。