CN111210477A - 一种运动目标的定位方法及系统 - Google Patents

一种运动目标的定位方法及系统 Download PDF

Info

Publication number
CN111210477A
CN111210477A CN201911365228.XA CN201911365228A CN111210477A CN 111210477 A CN111210477 A CN 111210477A CN 201911365228 A CN201911365228 A CN 201911365228A CN 111210477 A CN111210477 A CN 111210477A
Authority
CN
China
Prior art keywords
feature points
error
edge
preset
target
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201911365228.XA
Other languages
English (en)
Other versions
CN111210477B (zh
Inventor
周健
冯大权
周小安
万绍俊
温焕宇
王春琦
赖礼锋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen University
Original Assignee
Shenzhen University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen University filed Critical Shenzhen University
Priority to CN201911365228.XA priority Critical patent/CN111210477B/zh
Publication of CN111210477A publication Critical patent/CN111210477A/zh
Application granted granted Critical
Publication of CN111210477B publication Critical patent/CN111210477B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/13—Edge detection
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Automation & Control Theory (AREA)
  • Data Mining & Analysis (AREA)
  • Multimedia (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Image Analysis (AREA)

Abstract

本发明提供一种运动目标的定位方法及系统，其中方法包括：获取待定位目标的环境图像边缘角点特征、加速度、角速度及预设噪声协方差项；根据获取预设数量具有边缘角点特征的特征点的观测值及估计值，获取重投影误差；根据预设误差模型获取待定位目标的加速度及角速度的增量误差及惯性测量噪声误差；利用预设优化算法对预设噪声协方差项、重投影误差、增量误差及惯性测量噪声误差进行非线性优化，得到待定位目标的位姿信息。本发明利用角点特征及边缘线特征综合的方法提取特征点，增强了前端视觉跟踪的鲁棒性及准确性；考虑环境中噪声误差以及惯性测量单元误差的影响，将前端的数据误差进行后端非线性优化，提高了运动目标定位的准确性。

Description

一种运动目标的定位方法及系统
技术领域
本发明涉及机器视觉领域，具体涉及一种运动目标的定位方法及系统。
背景技术
随着计算机技术和人工智能的发展，运动机器人得到了快速发展并渗透到社会的各个领域。运动机器人定位算法是使机器人具备自主导航功能的基础。传统的相对定位算法主要分为里程计法和惯性导航法，里程计法是通过安装在车轮上的光电编码器计算车轮的运动从而估计出机器人的运动，惯性导航法则是通过陀螺仪，加速度计等惯性测量单元(Inertial measurement unit，IMU)实现定位。由于打滑和误差积累，纯视觉里程计和IMU定位的精度都较低，考虑到视觉和惯性传感器的这种优缺点互补性，融合视觉与IMU的视觉惯性里程计(Visual Inertial Odometry，VIO)算法大大提高了定位精度。然而VIO系统在纹理丰富的场景，大部分时候的结果的误差比较小，但在经过长走廊或纹理不够丰富的场景时，容易累积较大的误差。
发明内容
因此，本发明要解决的技术问题在于克服现有技术中的在纹理信息较少或者图像模糊时候，VIO系统对运动物体位姿估计及重建环境图像的误差较大的缺陷，从而提供一种运动目标的定位方法及系统。
为达到上述目的，本发明提供如下技术方案：
第一方面，本发明提供一种运动目标的定位方法，包括如下步骤：获取待定位目标的环境图像、加速度及角速度；获取环境图像中待定位目标的边缘角点特征及预设噪声协方差项；获取预设数量具有边缘角点特征的特征点的观测值及估计值，根据观测值及估计值获取待定位目标的重投影误差；根据预设误差模型获取待定位目标的加速度及角速度的增量误差及惯性测量噪声误差；利用预设优化算法对预设噪声协方差项、重投影误差、增量误差及惯性测量噪声误差进行非线性优化，得到待定位目标的位姿信息。
在一实施例中，获取环境图像中待定位目标的边缘角点特征的过程，包括以下步骤：获取滤除环境图像信息噪声后像素点的灰度值；根据像素点的灰度值，获取像素点的综合梯度；根据像素点的综合梯度，获取环境图像边缘的第一边缘点；根据非极大值抑制准则，去除环境图像边缘的第一边缘点中的伪边缘点，获取环境图像边缘的第二边缘点；利用滞后阈值对环境图像边缘的第二边缘点进行优化处理，获取环境图像边缘及边缘线特征；对环境图像边缘进行角点检测获取边缘角点特征。
在一实施例中，获取预设数量具有边缘角点特征的特征点的观测值及估计值，根据观测值及估计值获取待定位目标的重投影误差的过程，包括以下步骤：在当前帧上选取预设数量具有角点特征的特征点，利用金字塔光流方法追踪相邻帧对应的特征点作为观测特征点，获取观测特征点投影位置的观测值；获取特征点三维坐标按照当前估计位姿进行投影得到的估计值；将观测值与估计值进行比较得到待定位目标的重投影误差。
在一实施例中，在当前帧上选取预设数量具有角点特征的特征点，利用金字塔光流方法追踪相邻帧对应的特征点作为观测特征点的过程，包括以下步骤：获取当前帧上预设数量角点特征较强的特征点；基于角点特征较强的特征点，利用金字塔光流方法追踪在环境图像下一帧中对应的特征点作为匹配特征点；滤除匹配特征点中的异常点，获取观测特征点。
在一实施例中，获取预设数量具有边缘角点特征的特征点的观测值及估计值，根据观测值及估计值获取待定位目标的重投影误差的过程，还包括如下步骤：判断下一帧观测特征点的数量是否等于上一帧特征点的数量；当下一帧观测特征点的数量等于上一帧特征点的数量时，将下一帧观测特征点作为当前帧上预设数量角点特征较强的特征点。
在一实施例中，当下一帧观测特征点的数量小于上一帧特征点的数量时，获取下一帧上角点特征较强的特征点，将下一帧观测特征点及下一帧上角点特征较强的特征点作为当前帧上预设数量角点特征较强的特征点，其中下一帧观测特征点及下一帧上角点特征较强的特征点的数量总和，与上一帧特征点的数量相等。
在一实施例中，利用预设优化算法对预设噪声协方差项、重投影误差、增量误差及惯性测量噪声误差进行非线性优化，得到待定位目标的位姿信息的过程，包括以下步骤：构建基于预设噪声协方差项、环境图像所有帧的重投影误差、增量误差及惯性测量噪声误差的优化目标函数；利用基于滑动窗口的预设优化方法，对优化目标函数进行非线性优化，生成待定位目标的位姿信息。
第二方面，本发明提供一种运动目标的定位系统，包括：信息获取模块，用于获取待定位目标的环境图像、加速度及角速度；边缘角点特征及预设噪声协方差项获取模块，用于获取环境图像中待定位目标的边缘角点特征及预设噪声协方差项；重投影误差获取模块，用于获取预设数量具有边缘角点特征的特征点的观测值及估计值，根据观测值及估计值获取待定位目标的重投影误差；增量误差及惯性测量噪声误差获取模块，用于根据预设误差模型获取待定位目标的加速度及角速度的增量误差及惯性测量噪声误差；位姿信息生成模块，用于利用预设优化算法对预设噪声协方差项、重投影误差、增量误差及惯性测量噪声误差进行非线性优化，得到待定位目标的位姿信息。
第三方面，本发明实施例提供一种计算机设备，包括：至少一个处理器，以及与至少一个处理器通信连接的存储器，其中，存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器执行本发明实施例第一方面的运动目标的定位方法。
第四方面，本发明实施例提供一种计算机可读存储介质，计算机可读存储介质存储有计算机指令，计算机指令用于使计算机执行本发明实施例第一方面的运动目标的定位方法。
本发明技术方案，具有如下优点：
本发明提供的运动目标的定位方法及系统，利用角点特征及边缘线特征综合的方法提取特征点，对特征点在下一帧中的位置进行追踪及估计，获取重投影误差，增强了前端视觉跟踪的鲁棒性及准确性；考虑运动目标运动环境中噪声误差以及惯性测量单元的误差的影响，将前端的数据误差进行后端非线性优化，提高了运动目标定位的准确性。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的运动目标的定位方法的一个具体示例的示意图；
图2为本发明实施例提供的获取环境图像中待定位目标的边缘角点特征的一个具体示例的示意图；
图3为本发明实施例提供的获取重投影误差的一个具体示例的示意图；
图4为本发明实施例提供的获取观测特征点的一个具体示例的示意图；
图5为本发明实施例提供非线性优化过程的一个具体示例的示意图；
图6为本发明实施例提供的的运动小车运动轨迹的另一个具体示例的示意图；
图7为本发明实施例提供的运动小车运动轨迹的另一个具体示例的示意图；
图8为本发明实施例提供的运动目标的定位系统的一个具体示例的示意图；
图9为本发明实施例提供的计算机设备一个具体示例的组成图。
具体实施方式
下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
此外，下面所描述的本发明不同实施方式中所涉及的技术特征只要彼此之间未构成冲突就可以相互结合。
实施例1
本实施例提供一种运动目标的定位方法，应用于视觉定位导航技术领域，如图1所示，包括如下步骤：
步骤S1：获取待定位目标的环境图像、加速度及角速度。
VIO系统按照数据融合的方式可以分为基于滤波器和基于优化两种，按照是否把图像特征信息加入进状态向量进行分类，可以分为松耦合和紧耦合两种。本发明实施例利用双目视觉与IMU融合的紧耦合VIO系统对待定位目标进行实时位姿估计。双目摄影设备置于待定位目标上，用于实时采集实际环境中的图像信息。IMU由三个单轴加速度计和陀螺仪组成，惯导解算主要是通过加速度计测得的待定位目标加速度和陀螺测得的待定位目标相对于导航坐标系的角速度来对待定位目标的位置、姿态及速度进行解算。IMU传感器分为三轴的加速度计和三轴的陀螺仪，加速度计用于测量小车的实际的三轴的加速度，陀螺仪用于测量搭载小车的三轴的角速度，考虑运动过程中的白噪声和IMU的BIAS(偏置)误差，建立相应的误差模型，利用运动模型中的离散积分的中值法求出传感器的加速度值和角速度的值。
步骤S2：获取环境图像中待定位目标的边缘角点特征及预设噪声协方差项。
实际中，在纹理信息较少或者图像模糊时，传统的基于点特征的方法不能准确的估计待定位目标的位姿及其环境的地图重建，因此本发明实施例采用点线融合的方法，进行定位及地图重建。同时考虑到噪声对传感器的影响，在最终的优化目标函数中加入预设噪声协方差项，进行进一步优化。由于本发明实施例后端是基于滑动窗口的优化方法，因此获取环境图像中待定位目标的边缘角点特征及预设噪声协方差项是在滑动窗口下的，即一定之间内的边缘角点特征及预设噪声协方差项。
步骤S3：获取预设数量具有边缘角点特征的特征点的观测值及估计值，根据观测值及估计值获取待定位目标的重投影误差。
本发明实施例使用重投影误差，不仅考虑了单应矩阵的计算误差，也考虑了图像点的测量误差，所以定位精度会更高。
步骤S4：根据预设误差模型获取待定位目标的加速度及角速度的增量误差及惯性测量噪声误差。
本发明实施例利用双目视觉与IMU融合的紧耦合VIO系统对待定位目标进行实时位姿估计。紧耦合的方式就是利用IMU和环境图像信息融合来估计状态量，通过重新参数化，把关键帧之间的IMU测量值积分成相对运动的约束，避免了因为初始条件变化造成的重复积分，在下一个关键帧到来之前先对IMU数据预积分，得到预积分增量误差。同时由于噪声对IMU的影响，运用预设算法得到惯性测量噪声误差，包括：由于加速度计的随机游走造成的噪声误差，陀螺仪的随机游走的造成的噪声误差。
步骤S5：利用预设优化算法对预设噪声协方差项、重投影误差、增量误差及惯性测量噪声误差进行非线性优化，得到待定位目标的位姿信息。
本发明实施例使用融合算法将预设预设噪声协方差项、重投影误差、增量误差及惯性测量噪声误差融合到一个优化目标函数中，采用VINS-Fusion开源代码中的基于滑动窗口的后端优化方法(仅以此举例，不以此为限)对优化目标函数进行优化，从而得到准确的待定位目标的位姿信息，实现对运动目标的实时定位。
本发明实施例提供的运动目标的定位方法，利用角点特征及边缘线特征综合的方法提取特征点，对特征点在下一帧中的位置进行追踪及估计，获取重投影误差，增强了前端视觉跟踪的鲁棒性及准确性；考虑运动目标运动环境中噪声误差以及惯性测量单元的误差的影响，将前端的数据误差进行后端非线性优化，提高了运动目标定位的准确性。
在一具体实施例中，如图2所示，获取环境图像中待定位目标的边缘角点特征的过程，包括以下步骤：
步骤S21：获取滤除环境图像信息噪声后像素点的灰度值。
图象的边缘是指图象局部区域亮度变化显著的部分，该区域的灰度剖面一般可以看作是一个阶跃，即从一个灰度值在很小的缓冲区域内急剧变化到另一个灰度相差较大的灰度值。图象的边缘部分集中了图象的大部分信息，图象边缘的确定与提取对于整个图象场景的识别与理解是非常重要的，同时也是图象分割所依赖的重要特征，边缘检测主要是图象的灰度变化的度量、检测和定位。由于待定位目标所处的实际环境中存在噪声，易对传感器造成影响，因此本发明实施例需要获取滤除环境图像信息噪声后像素点的灰度值。本发明实施例使用高斯平滑滤波器卷积降图像噪声，采用一个size＝3的高斯内核。对于一个位置(m,n)的像素点，其灰度值为f(m,n)，那么经过高斯滤波后的灰度值gσ(m,n)将变为：
Figure BDA0002338236790000091
其中，gσ(m,n)为经过高斯滤波后的灰度值，f(m,n)为未滤波前的灰度值。
步骤S22：根据像素点的灰度值，获取像素点的综合梯度。
图像的边缘为灰度值变化大的像素点的集合，在图像中用梯度表示灰度值的变化程度和方向，可以通过点乘一个sobel或其它算子得到不同方向的梯度值gx(m,n)及gy(m,n)。综合梯度通过以下公式计算综合梯度值G(m,n)和综合梯度方向θ：
Figure BDA0002338236790000092
Figure BDA0002338236790000101
其中，G(m,n)为综合梯度值，gx(m,n)及gy(m,n)为不同方向的梯度值，θ为综合梯度方向。
步骤S23：根据像素点的综合梯度，获取环境图像边缘的第一边缘点。
步骤S24：根据非极大值抑制准则，去除环境图像边缘的第一边缘点中的伪边缘点，获取环境图像边缘的第二边缘点。
在高斯滤波过程中边缘有可能被放大，为了得到准确的边缘，本发明实施例利用非极大值抑制准则滤除伪边缘点，使用边缘宽度尽可能为1个像素点：如果一个像素点属于边缘，那么这个像素点在梯度方向上的梯度值是最大的，否则不是边缘，将其灰度值设为0。
步骤S25：利用滞后阈值对环境图像边缘的第二边缘点进行优化处理，获取环境图像边缘及边缘线特征。
本发明实施例使用Canny算法提取环境图像边缘，仅以此举例，不以此为限，也可以采取其他算法提取图像边缘。Canny算法利用滞后阈值对环境图像边缘的第二边缘点进行优化处理。滞后阈值为双阀值算法，包括高阈值及低阈值。双阈值筛选强弱边缘，如果边缘像素的梯度值>高阈值，则将其标记为强边缘像素；如果低阈值<边缘像素的梯度值<高阈值，则将其标记为弱边缘像素；如果边缘像素的梯度值<低阈值，则会被抑制。
步骤S26：对环境图像边缘进行角点检测获取边缘角点特征。
本发明实施例提取边缘线特征后，利用FAST角点检测方法(仅以此举例，不以此为限)，假定特征点周围的图像强度，通过检测候选像素周围一圈像素来决定是否接手一个角点，与中心点差异较大的像素如果组成连续的圆弧，并且弧长大于圆周长的3/4时，认为搜索到一个角点，获取边缘角点特征，进而提升特征匹配的精度，使系统中的待定位目标位姿的初始值更准确。
在一具体实施例中，如图3所示，获取预设数量具有边缘角点特征的特征点的观测值及估计值，根据观测值及估计值获取待定位目标的重投影误差的过程，包括以下步骤：
步骤S31：在当前帧上选取预设数量具有角点特征的特征点，利用金字塔光流方法追踪相邻帧对应的特征点作为观测特征点，获取观测特征点投影位置的观测值。
本发明实施例在当前帧中选取特征点后，对其进行追踪，获取其在下一帧中的匹配点。为了剔除异常点，先对前后帧中这些匹配点进行校正，先对特征点及匹配点进行畸变校正，再投影到以原点为球心，半径为1的球面上，再延伸到深度归一化平面上，获得最终校正后的位置。对于每一对匹配点，基于校正后的位置，用基础矩阵加随机抽样一致算法来筛选，以剔除异常点，获取观测特征点。
步骤S32：获取特征点三维坐标按照当前估计位姿进行投影得到的估计值。
由于待定位目标的运动环境较复杂，会造成图像及传感器获取的数据出现误差，因此需要根据当前帧中的特征点三维坐标，估计其在下一帧中的位姿，将估计位姿进行投影得到估计值。
步骤S33：将观测值与估计值进行比较得到待定位目标的重投影误差。
在计算机视觉即时定位与地图构建系统(SLAM)中优化求解相机运动位姿时(3D-2D)，会用到重投影误差。比如在计算平面单应矩阵和投影矩阵的时候，往往会使用重投影误差来构造代价函数，然后最小化这个代价函数，以优化单应矩阵或者投影矩阵。之所以使用重投影误差，是因为它不仅考虑了单应矩阵的计算误差，也考虑了图像点的测量误差，所以其精度会更高。因此根据观测特征点的位置，获取观测特征点投影位置的观测值。
在一具体实施例中，如图4所示，在当前帧上选取预设数量具有角点特征的特征点，利用金字塔光流方法追踪相邻帧对应的特征点作为观测特征点的过程，包括以下步骤：
步骤S311：获取当前帧上预设数量角点特征较强的特征点。
本发明实施例根据边缘角点特征，在初始帧中选取与角点特征匹配度较强的预设数量的特征点，之后在下一帧中尝试跟踪这些点。
步骤S312：基于角点特征较强的特征点，利用金字塔光流方法追踪在环境图像下一帧中对应的特征点作为匹配特征点。本发明实施例利用基于图像金字塔的分层结构与多分辨率特征的光流追踪特征点，使得特征跟踪窗口尺寸能够在不同级别的图像分辨率层次上动态扩展，同时提高了特征点匹配精度。
步骤S313：滤除匹配特征点中的异常点，获取观测特征点。
本发明实施例中，为了滤除异常点，需要对前后帧中特征点及匹配特征点进行校正，先进行畸变校正，再投影到以原点为球心，半径为1的球面上，再延伸到深度归一化平面上，获得最终校正后的特征点及匹配特征点进位置。对于每一对特征点及匹配特征点，基于其校正后的位置，用基础矩阵F加随机抽样一致算法来滤除异常点，获取观测特征点。
在一具体实施例中，获取预设数量具有边缘角点特征的特征点的观测值及估计值，根据观测值及估计值获取待定位目标的重投影误差的过程，还包括如下步骤：
判断下一帧匹配特征点的数量是否等于上一帧特征点的数量。本发明实施例中，在当前帧中选取了预设数量的特征点，例如150个特征点，利用金字塔光流方法追踪特征点在下一帧中的位置，获取匹配特征点，然后对特征点及匹配特征点进行校正，滤除异常点之后，获取观测特征点。观测特征点的个数可能不是150个，例如：观测特征点的数量为140个，为了继续追踪特征点在再下一帧中的位置，需要判断这些观测特征点的数量是否达到预设数量。
当下一帧观测特征点的数量等于上一帧特征点的数量时，将下一帧观测特征点作为当前帧上预设数量角点特征较强的特征点。
在一具体实施例中，当下一帧观测特征点的数量小于上一帧特征点的数量时，获取下一帧上角点特征较强的特征点，将下一帧观测特征点及下一帧上角点特征较强的特征点作为当前帧上预设数量角点特征较强的特征点，其中下一帧观测特征点及下一帧上角点特征较强的特征点的数量总和，与上一帧特征点的数量相等。
本发明实施例中，由于在下一帧的匹配特征点中已经滤除了异常点，所以观测特征点的数量小于当前帧中特征点的个数，因此为了继续追踪预设数量的特征点，需要在获取下一帧上角点特征较强的特征点，将下一帧观测特征点及下一帧上角点特征较强的特征点作为当前帧上预设数量角点特征较强的特征点，用于追踪特征点在再下一帧中的位置。例如：下一帧中观测特征点的个数为140个，特征点的预设数量为150个，因此需要在下一帧中获取10个角点特征较强的特征点，将140个观测特征点即10个下一帧10个角点特征较强的特征点作为下一帧上预设数量角点特征较强的特征点，由于环境图像式一帧一帧进行的，故下一帧上预设数量角点特征较强的特征点成为当前帧上预设数量角点特征较强的特征点。
在一具体实施例中，如图5所示，利用预设优化算法对预设噪声协方差项、重投影误差、增量误差及惯性测量噪声误差进行非线性优化，得到待定位目标的位姿信息的过程，包括以下步骤：
步骤S51：构建基于预设噪声协方差项、环境图像所有帧的重投影误差、增量误差及惯性测量噪声误差的优化目标函数。
步骤S52：利用基于滑动窗口的预设优化方法，对优化目标函数进行非线性优化，生成待定位目标的位姿信息。
本发明实施例将预设噪声协方差项、重投影误差、增量误差融合及惯性测量噪声误差在一个优化目标函数中，采用VINS-Fusion开源代码中的基于滑动窗口的后端优化方法，需要说明的是，本发明实施例中的算法仅用于以此举例，但不以本发明实施例中的算法为限。
为了验证本发明实施例提供的运动目标的定位方法的正确性，在运动小车上搭载双目相机，将运动小车置于弱文理长走廊的环境中，分别用激光SLAM传感器、基于点特征的vins-fusion开源系统及本发明实施例提成的基于点线综合特征的VIO系统测量的小车运动轨迹，如图6及图7所示。图6为用激光SLAM传感器直接测量出的小车的运动轨迹地图，图7(a)为基于点特征的vins-fusion开源系统测量出的小车的运动轨迹地图，图7(b)为基于点线综合特征的VIO系统测量出的小车的运动轨迹地图。对比图6及图7(a)，可知基于点特征的vins-fusion开源系统测量出的小车的运动轨迹地图最后出现了偏差，由于误差的存在，小车的轨迹地图不是闭合的，偏离了用激光SLAM传感器直接测量出的小车的运动轨迹。对比图6及图7(b)，可知本发明实施例提成的基于点线综合特征的VIO系统测出来的小车运动轨迹，由于加入了点线综合特征，小车的轨迹图与用激光SLAM传感器直接测量出的轨迹图，基本没有差异。综上，对比vins-fusion的开源框架，加入线特征后的VIO系统对在走廊弱纹理环境下的小车的定位更准确了，误差减少，所以小车的运动轨迹地图更接近真实的轨迹图。
本发明实施例提供的运动目标的定位方法，利用角点特征及边缘线特征综合的方法提取特征点，对特征点在下一帧中的位置进行追踪及估计，获取重投影误差，增强了前端视觉跟踪的鲁棒性及准确性；考虑运动目标运动环境中噪声误差以及惯性测量单元的误差的影响，将前端的数据误差进行后端非线性优化，提高了运动目标定位的准确性。
实施例2
本施例提供一种运动目标的定位系统，如图8所示，包括：
信息获取模块1，用于获取待定位目标的环境图像、加速度及角速度；此模块执行实施例1中的步骤S1所描述的方法，在此不再赘述。
边缘角点特征及预设噪声协方差项获取模块2，用于获取环境图像中待定位目标的边缘角点特征及预设噪声协方差项；此模块执行实施例1中的步骤S2所描述的方法，在此不再赘述。
重投影误差获取模块3，用于获取预设数量具有边缘角点特征的特征点的观测值及估计值，根据观测值及估计值获取待定位目标的重投影误差；此模块执行实施例1中的步骤S3所描述的方法，在此不再赘述。
增量误差及惯性测量噪声误差获取模块4，用于根据预设误差模型获取待定位目标的加速度及角速度的增量误差及惯性测量噪声误差；此模块执行实施例1中的步骤S4所描述的方法，在此不再赘述。
位姿信息生成模块5，用于利用预设优化算法对预设噪声协方差项、重投影误差、增量误差及惯性测量噪声误差进行非线性优化，得到待定位目标的位姿信息。此模块执行实施例1中的步骤S5所描述的方法，在此不再赘述。
本发明实施例提供的运动目标的定位系统，利用角点特征及边缘线特征综合的方法提取特征点，对特征点在下一帧中的位置进行追踪及估计，获取重投影误差，增强了前端视觉跟踪的鲁棒性及准确性；考虑运动目标运动环境中噪声误差以及惯性测量单元的误差的影响，将前端的数据误差进行后端非线性优化，提高了运动目标定位的准确性。
实施例3
本发明实施例提供一种计算机设备，如图9所示，包括：至少一个处理器401，例如CPU(Central Processing Unit，中央处理器)，至少一个通信接口403，存储器404，至少一个通信总线402。其中，通信总线402用于实现这些组件之间的连接通信。其中，通信接口403可以包括显示屏(Display)、键盘(Keyboard)，可选通信接口403还可以包括标准的有线接口、无线接口。存储器404可以是高速RAM存储器(Ramdom Access Memory，易挥发性随机存取存储器)，也可以是非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。存储器404可选的还可以是至少一个位于远离前述处理器401的存储装置。其中处理器401可以执行实施例1的运动目标的定位方法。存储器404中存储一组程序代码，且处理器401调用存储器404中存储的程序代码，以用于执行实施例1的运动目标的定位方法。
其中，通信总线402可以是外设部件互连标准(peripheral componentinterconnect，简称PCI)总线或扩展工业标准结构(extended industry standardarchitecture，简称EISA)总线等。通信总线402可以分为地址总线、数据总线、控制总线等。为便于表示，图9中仅用一条线表示，但并不表示仅有一根总线或一种类型的总线。
其中，存储器404可以包括易失性存储器(英文：volatile memory)，例如随机存取存储器(英文：random-access memory，缩写：RAM)；存储器也可以包括非易失性存储器(英文：non-volatile memory)，例如快闪存储器(英文：flash memory)，硬盘(英文：hard diskdrive，缩写：HDD)或固降硬盘(英文：solid-state drive，缩写：SSD)；存储器404还可以包括上述种类的存储器的组合。
其中，处理器401可以是中央处理器(英文：central processing unit，缩写：CPU)，网络处理器(英文：network processor，缩写：NP)或者CPU和NP的组合。
其中，处理器401还可以进一步包括硬件芯片。上述硬件芯片可以是专用集成电路(英文：application-specific integrated circuit，缩写：ASIC)，可编程逻辑器件(英文：programmable logic device，缩写：PLD)或其组合。上述PLD可以是复杂可编程逻辑器件(英文：complex programmable logic device，缩写：CPLD)，现场可编程逻辑门阵列(英文：field-programmable gate array，缩写：FPGA)，通用阵列逻辑(英文：generic arraylogic,缩写：GAL)或其任意组合。
可选地，存储器404还用于存储程序指令。处理器401可以调用程序指令，实现如本申请执行实施例1中的运动目标的定位方法。
本发明实施例还提供一种计算机可读存储介质，计算机可读存储介质上存储有计算机可执行指令，该计算机可执行指令可执行实施例1的运动目标的定位方法。其中，存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)、随机存储记忆体(RandomAccess Memory，RAM)、快闪存储器(Flash Memory)、硬盘(Hard Disk Drive，缩写：HDD)或固降硬盘(Solid-State Drive，SSD)等；存储介质还可以包括上述种类的存储器的组合。
显然，上述实施例仅仅是为清楚地说明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引申出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (10)

1.一种运动目标的定位方法，其特征在于，包括如下步骤：
获取待定位目标的环境图像、加速度及角速度；
获取环境图像中待定位目标的边缘角点特征及预设噪声协方差项；
获取预设数量具有边缘角点特征的特征点的观测值及估计值，根据所述观测值及估计值获取待定位目标的重投影误差；
根据预设误差模型获取待定位目标的加速度及角速度的增量误差及惯性测量噪声误差；
利用预设优化算法对所述预设噪声协方差项、重投影误差、增量误差及惯性测量噪声误差进行非线性优化，得到待定位目标的位姿信息。
2.根据权利要求1所述的运动目标的定位方法，其特征在于，所述获取环境图像中待定位目标的边缘角点特征的过程，包括以下步骤：
获取滤除所述环境图像信息噪声后像素点的灰度值；
根据像素点的灰度值，获取像素点的综合梯度；
根据所述像素点的综合梯度，获取环境图像边缘的第一边缘点；
根据非极大值抑制准则，去除环境图像边缘的第一边缘点中的伪边缘点，获取环境图像边缘的第二边缘点；
利用滞后阈值对所述环境图像边缘的第二边缘点进行优化处理，获取环境图像边缘及边缘线特征；
对所述环境图像边缘进行角点检测获取边缘角点特征。
3.根据权利要求2所述的运动目标的定位方法，其特征在于，所述获取预设数量具有边缘角点特征的特征点的观测值及估计值，根据所述观测值及估计值获取待定位目标的重投影误差的过程，包括以下步骤：
在当前帧上选取预设数量具有角点特征的特征点，利用金字塔光流方法追踪相邻帧对应的特征点作为观测特征点，获取观测特征点投影位置的观测值；
获取所述特征点三维坐标按照当前估计位姿进行投影得到的估计值；
将所述观测值与所述估计值进行比较得到待定位目标的重投影误差。
4.根据权利要求3所述的运动目标的定位方法，其特征在于，所述在当前帧上选取预设数量具有角点特征的特征点，利用金字塔光流方法追踪相邻帧对应的特征点作为观测特征点的过程，包括以下步骤：
获取当前帧上预设数量角点特征较强的特征点；
基于所述角点特征较强的特征点，利用金字塔光流方法追踪在环境图像下一帧中对应的特征点作为匹配特征点；
滤除匹配特征点中的异常点，获取观测特征点。
5.根据权利要求4所述的运动目标的定位方法，其特征在于，所述获取预设数量具有边缘角点特征的特征点的观测值及估计值，根据所述观测值及估计值获取待定位目标的重投影误差的过程，还包括如下步骤：
判断下一帧观测特征点的数量是否等于上一帧特征点的数量；
当下一帧观测特征点的数量等于上一帧特征点的数量时，将下一帧观测特征点作为当前帧上预设数量角点特征较强的特征点。
6.根据权利要求5所述的运动目标的定位方法，其特征在于，当下一帧观测特征点的数量小于上一帧特征点的数量时，获取下一帧上角点特征较强的特征点，将下一帧观测特征点及下一帧上角点特征较强的特征点作为当前帧上预设数量角点特征较强的特征点，其中下一帧观测特征点及下一帧上角点特征较强的特征点的数量总和，与上一帧特征点的数量相等。
7.根据权利要求6所述的运动目标的定位方法，其特征在于，利用预设优化算法对所述预设噪声协方差项、重投影误差、增量误差及惯性测量噪声误差进行非线性优化，得到待定位目标的位姿信息的过程，包括以下步骤：
构建基于预设噪声协方差项、环境图像所有帧的重投影误差、增量误差及惯性测量噪声误差的优化目标函数；
利用基于滑动窗口的预设优化方法，对优化目标函数进行非线性优化，生成待定位目标的位姿信息。
8.一种运动目标的定位系统，其特征在于，包括：
信息获取模块，用于获取待定位目标的环境图像、加速度及角速度；
边缘角点特征及预设噪声协方差项获取模块，用于获取环境图像中待定位目标的边缘角点特征及预设噪声协方差项；
重投影误差获取模块，用于获取预设数量具有边缘角点特征的特征点的观测值及估计值，根据所述观测值及估计值获取待定位目标的重投影误差；
增量误差及惯性测量噪声误差获取模块，用于根据预设误差模型获取待定位目标的加速度及角速度的增量误差及惯性测量噪声误差；
位姿信息生成模块，用于利用预设优化算法对所述预设噪声协方差项、重投影误差、增量误差及惯性测量噪声误差进行非线性优化，得到待定位目标的位姿信息。
9.一种计算机设备，其特征在于，包括：至少一个处理器，以及与所述至少一个处理器通信连接的存储器，其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器执行权利要求1-7中任一所述的运动目标的定位方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使所述计算机执行权利要求1-7中任一所述的运动目标的定位方法。
CN201911365228.XA 2019-12-26 2019-12-26 一种运动目标的定位方法及系统 Active CN111210477B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911365228.XA CN111210477B (zh) 2019-12-26 2019-12-26 一种运动目标的定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911365228.XA CN111210477B (zh) 2019-12-26 2019-12-26 一种运动目标的定位方法及系统

Publications (2)

Publication Number Publication Date
CN111210477A true CN111210477A (zh) 2020-05-29
CN111210477B CN111210477B (zh) 2023-07-04

Family

ID=70789346

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911365228.XA Active CN111210477B (zh) 2019-12-26 2019-12-26 一种运动目标的定位方法及系统

Country Status (1)

Country Link
CN (1) CN111210477B (zh)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111795686A (zh) * 2020-06-08 2020-10-20 南京大学 一种移动机器人定位与建图的方法
CN111811502A (zh) * 2020-07-10 2020-10-23 北京航空航天大学 一种运动载体多源信息融合导航方法及系统
CN111951262A (zh) * 2020-08-25 2020-11-17 杭州易现先进科技有限公司 Vio误差的修正方法、装置、系统和电子装置
CN112162561A (zh) * 2020-10-13 2021-01-01 上海美迪索科电子科技有限公司 一种地图构建优化方法、装置、介质及设备
CN112284380A (zh) * 2020-09-23 2021-01-29 深圳市富临通实业股份有限公司 一种基于光流和imu融合的非线性估计方法及系统
CN113108791A (zh) * 2021-03-05 2021-07-13 深圳大学 一种导航定位方法及导航定位设备
CN113108780A (zh) * 2021-03-30 2021-07-13 沈奥 一种基于视觉惯导slam算法的无人船自主导航方法
CN113112404A (zh) * 2021-04-02 2021-07-13 广州图匠数据科技有限公司 一种基于滑动窗口的图像拼接方法及装置
CN113155121A (zh) * 2021-03-22 2021-07-23 珠海深圳清华大学研究院创新中心 一种车辆定位方法、装置及电子设备
CN113467464A (zh) * 2021-07-17 2021-10-01 普达迪泰（天津）智能装备科技有限公司 一种用于无人车轨迹模拟测试的卫星定位运动轨迹方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190204084A1 (en) * 2017-09-29 2019-07-04 Goertek Inc. Binocular vision localization method, device and system
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190204084A1 (en) * 2017-09-29 2019-07-04 Goertek Inc. Binocular vision localization method, device and system
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111795686A (zh) * 2020-06-08 2020-10-20 南京大学 一种移动机器人定位与建图的方法
CN111795686B (zh) * 2020-06-08 2024-02-02 南京大学 一种移动机器人定位与建图的方法
CN111811502A (zh) * 2020-07-10 2020-10-23 北京航空航天大学 一种运动载体多源信息融合导航方法及系统
CN111951262A (zh) * 2020-08-25 2020-11-17 杭州易现先进科技有限公司 Vio误差的修正方法、装置、系统和电子装置
CN111951262B (zh) * 2020-08-25 2024-03-12 杭州易现先进科技有限公司 Vio误差的修正方法、装置、系统和电子装置
CN112284380A (zh) * 2020-09-23 2021-01-29 深圳市富临通实业股份有限公司 一种基于光流和imu融合的非线性估计方法及系统
CN112162561A (zh) * 2020-10-13 2021-01-01 上海美迪索科电子科技有限公司 一种地图构建优化方法、装置、介质及设备
CN113108791B (zh) * 2021-03-05 2023-08-04 深圳大学 一种导航定位方法及导航定位设备
CN113108791A (zh) * 2021-03-05 2021-07-13 深圳大学 一种导航定位方法及导航定位设备
CN113155121B (zh) * 2021-03-22 2024-04-02 珠海深圳清华大学研究院创新中心 一种车辆定位方法、装置及电子设备
CN113155121A (zh) * 2021-03-22 2021-07-23 珠海深圳清华大学研究院创新中心 一种车辆定位方法、装置及电子设备
CN113108780A (zh) * 2021-03-30 2021-07-13 沈奥 一种基于视觉惯导slam算法的无人船自主导航方法
CN113112404B (zh) * 2021-04-02 2023-04-28 广州图匠数据科技有限公司 一种基于滑动窗口的图像拼接方法及装置
CN113112404A (zh) * 2021-04-02 2021-07-13 广州图匠数据科技有限公司 一种基于滑动窗口的图像拼接方法及装置
CN113467464A (zh) * 2021-07-17 2021-10-01 普达迪泰（天津）智能装备科技有限公司 一种用于无人车轨迹模拟测试的卫星定位运动轨迹方法

Also Published As

Publication number Publication date
CN111210477B (zh) 2023-07-04

Similar Documents

Publication Publication Date Title
CN111210477B (zh) 一种运动目标的定位方法及系统
CN110322500B (zh) 即时定位与地图构建的优化方法及装置、介质和电子设备
CN112734852B (zh) 一种机器人建图方法、装置及计算设备
US10275649B2 (en) Apparatus of recognizing position of mobile robot using direct tracking and method thereof
CN110763251B (zh) 视觉惯性里程计优化的方法及系统
US9888235B2 (en) Image processing method, particularly used in a vision-based localization of a device
US10399228B2 (en) Apparatus for recognizing position of mobile robot using edge based refinement and method thereof
CN111830953B (zh) 车辆自定位方法、装置及系统
CN107016705A (zh) 计算机视觉系统中的地平面估计
US10268929B2 (en) Method and device for generating binary descriptors in video frames
CN110349212B (zh) 即时定位与地图构建的优化方法及装置、介质和电子设备
CN101216941A (zh) 剧烈光照变化下基于角点匹配与光流法的运动估计方法
CN105205459B (zh) 一种图像特征点类型的识别方法和装置
CN111609868A (zh) 一种基于改进光流法的视觉惯性里程计方法
CN112541423A (zh) 一种同步定位与地图构建方法和系统
CN112179373A (zh) 一种视觉里程计的测量方法及视觉里程计
CN113188509B (zh) 一种测距方法、装置、电子设备及存储介质
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN112837404B (zh) 一种平面对象的三维信息的构建方法及装置
CN111862146B (zh) 一种目标对象的定位方法及装置
Li et al. RD-VIO: Robust visual-inertial odometry for mobile augmented reality in dynamic environments
JP2018116147A (ja) 地図作成装置、地図作成方法及び地図作成用コンピュータプログラム
CN113959435A (zh) 一种基于多相机模型的车载环视在线slam系统及方法
Knorr Self-Calibration of Multi-Camera Systems for Vehicle Surround Sensing
CN117576200B (zh) 一种长周期移动机器人定位方法、系统、设备及介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant