(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 20221028076 5.X (22)申请日 2022.03.22 (71)申请人 中国电子科技 集团公司第五十四研 究所 地址 050081 河北省石家庄市中山西路589 号第五十四所卫星导航系统与装备技 术国家重点实验室 (72)发明人 赵茜　蔚保国　易卿武　李隽　 金炜桐　李爽　张衡　梁晓虎　 黄璐　贾浩男　李雅宁　王新健　 程建强　陈冲　祝瑞辉　 (74)专利代理 机构 河北东尚律师事务所 13124 专利代理师 王文庆　曲佳颖 (51)Int.Cl. G06V 20/10(2022.01)G06V 10/44(2022.01) G06V 10/75(2022.01) G06K 9/62(2022.01) G06T 7/215(2017.01) G06T 7/73(2017.01) G01C 21/20(2006.01) G01C 22/00(2006.01) (54)发明名称 一种动态场景 下的机器人视觉里程计方法 (57)摘要 本发明公开了一种动态场景下的机器人视 觉里程计方法， 属于机器人定位导航技术领域。 现实场景中一般存在多个运动物体， 在这种动态 场景中， 由于传统的视觉里程计假设场景中不存 在运动的物体， 引起机器人在动态场景中定位的 较大误差。 本发 明在视觉手段中融合多模型拟合 技术， 能够实现场景中各个运动模 型的运动参数 求解， 进一步判别出机器人的运动模型， 排除运 动物体的干扰， 从而实现较为准确的机器人自定 位。 本发明提升了机器人在动态场景中定位的精 度， 增强了机器人定位导航系统的准确性和鲁棒 性。 权利要求书2页 说明书5页 附图1页 CN 114782802 A 2022.07.22 CN 114782802 A 1.一种动态场景 下的机器人视 觉里程计方法， 其特 征在于， 包括以下步骤： (1)机器人在行进过程中进行立体图像序列采集， 并进行图像预处 理； (2)对采集到的初始时刻立体 图像和下一时刻立体图像， 对每一时刻左右图像进行特 征点提取和匹配， 以及对前后时刻的左图进行特征点提取和匹配， 并将两次提取和匹配的 特征点取交集， 获得 特征匹配点的三维坐标； (3)对前后时刻的特 征匹配点进行运动分割， 估计出机器人运动模型； (4)对运动模型进行参数估计获得基础矩阵， 对基础矩阵进行分接获得旋转矩阵R和平 移矩阵t， 并累乘位姿计算出当前帧的位姿； (5)将下一时刻作为初始时刻， 返回步骤(2)， 同时进行局部光束法平差， 校正机器人在 各时刻的位姿。 2.根据权利要求1中所述的一种动态场景下的机器人视觉里程计方法， 其特征在于， 步 骤(1)中机器人通过双目相机进行立体图像序列采集， 双目相机安装于 机器人平台上。 3.根据权利要求1中所述的一种动态场景下的机器人视觉里程计方法， 其特征在于， 所 述的步骤(3)的具体过程为： 首先在N个特征匹配点X＝[x1,x2,…,xN]中进行采样， 生成M个 模型假设， 然后计算所有点到所有模型假设的残差矩阵： 其中， R的第i行ri＝[ri(1),ri(2),…,ri(M)]是点xi到所有模型假设的残差， 将向量ri按照 从小到大的升序排序为 排列偏好向量 使得 将特征匹配点xi的排列偏好向量σi和特征匹配点xj的排列偏好向量σj之间的相关性定 义为： 其中， k＝M/10， M为模型假设的数量； 然后通过定义的半正定核矩阵S∈[0,1]N×N计算特征匹配点xi和特征匹配点xj之间的相 似度S(i,j)： ε(i,j)＝1 ‑f( σi, σj) 其中， ε(i,j)为 点xi和点xj之间的空间距离； 然后将相似度矩阵S(i,j)作为基于P ‑stable分布的局部敏感哈希检索算法的输入， 对特 征匹配点进 行分组， 将特征匹配点划分到多个簇中； 然后通过设计的模型选择迭代框架， 采 取构建能量函数并求取全局最小值的方法， 结合GRIC准则， 从获得的簇中剔除冗余簇， 选择 出和真实模型最接近的模型； 最后基于真实模型对应的残差信息， 对剩余特征匹配点执行 层次聚类将点重新划分到正确的运动模型中， 实现不同运动模型的内点分类。 4.根据权利要求1中所述的一种动态场景下的机器人视觉里程计方法， 其特征在于， 所 述的步骤(4)的具体过程 为： 首先使用RANSAC对获得的机器人运动模型进行参数估计， 获得基础矩阵， 再将基础矩权　利　要　求　书 1/2 页 2 CN 114782802 A 2阵转化为本质矩阵， 接着将 本质矩阵E 分解为旋转矩阵R和平移矩阵t， 即得到 当前时刻机器 人位姿， 然后设置初始 时刻的机器人坐标系作为统一的坐标系， 将计算出来的当前时刻机 器人位姿， 结合前一时刻计算出来的机器人位姿， 通过累乘变换得出当前帧机器人相对于 统一坐标系的位姿。 5.根据权利要求1中所述的一种动态场景下的机器人视觉里程计方法， 其特征在于， 所 述的步骤(5)中局部光束法平差的具体方法是： 使用滑动窗口截取当前时刻周围最近的h帧 图像， 构造 重投影误差方程， 校正机器人在各时刻的位姿， h为设定值。权　利　要　求　书 2/2 页 3 CN 114782802 A 3