一种视觉与惯性融合的室内定位方法技术

本发明专利技术公开了一种视觉与惯性融合的室内定位方法，输入测量数据和图像数据，其中IMU数据用于状态预测，而图像数据用于量测更新，基于扩展卡尔曼滤波器的框架，在状态预测阶段采用惯性器件的测量进行状态更新，在量测更新阶段采用特征点在图像帧上的投影残差作为测量进行状态校正，在扩展卡尔曼滤波器的设计中，选取包含当前以及过去多个相机位置对应IMU状态的滑动状态窗口作为状态变量；在特征跟踪阶段采用光流法建立图像中的特征关联关系；在滑动窗口更新阶段，只有当状态间存在足够大的平移时，才将当前IMU状态作为新的状态加入到滑动状态窗口中。本发明专利技术具有如下优点：可以在满足机载性能限制的要求下提供鲁棒、精确的导航性能。

【技术实现步骤摘要】
一种视觉与惯性融合的室内定位方法
本专利技术属于无人机导航定位
，特别涉及一种视觉与惯性融合的室内定位方法。
技术介绍
微小无人机在室内的自主飞行对导航系统的精度、鲁棒性以及定位频率都有很高的要求。由于室内不能获得稳定的GPS信号支持，无人机在室内的导航主要依靠其机载传感器进行组合导航。在常用的机载传感器中，相机能够获取含有丰富信息的图像数据，惯性测量器件(IMU)输出频率高且不易受环境影响，两者的有效融合可以给无人机提供稳定、鲁棒的导航性能。一般可以从实现框架上将视觉与惯性数据的融合方法分为松耦合和紧耦合两种。其中，松耦合方法中先将视觉与惯性器件分为两个子系统进行独立解算，然后将两个子系统的解算结果融合起来。紧耦合算法中则对视觉与惯性的原始测量数据进行联合优化，这样做的好处是可以得到最优的估计结果。由于受到机载计算性能的限制，无人机上通常采用松耦合的视觉惯性融合方法进行导航定位，难以得到最优的估计结果。
技术实现思路
本专利技术旨在至少解决上述技术问题之一。为此，本专利技术的目的在于提出一种视觉与惯性融合的室内定位方法，可以在满足机载性能限制的要求下提供鲁棒、精确的导航性能。为了实现上述目的，本专利技术的实施例公开了一种视觉与惯性融合的室内定位方法，包括以下步骤：S11：获取当前时刻的IMU测量数据，并根据当前时刻的IMU测量数据以及前一时刻的IMU状态数据对所述当前时刻的IMU状态数据进行更新，其中，所述IMU状态数据包括速度数据和位移数据；S12：根据所述当前时刻的IMU测量数据以及前一时刻的协方差矩阵进行更新，得到所述当前时刻的协方差矩阵；S21：获取当前帧的图像数据，如果所述当前帧的图像数据为第一幅图像，则在所述第一幅图像中提取第一预设阈值的FAST特征点作为初始光流跟踪点；如果所述当前帧的图像数据不是第一幅图像，则使用网格化的光流跟踪算法进行特征跟踪；S22：检测所述当前帧的图像数据与前一帧的图像数据间的平移值是否超过预设平移阈值，如果未超过所述预设平移阈值则进入步骤S23；如果超过所述预设平移阈值则进入步骤S24；S23：对当前帧的图像数据跟踪到的特征点进行量测更新，且在下一次更新中仍将第k帧作为前一帧进行特征跟踪，并结束当前帧图像的处理；S24：将当前帧的图像数据对应的IMU状态数据加入到滑动窗口的尾部，并对协方差矩阵进行更新，并进入步骤S25；S25：遍历所述当前帧的图像数据的所有特征点，若某个特征点在当前帧中跟踪失败且跟踪持续的帧数大于等于第二预设阈值，则对大于等于所述第二预设阈值的特征点进行量测更新；S26：根据滑动状态窗口中的状态数目和特征点更新情况对滑动状态窗口进行管理。进一步地，所述第一预设阈值为300。进一步地，所述网格化的光流跟踪算法包括：S21_1：从图像左上方像素开始对图像进行滑动窗口处理，选取3×3大小的窗口，对于窗口中心的像素点，将其与其邻域内的8个像素点进行比较，若中心点灰度值更大则赋1，否则赋0，最终将这8个0/1值组合成二进制数作为中心点的灰度值；S21_2：在光流跟踪过程中，将图像均匀分为8×8个网格，若跟踪过程中特征点数目小于300，则通过提取新的FAST特征点补足300个；S21_3：对于步骤S21_2中获得的特征点跟踪点对，采用RANSAC算法剔除匹配错误的点对。进一步地，在步骤S22中，所述检测所述当前帧的图像数据与前一帧的图像数据间的平移值的步骤包括：获取所述当前帧的图像数据与前一帧的图像数据的所有匹配点对；将所述所有匹配点对转化为以图像像素中心为原点的齐次坐标；根据所述所有匹配点对转化为以图像像素中心为原点的齐次坐标得到所述当前帧的图像数据与前一帧的图像数据间的平移值。进一步地，所述第二预设阈值为3。根据本专利技术实施例的视觉与惯性融合的室内定位方法，输入测量数据和图像数据，IMU数据用于状态预测，而图像数据用于量测更新，基于扩展卡尔曼滤波器的框架，在状态预测阶段采用惯性器件的测量进行状态更新，在量测更新阶段采用特征点在图像帧上的投影残差作为测量进行状态校正，在扩展卡尔曼滤波器的设计中，选取包含当前以及过去多个相机位置对应IMU状态的滑动状态窗口作为状态变量；在特征跟踪阶段采用光流法建立图像中的特征关联关系；在滑动窗口更新阶段，只有当状态间存在足够大的平移时，才将当前IMU状态作为新的状态加入到滑动状态窗口。本专利技术实施例的视觉与惯性融合的室内定位方法，可以在满足机载性能限制的要求下提供鲁棒、精确的导航性能。本专利技术的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本专利技术的实践了解到。附图说明本专利技术的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：图1是本专利技术一个实施例的视觉与惯性融合的室内定位方法的总体流程图。具体实施方式下面详细描述本专利技术的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本专利技术，而不能理解为对本专利技术的限制。在本专利技术的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本专利技术和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本专利技术的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。在本专利技术的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本专利技术中的具体含义。参照下面的描述和附图，将清楚本专利技术的实施例的这些和其他方面。在这些描述和附图中，具体公开了本专利技术的实施例中的一些特定实施方式，来表示实施本专利技术的实施例的原理的一些方式，但是应当理解，本专利技术的实施例的范围不受此限制。相反，本专利技术的实施例包括落入所附加权利要求书的精神和内涵范围内的所有变化、修改和等同物。以下结合附图描述本专利技术。图1是本专利技术一个实施例的视觉与惯性融合的室内定位方法的总体流程图。如图1所示，本专利技术的视觉与惯性融合的室内定位方法，包括以下步骤：S11:当获取到l+1时刻的IMU数据时，依据此测量数据以及时刻l的状态对l+1时刻的状态进行预测。通过时刻l+1的陀螺仪测量数据ωl+1使用数值积分方法计算时刻l到时刻l+1的旋转则l+1时刻的方位四元数估计如下：其中代表时刻l的本体方位四元数，代表时刻l+1本体方位四元数的估计。l+1时刻的位移和速度估计如下：其中表示τ时刻本体在全局坐标系下的加速度，表示τ时刻本体在全局坐标系下的速度，式(1-2)和(1-3)可由梯形公式近似求解。S12：当获取到l+1时刻的IMU数据时，依据时刻l+1的IMU数据对协方差矩阵进行更新。将时刻l的协方差矩阵分本文档来自技高网...

【技术保护点】
1.一种视觉与惯性融合的室内定位方法，其特征在于，包括以下步骤：S11：获取当前时刻的IMU测量数据，并根据当前时刻的IMU测量数据以及前一时刻的IMU状态数据对所述当前时刻的IMU状态数据进行更新，其中，所述IMU状态数据包括速度数据和位移数据；S12：根据所述当前时刻的IMU测量数据以及前一时刻的协方差矩阵进行更新，得到所述当前时刻的协方差矩阵；S21：获取当前帧的图像数据，如果所述当前帧的图像数据为第一幅图像，则在所述第一幅图像中提取第一预设阈值的FAST特征点作为初始光流跟踪点；如果所述当前帧的图像数据不是第一幅图像，则使用网格化的光流跟踪算法进行特征跟踪；S22：检测所述当前帧的图像数据与前一帧的图像数据间的平移值是否超过预设平移阈值，如果未超过所述预设平移阈值则进入步骤S23；如果超过所述预设平移阈值则进入步骤S24；S23：对当前帧的图像数据跟踪到的特征点进行量测更新，且在下一次更新中仍将第k帧作为前一帧进行特征跟踪，并结束当前帧图像的处理；S24：将当前帧的图像数据对应的IMU状态数据加入到滑动窗口的尾部，并对协方差矩阵进行更新，并进入步骤S25；S25：遍历所述当前帧的图像数据的所有特征点，若某个特征点在当前帧中跟踪失败且跟踪持续的帧数大于等于第二预设阈值，则对大于等于所述第二预设阈值的特征点进行量测更新；S26：根据滑动状态窗口中的状态数目和特征点更新情况对滑动状态窗口进行管理。...

【技术特征摘要】
1.一种视觉与惯性融合的室内定位方法，其特征在于，包括以下步骤：S11：获取当前时刻的IMU测量数据，并根据当前时刻的IMU测量数据以及前一时刻的IMU状态数据对所述当前时刻的IMU状态数据进行更新，其中，所述IMU状态数据包括速度数据和位移数据；S12：根据所述当前时刻的IMU测量数据以及前一时刻的协方差矩阵进行更新，得到所述当前时刻的协方差矩阵；S21：获取当前帧的图像数据，如果所述当前帧的图像数据为第一幅图像，则在所述第一幅图像中提取第一预设阈值的FAST特征点作为初始光流跟踪点；如果所述当前帧的图像数据不是第一幅图像，则使用网格化的光流跟踪算法进行特征跟踪；S22：检测所述当前帧的图像数据与前一帧的图像数据间的平移值是否超过预设平移阈值，如果未超过所述预设平移阈值则进入步骤S23；如果超过所述预设平移阈值则进入步骤S24；S23：对当前帧的图像数据跟踪到的特征点进行量测更新，且在下一次更新中仍将第k帧作为前一帧进行特征跟踪，并结束当前帧图像的处理；S24：将当前帧的图像数据对应的IMU状态数据加入到滑动窗口的尾部，并对协方差矩阵进行更新，并进入步骤S25；S25：遍历所述当前帧的图像数据的所有特征点，若某个特征点在当前帧中跟踪失败且跟踪持续的帧数大于等于第二预设阈值，则对大于等于所述第二预设阈值的特征点进行量测更新；S26：根据滑动状...

【专利技术属性】
技术研发人员：程农，田振，李清，李建，
申请(专利权)人：清华大学，
类型：发明
国别省市：北京,11