CN113537208A - 一种基于语义orb-slam技术的视觉定位方法及系统 - Google Patents

Abstract

本发明公开了一种基于语义ORB‑SLAM技术的视觉定位方法及系统，包括车载设备、云端服务器、地标；车载控制单元包括RGB‑D相机数据处理模块、语义识别与分割模块、位姿匹配模块、移动控制模块；通过相机数据处理模块对环境特征点提取与匹配、位姿推算、局部与非线性图优化得到全局地图，并提供地图保存与加载接口，实现AGV视觉定位技术；目标检测与匹配模块对已训练好的模型加载并推理，实现像素级语义分割，并计算地标信息，将语义标签和信息保存到全局地图；当AGV需要纯定位时，云端提供标签地标信息，再经过位姿匹配模块计算得到精确位姿；控制模块可全程控制机器人移动，辅助传感设备对环境充分扫描，实时输出移动机器人位姿。

Description

一种基于语义ORB-SLAM技术的视觉定位方法及系统

技术领域

本发明涉及移动机器人领域，尤其涉及一种结合深度学习的移动机器人视觉定位方法及系统。

背景技术

目前各种移动机器人已经应用在智能自动化行业，并发挥者非常重要的作用。又随着计算机视觉的高速发展，采用相机代替传统激光作为AGV的数据输入传感器不仅大大降低了成本，也使得AGV功能更加丰富与智能。然而要使机器人稳定工作，且具有导航、调度、避障等功能，必须依赖于精准、高效且可靠的定位技术。在降低成本的同时进一步追求提高机器人定位精度与稳定性，机器人智能性、可靠性也是今后移动机器人发展的必然趋势。因此，本发明的目的是为了解决机器人实时探索环境构建语义地图，而后加载地图后实时输出自身坐标，即使在环境特征点不足的情况下，依然能够通过本发明的重定位技术在降低视觉相机计算量的同时能精准有效地定到自身坐标。

发明内容

本发明的目的是为了解决机器人实时探索环境构建语义地图，而后加载地图后实时输出自身坐标，即使在环境特征点不足的情况下，依然能够通过本发明的重定位技术在降低视觉相机计算量的同时能精准有效地定到自身坐标。

本发明的目的是通过以下技术方案来实现的：一种基于语义ORB-SLAM技术的双目视觉定位方法，该方法包括以下步骤：

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

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

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

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

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

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

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

进一步地，步骤(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₂为两个像素点的像素坐标，所述基础矩阵即是同一空间点在不同相机视角下的投影到图像上的点的坐标关系，所述单应矩阵用于描述处于共同平面上的一些点投影到两张不同图像之间的变换关系，最后选取获得误差最小的位姿作为最终的位姿估计。

进一步地，步骤(2)具体过程如下：

步骤2-1、通过步骤(1)得到关键帧中地图点坐标，地图点对应的3D点坐标，相机拍摄该关键帧时的位姿，获取能观测到该地图点的所有关键帧，及该关键帧的临近关键帧，作为候选帧，所述临近帧即与当前候选帧有共同观测点的帧；

步骤2-2、在候选帧的临近帧通过投影匹配将描述子按位异或，异或结果中1的个数和为汉明距离，找到汉明距离最小地图点；

步骤2-3、以汉明距离最小地图点位姿为顶点，投影误差为边，加入图优化求最优解，得到旋转矩阵R和平移向量t，所述图优化即采用开源g2o图优化库通过高斯牛顿法迭代计算，使顶点与顶点之间边的误差最小最优的方法，根据步骤1-7，考虑从1到 n(n＞1)时刻得到的机器人坐标x，特征点对应的3D点坐标y为

x＝{x₁，...，x_n}，y＝{y₁，...，y_n}

其中，x_i(1＜i＜n)为i时刻机器人坐标，y_i(1＜i＜n)为i时刻机器人观测到的3D点坐标，根据微分中值公式：

f(x+Δx)≈f(x)+J(x)^TΔx

其中，Δx为引入的增量，J(x)^T为f(x)关于x的导数，是n×1的向量，目标函数为寻求最优增量Δx，使得||f(x+Δx||²最小，根据线性最小二乘法得：

令Δx的导数为零：

J(x)J(x)^TΔx＝-J(x)f(x)

给定初值机器人坐标x，迭代10次即可得到最优增量Δx，也就得到了最优机器人位姿x+Δx；

步骤2-4、遍历地图点通过优化前的旋转矩阵R和平移向量t投影到关键帧上，再用优化后的旋转矩阵R和平移向量t反向投影到3D坐标点，来更新地图点位置；

步骤2-5、以新的地图点信息更新特征点信息，关键帧信息。

进一步地，步骤(3)具体过程如下：

步骤3-1、遍历当前帧的共视关键帧，计算与当前帧的相似度得分，假设当前帧词袋向量为v_c，共视图中一个关键帧的词袋向量为v_i，则相似度得分：

s(v_c，v_i)是v_c与v_i的相似度得分；

步骤3-2、取步骤3-1中最小得分s_min为最小基准，大于s_min的关键帧设为回环候选关键帧，等待进一步检测；

步骤3-3、根据步骤3-2中得到的回环候选关键帧，找到与当前帧有共同单词的关键帧，共同单词最多的关键帧单词数量记为commonwords_max，定义共同单词最少的关键帧单词数量commonwords_min＝0.8*commonwords_max，剔除共视单词数量小于commonwords_min的关键帧以及不在当前帧的共视图中关键帧；

步骤3-4、再将剩余回环候选关键帧具有相同特征点的归为一组，计算每组相似性得分，最高得分记为AccScore_best，定义Score_min＝0.75*AccScore_best，进一步剔除小于Score_min的孤立回环候选关键帧，得到可靠的回环候选关键帧。

进一步地，步骤(4)具体过程如下：

步骤4-1、遍历步骤3-4得到的可靠的回环候选关键帧；

步骤4-2、采用RANSAC算法对当前帧与回环候选关键帧进行特征匹配，确定匹配数量和对应地图点；

步骤4-3、对当前帧和回环候选关键帧的匹配点对构造求解器，迭代5次求解相对位姿sim3变换，所述sim3变换就是使用n对回环帧和候选帧地图点对来进行相似变换的求解，得到当前帧与候选帧地图点之间的旋转矩阵R、平移向量t和尺度s；

步骤4-4、根据步骤4-3的结果，将地图点从当前帧投影到回环候选关键帧，根据地图点模长预测尺度信息，由此得到匹配半径，在投影点匹配半径内查找最优匹配，记录id，再从回环候选关键帧重投影到当前帧，以同样的方法比较两次重投影匹配的点对是否一致；

步骤4-5、根据关键帧中的特征点数量与阈值条件，过滤特征点数量低于阈值的关键帧，得到最终的回环帧；

步骤4-6、根据步骤4-5得到的回环帧，估算当前帧位姿，更新共视图所有关键帧位姿，然后矫正当前帧和共视图关键帧对应的所有地图点，融合重复地图点，最后再更新当前帧的共视关键帧，确保融入了新的回环帧；

步骤4-7、开启新的线程按照步骤(2)，对当前时刻之前的所有拍摄关键帧的相机位姿执行全局非线性优化。

进一步地，步骤(5)具体过程如下：

步骤5-1、检查关键帧序列中是否有新的关键帧，如有新的关键帧，将关键帧插入局部地图，所述局部地图为步骤(1)中保存的关键帧集合，地图点集合以及相机位姿集合，剔除地图中被所有其他关键帧观测到的比例低于0.25的地图点，以及当前地图点被观测次数少于3次和连续三帧未再被观测到的地图点；

步骤5-2、在共视关键帧及每个共视关键帧的临近帧中查找更多匹配的地图点对，并融合当前帧地图点与共视关键帧地图点，重新计算地图点描述子、平均观测方向以及深度信息；

步骤5-3、按照步骤(2)，对拍摄到共视关键帧的相机位姿，执行局部非线性优化；

步骤5-4、按照步骤1-6中的方法剔除冗余局部关键帧，得到最终的局部地图,再将新的局部地图拼接融合为全局地图。

进一步地，步骤(6)具体过程如下：

步骤6-1、加载已训练好的YOLOv5s网络模型权重文件，配置模型网络参数并创建推理引擎与上下文；

步骤6-2、遍历关键帧序列，对每一帧图像采用锚框方式执行推理，检测到目标物体，输出标签及外框，计算外框中心点的坐标，作为目标的位置信息，所述目标物体即是数据集中的类别。

步骤6-3、关联标签和位置信息，关联标签及关键帧ID，关联标签及外框中的特征点，存入全局地图，并上传云端服务器作为重定位时已知的地标信息。

步骤6-4、在步骤6-4中的基础上将外框及标签按照地标所在关键帧所在金字塔层的缩放比例存储到该标签对应关键帧ID所在的地图中，融合语义与点云地图，构建语义点云地图，上传云端服务器。

进一步地，步骤(7)具体过程如下：当机器人开启重定位，以机器人当前位置为坐标原点O_W，扫描机器人相机面向的环境，启动步骤6所述的目标识别，检测与分割步骤，如果检测到地标，得到对应标签，就得到了该标签对应的关键帧，地标框内的特征点以及地标位置信息，具体步骤如下：

步骤7-1、机器人从云端获取语义地图，并加载；

步骤7-2、机器人在某个世界坐标系下位姿(x_w，y_w，θ)，对应的相机位姿 (x_wc，y_wc，θ_wc)，将检测到地标表示为landmark₁，landmark₂，...，landmark_n，(n＞＝ 2)，n为地标数量，便保存当前图片image_cur，遍历像素点得到landmark_i，i∈(1，2，...，n) 投影到image_cur1下的坐标(u_i，v_i)，再根据步骤1-2公式：

反向求解landmark_i的相对相机坐标系下坐标向量landmark_cli＝(x_cli，y_cli，z_cli)^T，通常机器人只在地面上运行，也就是二维平面，故z_cli可忽略不计，最终landmark_cli＝(x_cli，y_cli)^T；

步骤7-3、然后以地标对应的标签为key检索云端服务器存储的地标key-value便可得到landmark_i，i∈(1，2，...，n)在世界坐标系下坐标landmark_wli＝(x_wli，y_wli)，

步骤7-4、遍历地标i，j(i∈(1，2，...，n-1)，j＝i+1，且j＜＝n)，将其以复数形式表达，得到：

landmark_wli，landmark_wlj为第i，j个地标相对世界坐标系下坐标向量，landmark_cli，landmark_clj为第i，j个地标相对相机坐标系下的坐标向量，x轴与y 轴位置坐标和：

其中，r(pose_i，j)，i(pose_i，j)分别为pose_i，j的实部和虚部。

机器人朝向θ和为：

其中，j是复数单位，α(landmark_wli-pose_i，j)是landmark_wli-pose_i，j的幅角，α(landmark_cli)是landmark_cli的幅角；

步骤7-5、相机位置如下：

(x_wc，y_wc)＝x_sum/n-i-1，y_sum/n-i-1

θ_wc＝norm(α(θ_sum/2*(n-i-1))

其中，α(θ_sum/2*(n-i-1)是θ_sum/2*(n-i-1)的幅角，n是地标的数量；

步骤7-6、进一步将步骤7-5得到的相机位姿pose_wc＝(x_wc，y_wc，θ_wc)，加上相机相对机器人坐标系的安装误差得到机器人在世界坐标系下的坐标，也就是机器人的定位；

步骤7-7、定位到机器人自身位姿后，在语义地图中以当前坐标为圆心，l为半径，提取特征点，与地图中存储的地图点匹配，进行回环检测，非线性优化，得到机器人精确位姿，最后按照调度任务控制机器人移动，匹配相机新扫描到的已探索过的环境和云端地图匹配，输出实时定位位姿。

步骤7-8，如果相机扫描到的地标少于2个，继续移动机器人至扫描到地标数量大于等于2个，重复步骤7-2到步骤7-7。

本发明还提供了一种实现基于语义ORB-SLAM技术的双目视觉定位方法的系统，该系统包括移动机器人车载设备、云端服务器、在普通环境或空旷区域或特征少的区域放置已训练好的可识别地标；所述普通环境为亮度、灰度、色彩可区分，可辨识的不包含已知地标的室内环境；所述特征少的区域为连续3帧提取到的ORB特征点不足100 个的区域。

所述移动机器人车载设备包括RGB-D相机和控制中心单元；

所述控制中心单元包括图像点云数据处理模块、语义识别与分割模块、位姿匹配模块和移动控制模块；

所述图像点云数据处理模块根据RGB-D相机扫描的环境原始点云数据提取ORB特征点，生成关键帧，根据关键帧信息计算描述子，匹配特征点与地图点，通过反向投影得到环境中3D点模糊位姿，经非线性优化得到移动机器人精确位姿，同时构建包括特征点、关键帧、地图点、位姿信息的全局地图；

所述语义识别与分割模块根据地标图片数据集，经YOLOv5s经深度神经网络训练出模型，经过加载推理后对像素语义识别与分割，得出地标信息，构建带有语义信息的全局地图，并将结果上传云端服务器；所述地标图片数据集为一半从已公开EuRoC数据集提取，另一半从实际环境中拍摄，手动添加标签的5种分类5000张图片组成，所述五种分类包括办公桌、办公椅、立方体办公柜、人、叉车托盘五种。

所述位姿匹配模块通过对RGB-D相机将环境的RGB通道视频流转换为连续的灰度图像帧，在图像帧上安照地标图片数据集中已有的5种类别通过模型进行分类推理，当移动机器人需要定位时，与云端服务器已知地标信息对比计算得到移动机器人自身模糊位置，再通过位姿匹配模块对该模糊位置进行非线性优化，得到移动机器人精确位姿；

所述移动控制模块以语义识别与分割模块是否得到地标模糊位置为结果，如过未匹配到目标地标或者匹配到的目标地标数量小于设定阈值，则由移动控制模块控制机器人移动一段距离,再进行位置匹配，直至匹配到的目标地标数量不小于阈值，结束机器人移动，辅助移动机器人获取精确定位。

本发明的有益效果是：本发明提供了一种基于语义ORB-SLAM技术的双目视觉定位方法及系统,仅采用双目实感RGB-D图像传感器、云端服务器、存在于环境中的已知分类的地标，实现特征提取，语义识别，地图构建与重载，实时定位与重定位，将SLAM 与深度学习结合，增加地图的可理解性与人机交互性，减少因特征点少导致定位偏移问题，提高地图加载后重定位失败或重定位耗时严重的问题，大大降低定位时对CPU 的内存占用率，实现快速稳定实时输出移动机器人位姿，极大提高了机器人的智能性，稳定性，可靠性以及工作效率。

附图说明

图1为本发明提供的一种基于语义ORB-SLAM技术的双目视觉定位方法流程图；

图2为本发明中移动机器人示意图；

图3为本发明提供的一种基于语义ORB-SLAM技术的双目视觉定位方法中特征提取流程图；

图4为本发明提供的一种基于语义ORB-SLAM技术的双目视觉定位方法中回环检测流程图；

图5为本发明提供的一种基于语义ORB-SLAM技术的双目视觉定位方法中位姿匹配流程图。

具体实施方式

下面结合附图进一步详细描述本发明的技术方案，但本发明的保护范围不局限于以下所述。

如图1所示，本发明提供了一种基于语义ORB-SLAM技术的双目视觉定位方法，该方法包括以下步骤：

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

步骤1-1、以移动机器人开机所在位置为坐标系原点，根据RGB-D相机扫描到的点云数据提取ORB特征点，在图像中选取像素p，设亮度为I_p，以I_p的20％为阈值T，以像素p为中心，选取半径为4的圆上的16个像素点，如果圆上至少有连续的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₂为两个像素点p的像素坐标，所述基础矩阵即是同一空间点在不同相机视角下的投影到图像上的点的坐标关系，所述单应矩阵通常描述处于共同平面上的一些点投影到两张不同图像之间的变换关系，最后选取获得误差最小的位姿作为最终的位姿估计。

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

步骤2-1、通过步骤(1)得到关键帧中地图点坐标，地图点对应的3D点坐标，相机拍摄该关键帧时的位姿，获取能观测到该地图点的所有关键帧，及该关键帧的临近关键帧，作为候选帧，所述临近帧即与当前候选帧有共同观测点的帧；

步骤2-2、在候选帧的临近帧通过投影匹配将描述子按位异或，异或结果中1的个数和为汉明距离，找到汉明距离最小地图点；

步骤2-3、以汉明距离最小地图点位姿为顶点，投影误差为边，加入图优化求最优解，得到旋转矩阵R和平移向量t，所述图优化即采用开源g2o图优化库通过高斯牛顿法迭代计算，使顶点与顶点之间边的误差最小最优的方法，根据步骤1-7，考虑从1到 n(n＞1)时刻得到的机器人坐标x，特征点对应的3D点坐标y为

x＝{x₁，...，x_n}，y＝{y₁，...，y_n}

其中，x_i(1＜i＜n)为i时刻机器人坐标，y_i(1＜i＜n)为i时刻机器人观测到的3D点坐标，根据微分中值公式：

f(x+Δx)≈f(x)+J(x)^TΔx

其中，Δx为引入的增量，J(x)^T为f(x)关于x的导数，是n×1的向量，目标函数为寻求最优增量Δx，使得||f(x+Δx||²最小，根据线性最小二乘法得：

令Δx的导数为零：

J(x)J(x)^TΔx＝-J(x)f(x)

给定初值机器人坐标x，迭代10次即可得到最优增量Δx，也就得到了最优机器人位姿x+Δx，也就得到了旋转矩阵R和平移向量t；

步骤2-4、遍历地图点通过优化前的旋转矩阵R和平移向量t投影到关键帧上，再用优化后的旋转矩阵R和平移向量t反向投影到3D坐标点，来更新地图点位置；

步骤2-5、以新的地图点信息更新特征点信息，关键帧信息。

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

步骤3-1、遍历当前帧的共视关键帧，计算与当前帧的相似度得分，假设当前帧词袋向量为v_c，共视图中一个关键帧的词袋向量为v_i，则相似度得分：

s(v_c，v_i)是v_c与v_i的相似度得分；

步骤3-2、取步骤步骤3-1中最小得分s_min为最小基准，大于s_min的关键帧设为回环候选关键帧，等待进一步检测；

步骤3-3、由步骤步骤3-2中的回环候选关键帧，通过倒排索引技术查询，快速找到与当前帧有共同单词的关键帧，共同单词最多的关键帧单词数量记为 commonwords_max，定义共同单词最少的关键帧单词数量commonwords_min＝0.8* commonwords_max，剔除共视单词数量小于commonwords_min的关键帧以及不在当前帧的共视图中关键帧；

步骤3-4、再将剩余回环候选关键帧具有相同特征点的归为一组，计算每组相似性得分，最高得分记为AccScore_best，定义Score_min＝0.75*AccScore_best，进一步剔除小于Score_min的孤立回环候选关键帧，得到可靠的回环候选关键帧。

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

步骤4-1、遍历步骤3-4得到的可靠回环候选关键帧；

步骤4-2、采用RANSAC算法对当前帧与回环候选关键帧进行特征匹配，确定匹配数量和对应地图点；

步骤4-3、对当前帧和回环候选关键帧的匹配点对构造求解器，迭代5次求解相对位姿sim3变换，所述sim3变换就是使用3对回环帧和候选帧地图点对来进行相似变换的求解，得到当前帧与候选帧地图点之间的旋转矩阵R、平移向量t和尺度s；具体为：已知两个关键帧有n对成功匹配的特征点坐标信息：

{r_l，i}，{r_r,i} (i＝1,2，...，n.)

两帧之间的相似变换有：

r_r＝sR(r_l)+t₀

其中，s为尺度信息，也是所在金字塔层的缩放，R(r_l)为将特征点r_l经旋转矩阵R的旋转，t₀为r_r到r_l的平移向量，构造重投影误差e_i：

e_i＝r_r，i-sR(r_l，i)-t₀

目标函数为重投影误差的平方和，

其中，’代表向量的转置，迭代求解上式，使重投影误差平方和最小，得到最优的s，R和t₀，更新回环候选关键帧。

步骤4-4、根据步骤4-3的结果，进行将地图点从当前帧投影到候选关键帧，根据地图点模长预测尺度信息，由此得到匹配半径，在投影点匹配半径内查找最优匹配，记录id，再从回环候选关键帧重投影到当前帧，以同样的方法比较两次重投影匹配的点对是否一致；

步骤4-5、根据关键帧中的特征点数量与阈值条件，过滤特征点数量低于阈值的关键帧，得到最终的回环帧；

步骤4-6、根据步骤4-5得到的回环帧，估算当前帧位姿，更新共视图所有关键帧位姿，然后矫正当前帧和共视图关键帧对应的所有地图点，融合重复地图点，最后再更新当前帧的共视关键帧，确保融入了新的回环帧；

步骤4-7、开启新的线程按照步骤(2)执行全局光束法平差优化，对当前时刻之前的所有拍摄关键帧的相机位姿执行全局非线性优化。

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

检查关键帧序列中是否有新的关键帧，如有新的关键帧，将关键帧插入局部地图，所述局部地图为步骤(1)中保存的关键帧集合，地图点集合以及相机位姿集合，剔除地图中被所有其他关键帧观测到的比例低于0.25的地图点，以及当前地图点被观测次数少于3次和连续三帧未再被观测到的地图点；

步骤5-2、在共视关键帧及每个共视关键帧的临近帧中查找更多匹配的地图点对，并融合当前帧地图点与共视关键帧地图点，重新计算地图点描述子、平均观测方向以及深度信息；

步骤5-3、按照步骤(2)，对拍摄到共视关键帧的相机位姿，执行局部非线性优化；

步骤5-4、按照步骤1-6中的方法剔除冗余局部关键帧，得到最终的局部地图,再将新的局部地图拼接融合为全局地图。

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

步骤6-1、加载已训练好的YOLOv5s网络模型权重文件，配置模型网络参数；

步骤6-2、创建推理引擎与上下文，包括用C++接口对步骤6-1已配置模型序列化，启动GPU加速，设定模型运行空间大小为8GB，设定相机数据输入路径，数据输出路径，再利用Tensorrt对其反序列化得到引擎与上下文；

步骤6-3、遍历关键帧序列，对每一帧图像采用锚框方式执行推理，检测到目标物体，输出标签及外框，计算外框中心点的坐标，作为目标的位置信息，所述目标物体即是数据集中的类别。

步骤6-4、关联标签和位置信息，关联标签及关键帧ID，关联标签及外框中的特征点，存入全局地图，并上传云端服务器作为重定位时已知的地标信息。

步骤6-5、在步骤6-4中的基础上将外框及标签按照地标所在关键帧所在金字塔层的缩放比例存储到该标签对应关键帧ID所在的地图中，融合语义与点云地图，构建语义点云地图，上传云端服务器。

(7)进行位姿匹配，当机器人在已建图环境中重新启动，通过新扫描到的地标，与云端服务器全局地图中地标匹配，获取各分类的地标信息，由地标信息定位出机器人在世界坐标系下位姿。如图5所示，具体过程如下：当机器人开启重定位，以机器人当前位置为坐标原点O_w，扫描机器人相机面向的环境，启动步骤6所述的目标识别，检测与分割步骤，如果检测到地标，得到对应标签，就得到了该标签对应的关键帧，地标框内的特征点以及地标位置信息，具体步骤如下：

步骤7-1、机器人从云端获取语义地图，并加载；

步骤7-2、机器人在某个世界坐标系下位姿(x_w，y_w，θ)，对应的相机位姿 (x_wc，y_wc，θ_wc)，将检测到地标表示为landmark₁，landmark₂，...，landmark_n，(n＞＝ 2)，n为地标数量，便保存当前图片image_cur，遍历像素点得到landmark_i，i∈(1，2，...，n) 投影到image_cur1下的坐标(u_i，v_i)，再根据步骤1-2公式：

反向求解landmark_i的相对相机坐标系下坐标向量landmark_cli＝(x_cli，y_cli，z_cli)^T，通常机器人只在地面上运行，也就是二维平面，故z_cli可忽略不计，最终landmark_cli＝(x_cli，y_cli)^T；

步骤7-3、然后以地标对应的标签为key检索云端服务器存储的地标key-value便可得到landmark_i，i∈(1，2，...，n)在世界坐标系下坐标landmark_wli＝(x_wli,y_wli)，

步骤7-4、遍历地标i，j(i∈(1，2，...，n-1)，j＝i+1，且j＜＝n)，将其以复数形式表达，得到：

landmark_wli，landmark_wlj为第i，j个地标相对世界坐标系下坐标向量，landmark_cli，landmark_clj为第i，j个地标相对相机坐标系下的坐标向量，x轴与y 轴位置坐标和：

其中，r(pose_i，j)，i(pose_i，j)分别为pose_i，j的实部和虚部。

机器人朝向θ和为：

其中，j是复数单位，α(landmark_wli-pose_i，j)是landmark_wli-pose_i，j的幅角，α(landmark_cli)是landmark_cli的幅角；

步骤7-5、相机位置如下：

(x_wc，y_wc)＝x_sum/n-i-1，y_sum/n-i-1

θ_wc＝norm(α(θ_sum/2*(n-i-1))

其中，α(θ_sum/2*(n-i-1)是θ_sum/2*(n-i-1)的幅角，n是地标的数量；

步骤7-6、进一步将步骤7-5得到的相机位姿pose_wc＝(x_wc，y_wc，θ_wc)，加上相机相对机器人坐标系的安装误差得到机器人在世界坐标系下的坐标，也就是机器人的定位；

步骤7-7、定位到机器人自身位姿后，在语义地图中以当前坐标为圆心，l为半径，提取特征点，与地图中存储的地图点匹配，进行回环检测，非线性优化，得到机器人精确位姿，最后按照调度任务控制机器人移动，匹配相机新扫描到的已探索过的环境和云端地图匹配，输出实时定位位姿。

步骤7-8，如果相机扫描到的地标少于2个，继续移动机器人至扫描到地标数量大于等于2个，重复步骤7-2到步骤7-7。

本发明还提供了一种基于ORB-SLAM技术的双目视觉定位系统，包括移动机器人车载设备、云端服务器，在普通环境或空旷区域或特征少的区域放置已训练好的可识别地标。所述普通环境为亮度、灰度、色彩可区分，可辨识的不包含已知地标的室内环境；所述特征少的区域为连续3帧提取到的ORB特征点不足100个的区域；所述特征为ORB 特征点，所述ORB特征点由关键点和描述子组成，关键点为包含方向和旋转不变性的角点、区块、边缘，描述子由128位0、1组成的用以表示关键点的向量。

所述移动机器人车载设备包括RGB-D相机和控制中心单元；

所述控制中心单元包括图像点云数据处理模块、语义识别与分割模块、位姿匹配模块和移动控制模块；

所述图像点云数据处理模块根据RGB-D相机扫描的环境原始点云数据提取ORB特征点，生成关键帧，根据关键帧信息计算描述子，匹配特征点与地图点，通过反向投影得到环境中3D点模糊位姿，经非线性优化得到移动机器人精确位姿，同时构建包括特征点、关键帧、地图点、位姿信息的全局地图；

所述语义识别与分割模块根据海量筛选出的地标图片数据集，经YOLOv5s经深度神经网络训练出模型，通过加载模型、配置网络、构建引擎，将环境的RGB通道视频流转换为连续的灰度图像帧，在图像帧上安装数据集中以有的5种类别进行推理，推理，对像素语义识别与分割，结合点云数据处理模块得到地标信息，构建带有语义信息的全局地图；并将结果上传云端服务器；所述地标图片数据集为一半从已公开EuRoC数据集提取，另一半从实际环境中拍摄，手动添加标签的5种分类5000张图片组成，所述五种分类包括办公桌、办公椅、立方体办公柜、人、叉车托盘五种。

所述位姿匹配模块通过对RGB-D相机原始数据的分类推理，当移动机器人需要定位时，与云端服务器已知地标信息对比计算得到移动机器人自身模糊位置，然后对该模糊位置进行回环检测等非线性优化，得到移动机器人精确位姿；

所述移动控制模块以地标信息匹配模块匹配是否得到模糊位置为结果，如过未匹配到目标地标或者匹配到的目标地标数量小于设定阈值，则由移动控制模块控制机器人移动一段距离,再进行位置匹配，直至匹配到的目标地标数量不小于阈值，结束机器人移动，辅助移动机器人获取精确定位。

如图2所示，所述移动机器人为全向驱动轮和从动轮组成的自行车式模型AGV，所述RGB-D相机为双目深度实感RGB-D相机，安装于车头中心正前方，镜头向下俯视，与水平方向成30°夹角。

上述实施例用来解释说明本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明作出的任何修改和改变，都落入本发明的保护范围。

Claims (9)

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

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

步骤2-1、通过步骤(1)得到关键帧中地图点坐标，地图点对应的3D点坐标，相机拍摄该关键帧时的位姿，获取能观测到该地图点的所有关键帧，及该关键帧的临近关键帧，作为候选帧，所述临近帧即与当前候选帧有共同观测点的帧；

步骤2-2、在候选帧的临近帧通过投影匹配将描述子按位异或，异或结果中1的个数和为汉明距离，找到汉明距离最小地图点；

步骤2-3、以汉明距离最小地图点位姿为顶点，投影误差为边，加入图优化求最优解，得到旋转矩阵R和平移向量t，所述图优化即采用开源g2o图优化库通过高斯牛顿法迭代计算，使顶点与顶点之间边的误差最小最优的方法，根据步骤1-7，考虑从1到n(n>1)时刻得到的机器人坐标x，特征点对应的3D点坐标y为

x＝{x₁,...,x_n},y＝{y₁,...,y_n}

其中，x_i(1<i<n)为i时刻机器人坐标，y_i(1<i<n)为i时刻机器人观测到的3D点坐标，根据微分中值公式：

f(x+Δx)≈f(x)+J(x)^TΔx

其中，Δx为引入的增量，J(x)^T为f(x)关于x的导数，是n×1的向量，目标函数为寻求最优增量Δx，使得||f(x+Δx||²最小，根据线性最小二乘法得：

令Δx的导数为零：

J(x)J(x)^TΔx＝-J(x)f(x)

给定初值机器人坐标x，迭代10次即可得到最优增量Δx，也就得到了最优机器人位姿x+Δx；

步骤2-4、遍历地图点通过优化前的旋转矩阵R和平移向量t投影到关键帧上，再用优化后的旋转矩阵R和平移向量t反向投影到3D坐标点，来更新地图点位置；

步骤2-5、以新的地图点信息更新特征点信息，关键帧信息。

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

步骤3-1、遍历当前帧的共视关键帧，计算与当前帧的相似度得分，假设当前帧词袋向量为v_c,共视图中一个关键帧的词袋向量为v_i，则相似度得分：

s(v_c,v_i)是v_c与v_i的相似度得分；

步骤3-2、取步骤3-1中最小得分s_min为最小基准，大于s_min的关键帧设为回环候选关键帧，等待进一步检测；

步骤3-3、根据步骤3-2中得到的回环候选关键帧，找到与当前帧有共同单词的关键帧，共同单词最多的关键帧单词数量记为commonwords_max，定义共同单词最少的关键帧单词数量commonwords_min＝0.8*commonwords_max，剔除共视单词数量小于commonwords_min的关键帧以及不在当前帧的共视图中关键帧；

步骤3-4、再将剩余回环候选关键帧具有相同特征点的归为一组，计算每组相似性得分，最高得分记为AccScore_best，定义Score_min＝0.75*AccScore_best，进一步剔除小于Score_min的孤立回环候选关键帧，得到可靠的回环候选关键帧。

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

步骤4-1、遍历步骤3-4得到的可靠的回环候选关键帧；

步骤4-2、采用RANSAC算法对当前帧与回环候选关键帧进行特征匹配，确定匹配数量和对应地图点；

步骤4-3、对当前帧和回环候选关键帧的匹配点对构造求解器，迭代5次求解相对位姿sim3变换，所述sim3变换就是使用n对回环帧和候选帧地图点对来进行相似变换的求解，得到当前帧与候选帧地图点之间的旋转矩阵R、平移向量t和尺度s；

步骤4-4、根据步骤4-3的结果，将地图点从当前帧投影到回环候选关键帧，根据地图点模长预测尺度信息，由此得到匹配半径，在投影点匹配半径内查找最优匹配，记录id，再从回环候选关键帧重投影到当前帧，以同样的方法比较两次重投影匹配的点对是否一致；

步骤4-5、根据关键帧中的特征点数量与阈值条件，过滤特征点数量低于阈值的关键帧，得到最终的回环帧；

步骤4-6、根据步骤4-5得到的回环帧，估算当前帧位姿，更新共视图所有关键帧位姿，然后矫正当前帧和共视图关键帧对应的所有地图点，融合重复地图点，最后再更新当前帧的共视关键帧，确保融入了新的回环帧；

步骤4-7、开启新的线程按照步骤(2)，对当前时刻之前的所有拍摄关键帧的相机位姿执行全局非线性优化。

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

步骤5-1、检查关键帧序列中是否有新的关键帧，如有新的关键帧，将关键帧插入局部地图，所述局部地图为步骤(1)中保存的关键帧集合，地图点集合以及相机位姿集合，剔除地图中被所有其他关键帧观测到的比例低于0.25的地图点，以及当前地图点被观测次数少于3次和连续三帧未再被观测到的地图点；

步骤5-2、在共视关键帧及每个共视关键帧的临近帧中查找更多匹配的地图点对，并融合当前帧地图点与共视关键帧地图点，重新计算地图点描述子、平均观测方向以及深度信息；

步骤5-3、按照步骤(2)，对拍摄到共视关键帧的相机位姿，执行局部非线性优化；

步骤5-4、按照步骤1-6中的方法剔除冗余局部关键帧，得到最终的局部地图,再将新的局部地图拼接融合为全局地图。

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

步骤6-1、加载已训练好的YOLOv5s网络模型权重文件，配置模型网络参数并创建推理引擎与上下文；

步骤6-2、遍历关键帧序列，对每一帧图像采用锚框方式执行推理，检测到目标物体，输出标签及外框，计算外框中心点的坐标，作为目标的位置信息，所述目标物体即是数据集中的类别。

步骤6-3、关联标签和位置信息，关联标签及关键帧ID，关联标签及外框中的特征点，存入全局地图，并上传云端服务器作为重定位时已知的地标信息。

步骤6-4、在步骤6-4中的基础上将外框及标签按照地标所在关键帧所在金字塔层的缩放比例存储到该标签对应关键帧ID所在的地图中，融合语义与点云地图，构建语义点云地图，上传云端服务器。

8.根据权利要求1所述的一种基于语义ORB-SLAM技术的视觉定位方法，其特征在于，步骤(7)具体过程如下：当机器人开启重定位，以机器人当前位置为坐标原点O_w，扫描机器人相机面向的环境，启动步骤6所述的目标识别，检测与分割步骤，如果检测到地标，得到对应标签，就得到了该标签对应的关键帧，地标框内的特征点以及地标位置信息，具体步骤如下：

步骤7-1、机器人从云端获取语义地图，并加载；

步骤7-2、机器人在某个世界坐标系下位姿(x_w,y_w,θ)，对应的相机位姿(x_wc,y_wc,θ_wc)，将检测到地标表示为landmark₁,landmark₂,...,landmark_n,(n＞＝2)，n为地标数量，便保存当前图片image_cur，遍历像素点得到landmark_i,i∈(1,2,...,n)投影到image_cur1下的坐标(u_i,v_i)，再根据步骤1-2公式：

反向求解landmark_i的相对相机坐标系下坐标向量landmark_cli＝(x_cli,y_cli,z_cli)^T，通常机器人只在地面上运行，也就是二维平面，故z_cli可忽略不计，最终landmark_cli＝(x_cli,y_cli)^T；

步骤7-3、然后以地标对应的标签为key检索云端服务器存储的地标key-value便可得到landmark_i,i∈(1,2,...,n)在世界坐标系下坐标landmark_wli＝(x_wli,y_wli)，

步骤7-4、遍历地标i,j(i∈(1,2,...,n-1),j＝i+1,且j＜＝n),将其以复数形式表达，得到：

landmark_wli,landmark_wlj为第i，j个地标相对世界坐标系下坐标向量，landmark_cli,landmark_clj为第i，j个地标相对相机坐标系下的坐标向量，x轴与y轴位置坐标和：

其中，r(pose_i,j)，i(pose_i,j)分别为pose_i,j的实部和虚部。

机器人朝向θ和为：

其中，j是复数单位，α(landmark_wli-pose_i,j)是landmark_wli-pose_i,j的幅角，α(landmark_cli)是landmark_cli的幅角；

步骤7-5、相机位置如下：

(x_wc,y_wc)＝x_sum/n-i-1,y_sum/n-i-1

θ_wc＝norm(α(θ_sum/2*(n-i-1))

其中，α(θ_sum/2*(n-i-1)是θ_sum/2*(n-i-1)的幅角，n是地标的数量；

步骤7-6、进一步将步骤7-5得到的相机位姿pose_wc＝(x_wc,y_wc,θ_wc)，加上相机相对机器人坐标系的安装误差得到机器人在世界坐标系下的坐标，也就是机器人的定位；

步骤7-7、定位到机器人自身位姿后，在语义地图中以当前坐标为圆心，l为半径，提取特征点，与地图中存储的地图点匹配，进行回环检测，非线性优化，得到机器人精确位姿，最后按照调度任务控制机器人移动，匹配相机新扫描到的已探索过的环境和云端地图匹配，输出实时定位位姿。

步骤7-8，如果相机扫描到的地标少于2个，继续移动机器人至扫描到地标数量大于等于2个，重复步骤7-2到步骤7-7。

9.一种实现权利要求1-8任一项所述的基于语义ORB-SLAM技术的视觉定位方法的系统，其特征在于：该系统包括移动机器人车载设备、云端服务器、在普通环境或空旷区域或特征少的区域放置已训练好的可识别地标；所述普通环境为亮度、灰度、色彩可区分，可辨识的不包含已知地标的室内环境；所述特征少的区域为连续3帧提取到的ORB特征点不足100个的区域。

所述移动机器人车载设备包括RGB-D相机和控制中心单元；

所述控制中心单元包括图像点云数据处理模块、语义识别与分割模块、位姿匹配模块和移动控制模块；

所述图像点云数据处理模块根据RGB-D相机扫描的环境原始点云数据提取ORB特征点，生成关键帧，根据关键帧信息计算描述子，匹配特征点与地图点，通过反向投影得到环境中3D点模糊位姿，经非线性优化得到移动机器人精确位姿，同时构建包括特征点、关键帧、地图点、位姿信息的全局地图；

所述语义识别与分割模块根据地标图片数据集，经YOLOv5s经深度神经网络训练出模型，经过加载推理后对像素语义识别与分割，得出地标信息，构建带有语义信息的全局地图，并将结果上传云端服务器；所述地标图片数据集为一半从已公开EuRoC数据集提取，另一半从实际环境中拍摄，手动添加标签的5种分类5000张图片组成,所述五种分类包括办公桌、办公椅、立方体办公柜、人、叉车托盘五种。

所述位姿匹配模块通过对RGB-D相机将环境的RGB通道视频流转换为连续的灰度图像帧，在图像帧上安照地标图片数据集中已有的5种类别通过模型进行分类推理，当移动机器人需要定位时，与云端服务器已知地标信息对比计算得到移动机器人自身模糊位置，再通过位姿匹配模块对该模糊位置进行非线性优化，得到移动机器人精确位姿；

所述移动控制模块以语义识别与分割模块是否得到地标模糊位置为结果，如过未匹配到目标地标或者匹配到的目标地标数量小于设定阈值，则由移动控制模块控制机器人移动一段距离,再进行位置匹配，直至匹配到的目标地标数量不小于阈值，结束机器人移动，辅助移动机器人获取精确定位。

Images