(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210220671.3 (22)申请日 2022.03.08 (71)申请人 安徽信息 工程学院 地址 241000 安徽省芜湖市湾沚区永和路1 号 (72)发明人 田德红　王伟　 (74)专利代理 机构 马鞍山诗韬知识产权代理事 务所(普通 合伙) 34245 专利代理师 章丽霞 (51)Int.Cl. G06T 7/73(2017.01) G06V 10/56(2022.01) G06V 10/762(2022.01) G06V 10/774(2022.01) G06K 9/62(2022.01)G01C 21/00(2006.01) B25J 9/16(2006.01) (54)发明名称 一种基于阈值约束的鲁棒闭环检测方法、 系 统、 存储介质及设备 (57)摘要 本发明公开了一种基于阈值约束的鲁棒闭 环检测方法、 系统、 存储介质及设备， 基于阈值约 束的鲁棒闭环检测方法包括： 利用视觉感知系统 采集图像信息， 基于阈值约束筛选表征环境信息 的关键帧， 并生成关键帧模板库； 根据关键帧所 包含的图像旋转和平移信息， 进行方向和位移编 码， 更新位姿感知信息； 根据场景的复杂程度不 同， 采用一种具有动态增减机制的图像特征训练 方法对关键帧图像特征信息进行学习， 并生成特 征向量集； 引入颜色深度图并结合图像匹配算法 实现运动过程中的闭环检测， 修正闭环错误。 本 发明可以解决现有技术中视觉SLAM易受复杂环 境因素干扰， 导致闭环准确率和定位精度低， 进 而导致所构建的二维拓扑图失真的问题。 权利要求书4页 说明书11页 附图6页 CN 114663506 A 2022.06.24 CN 114663506 A 1.一种基于阈值约束的鲁棒闭环检测方法， 其特征在于， 所述基于阈值约束的鲁棒闭 环检测方法包括： 利用视觉感知系统采集图像信息， 基于阈值约束筛选表征环境信息的关键帧， 并生成 关键帧模板库， 其中每一个关键帧均仅且只对应一个特 征向量； 根据所述关键帧所包含的图像旋转和平移信息， 进行方向和位移编码， 更新位姿感知 信息； 根据场景的复杂程度不同， 采用一种具有动态增减机制的图像特征训练方法对关键帧 图像特征信息进行 学习， 并生成特 征向量集， 每个关键帧仅且只被一个特 征向量表征； 引入颜色深度图并结合图像匹配算法实现运动过程中的闭环检测， 修 正闭环错 误。 2.根据权利要求1所述的基于 阈值约束的鲁棒闭环检测方法， 其特征在于， 所述基于 阈 值约束筛 选表征环境信息的关键帧包括： 将第一帧设置为关键帧， 且在 当前帧的位移量达到一定位移阈值或者旋转角度达到一 定角度阈值时将该当前帧设置为关键帧。 3.根据权利要求1所述的基于 阈值约束的鲁棒闭环检测方法， 其特征在于， 所述根据 所 述关键帧图像所包含的图像旋转和平移信息， 进行方向和 位移编码， 更新位姿感知信息包 括： 通过构建最小二乘法和SVD分解方法计算两帧间的运动向量， 求解旋转矩阵R和平移向 量t： 式中， pi和p′i分别为第一帧和第二帧图像中的第i点， ei为第i对点的误差项， R和t分别 为两帧间的旋转矩阵和平 移向量； 求解得欧式变换R， t， 使得pi和p′i一一对应， 然后定义第i对点的误差项ei， 运用SVD方 法求解式中的ICP问题。 最后， 构建最小二乘问题， 求出使误差平方和达 到最小的R和t； 如果当前帧与上一帧之间的平移向量t大于平移阈值E， 则说明两帧间相距较远， 选取 当前帧为关键帧， 反之， 则比较 当前帧与上一帧之 间的旋转矩阵R； 若旋转矩阵R大于旋转阈 值A， 则说明两帧间旋转角度过大， 选取当前帧为关键帧， 反之则将当前帧设为初始帧， 继续 进行判断R和t直到图数量 不再增加。 4.根据权利要求3所述的基于 阈值约束的鲁棒闭环检测方法， 其特征在于， 所述根据场 景的复杂程度不同， 采用一种具有动态增减机制的图像特征训练方法对关键帧图像特征信 息进行学习， 并生成特 征向量集， 每个关键帧仅且只被一个特 征向量表征包括： 步骤31， 样本到相近的聚类集合分配规则如式(1)所示， 更新聚类中心规则如式(2)所 示： 权　利　要　求　书 1/4 页 2 CN 114663506 A 2其中， 为第i个聚类簇中样本的个数， xj为第i个聚类簇中的第 j个样本， 为上一 次迭代之前的聚类中心， 为迭代之后的聚类中心， ε为前一次聚类中心与当前聚类中 心的误差阈值； 重复步骤31， 直到满足 公式(3)； 其中， Rd为增长阈值， Da为缩减阈值； 遍历样本判定若满足式(4)、 (5)， 则聚类完成， 若不满足式(4)， 则此样本与此聚类簇的 区分度较大， 是区别于场景 的显著性区域， 则将当前聚类簇样本点与聚类中心最远的点设 为一个新的聚类中心； 若不满足式(5)， 则说明二聚类中心过近， 则合并二聚类中心， 采用二 聚类中心之间连线的中点 为新的聚类中心点， 直到满足 公式(4)、 (5)， 则聚类完成； 步骤32， 将动态增减机制的图像特征训练方法提取出来的特征向量v＝(v1,v2,…,vk) 中的元素映射到[0， 1]区间， 经 过降维后生成新的特 征向量： 其中， v′j是降维后特征向量的元素， vi是第i个特征向量， vmin为从表征关键帧信息的特 征向量中的最小元 素值， vmax为特征向量中的最大值元 素值； 步骤33， 将降维后生成新的特征向量归一化至特征向量集， 每个关键帧仅且只被一个 特征向量表征。 5.根据权利要求1所述的一种基于 阈值约束的鲁棒闭环检测方法， 其特征在于， 所述引 入颜色深度图并结合图像匹配算法实现运动过程中的闭环检测， 修 正闭环错 误包括： 利用颜色图和深度图结合的颜色深度图中的扫描线强度分布进行图像匹配， 对灰度图 片所有像素列强度求和并归一化； 将图像扫描线强度分布间的平均绝对强度差值称为强度 偏移量， 用g(c)表示； 其中， Ij和Ik是被比较的图像扫描 线强度分布， c是分布偏移量， b是图像的宽度； 通过下述公式获取图像的匹配度 度量G： G＝ μR|giR(c)‑g(c)|+ μD|giD(c)‑g(c)|； 其中， μR和 μD分别为颜色图和深度图的权值， 且 μR+ μD＝1； giR(c)为颜色图的扫描强度分 布， giD(c)为深度图的扫描强度分布； 在连续的图像 中Ij和Ik像素的最小偏移量cm是对二幅 图像的匹配度 度量的最小值；权　利　要　求　书 2/4 页 3 CN 114663506 A 3