一种基于视觉和惯性测量单元的无人飞行器位姿估计方法与流程

本发明涉及无人飞行器位姿估计方法，尤其涉及一种基于视觉和惯性测量单元的无人飞行器位姿估计方法。

背景技术：

在传统无人飞行器视觉导航中，传统方法是基于gps/imu融合的位姿估计来导航。imu具有较高的输出频率，作为短期内的位姿状态；gps刷新频率较低，不存在累积误差；基于滤波的方法，利用imu和gps传感器数据来估计无人飞行器的位姿。

传统的基于gps/imu融合的位姿估计，在无gps信号情况该方法存在累计误差，如在室内的导航。另外gps精度有限，因此传统基于gps/imu融合的位姿估计的精度不高；gps的刷新频率较低，在实时性要求高的位姿估计应用中，基于gps/imu融合的位姿估计不能有效地工作。

技术实现要素：

为了解决现有技术中的问题，本发明提供了一种基于视觉和惯性测量单元的无人飞行器位姿估计方法。

本发明提供了一种基于视觉和惯性测量单元的无人飞行器位姿估计方法，包括并行的跟踪线程、局部地图线程和深度滤波器线程，所述跟踪线程用于跟踪摄像机的相对运动、视觉信息和惯性信息融合，所述局部地图线程用于检测新的fast角点，估计三角化测量3d点的深度，并在初始化时对齐视觉和惯性帧，所述深度滤波器线程用于更新3d点的种子。

作为本发明的进一步改进，所述跟踪线程通过半直接法实现视觉里程计的初始化，跟踪特征点和估计摄像机姿态，先是计算imu预积分，获取imu帧中的运动状态并根据外参矩阵将其转换到相机帧，作为相机位姿的先验信息，然后基于稀疏模型的图像对齐估计相对运动，特征块对齐优化3d点在当前图像帧的二维投影坐标，进一步精细化3d点坐标和相机位置姿态，最后通过连续帧之间的因子图优化来更新imu状态的姿态，速度和偏移。

作为本发明的进一步改进，在局部地图线程中，检测帧队列中新关键帧上的fast特征点，并在参考帧中通过光流跟踪新特征点，找出新关键帧和参考帧之间的特征点对应关系，通过参考帧和新关键帧之间的特征点的对应关系和相对姿态，对新特征点进行三角化处理，并获得新关键帧中新特征的深度，将新特征点添加到深度过滤器中作为种子点进行优化，通过光速平差法优化地图中的局部关键帧。

作为本发明的进一步改进，在深度滤波器线程中，深度滤波器是运行概率深度滤波器，以优化新特征点的深度，当新特征点深度的误差低于阈值时，新特征点的深度收敛，将新特征点作为3d点插入到地图中，作为候选3d点，并且这些新的3d点立即用于跟踪线程。

作为本发明的进一步改进，所述跟踪线程包括以下步骤：

s101、判断视觉-imu对齐初始化是否完成，如果否，则跳到步骤s103，如果是，则进入下一步骤；

s102、计算imu预积分，imu相对位姿先验求解，并作为先验位姿；

s103、半稠密视觉跟踪，直接法图像对齐、特征点对齐、位姿优化、3d点结构优化；

s104、视觉-imu信息融合；

s105、判断是否为关键帧，如果是，则进入关键帧队列，进行局部地图线程，如果否，则进行帧队列，进行深度滤波器线程。

作为本发明的进一步改进，所述局部地图线程包括以下步骤：

s201、fast角点检测和klt光流跟踪新特征点；

s202、三角化新特征点，获取深度信息，插入新队列至深度滤波器线程；

s203、光束平差法优化；

s204、判断视觉-imu对齐是初始化是否完成，如果是，则进行地图更新，如果否，则进行光束平差法优化和视觉-imu对齐初始化。

作为本发明的进一步改进，所述深度滤波器线程包括以下步骤：

s301、深度滤波器更新种子点深度和不确定性；

s302、判断种子点是否深度收敛，如果是，则向地图插入新种子点作为3d结构候选点。

本发明的有益效果是：通过上述方案，采用基于视觉和惯性测量单元信息融合的方法，通过跟踪、深度滤波、局部地图三个并行线程，可以达到较高的精度,并且有较高的运行效率，在计算能力受限的嵌入式设备上也能达到较快的估计速度，能满足实时性较高的使用场合。

附图说明

图1是本发明一种基于视觉和惯性测量单元的无人飞行器位姿估计方法的示意图。

图2是基于因子图优化的视觉-惯性融合示意图。

图3是在公共数据集theeurocmavdataset位置估计定性对比在mh_01_easy数据集进行实验对比的示意图。

图4是在公共数据集theeurocmavdataset位置估计定性对比在mh_03_medium数据集进行实验对比的示意图。

图5是在公共数据集theeurocmavdataset位置估计定性对比在mh_05_difficult数据集进行实验对比的示意图。

图6是在公共数据集theeurocmavdataset与okvis方法求解的位置的定性对比在mh_01_easy数据集进行实验对比的示意图。

图7是在公共数据集theeurocmavdataset与okvis方法求解的位置的定性对比在mh_03_medium数据集进行实验对比的示意图。

图8是在公共数据集theeurocmavdataset与okvis方法求解的位置的定性对比在mh_05_difficult数据集进行实验对比的示意图。

具体实施方式

下面结合附图说明及具体实施方式对本发明作进一步说明。

如图1所示，一种基于视觉和惯性测量单元的无人飞行器位姿估计方法，包括并行的跟踪线程、局部地图线程和深度滤波器线程，所述跟踪线程用于跟踪摄像机的相对运动、视觉信息和惯性信息融合，所述局部地图线程用于检测新的fast角点，估计三角化测量3d点的深度，并在初始化时对齐视觉和惯性帧，所述深度滤波器线程用于更新3d点的种子。

跟踪线程通过半直接法实现视觉里程计的初始化，跟踪特征点和估计摄像机姿态。第一步是计算imu预积分，获取imu帧中的运动状态并根据外参矩阵将其转换到相机帧，作为相机位姿的先验信息，然后基于稀疏模型的图像对齐估计相对运动，特征块对齐优化3d点在当前图像帧的二维投影坐标，进一步精细化3d点坐标和相机位置姿态，最后通过连续帧之间的因子图优化来更新imu状态的姿态，速度和偏移。

在局部地图线程中，检测帧队列中新关键帧上的fast特征点，并在参考帧中通过光流跟踪这些新些特征点点，找出新关键帧和参考帧之间的特征点对应关系。通过参考帧和新关键帧之间的特征点的对应关系和相对姿态，本发明可以对这些新特征点进行三角化处理，并获得新关键帧中新特征的深度。然而，新关键帧的新特征点的深度存在比较大的误差。因此，这些新特征点需要添加到深度过滤器中作为种子点进行优化。最后，本发明通过光速平差法优化地图中的局部关键帧。

深度滤波器是运行概率深度滤波器，以优化新特征的深度。当特征点深度的误差低于阈值时，特征点的深度收敛，本发明将新的特征点作为3d点插入到地图中，作为候选3d点，并且这些新的3d点可以立即用于跟踪线程。

图2是视觉-惯性融合方法，用因子图表示状态(位置、速度、imu角速度和加速度的偏置)最大后验估计，通过最小化后验估计的负对数来求解视觉-惯性融合的位姿估计。

图3至图5是在公共数据集theeurocmavdataset位置估计定性对比：分别在mh_01_easy、mh_03_medium、mh_05_difficult数据集进行实验对比。

表1在公共数据集theeurocmavdataset位置定量对比

由图3至图5和表1可以看出，本发明的估计方法求解的位置误差很小。

图6至图8是在公共数据集theeurocmavdataset与okvis方法求解的位置的定性对比：分别在mh_01_easy、mh_03_medium、mh_05_difficult数据集进行实验对比。

表2在公共数据集theeurocmavdataset与okvis求解的位置误差的定量对比

由图6至图8和表2可以看出，本发明的估计方法求解的位置精度在三个数据集上都比okvis方法高。

表3在公共数据集mh_01_easy上，三个线程的运行时间

其中实验运行于intelcorei56200u，2.3ghzcpuandan16gbram笔记本电脑，实时性可以达到100hz的估计速度，在嵌入式设备上如树莓派也能达到30hz。

本发明提供的一种基于视觉和惯性测量单元的无人飞行器位姿估计方法，采用基于视觉和惯性测量单元信息融合的方法，通过跟踪、深度滤波、局部地图三个线程，对imu数据进行预积分，得到位姿的先验，对视觉数据跟踪获取位姿估计，并通过因子优化对视觉和惯性测量单元数据进行融合，可以达到很高的精度(位置误差≤1m),并且有较高的运行效率，在计算能力受限的嵌入式设备上也能达到30hz的估计速度，能满足实时性较高的使用场合。

本发明提供的一种基于视觉和惯性测量单元的无人飞行器位姿估计方法，主要应用于无人飞行器位姿估计，是基于单目视觉和惯性测量单元的估计方法，该发明包括以下三个部分：1)单目视觉slam初始化、视觉slam与imu对齐初始化；2)imu预积分、半稠密视觉里程计跟踪以及基于图优化的视觉惯性信息融合；3)后端三角测量法获取fast特征点的深度、深度滤波器和光束平差法优化。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。