(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210467034.6 (22)申请日 2022.04.29 (71)申请人 中国人民解 放军火箭军工程大 学 地址 710025 陕西省西安市灞桥区同心路2 号 (72)发明人 赵军阳　姚鑫玲　周召发　张志利　 徐泽乾　 (74)专利代理 机构 西安研创天下知识产权代理 事务所(普通 合伙) 61239 专利代理师 彭娜娜 (51)Int.Cl. G06T 7/73(2017.01) G06T 7/13(2017.01) G06T 7/136(2017.01) G06V 10/44(2022.01)G06V 10/80(2022.01) (54)发明名称 一种基于亚像素级特征合并优化的视觉惯 性定位方法 (57)摘要 本发明公开了一种基于亚像素级特征合并 优化的视觉惯性定位方法， 针对前后两帧图像， 采取点线特征双线程提取方式： 利用亚像素级点 特征提取算法对图像进行特征检测， 获得亚像素 级角点； 通过线特征提取获得亚像级线特征， 使 点线特征提取精度达到亚像素级。 对获得的点线 特征进行匹配， 获得相邻两帧图像的帧间位姿， 并与惯性器件测量获得的载体位姿信息融合， 从 而获得载体的位置信息。 由于线特征数据量大， 使得系统后端优化时间长， 因此在 滑动窗口内将 重复线特征合并， 减少待估计状态量维度， 增加 有效线特征比例。 本发明降低了算法所需的硬件 设施成本， 减少了优化所需的计算量， 提高了运 行速度， 使定位精度更加准确。 权利要求书3页 说明书6页 附图3页 CN 114972503 A 2022.08.30 CN 114972503 A 1.一种基于亚像素级特 征合并优化的视 觉惯性定位方法， 其特 征在于， 包括以下步骤， S1： 对相机图像进行 预处理， 得到灰度图像， 作为视 觉系统的输入图像； S2： 利用多 级像素特征检测算法， 对输入图像的点特 征进行提取； S3： 在步骤S2进行点特 征提取的同时， 对输入图像进行线特 征提取； S4： 将步骤S2中提取的点特 征和步骤S3中提取的线特 征进行融合， 实现视 觉惯性定位。 2.根据权利要求1所述的一种基于亚像素级特征合并优化的视觉惯性定位方法， 其特 征在于， 步骤S2的具体操作包括以下步骤， S201： 利用像素级特 征检测方法， 对输入图像进行第一轮角点检测； S202： 在已检测出的角点上利用亚像素级角点检测方法进行二次筛 选； S203： 对亚像素级角点检测结果进行图像边缘化约束， 剔除不在图像边缘的亚像素级 角点； S204： 使用LK光流法对步骤S203中最终得到的亚像素级角点进行特征匹配， 获得输入 图像基于点特 征的帧间位姿。 3.根据权利要求2所述的一种基于亚像素级特征合并优化的视觉惯性定位方法， 其特 征在于， 步骤S201的具体操作包括以下步骤， S2011： 在输入图像中选择一个像素点p， 以p为中心， 3个像素点为半径， 确定一个半径 为3， 边缘为16个像素点的检测邻域； S2012： 确定像素点p的灰度值， 将圆周上16个像素点的灰度值与p的灰度值做差， 若连 续有n个像素点的差值大于 设定阈值， 则令p点为该检测邻域的角点， 并记录连续n个像素点 的差值和， 记为 S2013： 以步骤S2011中确定的检测邻域中的其他像素点作为新的像素点p， 重复上述步 骤S2011‑S2012， 判断该检测邻域内是否存在其他满足特点的特征点， 直至遍历 输入图像中 的所有像素点； S2014： 将输入图像进行区域划 分， 选取某一区域内差值和 最大的角点作为该区 域的角点， 同时舍去该区域内其 余角点。 4.根据权利要求3所述的一种基于亚像素级特征合并优化的视觉惯性定位方法， 其特 征在于， 步骤S202的具体操作包括以下步骤， S2021： 将 步骤S2014中选取出来的角点作为基点q0， 以q0为中心取一窗口作为亚像素点 邻域； S2022： 利用Sobel算子对窗口内pi点求像素梯度Gi， 其中， pi为以q0为中心的亚像 素点邻 域中的第i个 像素点； S 2 0 2 3 ：利 用 最 小 二 乘 法 将 pi点 和 对 应 的 像 素 梯 度 Gi代 入 公 式 中， 建立系统方法求解， 得到新的亚像素点q1； 式中， ωi表 示高斯权 重； S2024： 以q1为中心， 重复步骤S2021 ‑2023进行迭代计算， 得到新的亚像素点qi， 直至得 到最优解。权　利　要　求　书 1/3 页 2 CN 114972503 A 25.根据权利要求4所述的一种基于亚像素级特征合并优化的视觉惯性定位方法， 其特 征在于， 步骤S3的具体操作包括以下步骤， S301： 依序分析输入图像的图像帧， 计算图像帧中每个像素点的梯度值和线区域角度， 设置梯度阈值， 将大于梯度阈值的像素点进行合并， 得到线特 征； S302： 根据线特 征的距离、 长度和数目， 对步骤S2021中提取的线特 征进行筛 选； S303： 使用LBD描述子匹配线特 征， 获得输入图像 基于线特 征的帧间位姿。 6.根据权利要求5所述的一种基于亚像素级特征合并优化的视觉惯性定位方法， 其特 征在于， 步骤S3 01的具体操作包括以下步骤， S3011： 计算输入图像的图像帧中像素点的梯度 值和线区域角度， 设置梯度阈值ρ， 将梯 度值小于ρ 的像素点进行抑制， 其 余符合条件的像素点 坐标放置 到同一个空间中； S3012： 设定方向阈值 选取所有满足梯度阈值的像素点中梯度值最大的像 素点， 并将该像素点的坐标值的坐标从空间中取 出作为种子像素se ed； S3013： 以seed为标准， 选取像素梯度方向在方向阈值 内的像素点， 若像素点符合条 件， 则将其从空间中取 出， 使像素不被重复判断； S3014： 每找到一个符合阈值的像素点j， 将图像帧中符合阈值的像素点组成的区域角 度更新为 式中， 下标j用于遍历区域中的所有像 素， level_l ine_angelj为步骤S3011中计算得到的图像帧中像素点的线区域角度； S3015： 将区域中的所有像素进行判断， 获得线段支持域， 将线段支持域中的零散点进 行拟合， 获得 该线段支持域的最小外 接矩形， 并对该矩形进行修 正， 得到线特 征。 7.根据权利要求6所述的一种基于亚像素级特征合并优化的视觉惯性定位方法， 其特 征在于， 步骤S4的具体操作包括以下步骤， S401： 选取关键帧； S402： 在滑动窗口内， 设置三维空间、 二维平面线特征距离阈值两个条件， 将重复线特 征进行合并； S403： 在滑动窗口中， 对每个图像帧重复上述步骤S401～步骤S402， 选取关键帧序列， 合并重复线 特征， 计算每个特征的误差状态函数， 获得帧间位姿矩阵， 从而实现视觉惯性定 位目标。 8.根据权利要求7所述的一种基于亚像素级特征合并优化的视觉惯性定位方法， 其特 征在于， 步骤S401的具体操作包括以下步骤， 对相机输入的图像帧序列进 行筛选， 最新的关 键帧与最近插入的关键帧之间有15个连续的图像帧； 当前帧的点、 线跟踪数目必须达到20 个； 最新的图像帧与当前关键帧的帧间位姿超过预先设定的阈值时， 将该关键帧剔除， 引入 新的关键帧。 9.根据权利要求8所述的一种基于亚像素级特征合并优化的视觉惯性定位方法， 其特 征在于， 步骤S402的具体操作包括以下步骤， S4021： 对关键帧和当前帧中的线特征进行编号， 将当前帧 的线特征linei转到曼哈顿坐 标系， 设置三维空间线特征距离阈值为threshold1， 若关键帧中的线特征linej与当前帧中 的线特征linei的距离小于thresho ld1， 将两线特 征转到二维平面；权　利　要　求　书 2/3 页 3 CN 114972503 A 3