一种单目视觉惯性里程计系统及方法与流程

本发明涉及移动载体的自主导航控制技术领域，尤其涉及一种单目视觉惯性里程计系统及方法。

背景技术：

近年来，单目视觉和惯导融合的里程计系统被广泛应用于移动载体的自主导航控制领域，主要用于在缺乏绝对定位技术的场景中实现移动载体的导航定位以及利用自身的相对定位系统辅助移动载体的导航定位。因此提高里程计系统的定位精度和实时性是保障移动载体准确导航定位的关键。

目前，关于单目视觉和惯导融合的里程计系统技术方案可以分为两类：一是单目视觉模块和惯导模块采用松耦合融合方式，分别让单目视觉和惯导两个模块独立运行，并进行单独解算，然后将各自解算的结果进行融合；二是单目视觉模块和惯导模块采用紧耦合融合方式，充分考虑单目视觉和惯导两个模块之间的状态关联性，将视觉模块的图像特征放到状态向量中，然后再解算姿态信息。现有技术中，采用扩展卡尔曼滤波器将单目相机与imu信息相融合的方法，但是其定位精度低，不便于应用于对定位精度较高的导航载体。针对上述问题发展的单目视觉和惯性紧耦合的方法，虽然提高了精度，但是计算复杂度高，对硬件设备资源要求高且不具备实时处理的能力。

现有技术存在以下缺点：一是单目视觉里程计尺度不确定性导致定位精度差；二是计算复杂度高，需要大型视觉识别系统设备，实时性差，不便于推广应用。

技术实现要素：

鉴于上述的分析，本发明旨在提供一种单目视觉惯性里程计系统及方法，以解决现有的里程计系统定位精度低和实时性差的问题。

一方面，本发明提出了一种单目视觉惯性里程计系统，包括：惯导模块、单目视觉模块、控制模块和姿态输出模块。惯导模块用于感应测量载体的运动信息；单目视觉模块用于通过图像帧获取载体周围环境信息；控制模块包括预处理模块和repe(ransac-extendedkalmanparticlefilter)滤波器；预处理模块用于根据运动信息采用捷联惯导算法解算得到载体的惯导信息，并根据单目视觉模块提供的连续三帧图像信息利用特征点匹配算法对图像特征点进行粗匹配，得到粗匹配特征点组；repf滤波器，用于基于解算的载体惯导信息对得到的粗匹配特征点组进行精匹配得到最佳匹配点组，并利用全部最佳匹配点组解算得到载体的姿态信息；姿态输出模块用于输出控制模块解算得到的姿态信息。

进一步的，惯导模块包括固联在载体上陀螺仪和加速度计，所述陀螺仪用于感应测量载体轴向的角速度，所述加速度计用于感应测量载体的线速度。

进一步的，单目视觉模块提供的连续三帧图像信息分别为当前帧数据、前一帧数据和前两帧数据。

进一步的，预处理模块执行下述流程得到粗匹配点：

利用fast特征算法从单目视觉模块提供的连续三帧图像中分别提取特征点，并利用lk光流跟踪进行特征点匹配；

从lk光流跟踪得到的匹配特征点组中随机选取八组匹配点组，并采用八点法估计得到单目视觉模块相机的外参数，根据所述相机外参数及图像间的对极几何关系确定局内匹配点组数，保留匹配点组数最大的匹配点组以及与所述匹配点组数最大的匹配点组相匹配的匹配点组，剔除其它匹配点组，得到粗匹配点组。

进一步的，repe滤波器执行下述流程得到最佳匹配点组：

随机选取一组粗匹配提取的匹配点组，与三帧图像对应的惯导信息进行融合，得到系统状态量；利用repf滤波算法估计系统的误差量，与惯导信息进行融合，得到系统真实状态量；根据所述系统真实状态量及三帧图像几何中的“点-线-点”的映射关系，得到三焦点张量；

判断其它匹配点组的约束关系，是否满足所述三焦点张量的约束关系，如果满足约束关系，则判断为局内匹配点组并保留；

多次随机取样进行局内匹配点组判断，保留局内匹配点组数最大的匹配点组以及与所述匹配点组数最大的匹配点组相匹配的匹配点组，将其它未被选中的匹配点组剔除掉，得到最佳匹配点组。

进一步的，repf滤波算法利用全部最佳匹配点组更新系统真实的状态量以解算得到载体的姿态信息。

根据上述技术方案，本发明的有益效果如下：

1.惯导模块与单目视觉模块具有互补性，充分利用两个模块的优势，将两个模块相融合，解决单目视觉里程计尺度不确定性问题，提高了单目视觉里程计的适应能力和定位精度，使其能够在多种场景中应用；

2.控制模块中采用计算复杂度较低的repf滤波器算法处理惯导和单目视觉模块提供的信息，在保证系统高精度性能的前提下，降低控制模块处对处理器性能的要求，提高了里程计系统的实时性。

另一方面，本发明还提出了一种单目视觉惯性里程计系统的工作方法，包括以下步骤：

惯导模块和单目视觉模块分别向控制模块发送运动信息和连续三帧图像信息；

预处理模块根据运动信息采用捷联惯导算法解算得到载体的惯导信息，并根据单目视觉模块提供的连续三帧图像信息利用特征点匹配算法对图像特征点进行粗匹配，得到粗匹配特征点组；

repf滤波器基于解算的载体惯导信息对得到的图像粗匹配特征点组进行精匹配得到最佳匹配点组，并利用全部最佳匹配点组解算得到载体的姿态信息；

控制模块将载体的姿态信息发送至姿态输出模块。

进一步的，预处理模块执行下述流程得到粗匹配点组：

利用fast特征算法从单目视觉模块提供的连续三帧图像中分别提取特征点，并利用lk光流跟踪进行特征点匹配；

从lk光流跟踪得到的匹配特征点组中随机选取八组匹配点组，并采用八点法估计得到单目视觉模块相机的外参数，根据所述相机外参数及图像间的对极几何关系确定局内匹配点组数，保留匹配点组数最大的匹配点组以及与所述匹配点组数最大的匹配点组相匹配的匹配点组，剔除其它匹配点组，得到粗匹配点组。

进一步的，repe滤波器执行下述流程得到最佳匹配点组：

随机选取一组粗匹配提取的匹配点组，与三帧图像对应的惯导信息进行融合，得到系统状态量；利用repf滤波算法估计系统的误差量，与惯导信息进行融合，得到系统真实状态量；根据所述系统真实状态量及三帧图像几何中的“点-线-点”的映射关系，得到三焦点张量；

判断其它匹配点组的约束关系，是否满足所述三焦点张量的约束关系，如果满足约束关系，则判断为局内匹配点组并保留；

多次随机取样进行局内匹配点组判断，保留局内匹配点组数最大的匹配点组以及与所述匹配点组数最大的匹配点组相匹配的匹配点组，将其它未被选中的匹配点组剔除掉，得到最佳匹配点组。

进一步的，repf滤波算法利用全部最佳匹配点组更新系统真实的状态量以解算得到载体的姿态信息。

由于本发明中的里程计系统工作方法与上述里程计系统原理相同，所以该方法也具有与上述里程计系统相应的技术效果。

本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所特别指出的内容中来实现和获得。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制。

图1为单目视觉惯性里程计系统的组成结构框图；

图2为单目视觉惯性里程计系统的工作流程图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。

系统实施例：

本发明的一个具体实施例，公开了一种单目视觉惯性里程计系统。如图1所示，包括：惯导模块、单目视觉模块、控制模块和姿态输出模块。惯导模块用于感应测量载体的运动信息；单目视觉模块用于通过图像帧获取载体周围环境信息；控制模块包括预处理模块和repe(ransac-extendedkalmanparticlefilter)滤波器；预处理模块用于根据运动信息采用捷联惯导算法解算得到载体的惯导信息，并根据单目视觉模块提供的连续三帧图像信息利用特征点匹配算法对图像特征点进行粗匹配，得到粗匹配特征点组；repf滤波器，用于基于解算的载体惯导信息对得到的粗匹配特征点组进行精匹配得到最佳匹配点组，并利用全部最佳匹配点组解算得到载体的姿态信息；姿态输出模块用于输出控制模块解算得到的姿态信息。

其中，载体可以是飞机、无人机、潜艇、移动机器人等需要智能导航定位系统的移动载体。预处理模块解算出的惯导信息包括位置、姿态和速度信息；单目视觉模块通过图像帧获得的载体周围环境信息包括位置和姿态信息等；姿态输出模块输出的姿态信息包括位置、姿态和速度等信息。

进一步的，惯导模块包括固联在载体上陀螺仪和加速度计，所述陀螺仪用于感应测量载体轴向的角速度，所述加速度计用于感应测量载体的线速度。

其中，采用捷联惯导算法解算运动信息得到载体的惯导信息；先根据陀螺仪输出的角速度建立导航坐标系，再根据加速度计输出的线速度解算出载体在导航坐标系中的速度、位置和姿态等信息。

进一步的，现有技术中通常仅采用两帧图像数据来获得载体周围环境信息，但是为了提高里程计系统定位导航的精度，本发明的单目视觉模块提供了连续三帧图像数据，即当前帧数据、前一帧数据和前两帧数据；当采集到新的图像数据时，则前述当前帧数据变为前一帧数据，前述前一帧数据变为前辆帧数据，采集到的新图像数据作为当前帧数据，以此循环实现里程计系统定位导航的实时性。

进一步的，预处理模块执行下述流程得到粗匹配点组：

利用fast特征算法从单目视觉模块提供的连续三帧图像中分别提取特征点，并利用lk光流跟踪进行特征点匹配；

从lk光流跟踪得到的匹配特征点组中随机选取八组匹配点组，并采用八点法估计得到单目视觉模块相机的外参数，根据所述相机外参数及图像间的对极几何关系确定局内匹配点组数，保留匹配点组数最大的匹配点组以及与所述匹配点组数最大的匹配点组相匹配的匹配点组，剔除其它匹配点组，得到粗匹配点组。

其中，采用八点法估计得到单目视觉模块相机的外参数包括：本质矩阵以及相机的旋转矩阵和平移量，用以剔除偏差较大的匹配点组，以减少计算量。

进一步的，repe滤波器执行下述流程得到最佳匹配点组：

随机选取一组粗匹配提取的匹配点组，与三帧图像对应的惯导信息进行融合，得到系统状态量；利用repf滤波算法估计系统的误差量，与惯导信息进行融合，得到系统真实状态量；根据所述系统真实状态量及三帧图像几何中的“点-线-点”的映射关系，得到三焦点张量；

系统的状态量包括坐标、位置、矩阵、旋转量以及惯导运动方程等，根据系统的状态量，利用repf滤波算法可以估计系统的误差量；再根据系统的误差量并与惯导信息相融合，可以得到系统真实的状态量；根据系统真实的状态量去解算载体的位置、速度、姿态以及轨迹等信息。

其中，“点-线-点”的映射关系具体为：单目视觉模块可以提供前两帧图像、前一帧图像和当前帧图像，将空间中的一条直线在三帧图像分别投影成三条直线；当前帧图像和前一帧图像为相邻两帧图像，可以利用相邻两帧图像间的对极几何约束关系确定空间中该直线上的某一点在两个成像平面映射点之间的映射关系，同理可以得到前两帧图像和前一帧图像两个映射点之间关系；空间直线和投影在前一帧图像的投影线形成一个平面，结合两个相邻帧图像的对极几何约束关系，可以推导出空间中该直线上的某一点分别在前两帧图像和当前帧图像之间的映射关系；

判断其它匹配点组的约束关系，是否满足所述三焦点张量的约束关系，如果满足约束关系，则判断为局内匹配点组并保留；

多次随机取样进行局内匹配点组判断，保留局内匹配点组数最大的匹配点组以及与所述匹配点组数最大的匹配点组相匹配的匹配点组，将其它未被选中的匹配点组剔除掉，得到最佳匹配点组；

先对特征点进行粗匹配再进行精匹配，大大降低了计算量和计算复杂程度，从而提高了里程计系统的实时性。

重复上述步骤，直至得到全部最佳匹配点，并利用全部最佳匹配点组解算得到载体的姿态信息；利用全部最佳匹配点组解算载体的姿态信息可以避免单一匹配点组的单一性和偶然性并提高里程计系统的定位导航精度。

进一步的，为了实现里程计系统的实时性，需要判断是否采集到新的图像数据和惯导数据。若有新的数据，则需要重复前述步骤，直至整个里程计系统采集和处理完成。

进一步的，repf滤波算法利用全部最佳匹配点组更新系统真实的状态量以解算得到载体的姿态信息。

本发明实施例中的单目视觉惯性里程计系统用于移动载体的自主导航控制。首先，惯导模块与单目视觉模块相融合，解决了单目视觉里程计尺度不确定性问题，提高了单目视觉里程计的适应能力和定位精度；其次，应用控制模块中的两级优化，采用预处理模块对数据进行粗处理，再利用计算复杂度较低的repf滤波器算法对预处理后的数据进一步优化，降低了控制模块处对处理器性能的要求，提高了里程计系统的实时性。从而实现了里程计系统对高定位精度和实时性的要求。

方法实施例：

参见图2，图2为本发明实施例提出的单目视觉惯性里程计系统的工作流程图。如图所示，包括以下步骤：

惯导模块和单目视觉模块分别向控制模块发送运动信息和连续三帧图像信息；

预处理模块根据运动信息利用捷联惯导算法解算得到载体的惯导信息，并根据单目视觉模块提供的连续三帧图像信息利用特征点匹配算法对图像特征点进行粗匹配，得到粗匹配特征点组；

repf滤波器基于解算的载体惯导信息对得到的图像粗匹配特征点组进行精匹配得到最佳匹配点组，并利用全部最佳匹配点组解算得到载体的姿态信息；

控制模块将载体的姿态信息发送至姿态输出模块。

其中，运动信息包括载体的角速度和线速度，惯导信息包括载体的位置、速度和姿态等信息；连续三帧图像通过获取周围环境信息而提供载体的位置和姿态等信息；控制模块输出的姿态信息包括位置、速度和姿态等信息。

进一步的，预处理模块执行下述流程得到粗匹配点组：

利用fast特征算法从单目视觉模块提供的连续三帧图像中分别提取特征点，并利用lk光流跟踪进行特征点匹配；

从lk光流跟踪得到的匹配特征点组中随机选取八组匹配点组，并采用八点法估计得到单目视觉模块相机的外参数，根据所述相机外参数及图像间的对极几何关系确定局内匹配点组数，保留匹配点组数最大的匹配点组以及与所述匹配点组数最大的匹配点组相匹配的匹配点组，剔除其它匹配点组，得到粗匹配点组。

进一步的，repe滤波器执行下述流程得到最佳匹配点组：

随机选取一组粗匹配提取的匹配点组，与三帧图像对应的惯导信息进行融合，得到系统状态量；利用repf滤波算法估计系统的误差量，与惯导信息进行融合，得到系统真实状态量；根据所述系统真实状态量及三帧图像几何中的“点-线-点”的映射关系，得到三焦点张量；

判断其它匹配点组的约束关系，是否满足所述三焦点张量的约束关系，如果满足约束关系，则判断为局内匹配点组并保留；

多次随机取样进行局内匹配点组判断，保留局内匹配点组数最大的匹配点组以及与所述匹配点组数最大的匹配点组相匹配的匹配点组，将其它未被选中的匹配点组剔除掉，得到最佳匹配点组；

重复上述步骤，直至得到全部最佳匹配点，并利用全部最佳匹配点解算载体的姿态信息。

进一步的，判断是否采集到新的图像数据和惯导数据。若有新数据，则重复前述步骤，直至整个里程计系统采集和处理完成。

进一步的，repf滤波算法利用全部最佳匹配点组更新系统真实的状态量以解算得到载体的姿态信息。

本发明实施例中的单目视觉惯性里程计系统的工作方法用于移动载体的自主导航控制。首先，惯导模块与单目视觉模块相融合，解决了单目视觉里程计尺度不确定性问题，提高了单目视觉里程计的适应能力和定位精度；其次，应用控制模块中的两级优化，采用预处理模块对数据进行粗处理，再利用计算复杂度较低的repf滤波器算法对预处理后的数据进一步优化，降低了控制模块处对处理器性能的要求，提高了里程计系统的实时性。从而实现了里程计系统对高定位精度和实时性的要求。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。