(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210179993.8 (22)申请日 2022.02.25 (71)申请人 淄博师范高等专科 学校 地址 255100 山东省淄博市淄川区文汇街 28号 (72)发明人 陈永平　吕莎莎　宗延雷　 (74)专利代理 机构 上海互顺专利代理事务所 (普通合伙) 31332 专利代理师 韦志刚　曹月明 (51)Int.Cl. G06V 20/10(2022.01) G06V 20/30(2022.01) G06T 7/73(2017.01) G06T 7/246(2017.01) G06T 5/00(2006.01)G06Q 50/20(2012.01) G06K 9/62(2022.01) G06F 16/29(2019.01) G06V 10/762(2022.01) G06V 10/74(2022.01) (54)发明名称 一种教学机 器人系统及控制方法 (57)摘要 根据本公开的示例实施例， 提供了一种教学 机器人系统及控制方法。 该系统包括机器人主体 和控制设备， 控制设备安装有引导模块， 引导模 块用于捕获环境的红绿蓝三通道 图像帧和深度 信息， 从红绿蓝三通道图像帧中提取ORB特征， 将 ORB特征与局部地图点进行匹配， 然后利用非线 性优化最小化重投影误差来计算获得图像采集 装置的位姿， 计算帧间匹配的内点、 旋转向量和 平移向量， 基于旋转向量和平移向量来进行运动 估计， 计算出帧间相对移动距离， 基于特征点追 踪和视觉最小变化选择关键帧， 基于当前选择的 关键帧， 维护和拓展新的局部地图点， 以及基于 光束平差法优化局部地图中所有关键帧的位姿 和所有地图点。 由此， 本发明能够实现地图定位 和导航。 权利要求书3页 说明书7页 附图2页 CN 114549991 A 2022.05.27 CN 114549991 A 1.一种教学机器人系统， 包括： 机器人主体和控制设备； 控制设备安装有引导模块； 以及 引导模块用于经由图像采集装置捕 获环境的红绿蓝三通道图像帧和深度信 息， 从红绿 蓝三通道图像帧中提取ORB特征， 将ORB特征与局部地图点进行匹配， 图像上最小海明距离 的ORB特征点被选为匹配点， 然后利用非线性优化最小化重投影误差来计算获得图像采集 装置的位姿， 采用随机采样一致性算法以及基于深度信息对匹配点进行采样， 从而剔除外 点、 抑制噪声， 计算帧间匹配的内点、 旋转向量和平移向量， 基于旋转向量和平移向量来进 行运动估计， 计算出帧间相对移动距离， 基于特征点追踪和视觉最小变化选择关键帧， 通过 关键帧之间的图像可追踪点进行二次判断， 删除冗余关键帧， 对当前选择 的关键帧进行环 形闭合检测， 以确定当前选择 的关键帧中的当前场景是否重复出现， 基于当前选择 的关键 帧， 维护和拓展新的局部地图点， 以及基于光束平差法优化局部地图中所有关键 帧的位姿 和所有地图点。 2.根据权利要求1所述的系统， 其中所述引导模块用于基于以下公式来计算出帧间相 对移动距离： D＝||Δt||+min(2π ‑||r||， ||r||)， 其中r为帧间旋转向量， t为平移向量， 以 及如果确定帧间相对移动距离小于帧间最大移动距离并且大于帧间最小移动距离， 则确定 跟踪到的帧间特征点的数量是否大于预定数量， 如果确定跟踪到的帧间特征点的数量大于 预定数量， 则确定 当前帧与先前关键帧之 间匹配的特征点的数量占先前关键帧中的特征点 的数量的比例是否大于预定比例， 如果确定当前帧与先前关键帧之 间匹配的特征点的数量 占先前关键帧中的特 征点的数量的比例大于预定比例， 则将当前帧选择为关键帧。 3.根据权利要求1所述的系统， 其中所述引导模块用于确定能否在3个先前关键帧中找 到当前选择的关键帧中的90％的特征点， 如果确定能够在3个先前关键帧中找到当前选择 的关键帧中的90％的特 征点， 则确定当前选择的关键帧为冗余， 并删除所选择的关键帧。 4.根据权利要求1所述的系统， 其中所述引导模块用于利用K均值 聚类法将训练集中的 所有特征向量分成K类， 对于每一类， 递归采用同样方式进 行聚类， 直到达到预定层数， 完成 词汇树构建， 基于单词的出现频率确定词 汇树的叶节点的权重wi＝ln(N/Ni)， N为数据库中 图像数量的总和， Ni为数据库中具有单词i的图像的数量总和， 待检索图像的特征向量和数 据库图像的特 征向量可如下表示： 其中ni和mi分别表示待检索图像和数据库图像中具有单词i的数量， 计 算待检索图像和数据库图像之间的相似度如下： 根据该公式计算当前选择的关键帧与先前关键帧之 间的相似度， 基于相似度， 确定当前选择的关键帧中的当前场景 是否重复出现。 5.根据权利要求4所述的系统， 其中所述引导模块用于建立用于先前关键帧的倒排索 引， 记录包含特定单词的所有 先前关键帧， 对于选择的关键帧， 计算选择的关键帧中包含的 单词表， 对于单词表中的每个单词， 通过倒排索引查找具有 该单词的所有 先前关键帧， 并记权　利　要　求　书 1/3 页 2 CN 114549991 A 2录每个先前关键帧与选择的关键帧共同包含的单词的个数， 在与选择的关键帧共同包含单 词的所有 先前关键帧中确定共同单词个数超过预定个数的先前关键帧， 计算确定的先前关 键帧与选择 的关键帧之间的上述相似度， 如果确定相似度超过预定值， 则确定选择 的关键 帧中的当前场景重复出现。 6.根据权利要求1所述的系统， 其中所述引导模块还用于如果确定连续的10个图像帧 均不匹配， 则将当前图像帧确定为关键帧， 并对当前图像帧进 行闭环检测， 以期待找到所有 与当前图像帧相似的场景并建立帧间约束。 7.一种用于教学机器人的控制方法， 包括： 经由机器人的图像采集装置捕 获环境的红绿蓝三通道图像帧和深度信 息， 从红绿蓝三 通道图像帧中提取ORB特征， 将ORB特征与局部地图点进 行匹配， 图像上最小海明距离的ORB 特征点被选为匹配点， 然后利用非线性优化最小化重投影误差来计算获得图像采集装置的 位姿， 采用随机采样一致性算法以及基于深度信息对匹配点进 行采样， 从而剔除外点、 抑制 噪声， 计算帧间匹配的内点、 旋转向量和平移向量， 基于旋转向量和平移向量来进 行运动估 计， 计算出帧间相对移动距离， 基于特征点追踪和视觉最小变化选择关键帧， 通过关键帧之 间的图像可追踪点进行二次判断， 删除冗余关键帧， 对当前选择 的关键帧进行环形闭合检 测， 以确定 当前选择的关键帧中的当前场景是否重复出现， 基于 当前选择的关键帧， 维护和 拓展新的局部地图点， 以及基于光束平差法优化局部地图中所有关键帧的位姿和所有地图 点。 8.根据权利要求7所述的方法， 其中计算出帧间相对移动距离包括基于以下公式来计 算出帧间相对移动距离： D＝||Δt||+min(2 π ‑||r||， ||r||)， 其中r为帧间旋转向量， t为平 移向量； 以及选择关键帧包括如果确定帧间相对移动距离小于帧间最大移动距离并且大于 帧间最小移动距离， 则确定跟踪到的帧间特征点的数量是否大于预定数量， 如果确定跟踪 到的帧间特征点的数量大于预定数量， 则确定 当前帧与先前关键帧之 间匹配的特征点的数 量占先前关键帧中的特征点的数量的比例是否大于预定比例， 如果确定当前帧与先前关键 帧之间匹配的特征点的数量占先前关键帧中的特征点的数量的比例大于预定比例， 则将当 前帧选择为关键帧。 9.根据权利要求7 所述的方法， 其中删除冗余关键帧包括： 确定能否在3个先前关键帧中找到当前选择的关键帧中的90％的特征点， 如果确定能 够在3个先前关键帧中找到当前选择 的关键帧中的90％的特征点， 则确定当前选择的关键 帧为冗余， 并删除所选择的关键帧。 10.根据权利要求7 所述的方法， 其中环形闭合检测包括： 利用K均值聚类法将训练集中的所有特征向量分成K类， 对于每一类， 递归采用同样方 式进行聚类， 直到达到预定层数， 完成词汇树构建， 基于单词的出现频率确定词汇树的叶节 点的权重wi＝ln(N/Ni)， N为数据库中图像数量的总和， Ni为数据库中具有单词i的图像的数 量总和， 待检索图像的特征向量和数据库图像的特征向量可如下表示： 其 中ni和mi分别表示待检索图像和数据库图像中具有单词i的数量， 计 算待检索图像和数据库权　利　要　求　书 2/3 页 3 CN 114549991 A 3