CN113327270A - 视觉惯导方法、装置、设备及计算机可读存储介质 - Google Patents

视觉惯导方法、装置、设备及计算机可读存储介质 Download PDF

Info

Publication number
CN113327270A
CN113327270A CN202010131515.0A CN202010131515A CN113327270A CN 113327270 A CN113327270 A CN 113327270A CN 202010131515 A CN202010131515 A CN 202010131515A CN 113327270 A CN113327270 A CN 113327270A
Authority
CN
China
Prior art keywords
value
estimation
observation
state
inertial navigation
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.)
Pending
Application number
CN202010131515.0A
Other languages
English (en)
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.)
Syrius Technology Shenzhen Co Ltd
Original Assignee
Syrius Technology Shenzhen Co Ltd
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 Syrius Technology Shenzhen Co Ltd filed Critical Syrius Technology Shenzhen Co Ltd
Priority to CN202010131515.0A priority Critical patent/CN113327270A/zh
Publication of CN113327270A publication Critical patent/CN113327270A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Optics & Photonics (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种视觉惯导方法、装置、设备及计算机可读存储介质。其中，该方法包括：通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值；确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项；根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计。实现了一种融合深度信息的视觉惯导里程计方案，提高了里程计的准确性，增强了里程计在弱光照或弱纹理环境下的稳定性，同时，节省了系统开销。

Description

视觉惯导方法、装置、设备及计算机可读存储介质
技术领域
本发明涉及惯导技术领域，尤其涉及一种视觉惯导方法、装置、设备及计算机可读存储介质。
背景技术
现有技术中，VIO(Visual Inertial Odometry，视觉惯导里程计)是指利用视觉传感器(camera)以及惯导传感器(IMU，Inertial measurement unit)数据估计系统本体当前位姿的一种算法。这种算法在自主移动视觉模组，无人车，无人机定位导航、AR(AugmentedReality，增强现实)等方面都有着重要应用。视觉惯导里程计作为移动视觉模组相对定位的有效传感器，为视觉模组提供了实时的位姿信息。与纯视觉里程计相比，视觉惯导里程计具有以下优点：
1.能通过IMU获取绝对尺度的估计；
2.对物体运动过快导致图像畸变的情况有更鲁棒表现；
3.在短暂视觉跟踪失败时，能依靠IMU数据继续保证系统正常运行。
尽管理论上视觉传感器和惯导传感器的优点和缺点具有互补性，但由于低成本惯导传感器导致视觉惯导里程计仍然具有一些不足，一是，对于民用级别等低成本的惯导传感器，例如手机上使用的惯导传感器，或者车载千元级别的惯导传感器，由于精度不准确，本身噪音和零偏较大，导致在运行过程中可能会出现漂移的现象，且随着时间的累计误差会越来越大，从而导致出现不准确的状态估计，二是，在弱光照环境以及低纹理环境下，视觉惯导里程计的稳定性较弱。
发明内容
为了解决现有技术中视觉惯导里程计准确性和稳定性较差、计算效率不高的技术缺陷，本发明提出了一种视觉惯导方法、装置、设备及计算机可读存储介质。其中，首先，提出了一种视觉惯导方法，该方法包括：
通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值；
确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项；
根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计。
本发明还提出了一种视觉惯导装置，该装置包括：
状态预测模块，用于通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值；
残差构建模块，用于确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项；
位姿修正模块，用于根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计。
本发明还提出了一种视觉惯导设备，该视觉惯导设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如上所述的视觉惯导方法的步骤。
本发明还提出了一种计算机可读存储介质，该计算机可读存储介质上存储有视觉惯导程序，所述视觉惯导程序被处理器执行时实现如上所述的视觉惯导方法的步骤。
本发明的有益效果在于，通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值；确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项；根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计。实现了一种融合深度信息的视觉惯导里程计方案，提高了里程计的准确性，增强了里程计在弱光照或弱纹理环境下的稳定性，同时，节省了系统开销。
附图说明
下面将结合附图及实施例对本发明作进一步说明，附图中：
图1是本发明实施例提供的视觉惯导方法的第一流程图；
图2是本发明实施例提供的视觉惯导方法的第二流程图；
图3是本发明实施例提供的视觉惯导方法的第三流程图；
图4是本发明实施例提供的视觉惯导方法的第四流程图；
图5是本发明实施例提供的视觉惯导方法的第五流程图；
图6是本发明实施例提供的视觉惯导方法的第六流程图；
图7是本发明实施例提供的视觉惯导方法的第七流程图；
图8是本发明实施例提供的视觉惯导方法的第八流程图；
图9是本发明实施例提供的视觉惯导装置的结构框图。
具体实施方式
应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
在后续的描述中，使用用于表示元件的诸如“模块”、“部件”或“单元”的后缀仅为了有利于本发明的说明，其本身没有特定的意义。因此，“模块”、“部件”或“单元”可以混合地使用。
实施例一
图1是本发明实施例提供的视觉惯导方法的第一流程图。本实施例提出了一种视觉惯导方法，该方法包括：
S1、通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值；
S2、确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项；
S3、根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计。
在本实施例中，首先，通过IMU数据累计的方式，将视觉模组的位姿由上一时刻预测当前状态。其中，需要首先说明的是，本实施例的视觉惯导方法适用于导航仪、视觉模组等具备自主定位定位功能的设备或模块。本实施例的整体方案是，采用滤波的方法，通过前一时刻的位置和姿态以及当前时刻的传感器数据，估计出当前时刻系统所处的位置和姿态。具体的，在本实施例中，以具备基础视觉惯导组件的视觉模组为例进行说明，该视觉模组配置有相应的视觉组件和惯导组件，其中，通过视觉组件获取所需的图像信息和深度信息，通过惯导组件获取所需的惯导传感数据。在本实施例的先前步骤中，首先，对视觉模组的位姿进行初始化，然后，在开始执行视觉惯导任务的过程中，考虑到惯导组件获取惯导传感数据的频率高于视觉组件获取图像信息的频率，因此，通过IMU数据累计的方式将视觉模组的位姿由初始状态预测当前时刻状态。
在本实施例中，首先对视觉模组状态进行初始化，初始化包括初始化世界坐标系，视觉模组状态以及IMU加速度计以及角速度计的bias，再根据所述视觉模组获取的RGB图像或者灰度图结合深度图确定特征点在世界坐标系中的坐标估计值，结合所述坐标估计值计算对应的观测值估计，与实际观测值进行残差项的构建，在利用残差项对视觉模组状态进行修正得到当前时刻的视觉模组位姿估计值。其中，由于IMU频率比视觉模组频率高，因此每获取一帧图像，需要利用IMU数据累计通过上一时刻状态预测当前时刻状态。随后对前一帧图像和后一帧图像进行特征提取与匹配，得到与当前时刻有相关性的特征点及其观测值和对应深度，对于无效深度的特征点，其对应的观测值视为2D观测，否则视为3D观测。并根据该观测值分别计算该特征点在世界坐标系下的坐标估计值，利用所述特征点世界坐标估计值并反投影到多相机状态计算对应的观测值估计值(包含2D观测估计值以及3D观测估计值)，将观测值估计值与实际观测值进行差值运算得到2D残差项和3D残差项。随后，首先利用2D残差项修正视觉模组的位姿预测得到第一位姿估计，最后利用3D残差项修正第一位姿估计值，得到当前时刻视觉模组的位姿估计。
在本实施例中，得到当前时刻视觉模组的位姿估计后，对无用信息进行删除，具体为过于旧的关键帧，以及已无任何关联特征点的关键帧，并对滤波器状态中相应信息进行删除。进一步的，重复执行上述状态更新操作、残差获取操作以及位姿修正操作，实时获取最新状态下的位姿信息，直至本次视觉惯导任务执行完毕。
本实施例的有益效果在于，通过IMU数据累计将视觉模组的位姿由初始状态预测至当前状态；然后，根据所述视觉模组获取RGB图像或者灰度图结合深度图确定特征点在世界坐标系中的坐标估计值，根据深度值有效与否以及是否在预设阈值范围内对特征点进行分类，根据对应类别计算特征点的2D观测值估计值，和3D观测值估计值，分别与对应实际观测值进行残差项构建；最后，根据所述残差项对当前状态下的位姿进行修正，首先利用2D残差进行修正，在利用3D残差进行修正，得到最新状态下的位姿估计。实现了一种融合深度信息的视觉惯导里程计方案，提高了里程计的准确性，增强了里程计在弱光照或弱纹理环境下的稳定性，同时，节省了系统开销。
实施例二
图2是本发明实施例提供的视觉惯导方法的第二流程图。基于上述实施例，通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值，包括：
S11、在上一时刻到当前时刻的时间段内，对所述惯导传感数据中的加速度计数值以及角速度计数值进行累计，同时，结合所述视觉模组在上一时刻的状态估计，得到所述状态预测值。
在本实施例中，按预设的图像获取频率获取所需的RGB图像或者灰度图结合深度图像时，由于惯导组件获取惯导传感数据的频率高于视觉组件获取图像信息的频率，因此，在初始时刻到当前时刻的时间段内，对惯导传感数据中的加速度计数值以及角速度计数值进行累计，从而得到视觉模组在当前状态下的状态预测值。
具体的，例如，首先，确定视觉模组在一个t时刻的姿态，然后，将该t时刻之后的delta t时间记为t+1时刻，为了得到该t+1时刻的状态估值，在本实施例中，将利用惯导传感数据中的加速度计数值以及角速度计数值，进行相应的累计，从而得到上述t+1时刻的状态预测值。
可选的，在本实施例中，在对惯导传感数据中的加速度计数值以及角速度计数值进行累计之前，还需判断当前已有IMU数据是否可以进行初始化，具体的：
首先，获取两段N(0.5<N<20)秒的IMU数据段，第一段数据为相对较新时刻数据，第二段数据为旧时刻数据，用第一段IMU数据段判断预设时间段内的惯导传感数据的方差是否大于预设阈值；
然后，若上述惯导传感数据的方差大于所述预设阈值，则进行下一步初始化步骤，若惯导传感数据的方差小于所述预设阈值，则判断为初始化失败，则继续获取惯导传感数据，直到第一段数据内IMU数值的方差大于阈值，表示IMU得到充分激励。其中，第一段数据主要是判断开始模组开始运动，可以理解的是，第一段数据是较旧的数据，它实际上的是模组在开始运动之前静止的数据，它将会被用来进行初始化。
可选的，在本实施例中，通过当前已有IMU数据确定可执行初始化之后，再执行初始化操作，具体的：
首先，当所述第一段IMU数据段被判断为有充分激励，获取第二段IMU数据段进行具体初始化；
然后，所述激励若足够，则利用第二段IMU数据段，即旧IMU数据段对IMU状态进行初始化，首先对所述IMU数据段进行累加求平均值，对该平均值求向量模长视为重力加速度常量的估计值，随后构建重力向量(0，0，-gravity_norm)，并结合所述加速度平均值，求解IMU与世界坐标系的旋转四元数。由于此段数据被获取时IMU处于相对静止状态，所以IMU的零偏设定为IMU噪声，至此初始化成功。
需要理解的是，在本实施例中，如上例所述，若当计算得到的方差大于上述设定的阈值，则表示上述惯导被有效激励，可以进行初始化，利用所述第二IMU数据段计算加速度计与角速度计的偏置量bias，其中，偏置量bias表示当前惯导传感组件所获取的测量数据与真实数据相差部分。
本实施例的有益效果在于，一方面，通过在上一时刻到当前时刻的时间段内，对所述惯导传感数据中的加速度计数值以及角速度计数值进行累计，提供了初始化的判定依据和初始化的执行方案，另一方面，结合所述视觉模组在上一时刻的状态估计，从而得到视觉模组在当前状态下的位姿预测，为后续进行估值修正提供了数据基础。
实施例三
图3是本发明实施例提供的视觉惯导方法的第三流程图。基于上述实施例，确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项，包括：
S21、根据所述视觉模组获取的RGB图像或灰度图像进行特征提取和匹配，得到被跟踪的特征点及其观测值。
在本实施例中，当完成初始化后，首先，通过视觉模组获取相应的RGB图像或灰度图像，然后，根据当前的定位需求以及相应的参数配置，执行相关的特征提取和匹配操作，得到被跟踪的特征点及其观测值。
在本实施例中，通过对所述视觉模组获取的RGB图像或灰度图像进行特征提取和匹配，得到被跟踪的特征点及其观测值，从而为后续的坐标转换以及差值计算提供了数据基础。
实施例四
图4是本发明实施例提供的视觉惯导方法的第四流程图。基于上述实施例，确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项，包括：
S22、结合所述多相机状态对所述特征点的观测值，计算所述特征点在世界坐标系中的坐标估计值，再将所述坐标估计值投影到所述多相机状态中，得到所述特征点的观测值估计值。
在本实施例中，以某一特定点，以及C1、C2、C3表示三个时刻的相机状态为例作说明。首先，根据上述三个时刻的相机状态C1、C2、C3得到对该特征点的实际观测值m1、m2、m3；然后，利用上述三个时刻的相机状态C1、C2、C3以及三个实际观测值m1、m2、m3计算该特征点在世界坐标系中的坐标估计值，然后，将此坐标估计值反投影至上述三个时刻的相机状态C1、C2、C3，得到该特征点对于上述三个时刻的相机状态C1、C2、C3的三个观测值估计值。可以理解的是，本实施例针对某一特征点，不限于采用三个时刻的相机状态，而可以根据实际的定位需求以及相应的参数配置，采用相应规模的相机状态。
本实施例的有益效果在于，通过结合所述多相机状态对所述特征点的观测值，计算所述特征点在世界坐标系中的坐标估计值，再将所述坐标估计值投影到所述多相机状态中，得到所述特征点的观测值估计值，为后续提供了相应的计算方案和数据基础。
图5是本发明实施例提供的视觉惯导方法的第五流程图。基于上述实施例，结合所述多相机状态对所述特征点的观测值，计算所述特征点在世界坐标系中的坐标估计值，再将所述坐标估计值投影到所述多相机状态中，得到所述特征点的观测值估计值，包括：
S221、根据所述观测值确定所述特征点在对应的深度图中的深度值，以及，确定所述深度值是否处于预设的深度阈值范围；
S222、若所述深度值处于预设的深度阈值范围，则通过加权最小二乘法计算所述特征点在所述世界坐标系中的、或对于相应相机状态的坐标估计值，并将计算得到的坐标反投影到所述多相机状态中，得到所述多相机状态对所述特征点的第二观测值估计值；
S223、若所述深度值未处于所述深度阈值范围，则通过三角化法计算所述特征点在所述世界坐标系中的、或对于相应相机状态的坐标估计值，并将计算得到的坐标反投影到所述多相机状态中，得到所述多相机状态对所述特征点的第一观测值估计值。
具体的，为了确定当前图像帧中是否包含有效的深度信息，在本实施例中，首先根据视觉组件的参数信息以及相应的计算需求，设定预设的深度阈值范围，然后，若所述特征点的深度值不在所述深度阈值范围内，则表明该特征点的深度值是不准确的，利用多相机状态对特征点的二维观测通过三角化的计算方案得到所述特征点在世界坐标系中的坐标估计值，记为第一观测值估计值，若所述特征点的深度值在所述深度阈值范围内，则表明该特征点的深度值是有效的或准确的，在此，利用多相机状态对特征点的二维观测以及深度观测(即三维观测)通过所述加权最小二乘法的计算方案得到所述特征点在世界坐标系中的坐标估计值，记为第二观测值估计值。
为了进一步解释本实施例的实施方案，以下将通过具体示例说明如何通过带有深度信息的特征点计算得到世界坐标系下的坐标估计值。
首先，构建3D特征点不确定模型，每一帧RGB image(红、绿、蓝三色色彩模式图像)对应一帧range image(深度图)，其中，RGB image像素点的深度值是range image对应坐标的值。其中，对于一像素点pj，视其对应深度值dj为随机变量，且服从均值为
Figure BDA0002395887340000091
方差为
Figure BDA0002395887340000092
的高斯分布。同样假设该像素点在图像上的坐标(μj，vj)也服从高斯分布
Figure BDA0002395887340000093
其分布由该像素周围的深度值决定。
具体的，设某一特征点的在相机坐标系下的观测深度值为
Figure BDA0002395887340000094
表示在相机位姿i下观测特征点fj的深度观测值，将
Figure BDA0002395887340000095
建模成符合混合高斯分布，并且由该像素点的周围像素的深度值决定其分布，由此得到深度观测值的不确定模型，根据相机模型可以推出该特征点在相机坐标系下
Figure BDA0002395887340000096
值的分布,对应期望为
Figure BDA0002395887340000101
最终在相机位姿i下观测特征点fj的观测值为
Figure BDA0002395887340000102
对应协方差∑i
对于同一个特征点，我们有多组观测u＝{u1 … un}以及对应协方差∑＝{∑1 ...∑n}
世界坐标系下的特征点坐标与camera坐标系下坐标的映射关系：
Figure BDA0002395887340000103
利用加权最小二乘法即可求得该特征点在世界坐标系下的坐标的估计值
Figure BDA0002395887340000104
其中，
Figure BDA0002395887340000105
表示i时刻，IMU坐标系与相机坐标系之间的旋转矩阵，以此类推，左边角标分别可以是G世界坐标系，C相机坐标系以及I IMU坐标系；
P一般指坐标值，例如cPI表示相机坐标系下IMU的坐标；
约定带有～上标的符号为误差值，带有^上标的符号为测量值或者估计值。
由于使用加权最小二乘法求解，省去了类似LM、GN等算法的迭代过程，从而降低计算量，提高计算效率。
至此，已阐述完毕带有深度信息的特征点如何计算世界坐标系坐标
Figure BDA0002395887340000106
但在实际任务执行过程中，并非所有特征点都带有有效的深度信息，本实施例的解决方案是，给深度设定一个有效阈值，深度低于该阈值，且该特征点周围像素均有有效深度，那么该特征点被视为有效深度的特征点，归类为3D特征，并用加权最小二乘进行计算
Figure BDA0002395887340000111
除此之外的特征点，归类为2D特征，相应的，采用三角化的做法计算
Figure BDA0002395887340000112
需要说明的是，上述方案基于主流的MSCKF(A multi-state constraint Kalmanfilter for vision-aided inertial navigation)，并融合深度传感器信息，状态向量数学模型与MSCKF一致。
具体的，以下对本实施例所采用的状态向量作相应说明：
IMU状态向量：
Figure BDA0002395887340000113
上述状态向量中各项参数分别表示世界坐标系到IMU坐标系的转换四元数，IMU在世界坐标系下的坐标，IMU本体在世界坐标系下的速度，角加速度计的偏置量，加速度计的偏置量，IMU坐标系到相机坐标系的转换四元数，相机在IMU坐标系下的坐标，以及IMU与相机之间获取数据的时间差。
滤波器状态向量：
Figure BDA0002395887340000114
其中，第一项为IMU当前时刻的状态，后面N项为滑动窗口大小的相机状态。需要说明的是，本实施例的IMU状态即IMU在世界坐标系中的坐标以及与世界坐标之间的转换关系。
本实施例的有益效果在于，通过确定预设的深度阈值范围；若所述特征点的深度值不在所述深度阈值范围内，则通过三角化计算所述特征点在世界坐标系中的坐标估计值，得到第一观测值估计值，若所述特征点的深度值在所述深度阈值范围内，则通过所述加权最小二乘法计算所述特征点在世界坐标系中的坐标估计值，得到第二观测值估计值。本实施例提高了视觉惯导任务的计算量，节省了系统开销，提高了处理效率。
实施例六
图6是本发明实施例提供的视觉惯导方法的第六流程图。基于上述实施例，所述确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项，还包括：
S23、对所述第二观测值估计值和所述观测值进行差值计算，得到三维残差项；
S24、对所述第一观测值估计值和所述观测值进行差值计算，得到二维残差项。
在本实施例中，一方面，对所述第二观测值估计值和所述观测值进行差值计算，得到三维残差项，另一方面，对所述第一观测值估计值和所述观测值进行差值计算，得到二维残差项。
具体的，在本实施例中，分别通过不具备深度特征的残差以及具备深度特征的残差对上述位姿估值进行修正，从而得到所述最新状态下的位姿估值。
如上例所述，当得到上述估值之后，便可以求出特征点观测的估计值与实际观测值之间的残差r，由于此残差与特征点有相关性，需要将相关部分投影到左零空间得到
Figure BDA0002395887340000121
然后通过以下公式更新状态向量以及协方差矩阵。对2Dfeatures和3Dfeatures分别进行一次修正；最后得到相应的Xt|t。
具体的，更新公式如下：
Figure BDA0002395887340000122
Figure BDA0002395887340000123
Figure BDA0002395887340000124
Figure BDA0002395887340000131
本实施例的有益效果在于，通过对所述第二观测值估计值和所述观测值进行差值计算，得到三维残差项，以及，对所述第一观测值估计值和所述观测值进行差值计算，得到二维残差项，为后续的位姿估计分别提供了两次迭代修正的数据基础。
实施例七
图7是本发明实施例提供的视觉惯导方法的第七流程图。基于上述实施例，根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计，包括：
S31、通过所述二维残差项对所述状态预测值进行第一次修正；
S32、通过所述三维残差项对所述第一次修正后的位姿估计进行第二次修正，得到当前时刻的位姿估计。
可选的，在本实施例中，若在上述实施例中，特征点的深度值均处于预设的深度阈值范围，则不生成上述二维残差项，仅通过三维残差项对所述状态预测值进行修正，得到当前时刻的位姿估计。
可选的，为了确定特征点的深度值，以及进一步确定深度值是否处于上述预设的深度阈值范围内，在本实施例中，通过深度相机获取深度图；
可选的，在本实施例中，通过3D激光测距模块、或者立体摄影模块等其它具备测距功能的组件获取特征点的深度值，从而便于确定该深度值是否处于上述预设的深度阈值范围内。
本实施例的有益效果在于，通过所述观测值确定所述特征点在对应的深度图中的深度值，以及，确定所述深度值是否处于预设的深度阈值范围；若所述深度值处于预设的深度阈值范围，则通过加权最小二乘法计算所述特征点在所述世界坐标系中的、或对于相应相机状态的坐标估计值，并将计算得到的坐标反投影到所述多相机状态中，得到所述多相机状态对所述特征点的第二观测值估计值；若所述深度值未处于所述深度阈值范围，则通过三角化法计算所述特征点在所述世界坐标系中的、或对于相应相机状态的坐标估计值，并将计算得到的坐标反投影到所述多相机状态中，得到所述多相机状态对所述特征点的第一观测值估计值。从而得到所述最新状态下的位姿估计，提高了视觉惯导任务的计算质量，节省了系统开销，提高了处理效率。
实施例八
图8是本发明实施例提供的视觉惯导方法的第八流程图。基于上述实施例，在本实施例中，给出了一个视觉惯导任务的完整执行流程。具体的，在本实施例中，首先，通过传感器获取所需的数据，其中，该数据包括两个部分，一是包含深度信息的图像数据，二是惯导传感数据；然后，通过预设时间段内惯导传感数据进行初始化。首先判断该时间段内IMU数据的方差是否足够大，若足够大则表示可以继续进行初始化，否则继续获取数据，直到方差足够大，即IMU被有效激励，利用前一段时间内的加速度计和角速度计的值计算对应的加速度和角速度的bias值和IMU坐标系与世界坐标系的转换关系，将IMU在世界坐标系下的速度设置为0，并且认为IMU此时处于世界坐标系的(0，0，0)坐标，至此，认为系统初始化成功。在下一帧图像被获取时，则通过IMU数据累计的方式得到当前时刻系统状态的预测，并对RGB图像或者灰度图进行特征点提取；直到第二帧图像被获取，IMU数据累计之后得到第二帧图像获取时刻的IMU状态预测。并对RGB图像或者灰度图进行特征点的提取与匹配，得到特征点包含被跟踪的特征点，根据特征点深度信息进行有效性判断，得到相应的2D features和3D features，对2D features进行三角化计算的到对应的世界坐标系坐标值估值，对3Dfeatures进行加权最小二乘法计算特征点在世界坐标系的坐标值估值。利用所述特征点在世界坐标系下的坐标估计值计算的到对应的二维观测值估计值和三维观测值估计值，进而得到二维残差项和三维残差项。并利用二维残差项对系统状态进行第一次修正，再利用三维残差项对系统再次进行修正得到最终系统状态估计。同时，在一个视觉惯导任务的执行周期内，重复执行上述状态更新操作、残差获取操作以及位姿修正操作，实时获取最新状态下的位姿信息，直至本次视觉惯导任务执行完毕。
可以理解的是，在本实施例中，将深度信息融合到视觉惯导方案中，并且有选择性地充分利用深度信息，将原本只有二维观测(图像的u，v坐标值)的特征点信息转化成三维信息(u，v，d)观测，一方面，使得本视觉惯导方案有更加准确地恢复尺度，另一方面，在计算特征点在世界坐标系的坐标时采用的是加权最小二乘法，能有效节省类似LM算法迭代求解带来的计算量。同时，由于加权最小二乘法求解特征点坐标比三角化方法有更高的准确度以及更高成功率，因此，在特征点稀少，或者环境光照较弱时依然能维持视觉惯导任务的正确运行。
实施例九
图9是本发明实施例提供的视觉惯导装置的结构框图。
本发明还提出了一种视觉惯导装置，该装置包括：
状态预测模块10，用于通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值；
残差构建模块20，用于确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项；
位姿修正模块30，用于根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计。
需要说明的是，上述装置实施例与方法实施例属于同一构思，其具体实现过程详细见方法实施例，且方法实施例中的技术特征在装置实施例中均对应适用，这里不再赘述。
实施例十
本发明还提出了一种视觉惯导设备，其特征在于，所述视觉惯导设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如上所述的视觉惯导方法的步骤。
需要说明的是，上述设备实施例与方法实施例属于同一构思，其具体实现过程详细见方法实施例，且方法实施例中的技术特征在设备实施例中均对应适用，这里不再赘述。
实施例十一
本发明还提出了一种计算机可读存储介质，该计算机可读存储介质上存储有视觉惯导程序，所述视觉惯导程序被处理器执行时实现如上所述的视觉惯导方法的步骤。
需要说明的是，上述介质实施例与方法实施例属于同一构思，其具体实现过程详细见方法实施例，且方法实施例中的技术特征在介质实施例中均对应适用，这里不再赘述。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。
上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。

Claims (10)

1.一种视觉惯导方法，其特征在于，所述方法包括：
通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值；
确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项；
根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计。
2.根据权利要求1所述的视觉惯导方法，其特征在于，所述通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值，包括：
在上一时刻到当前时刻的时间段内，对所述惯导传感数据中的加速度计数值以及角速度计数值进行累计，同时，结合所述视觉模组在上一时刻的状态估计，得到所述状态预测值。
3.根据权利要求1所述的视觉惯导方法，其特征在于，所述确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项，包括：
根据所述视觉模组获取的RGB图像或灰度图像进行特征提取和匹配，得到被跟踪的特征点及其观测值。
4.根据权利要求1所述的视觉惯导方法，其特征在于，所述确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项，包括：
结合所述多相机状态对所述特征点的观测值，计算所述特征点在世界坐标系中的坐标估计值，再将所述坐标估计值投影到所述多相机状态中，得到所述特征点的观测值估计值。
5.根据权利要求4所述的视觉惯导方法，其特征在于，所述结合所述多相机状态对所述特征点的观测值，计算所述特征点在世界坐标系中的坐标估计值，再将所述坐标估计值投影到所述多相机状态中，得到所述特征点的观测值估计值，包括：
根据所述观测值确定所述特征点在对应的深度图中的深度值，以及，确定所述深度值是否处于预设的深度阈值范围；
若所述深度值处于预设的深度阈值范围，则通过加权最小二乘法计算所述特征点在所述世界坐标系中的、或对于相应相机状态的坐标估计值，并将计算得到的坐标反投影到所述多相机状态中，得到所述多相机状态对所述特征点的第二观测值估计值；
若所述深度值未处于所述深度阈值范围，则通过三角化法计算所述特征点在所述世界坐标系中的、或对于相应相机状态的坐标估计值，并将计算得到的坐标反投影到所述多相机状态中，得到所述多相机状态对所述特征点的第一观测值估计值。
6.根据权利要求5所述的视觉惯导方法，其特征在于，所述所述确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项，还包括：
对所述第二观测值估计值和所述观测值进行差值计算，得到三维残差项；
以及，对所述第一观测值估计值和所述观测值进行差值计算，得到二维残差项。
7.根据权利要求6所述的视觉惯导方法，其特征在于，所述根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计，包括：
通过所述二维残差项对所述状态预测值进行第一次修正；
以及，通过所述三维残差项对所述第一次修正后的位姿估计进行第二次修正，得到当前时刻的位姿估计。
8.一种视觉惯导装置，其特征在于，所述装置包括：
状态预测模块，用于通过惯导传感数据累计以及视觉模组在上一时刻的状态估计，得到当前时刻的状态预测值；
残差构建模块，用于确定被跟踪的特征点及其观测值，通过多相机状态确定所述特征点的观测值估计值，并对所述观测值估计值和所述观测值进行差值计算，得到残差项；
位姿修正模块，用于根据所述残差项对所述状态预测值进行修正，得到当前时刻的位姿估计。
9.一种视觉惯导设备，其特征在于，所述视觉惯导设备包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如权利要求1-7任一项所述的视觉惯导方法的步骤。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有视觉惯导程序，所述视觉惯导程序被处理器执行时实现如权利要求1至7中任一项所述的视觉惯导方法的步骤。
CN202010131515.0A 2020-02-28 2020-02-28 视觉惯导方法、装置、设备及计算机可读存储介质 Pending CN113327270A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010131515.0A CN113327270A (zh) 2020-02-28 2020-02-28 视觉惯导方法、装置、设备及计算机可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010131515.0A CN113327270A (zh) 2020-02-28 2020-02-28 视觉惯导方法、装置、设备及计算机可读存储介质

Publications (1)

Publication Number Publication Date
CN113327270A true CN113327270A (zh) 2021-08-31

Family

ID=77412862

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010131515.0A Pending CN113327270A (zh) 2020-02-28 2020-02-28 视觉惯导方法、装置、设备及计算机可读存储介质

Country Status (1)

Country Link
CN (1) CN113327270A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113870367A (zh) * 2021-12-01 2021-12-31 腾讯科技（深圳）有限公司 相机外参的生成方法、装置、设备、存储介质和程序产品
CN114625121A (zh) * 2022-01-24 2022-06-14 成都理工大学 基于多传感器融合的自主巡检探索小车系统及导航方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113870367A (zh) * 2021-12-01 2021-12-31 腾讯科技（深圳）有限公司 相机外参的生成方法、装置、设备、存储介质和程序产品
CN113870367B (zh) * 2021-12-01 2022-02-25 腾讯科技（深圳）有限公司 相机外参的生成方法、装置、设备、存储介质和程序产品
CN114625121A (zh) * 2022-01-24 2022-06-14 成都理工大学 基于多传感器融合的自主巡检探索小车系统及导航方法

Similar Documents

Publication Publication Date Title
CN111811506B (zh) 视觉/惯性里程计组合导航方法、电子设备及存储介质
JP6760114B2 (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
CN109443348B (zh) 一种基于环视视觉和惯导融合的地下车库库位跟踪方法
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN111795686B (zh) 一种移动机器人定位与建图的方法
CN112233177B (zh) 一种无人机位姿估计方法及系统
CN112219087A (zh) 位姿预测方法、地图构建方法、可移动平台及存储介质
CN109903330B (zh) 一种处理数据的方法和装置
KR101985344B1 (ko) 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치
CN109141411B (zh) 定位方法、定位装置、移动机器人及存储介质
CN110749308B (zh) 使用消费级gps和2.5d建筑物模型的面向slam的室外定位方法
CN113899364B (zh) 定位方法及装置、设备、存储介质
CN113740871B (zh) 一种在高动态环境下的激光slam方法、系统设备及存储介质
CN113327270A (zh) 视觉惯导方法、装置、设备及计算机可读存储介质
CN114723779A (zh) 一种车辆定位方法、装置及计算机可读存储介质
CN113503873A (zh) 一种多传感器融合的视觉定位方法
WO2023087681A1 (zh) 定位初始化方法、装置、计算机可读存储介质和计算机程序产品
CN112179373A (zh) 一种视觉里程计的测量方法及视觉里程计
CN117367427A (zh) 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法
CN114812601A (zh) 视觉惯性里程计的状态估计方法、装置、电子设备
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN113034538B (zh) 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备
CN113847907A (zh) 定位方法及装置、设备、存储介质
CN111829552B (zh) 一种视觉惯性系统的误差修正方法和装置
CN112037261A (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
CB02 Change of applicant information

Address after: 518000 Room 401, block D, building 7, Shenzhen International Innovation Valley, Dashi 1st Road, Xili community, Xili street, Nanshan District, Shenzhen, Guangdong

Applicant after: Juxing Technology (Shenzhen) Co., Ltd

Address before: 518000 building 101, building R3b, Gaoxin industrial village, No.018, Gaoxin South 7th Road, community, high tech Zone, Yuehai street, Nanshan District, Shenzhen City, Guangdong Province

Applicant before: Juxing Technology (Shenzhen) Co., Ltd

CB02 Change of applicant information