CN115993132A - 视觉惯性里程计初始化方法、装置及飞行汽车 - Google Patents

Abstract

本申请涉及一种视觉惯性里程计初始化方法、装置及飞行汽车。该方法包括：获取视觉惯导里程计检测到的图像帧中的特征点，根据所述特征点确定相邻图像帧之间的单应矩阵；根据所述单应矩阵确定对应的多组位姿数据，根据预设方式从所述多组位姿数据中确定目标位姿数据；根据所述单应矩阵和第一预设算法确定尺度因子，根据所述尺度因子和所述目标位姿数据，确定实际位姿数据。本申请提供的方案，对于飞行器从地面起飞的阶段可以较好地实现视觉惯性里程计的初始化。

Description

视觉惯性里程计初始化方法、装置及飞行汽车

技术领域

本申请涉及飞行汽车技术领域，尤其涉及一种视觉惯性里程计初始化方法、装置及飞行汽车。

背景技术

目前，VIO(Visual-Inertial Odometry，视觉惯性里程计)综合了IMU(InertialMeasurement Unit，惯性测量单元)和视觉传感器各自的特点，主要通过IMU和视觉传感器例如相机的输入数据，估计出当前载体的运动状态信息，其中包括速度、姿态和位置等。

VIO系统中包括初始化过程。初始化过程主要是给状态估计变量找到一个较好的初值。目前的视觉惯性里程计初始化方案都是针对一些比较普遍的场景进行初始化，例如可以将视觉惯性里程计分为动态初始化和静态初始化，首先进行动态初始化，根据动态初始化的结果判断系统是否满足静态初始化条件，如果满足，则继续进行静态初始化。

但是，对于飞行器从地面起飞的阶段，相关技术中的初始化方案并不能得到很好的初始化效果。

发明内容

为解决或部分解决相关技术中存在的问题，本申请提供一种视觉惯性里程计初始化方法、装置及飞行汽车，对于飞行器从地面起飞的阶段可以较好地实现视觉惯性里程计的初始化。

本申请第一方面提供一种视觉惯性里程计初始化方法，包括：

获取视觉惯导里程计检测到的图像帧中的特征点，根据所述特征点确定相邻图像帧之间的单应矩阵；

根据所述单应矩阵确定对应的多组位姿数据，根据预设方式从所述多组位姿数据中确定目标位姿数据；

根据所述单应矩阵和第一预设算法确定尺度因子，根据所述尺度因子和所述目标位姿数据，确定实际位姿数据。

在一实施方式中，所述方法还包括：

根据所述单应矩阵确定所述相邻图像帧的特征点的速度关系；

根据所述相邻图像帧的特征点的速度关系、相邻图像帧的特征点的速度和第二预设算法，更新所述相邻图像帧的特征点的速度。

在一实施方式中，所述根据预设方式从所述多组位姿数据中确定目标位姿数据，包括：

根据先验法向量从所述多组位姿数据中选择先验法向量与单应法向量二范数符合预设关系的一组位姿数据为目标位姿数据。

在一实施方式中，所述根据所述单应矩阵和第一预设算法确定尺度因子，包括：

根据所述单应矩阵和PnP算法确定尺度因子。

在一实施方式中，所述根据所述单应矩阵和PnP算法确定尺度因子，包括：

获取所述特征点的深度信息，根据PnP算法和所述深度信息确定所述特征点从相机坐标系到世界坐标系的转换位姿；

根据所述转换位姿和所述单应矩阵确定的对应位姿数据，确定尺度因子。

在一实施方式中，所述第二预设算法包括高斯牛顿迭代法。

在一实施方式中，所述获取视觉惯导里程计检测到的图像帧中的特征点，包括：

获取视觉惯导里程计检测到的图像帧中大于预设数量阈值的特征点，并通过RANSAC算法删除异常的特征点。

本申请第二方面提供一种视觉惯性里程计初始化装置，包括：

单应矩阵确定模块，用于获取视觉惯导里程计检测到的图像帧中的特征点，根据所述特征点确定相邻图像帧之间的单应矩阵；

解算选择模块，用于根据所述单应矩阵确定对应的多组位姿数据，根据预设方式从所述多组位姿数据中确定目标位姿数据；

尺度因子及位姿模块，用于根据所述单应矩阵和第一预设算法确定尺度因子，根据所述尺度因子和所述目标位姿数据，确定实际位姿数据。

在一实施方式中，所述装置还包括：

速度迭代更新模块，用于根据所述单应矩阵确定所述相邻图像帧的特征点的速度关系；根据所述相邻图像帧的特征点的速度关系、相邻图像帧的特征点的速度和第二预设算法，更新所述相邻图像帧的特征点的速度。

本申请第三方面提供一种飞行汽车，包括如上所述的视觉惯性里程计初始化装置。

本申请第四方面提供一种飞行汽车，包括：

处理器；以及

存储器，其上存储有可执行代码，当所述可执行代码被所述处理器执行时，使所述处理器执行如上所述的方法。

本申请第五方面提供一种计算机可读存储介质，其上存储有可执行代码，当所述可执行代码被电子设备的处理器执行时，使所述处理器执行如上所述的方法。

本申请提供的技术方案可以包括以下有益效果：

本申请的技术方案，先在获取视觉惯导里程计检测到的图像帧中的特征点后，根据所述特征点确定相邻图像帧之间的单应矩阵；然后根据所述单应矩阵确定对应的多组位姿数据，根据预设方式从所述多组位姿数据中确定目标位姿数据；根据所述单应矩阵和第一预设算法确定尺度因子，根据所述尺度因子和所述目标位姿数据，确定实际位姿数据。本申请充分考虑了飞行器从地面静止起飞具有的特殊性，即飞行器起飞的过程中，下视相机检测到的特征点几乎都在一个平面上，因此本申请将单应矩阵的约束加入到了视觉惯性里程计的初始化过程中，使用单应矩阵进行视觉惯性里程计的初始化。通过上述处理，本申请对于飞行器从地面起飞的阶段可以较好地实现视觉惯性里程计的初始化，对于飞行器以不同的上升姿态起飞都可以得到鲁棒性和准确性的定位结果。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。

附图说明

通过结合附图对本申请示例性实施方式进行更详细地描述，本申请的上述以及其它目的、特征和优势将变得更加明显，其中，在本申请示例性实施方式中，相同的参考标号通常代表相同部件。

图1是本申请实施例示出的视觉惯性里程计初始化方法的流程示意图；

图2是本申请另一实施例示出的视觉惯性里程计初始化方法的流程示意图；

图3是本申请实施例示出的飞行器从地面起飞过程的示意图；

图4是本申请实施例示出的视觉惯性里程计初始化装置的结构示意图；

图5是本申请实施例示出的电子设备的结构示意图。

具体实施方式

下面将参照附图更详细地描述本申请的实施方式。虽然附图中显示了本申请的实施方式，然而应该理解，可以以各种形式实现本申请而不应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了使本申请更加透彻和完整，并且能够将本申请的范围完整地传达给本领域的技术人员。

在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。

应当理解，尽管在本申请可能采用术语“第一”、“第二”、“第三”等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本申请的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

相关技术中，目前的视觉惯性里程计初始化方案都是针对一些比较普遍的场景进行初始化。对于飞行器从地面起飞的情况，相关技术中的初始化方案并不能得到很好的初始化效果，其主要原因包括：1)因为飞行器安装的是双目下视相机，即飞行器在地面静止时，相机距离地面大概只有20～30厘米，这个距离下双目相机有很大的盲区，所以检测到的特征点一般很少，这导致某些初始化方案会完全失效，因为这些方案都需要检测到足够的特征点。2)飞行器在起飞的过程中，检测到的特征点都是在一个平面上，这是一个很强的约束关系，相关技术中的初始化方案都是一些普遍的方案，并没有针对这种特定情况进行建模，因此初始化效果较差。

为解决上述问题，本申请充分考虑了飞行器从地面起飞时的特殊性，提供一种视觉惯性里程计初始化方法，对于飞行器从地面起飞的阶段可以较好地实现视觉惯性里程计的初始化。

以下结合附图详细描述本申请实施例的技术方案。

图1是本申请实施例示出的视觉惯性里程计初始化方法的流程示意图。

参见图1，该方法包括：

S101、获取视觉惯导里程计检测到的图像帧中的特征点，根据特征点确定相邻图像帧之间的单应矩阵。

其中，获取视觉惯导里程计检测到的图像帧中大于预设数量阈值的特征点，并通过RANSAC(RandomSAmpleConsensus，随机采样一致)算法删除异常的特征点，然后再根据特征点确定相邻图像帧之间的单应矩阵。

S102、根据单应矩阵确定对应的多组位姿数据，根据预设方式从多组位姿数据中确定目标位姿数据。

其中，根据先验法向量从多组位姿数据中选择先验法向量与单应法向量二范数符合预设关系的一组位姿数据为目标位姿数据。

S103、根据单应矩阵和第一预设算法确定尺度因子，根据尺度因子和目标位姿数据，确定实际位姿数据。

其中，根据单应矩阵和PnP算法确定尺度因子。例如，获取特征点的深度信息，根据PnP算法和深度信息确定特征点从相机坐标系到世界坐标系的转换位姿；根据转换位姿和单应矩阵确定的对应位姿数据，确定尺度因子。

需说明的是，本申请方案还可以包括：根据单应矩阵确定相邻图像帧的特征点的速度关系；根据相邻图像帧的特征点的速度关系、相邻图像帧的特征点的速度和第二预设算法，更新相邻图像帧的特征点的速度。其中，第二预设算法包括高斯牛顿迭代法。

从该实施例可以看出，本申请的技术方案，先在获取视觉惯导里程计检测到的图像帧中的特征点后，根据特征点确定相邻图像帧之间的单应矩阵；然后根据单应矩阵确定对应的多组位姿数据，根据预设方式从多组位姿数据中确定目标位姿数据；根据单应矩阵和第一预设算法确定尺度因子，根据尺度因子和目标位姿数据，确定实际位姿数据。本申请充分考虑了飞行器从地面静止起飞具有的特殊性，即飞行器起飞的过程中，下视相机检测到的特征点几乎都在一个平面上，因此本申请将单应矩阵的约束加入到了视觉惯性里程计的初始化过程中，使用单应矩阵进行视觉惯性里程计的初始化。通过上述处理，本申请对于飞行器从地面起飞的阶段可以较好地实现视觉惯性里程计的初始化，对于飞行器以不同的上升姿态起飞都可以得到鲁棒性和准确性的定位结果。

图2是本申请另一实施例示出的视觉惯性里程计初始化方法的流程示意图；图3是本申请实施例示出的飞行器从地面起飞过程的示意图。

对于任意一个配备双目下视相机-IMU传感器系统的飞行器，从地面静止起飞时都会遇到检测到的特征点数量较少以及特征点跟踪不准确的问题。因此，相关技术中的视觉惯性里程计初始化方案应用于此种场景，初始化效果较差，甚至无法成功完成初始化。针对该情况，本申请方案充分考虑了飞行器从地面起飞时的特殊性(即飞行器起飞的过程中，下视相机检测到的特征点几乎都在一个平面上)，因此提出使用单应矩阵进行视觉惯性里程计的初始化，设计了一种基于单应矩阵的视觉惯性里程计初始化方案。该方案可以处理飞行器以各种不同姿态起飞的情况，对这些情况都可以得到准确和鲁棒的初始化结果，其中初始化结果例如可以包括位置、姿态、速度等。需说明的是，本申请中的飞行器例如可以是飞行汽车但不局限于此。

本申请方案使用于任意飞行器从地面起飞时的视觉惯性里程计的初始化过程。本申请的初始化该过程可以主要可以分为四个步骤：确定两图像帧之间的单应矩阵、基于先验法向量选择正确解、将单应矩阵联合PnP算法恢复尺度因子、对图像帧速度进行迭代更新。以下对本申请方案进行详细介绍。

参见图2，该方法包括：

S201、确定视觉惯导里程计检测到的相邻两图像帧之间的单应矩阵。

如图3所示，由于飞行器是从水平面静止起飞，所以飞行器在空中检测到的特征点都是在同一个平面。单应矩阵描述了平面上的特征点在两个不同的相机坐标系下的变换关系，即对于某一个点p，在相机坐标系1下的坐标为p₁，在相机坐标系2下的坐标为p₂，则存在关系

其中单应矩阵

表示点p在坐标系1与坐标系2之间的变换关系。其中相机坐标系1例如可以是t时刻的图像帧的相机坐标系，相机坐标系2例如可以是t+1时刻的图像帧的相机坐标系。

单应矩阵等同于透视变换中使用的矩阵。透视变换描述了两个平面之间的映射关系，之所以称为单应矩阵是因为两个平面之间的关系是确定的，只能通过唯一的矩阵来表示这个变换，故称之为单应矩阵。单应矩阵通常描述处于共同平面上的一些点在两张图像间的变换关系。因此单应矩阵是在一个图像中的点映射到另一图像中的对应点的变换。

因此，在飞行器起飞的过程中，下视相机在不同时刻都可以检测到特征点在图像平面的坐标，例如特征点p在t时刻的图像帧中的坐标p₁和在t+1时刻的图像帧中的坐标p₂；根据这些坐标就可以计算出对应的单应矩阵。

需要注意的是，如果检测到的特征点比较少，则恢复出的单应矩阵可能存在一些误差，因此可以将特征点检测数量的预设数量阈值设置为100，即当检测到的特征点数量大于100时，计算单应矩阵；如果检测到的特征点数量小于100，就不使用单应矩阵进行初始化，而是直接使用IMU计算的位姿作为飞行器当前时刻的位姿。

另外，检测到的异常的特征点也会影响恢复出的单应矩阵的准确性。因此，本申请实施例对于检测到的特征点，可以进一步使用RANSAC(RandomSAmpleConsensus，随机采样一致)算法删除异常的特征点，以提升初始化位姿的精度。RANSAC是一种迭代方法，用于从一组包含异常值的观察数据中估计数学模型的参数。简言之，RANSAC是一种滤除异常值的常用算法。RANSAC从样本中随机抽选出一个样本子集，使用最小方差估计算法对这个子集计算模型参数，然后计算所有样本与该模型的偏差，再使用一个预先设定好的阈值与偏差比较，当偏差小于阈值时，该样本点属于模型内样本点，简称内点，否则为模型外样本点，简称外点。

S202、根据单应矩阵解算位姿数据，基于先验法向量从解算的单应矩阵对应的位姿数据中选择正确解。

本申请方案基于先验法向量选择正确解，即从多解中找出正确的解。

在得到单应矩阵后，根据得到的单应矩阵就可以恢复不同时刻飞行器的位姿数据，位姿数据包括旋转量R和平移量p。

一般情况下，从单应矩阵解算恢复出的旋转量R和平移量p有四组解，但是只有一组解才是正确的。因此，需要使用一些方法从这些解中得到正确的那组解。其中，可以根据先验法向量从多组位姿数据中选择先验法向量与单应法向量二范数符合预设关系的一组位姿数据为目标位姿数据。

本申请根据特征点的深度为正值这个条件，可以排除掉其中两组解。然后还剩下两组解，有一组是正确的，有一组是错误的。为了排除剩下的这个错误的解，本申请可以借助一些先验的信息。在本申请方案中，主要考虑飞行器是从地面静止起飞这个条件，设计了一种选择正确解的方案。飞行器在地面静止时，对应的法向量为[0,0,1]，这个就是先验条件。此外，由于法向量的本质是向量，所以它与坐标系的位置无关，只与旋转有关系。在飞行器起飞的过程中，IMU可以得到任意时刻飞行器的旋转量例如旋转矩阵。因此，根据得到的旋转矩阵和初始时刻的法向量[0,0,1]，就可以计算得到水平面在任意时刻的法向量。然后余下的两组解都对应着一个法向量，将两组解对应着的每一个法向量都与计算得到的这个水平面在任意时刻的法向量进行比较，此时可以使用二范数作为评价指标，其中比较关系符合预设关系例如二范数更小的那个法向量作为正确的解。这样就可以得到正确的旋转量R和平移量p。

需说明的是，本申请还可以使用组合导航给出的飞行器位姿计算任一时刻的法向量。

S203、将单应矩阵联合PnP算法恢复尺度因子。

由于计算单应矩阵使用的是特征点在图像平面或者归一化相机平面的坐标，因此从单应矩阵计算出的平移量p都是包含尺度信息，即计算出的平移不是飞行器真正的平移，相差了一个尺度因子s。所以在用先验法向量得到正确的解后，需要恢复出平移对应的尺度因子s。

由于飞行器使用的是双目相机，因此可以根据双目相机得到任意一个特征点的深度信息，然后可以使用PnP算法计算出归一化相机平面的相机坐标系到图像平面的世界坐标系的位姿。

另外，根据单应矩阵恢复出的位姿，同样可以计算得到相机坐标系到世界坐标系的位姿，这样就形成了一个约束关系，如下式所示：

其中，s是尺度因子，

是单应矩阵计算出的平移量，

是真实世界的平移量，也即利用PnP算法算出的真实世界的平移量。

根据上式(1)就可以计算出尺度因子s。需要注意的是，尺度因子s是一个标量，而两个平移

和

都是三维向量，因此，为了更准确地计算尺度因子s，可以使用最小二乘法计算s，即：

根据计算的尺度因子s，就可以恢复出飞行器在任意时刻的旋转量

和平移量

本申请还可以利用GPS计算出尺度因子，即将单应矩阵矩阵联合组合导航恢复尺度因子，描述如下：

目前的组合导航系统都可以得到机体在任意时刻的位姿(位置和姿态)。其中可以使用组合导航系统提供的飞行器的方位角估计相机坐标系到世界坐标系的位姿。首先，组合导航给出的位姿一般是在世界坐标系下，因此首先需要将变量转换到相机坐标系：

上式(3)中，

和

是组合导航给出的机体的旋转量和平移量；

和

是相机与IMU之间的外参。

根据上式(3)，即可得到两图像帧之间的真实平移为

然后根据公式(1)和公式(2)同样可以计算尺度因子s。

S204、对图像帧速度进行迭代更新。

由于IMU计算的平移需要根据速度进行积分，因此测量值有很小的波动都会造成计算出的平移的有较大的误差。误差过大会导致在初始化过程中解算的姿态很不准确。因此，本申请方案还可以基于单应矩阵对飞行器的速度进行了优化。

飞行器的速度可以用图像帧速度表示。为了得到前后图像帧速度的关系，需要得到图像帧的前后帧特征点位置的关系。即：

上式(4)即为平面上的特征点在两个相机坐标系之间的变换关系。

其中，

是单应矩阵，H₁、H₂和H₃分别是单应矩阵中的子矩阵块，即单应矩阵由H₁、H₂、H₃和1构成，H₁、H₂、H₃是单应矩阵的一部分。

其中，单应矩阵可以根据提取的特征点计算得到，在得到的单应矩阵中包括相应的子矩阵块。

如前描述，速度是对位置的求导，因此，可以得到特征点在两个相机坐标系下的速度v1和v2的关系为：

上式即为特征点在两个归一化相机坐标系下的速度关系。

然后，可以将飞行器的速度转化为图像帧的特征点在归一化相机坐标系下的速度，根据式(5)的约束关系，即可使用高斯牛顿迭代法对飞行器的速度进行进行迭代更新，从而实现速度优化。

高斯牛顿迭代法的基本思想是使用泰勒级数展开式去近似地代替非线性回归模型，然后通过多次迭代，多次修正回归系数，使回归系数不断逼近非线性回归模型的最佳回归系数，最后使原模型的残差平方和达到最小。有时候为了拟合数据，例如根据重投影误差求相机位姿，常常将求解模型转化为非线性最小二乘问题。高斯牛顿迭代法正是用于解决非线性最小二乘问题，达到数据拟合、参数估计和函数估计的目的。高斯牛顿迭代法是解决非线性最小二乘问题的最基本方法。

本申请方案使用优化后的速度可以进一步提升初始化阶段求解出的位姿的准确性。

综上所描述，本申请实施例中，将单应矩阵的约束加入到了视觉惯性里程计的初始化步骤中，并提出了选择正确解、恢复尺度因子、优化飞行器速度的方案。由于本申请方案充分考虑了飞行器从地面静止起飞具有的特殊性，因此对于飞行器以不同的上升姿态起飞(例如正常垂直起飞，自旋转起飞，侧移起飞)的情况，都可以得到鲁棒性和准确性的定位结果。并且由于目前多数飞行器都是从水平面静止起飞的，所以本申请方案可以应对大多数飞行器起飞情况，具有更强的推广性。

与前述应用功能实现方法实施例相对应，本申请还提供了一种视觉惯性里程计初始化装置、飞行汽车、电子设备及相应的实施例。

图4是本申请实施例示出的视觉惯性里程计初始化装置的结构示意图。

参见图4，本申请实施例提供的视觉惯性里程计初始化装置40包括：单应矩阵确定模块41、解算选择模块42、尺度因子及位姿模块43。

单应矩阵确定模块41，用于获取视觉惯导里程计检测到的图像帧中的特征点，根据特征点确定相邻图像帧之间的单应矩阵。其中，获取视觉惯导里程计检测到的图像帧中大于预设数量阈值的特征点，并通过RANSAC算法删除异常的特征点，然后再根据特征点确定相邻图像帧之间的单应矩阵。

解算选择模块42，用于根据单应矩阵确定对应的多组位姿数据，根据预设方式从多组位姿数据中确定目标位姿数据。其中，根据先验法向量从多组位姿数据中选择先验法向量与单应法向量二范数符合预设关系的一组位姿数据为目标位姿数据。

尺度因子及位姿模块43，用于根据单应矩阵和第一预设算法确定尺度因子，根据尺度因子和目标位姿数据，确定实际位姿数据。其中，根据单应矩阵和PnP算法确定尺度因子。例如，获取特征点的深度信息，根据PnP算法和深度信息确定特征点从相机坐标系到世界坐标系的转换位姿；根据转换位姿和单应矩阵确定的对应位姿数据，确定尺度因子。

其中，视觉惯性里程计初始化装置40还包括：速度迭代更新模块44。

速度迭代更新模块44，用于根据单应矩阵确定相邻图像帧的特征点的速度关系；根据相邻图像帧的特征点的速度关系、相邻图像帧的特征点的速度和第二预设算法，更新相邻图像帧的特征点的速度。其中，第二预设算法包括高斯牛顿迭代法。

本申请提供的视觉惯性里程计初始化装置，先在获取视觉惯导里程计检测到的图像帧中的特征点后，根据特征点确定相邻图像帧之间的单应矩阵；然后根据单应矩阵确定对应的多组位姿数据，根据预设方式从多组位姿数据中确定目标位姿数据；根据单应矩阵和第一预设算法确定尺度因子，根据尺度因子和目标位姿数据，确定实际位姿数据。本申请充分考虑了飞行器从地面静止起飞具有的特殊性，即飞行器起飞的过程中，下视相机检测到的特征点几乎都在一个平面上，因此本申请将单应矩阵的约束加入到了视觉惯性里程计的初始化过程中，使用单应矩阵进行视觉惯性里程计的初始化。通过上述处理，本申请对于飞行器从地面起飞的阶段可以较好地实现视觉惯性里程计的初始化，对于飞行器以不同的上升姿态起飞都可以得到鲁棒性和准确性的定位结果。

本申请还提供一种飞行汽车，包括如图4中的视觉惯性里程计初始化装置40。

关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不再做详细阐述说明。

图5是本申请实施例示出的电子设备的结构示意图。该电子设备例如可以是飞行汽车但不局限于此。

参见图5，电子设备1000包括存储器1010和处理器1020。

处理器1020可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器1010可以包括各种类型的存储单元，例如系统内存、只读存储器(ROM)和永久存储装置。其中，ROM可以存储处理器1020或者计算机的其他模块需要的静态数据或者指令。永久存储装置可以是可读写的存储装置。永久存储装置可以是即使计算机断电后也不会失去存储的指令和数据的非易失性存储设备。在一些实施方式中，永久性存储装置采用大容量存储装置(例如磁或光盘、闪存)作为永久存储装置。另外一些实施方式中，永久性存储装置可以是可移除的存储设备(例如软盘、光驱)。系统内存可以是可读写存储设备或者易失性可读写存储设备，例如动态随机访问内存。系统内存可以存储一些或者所有处理器在运行时需要的指令和数据。此外，存储器1010可以包括任意计算机可读存储媒介的组合，包括各种类型的半导体存储芯片(例如DRAM，SRAM，SDRAM，闪存，可编程只读存储器)，磁盘和/或光盘也可以采用。在一些实施方式中，存储器1010可以包括可读和/或写的可移除的存储设备，例如激光唱片(CD)、只读数字多功能光盘(例如DVD-ROM，双层DVD-ROM)、只读蓝光光盘、超密度光盘、闪存卡(例如SD卡、min SD卡、Micro-SD卡等)、磁性软盘等。计算机可读存储媒介不包含载波和通过无线或有线传输的瞬间电子信号。

存储器1010上存储有可执行代码，当可执行代码被处理器1020处理时，可以使处理器1020执行上文述及的方法中的部分或全部。

此外，根据本申请的方法还可以实现为一种计算机程序或计算机程序产品，该计算机程序或计算机程序产品包括用于执行本申请的上述方法中部分或全部步骤的计算机程序代码指令。

或者，本申请还可以实施为一种计算机可读存储介质(或非暂时性机器可读存储介质或机器可读存储介质)，其上存储有可执行代码(或计算机程序或计算机指令代码)，当可执行代码(或计算机程序或计算机指令代码)被电子设备(或服务器等)的处理器执行时，使处理器执行根据本申请的上述方法的各个步骤的部分或全部。

以上已经描述了本申请的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术的改进，或者使本技术领域的其他普通技术人员能理解本文披露的各实施例。

Claims (10)

1.一种视觉惯性里程计初始化方法，其特征在于，包括：

获取视觉惯导里程计检测到的图像帧中的特征点，根据所述特征点确定相邻图像帧之间的单应矩阵；

根据所述单应矩阵确定对应的多组位姿数据，根据预设方式从所述多组位姿数据中确定目标位姿数据；

根据所述单应矩阵和第一预设算法确定尺度因子，根据所述尺度因子和所述目标位姿数据，确定实际位姿数据。

2.根据权利要求1所述的方法，其特征在于，所述方法还包括：

根据所述单应矩阵确定所述相邻图像帧的特征点的速度关系；

根据所述相邻图像帧的特征点的速度关系、相邻图像帧的特征点的速度和第二预设算法，更新所述相邻图像帧的特征点的速度。

3.根据权利要求1所述的方法，其特征在于，所述根据预设方式从所述多组位姿数据中确定目标位姿数据，包括：

根据先验法向量从所述多组位姿数据中选择先验法向量与单应法向量二范数符合预设关系的一组位姿数据为目标位姿数据。

4.根据权利要求1所述的方法，其特征在于，所述根据所述单应矩阵和第一预设算法确定尺度因子，包括：

根据所述单应矩阵和PnP算法确定尺度因子。

5.根据权利要求4所述的方法，其特征在于，所述根据所述单应矩阵和PnP算法确定尺度因子，包括：

获取所述特征点的深度信息，根据PnP算法和所述深度信息确定所述特征点从相机坐标系到世界坐标系的转换位姿；

根据所述转换位姿和所述单应矩阵确定的对应位姿数据，确定尺度因子。

6.根据权利要求2所述的方法，其特征在于：

所述第二预设算法包括高斯牛顿迭代法。

7.根据权利要求1至6任一项所述的方法，其特征在于，所述获取视觉惯导里程计检测到的图像帧中的特征点，包括：

获取视觉惯导里程计检测到的图像帧中大于预设数量阈值的特征点，并通过RANSAC算法删除异常的特征点。

8.一种视觉惯性里程计初始化装置，其特征在于，包括：

单应矩阵确定模块，用于获取视觉惯导里程计检测到的图像帧中的特征点，根据所述特征点确定相邻图像帧之间的单应矩阵；

解算选择模块，用于根据所述单应矩阵确定对应的多组位姿数据，根据预设方式从所述多组位姿数据中确定目标位姿数据；

尺度因子及位姿模块，用于根据所述单应矩阵和第一预设算法确定尺度因子，根据所述尺度因子和所述目标位姿数据，确定实际位姿数据。

9.根据权利要求8所述的装置，其特征在于，所述装置还包括：

速度迭代更新模块，用于根据所述单应矩阵确定所述相邻图像帧的特征点的速度关系；根据所述相邻图像帧的特征点的速度关系、相邻图像帧的特征点的速度和第二预设算法，更新所述相邻图像帧的特征点的速度。

10.一种飞行汽车，其特征在于，包括如权利要求8至9中任一项所述的视觉惯性里程计初始化装置。

Images