(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210548983.7 (22)申请日 2022.05.20 (71)申请人 电子科技大 学长三角研究院 （衢州） 地址 324000 浙江省衢州市柯 城区芹江东 路288号创新大厦1号楼18楼 (72)发明人 郭贤生　韩璐飞　司皓楠　张明恒　 黄河　黄健　张玉坤　李林　 钱博诚　 (74)专利代理 机构 成都点睛专利代理事务所 (普通合伙) 51232 专利代理师 孙一峰 (51)Int.Cl. G06T 7/73(2017.01) G06T 7/66(2017.01) G06T 7/246(2017.01)G06V 20/10(2022.01) G06V 20/40(2022.01) G06V 10/44(2022.01) G06V 10/74(2022.01) G06V 10/764(2022.01) G06K 9/62(2022.01) (54)发明名称 一种基于视觉slam的机 器人自主定位方法 (57)摘要 本发明属于机器人定位技术领域， 具体涉及 一种基于视觉slam的机器人自主定位方法。 本发 明的方法主要是： 通过获取的图片， 从图片中获 取FAST角点作为特征点， 然后对机器人位姿状态 初始化后， 通过获取图片中相同特征点进行特征 匹配， 进而进行追踪。 本发明通过跟踪连续帧之 间的视觉 特征， 并在新获取到的图像帧中不断追 踪新的特 征， 从而实现机 器人实时定位。 权利要求书1页 说明书4页 附图3页 CN 114842081 A 2022.08.02 CN 114842081 A 1.一种基于 视觉slam的机器人自主定位方法， 其特 征在于， 包括以下步骤： S1、 通过测量模块获取机器人运动过程中的连续图片， 并通过树莓派模块将图像上传 至上位机的图像处理系统中对图像进 行处理； 所述测量模块为内置IMU的相机， 图像处理系 统在相机拍摄的图像中选取特征点进 行跟踪， 具体方法为： 根据ORB特征对 FAST角点进 行提 取， 找出图像中的角点作为特征点， 即一像素与相 邻像素差别大于阈值的点； 再计算每个特 征点的描述子， 即将特征点对其相邻的任意一个像素作比较， 相邻的像素比关键点大则取 1， 反之则取0； 将两张图象相似的描述字进行匹配， 根据匹配的结果 来预测相机的运动； S2、 机器人位姿状态初始化， 并在相机拍摄的图片中找到相似或相同的特 征点； S3、 将不同图片中寻找到的特 征点进行 特征匹配， 进行正常序列特 征跟踪， 具体为： 对于列表中还没有深度的特征点， 用奇异值分解并计算出坐标， 使其在被观察到的每 帧图像上的重投影误差最小， 特征点的坐标采用被观察到的第一帧图像的相机坐标系的深 度表示， 并用ceres优化； 结合各关键帧的位姿、 相机外参、 边缘化的信息、 预积分的误差、 每 个特征点的重投影误差、 回环闭环误差进行优化； S4、 通过对迭代最近的特征点进行ICP计算， 预测相机的运动 轨迹； 得到旋转矩阵R和平 移矩阵t； S5、 图像处 理系统将结果返回至上位机进行信号处 理， 获得驱动信号； S6、 上位机将驱动信号发送到树莓派模块， 由树莓派模块反馈给 单片机； S7、 单片机向机器人的驱动系统发送驱动信号， 控制机器人运动。权　利　要　求　书 1/1 页 2 CN 114842081 A 2一种基于视觉sla m的机器人自主定位方 法 技术领域 [0001]本发明属于机器人定位技术领域， 具体涉及一种基于视觉slam的机器人自主定位 方法。 背景技术 [0002]视觉‑惯性子系统(VIO)模块在人工智能技术中有着广泛的应用， 如机器人导航、 自动驾驶、 虚拟现实和增强现实。 视觉 ‑惯性系统的主要优点是可以观察公制尺度， 以及横 摇和俯仰角度， 这对于导航任务的状态估计功能非常重要。 此外， 加入IMU的测量也可以显 著改善运动跟踪性能， 弥补由于光照变化、 无纹理区域、 运动模糊损失的视觉轨迹。 V IO不仅 广泛应用于地 面机器人， 还有无 人机、 移动设备等。 [0003]然而， 有几个问题严重影响VIO的普及和使用。 第一： 严格的初始化。 由于缺乏的距 离测量， 因此很难将视觉结构与惯性测量直接融合， 也就是说VIO是一个高度非线性的系 统。 在多数情况下， 系统应该从一个已知的固定位置出发， 并在开始时缓慢移动， 但这限制 了其在实 践中的使用。 另一个问题是VIO在长期的使用中， 漂移是不可避免的。 发明内容 [0004]针对上述问题， 本发明提出了一种基于 视觉slam的机器人自主定位方法。 。 [0005]本发明的技 术方案为： [0006]一种基于 视觉slam的机器人自主定位方法， 包括以下步骤： [0007]S1、 通过测量模块获取机器人运动过程中的连续图片， 并通过树莓派模块将图像 上传至上位机的图像处理系统中对图像进 行处理； 所述测量模块为内置IMU的相机， 图像处 理系统在相机拍摄的图像中选取特征点进行跟踪， 具体方法为： 根据ORB特征对 FAST角点进 行提取， 找出图像中的角点作为特征点， 即一像素与相 邻像素差别大于阈值的点； 再计算每 个特征点的描述子， 即将特征点对其相邻的任意一个像素作比较， 相邻的像素比关键点大 则取1， 反之则取0； 将两张图象相似的描述字进行匹配， 根据匹配的结果来预测相机的运 动； [0008]S2、 机器人位姿状态初始化， 并在相机拍摄的图片中找到相似或相同的特 征点； [0009]S3、 将不同图片中寻找到的特 征点进行 特征匹配， 进行 特征跟踪； [0010]S4、 通过对迭代最近的特征点进行ICP计算， 预测相机的运动轨迹； 得到旋转矩阵R 和平移矩阵t； 具体为： [0011]先计算第n点和第n ‑1点的误差项： 定义两组点Pn和Pn ‑1， 并定义这对点的误差为 e， 则误差的平方和达 到最小的旋转矩阵R和平 移矩阵t： [0012] [0013]定义两组点的质心， 带入误差函数化简； [0014]计算两组点的去质心坐标；说　明　书 1/4 页 3 CN 114842081 A 3