[发明专利]一种基于语义ORB-SLAM技术的视觉定位方法及系统

权利要求书

1.一种基于语义ORB-SLAM技术的视觉定位方法，其特征在于，该方法包括以下步骤：

(1)对图像点云数据特征追踪处理，采用相机对移动机器人所在环境进行扫描，提取环境中ORB特征点，计算特征点描述子及特征点对应3D点坐标，分布8层金字塔，同时生成关键帧信息，计算3D点投影到关键帧中的地图点坐标和相机粗略位姿；

(2)对图像点云数据关键帧坐标、地图点坐标、相机坐标、移动机器人位姿，采用高斯牛顿法进行非线性图优化处理，并更新优化后的坐标；

(3)对图像点云数据回环候选帧及回环候选帧的临近帧进行位置识别，计算与当前帧的相似度得分，取得分超过阈值的作为可靠回环候选帧；

(4)对图像点云数据可靠回环候选帧进行几何验证，通过特征匹配，计算回环候选帧地图点坐标，相机位姿，与当前帧地图点，相机位姿融合并更新当前帧及临近帧坐标；

(5)对图像点云数据坐标经非线性优化的，位置经回环检测后的地图点、关键帧、相机位姿加入局部地图，再将局部地图不断拼接融合得到全局地图；

(6)对地图进行语义分割与目标识别，通过相机加载模型，实时推理分类，得出输入分类范畴的地标，并计算地标相对相机坐标系下坐标，同时在全局地图分割地标，加入标签融合全局地图，上传云端服务器；

(7)进行位姿匹配，当机器人在已建图环境中重新启动，通过新扫描到的地标，与云端服务器全局地图中地标匹配，获取各分类的地标信息，由地标信息定位出机器人在世界坐标系下位姿。

2.根据权利要求1所述的一种基于语义ORB-SLAM技术的视觉定位方法，其特征在于，步骤(1)具体过程如下：

步骤1-1、以移动机器人开机所在位置为坐标系原点，根据RGB-D相机扫描到的点云数据提取ORB特征，在图像中选取像素p，设亮度为I_p，以I_p的20％为阈值T，以像素p为中心，根据需求设定半径，选取圆上的若干个像素点，如果圆上至少有连续11个点的亮度大于I_p+T或小于I_p-T，像素p就是一个特征点，然后使用灰度质心法计算特征点方向，增加旋转不变性。

步骤1-2、根据以下公式计算3D点投影到图像坐标系下特征点坐标：

其中，(u,v)为图像坐标系下特征点坐标，(x,y,z)为环境中3D点在相机坐标系的坐标，(f_x,f_y)为像素坐标系与成像平面之间的关系，代表了u轴与v轴上的缩放，(c_x,c_y) 是相机光学中心点，为相机内参，记为K，属于相机内置参数，可从相机配置中直接读取，不可更改。

步骤1-3、将从拍摄背景到成像平面之间的空间分为8层金字塔，引入尺度scale概念，实现尺度不变性，将图片以1.2：1的比例进行缩放，然后将所有特征点分配到对应的层，按照以特征为中心，11.25°步进的方式将圆周分32等份，循环16次，共选取256个点对，比较点对像素大小关系，并以0和1表示，组成特征描述向量；

步骤1-4、对超过阈值数量1000个特征点的图像，以相机位姿、关键点以及地图点组成关键帧，并将其按照时间先后顺序存放在关键帧序列。如果两个关键帧KP1和KP2观测到的相同关键点数量超过15个，认为KP1与KP2具有共视关系，便生成以KP1、KP2为节点，相同关键点数量为边的共视图，所述地图点即与关键帧特征点id对应的3D空间点在该图像上的投影，所述关键帧同时作为回环检测的候选帧；

步骤1-5、更新地图点平均观测方向以及地图点距离相机光心的距离，遍历观测到的关键帧，获取不同光心坐标向量O_wi，定义观测方向nomal为关键点在世界坐标系下的坐标向量减去相机光心向量：

nomal＝p_w-O_wi

式中，p_w为关键点世界坐标。

步骤1-6、采取方向光流方法删除冗余关键帧，如果当前关键帧距离前一个关键帧未超过20帧，剔除该关键帧，如果当前关键帧中特征点数量少于1000，剔除该关键帧，如果当前关键帧中90％的地图点可以被其他关键帧看到，剔除该关键帧；

步骤1-7、按照在两个不同关键帧中相同特征点数量由大到小的顺序，获取前10个与当前帧存在相同特征点的其他关键帧，记为共视关键帧，遍历共视关键帧，针对PnP问题使用RANSAC随机采样一致性算法和EPnP算法，在RANSAC算法框架下迭代使用EPnP求解位姿T_cw，在3D-2D匹配点中随机选取4个匹配点对，根据这4个匹配点对使用EPnP算法求得一个粗略的位姿T_cw，在所有3D-2D点中的3D点重投影为2D点，并计算重投影误差，根据误差阈值大小将点分为局内点和局外点，剔除局外点，采用局内点迭代按照如下公式求解基础矩阵F和单应矩阵H：

p₂＝Hp₁

其中，p₁,p₂为两个像素点的像素坐标，所述基础矩阵即是同一空间点在不同相机视角下的投影到图像上的点的坐标关系，所述单应矩阵用于描述处于共同平面上的一些点投影到两张不同图像之间的变换关系，最后选取获得误差最小的位姿作为最终的位姿估计。