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

本发明属于无人机导航定位技术领域，特别涉及一种视觉与惯性融合的室内定位方法。

背景技术

微小无人机在室内的自主飞行对导航系统的精度、鲁棒性以及定位频率都有很高的要求。由于室内不能获得稳定的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的协方差矩阵分块为如下形式：

其中pii,l表示当前imu状态对应的协方差矩阵，pcc,l代表保存的n个过去的imu状态对应的协方差矩阵。

时刻l+1的协方差矩阵更新为：

其中，

式(1-6)中:

运算如式(1-10)中所示：

式(1-7)中：

其中σgc,σwgc分别为陀螺仪测量噪声以及偏差量噪声的标准差，σac,σwac分别为加速度测量噪声以及偏差量噪声的标准差。

对于图像数据的处理(记新获得的图像为第k+1帧，前一帧图像为第k帧)：

s21：对于获取的图像数据，若为第一幅图像，则在图像中提取300个fast特征点作为初始光流跟踪点，若不是第一幅图像，则使用网格化的光流跟踪算法进行特征跟踪。其步骤如下：

s21_1：对图像进行如下的预处理以减少光照变化带来的影响：从图像左上方像素开始对图像进行滑动窗口处理，选取3×3大小的窗口，对于窗口中心的像素点，将其与其邻域内的8个像素点进行比较，若中心点灰度值更大则赋1，否则赋0，最终将这8个0/1值组合成二进制数作为中心点的灰度值；

s21_2：在光流跟踪过程中，将图像均匀分为8×8个网格，若跟踪过程中特征点数目小于300，则通过提取新的fast特征点补足，而提取fast特征点的原则是使特征点尽量均匀的分布到8×8的网格块中去；

s21_3：对于上一步中获得的特征点跟踪点对，采用ransac算法(常用的剔除外点算法)剔除匹配错误的点对。

s22：检测前后两帧图像间的平移是否超过预设平移阈值，本发明中采用如下方法进行检测：设第k帧图像和第k+1帧图像间的共有m对匹配点对：先将其转化为以图像像素中心为原点的齐次坐标，并归一化记为计算下式：

其中表示帧k到帧k+1的旋转矩阵，若式(2-1)求得的t＜0.01，则判定平移量未超过预设平移阈值，反之判断超过预设平移阈值。

s23：若s22中判定的平移量很小，则不改变滑动窗口中的状态，而是对当前图像帧跟踪到的特征点进行一次量测更新(量测更新的步骤与s25中相似，将在s25中详细介绍)，且在下一次更新中仍将第k帧作为前一帧进行特征跟踪，并结束当前帧图像的处理。

s24：若s22中判定的平移量足够大，则将当前帧对应的imu状态加入到滑动窗口的尾部，此时协方差矩阵作以下的相应更新：

其中pk|k表示获取第k+1帧图像时刻系统的协方差矩阵，pk+1|k为将第k+1帧图像对应的imu状态加入滑动窗口后的协方差矩阵，式(2-2)中：

s25：遍历所有特征点，若其在当前帧中跟踪失败且跟踪持续的帧数大于等于3，则对其进行三角化并将其运用到量测更新中。假设共有m个特征点满足上述条件并将它们记作fi,i＝1…m。

对于特征点fi首先通过三角化计算出其在全局坐标系下的坐标将估计的特征点坐标投影到像素平面得到的投影点与特征点的跟踪位置间的像素差建模为量测误差，假设特征点fi共在n个图像帧上被观测到，则其对应的量测误差为：

其中，ni为像素噪声：

且

式(2-8)中fx，fy分别代表焦距与x,y方向1个像素距离的比值，[xijyijzij]为特征点在第j帧相机坐标系下的坐标。

构造的左零空间矩阵并将其左乘到式(2-4)的两侧消去包含的项得到如下形式：

将所有m个待跟新的特征点写成(2-9)种形式，并叠加到一起如下式中所示形式：

记为：

对式(2-11)中所得结果采用标准卡尔曼公式进行更新：

δx＝kr(2-13)

pk+1|k+1＝(i-kh)pk+1|k(i-kh)^t+krnk^t(2-14)

其中rn为式(2-11)中像素误差对应的方差阵。用求得的δx更新状态变量即可得到无人机当前时刻的位置、速度等状态。

s26：为了限制计算量的增加，本发明中在状态窗口中只保存过去的20个状态，所以当状态窗口中状态数超过20时要进行相应的处理。本发明中采用的策略是：首先检查所有状态对应的图像帧，若其观测到的特征点已经全部用于量测更新，则直接将其删除并对协方差矩阵进行相应的更新；若没有满足前述条件的状态帧，则将状态窗口中的第2帧删除，并对其观测到的仍旧处于持续跟踪状态的特征点进行一次步骤s25中所述的量测更新。

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

另外，本发明实施例的视觉与惯性融合的室内定位方法的其它构成以及作用对于本领域的技术人员而言都是已知的，为了减少冗余，不做赘述。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同限定。