CN110793526A - 基于可穿戴单目视觉和惯性传感器融合的行人导航方法及系统 - Google Patents

Abstract

本发明公开了一种基于可穿戴单目视觉和惯性传感器融合的行人导航方法及系统，包括：分别得到基于四元数的方向估计，用于位置估计的补偿后的加速度和补偿后的角速度；得到从相机坐标中的三维点到图像坐标中的二维点之间的映射；利用自适应增益方向滤波器来辅助消除不匹配的特征点；利用自适应帧率的单目视觉算法恢复连续两帧图像之间的相对位移；将基于四元数的方向估计、用于位置估计的补偿后的加速度以及连续两帧图像之间的相对位移输入到自适应增益方向滤波器/线性卡尔曼滤波器，得到被测目标的方向和位置估计。本发明对单目相机的帧率进行了调整，不仅能实时获得精确的方向，减少计算负荷，还能滤除加速度和角速度中的噪声和偏差。

Description

基于可穿戴单目视觉和惯性传感器融合的行人导航方法及 系统

技术领域

本发明涉及室内导航定位技术领域，尤其涉及一种基于可穿戴单目视觉和惯性传感器融合的行人导航方法及系统。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

最近，人口老龄化已成为全世界的一个亟待解决的问题，我们知道，随着人们年龄的增长，老人的各种生活能力下降，比如听力和视力下降、记忆力下降、健康问题增多等等。因此，他们在日常活动时经常需要一些外在帮助，比如导航系统，可以有效地引导他们，使他们的生活更容易和方便。考虑到人们需要花费大量时间待在如家里、学校、办公楼、购物中心等室内场所，因此，开发一种适用于室内或自由生活环境的可穿戴导航系统是十分必要的。

单目视觉和惯性传感器之间的融合是由人类和动物的前庭(内耳)和视觉(眼睛)系统发生的事情所激发的，将单目视觉传感器和惯性传感器相结合的基本原理是它们的互补特性，惯性传感器在快速运动期间提供具有高更新速率的良好信号，然而，直接对惯性传感器的线性加速度进行二重积分会导致在位置估计中产生漂移误差，这也是限制惯性导航精度和性能的一个重要问题。相比之下，单目视觉传感器以较低的速率提供长期、精确的自运动估计，但在快速和不可预测的运动下存在模糊特征，比例因子丢失以及处理速度慢的缺点。单目视觉-惯性传感器融合的目的就是利用视觉和惯性传感器的互补特性来克服各自带来的缺点。

尽管融合单目视觉和惯性传感器的动机是它们的互补特性，但要获得鲁棒的融合结果，需要解决以下挑战。首先，漂移累积是惯性传感器的主要缺点，降低该漂移累积不仅要借助于单目视觉传感器，而且要利用惯性传感器本身的特性，如加速度计中的重力加速度和磁力计中的地磁方向。其次，从加速度计中获取准确的读数是准确估计位置的关键，因为位置的估计是通过对该数据的二重积分得到的。第三，由于相机过快的运动，容易造成运动模糊，从而导致特征点的错误匹配，甚至丢失。最后，现有的融合算法通常是对非线性模型采用迭代算法进行处理，而其收敛性不能保证。

发明内容

为了解决上述问题，本发明提出了一一对应的解决方案。首先，提出了一种基于单目相机平移的特点，利用自适应增益方向滤波器来辅助消除特征点匹配错误的算法，该算法与RANSAC算法相比，显著提高了精度并降低了计算量。其次，提出了基于现有的自适应增益方向滤波器的单目相机的帧率调整算法，可用于检测连续两幅图像之间的运动，并对单目相机的帧率进行实时调整，以防止运动模糊，从而不仅可以恢复更准确的baseline(基线)，而且可降低计算速度和内存。第三，提出了一种基于线性模型的融合算法，从而更进一步降低计算复杂度。最后，可将在单目相机跟踪中丢失的绝对比例因子引入到上述融合算法中，作为其中的一个状态变量，和惯性传感器数据进行融合迭代而使其更新并最终收敛。

在一些实施方式中，采用如下技术方案：

基于可穿戴单目视觉和惯性传感器融合的行人导航方法，其特征在于，包括：

通过惯性传感器获得陀螺仪的测量值、加速度计的测量值和磁力计的测量值；

将上述得到的值输入自适应增益方向滤波器，得到基于四元数的方向估计，用于位置估计的补偿后的加速度和补偿后的角速度；

通过视觉传感器得到从相机坐标中的三维点到图像坐标中的二维点之间的映射；

基于SIFT算法进行特征点提取；利用自适应增益方向滤波器来辅助消除不匹配的特征点；

利用自适应帧率的单目视觉算法恢复连续两帧图像之间的相对位移；

将基于四元数的方向估计、用于位置估计的补偿后的加速度以及连续两帧图像之间的相对位移输入到自适应增益方向滤波器/线性卡尔曼滤波器，最终得到被测目标的方向和位置估计。

在另一些实施方式中，采用如下技术方案：

一种终端设备，其包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，所述指令适于由处理器加载并执行上述的基于可穿戴单目视觉和惯性传感器融合的行人导航方法。

在另一些实施方式中，采用如下技术方案：

一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行上述的基于可穿戴单目视觉和惯性传感器融合的行人导航方法。

与现有技术相比，本发明的有益效果是：

本发明利用自适应增益方向滤波器来辅助消除不匹配特征点，显著降低了计算量。基于自适应增益方向滤波器的单目相机的帧率调整算法，用于检测连续两幅图像之间的显著运动。为了避免运动模糊，本发明对单目相机的帧率进行了调整，之后通过线性卡尔曼滤波器算法进行处理，不仅能实时获得精确的方向，减少计算负荷，还能滤除加速度和角速度中的噪声和偏差。

本发明利用惯性传感器进行方向预估，可以根据纯相机平移的性质，消除不匹配的特征点，因此与RANSAC算法相比可以显著减少计算时间。此外，采用自适应帧率的单目相机，不仅避免了基于补偿角速度和加速度的运动模糊，而且在静态运动中，产生了一种对视觉进行零速度更新的效果。

附图说明

图1为是本发明实施例中提出的位置估计方法的架构图；

图2为本发明实施例中世界坐标系(e)、传感器坐标系(s)与摄像机坐标系(c)之间的关系，以及基于单位四元数方向估计的相应变换；

图3(a)为本发明实施例中两个连续帧之间的对极几何示意图；

图3(b)为本发明实施例中单目相机原点的纯平移运动的特性示意图；

图4为本发明实施例中在走廊环境中的两个连续图像中的检测和匹配的特征点；

图5的实验环境为二维平面，行走距离为12米，在xy平面上直线运动的最终位置的估计结果；(a)结果的对比；(b)惯性数据；(c)比例因子估计。

图6为半圆轨迹的估计结果；(a)方向比较的结果；(b)估计的半圆轨迹。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本发明使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

术语解释：

尺度不变特征变换(SIFT)；随机样本一致性(RANSAC)；零速度修正算法(ZUPT)；惯性测量单元(IMU)；卡尔曼滤波器(KF)；扩展的卡尔曼滤波器(EKF)。

实施例一

本发明要解决的主要技术问题有如下几个方面：

1)解决了单纯利用惯性传感器而带来的漂移误差累计的缺点；

2)提出了一种基于自适应增益方向滤波器的相机帧率调整算法，解决了相机产生的运动模糊、视觉特征点不匹配以及单目相机跟踪中丢失的绝对比例因子的问题；

3)实时获得准确的方向，还能去除加速度和角速度中的噪声和偏差。

基于此，本实施例提出了基于可穿戴单目视觉和惯性传感器融合的行人导航方法，它是以用于传感器融合的自适应增益方向滤波器/线性卡尔曼滤波器方法为基础，系统的组成图如图1所示，主要包括以下步骤：

1)自适应增益方向滤波器对惯性传感器得到的加速度数据，角速度数据，磁力计数据进行处理；

2)自适应增益方向滤波器-辅助帧率调整算法对视觉传感器所得到的数据进行处理；

3)通过自适应增益方向滤波器--辅助特征异常值剔除算法，来消除不匹配的二维的特征点；

4)将基于四元数的方向估计、用于位置估计的补偿后的加速度以及无比例因子的baseline(基线)恢复(即连续两帧图像之间的相对位移)输入到自适应增益方向滤波器/线性卡尔曼滤波器，最终得到被测目标的方向和位置估计。

下面对本发明基于可穿戴单目视觉和惯性传感器融合的行人导航方法进行详细的说明。

(1)传感器模型

本实施例使用惯性传感器和单目视觉传感器的测量数据来确定方向和位置估计。为了更好地描述问题，本实施例中使用四元数来表示方向。为了简洁明了，首先对数学变量和符号的定义进行描述：

t-时间；s-传感器坐标系；c-相机坐标系；e-世界坐标系；^eg-世界坐标系下的重力值；^sa-传感器坐标系下的加速度测量值；^sω-传感器坐标系下的角速度测量值；-t时刻，传感器坐标系s到世界坐标系e下的方向；

-相机坐标系到传感器坐标系的相对方向；

-相机坐标系到传感器坐标系的相对平移；^sω_c，t-t时刻，传感器坐标系下补偿的角速度；^sa_b，t-t时刻，传感器坐标系下补偿的加速度；f_s-传感器的采样频率；f-焦距；(x，y)^T-二维图像点；(X，Y.Z)^T-三维点；(c_x，c_y)^T-相机主点；K-相机内部参数；F-基本矩阵；E-本质矩阵；B-连续两次之间的baseline(基线)(即连续两帧图像之间的相对位移)；

-从坐标系2到坐标系1的相对旋转矩阵；l-极线；e-极点；f_c-相机采样率；λ-比例因子的倒数；T-表示相机的自运动。

1)惯性传感器

陀螺仪的测量值加速度计的测量值和磁力计的测量值

的测量模型是基于惯性传感器模块的局限性而建立的，局限性代表惯性传感器的固有缺陷：漂移误差的累积，运动估计精度低，存在噪声干扰等。

本实施例中，根据惯性传感器模块的局限性建立了一个由陀螺仪的测量值、加速度计的测量值和磁力计的测量值组成的测量模型。

所提出的包含这些模型的算法已经在LPMS-B2惯性传感器上实现，将这些测量值作为自适应增益方向滤波器的输入，如图1所示。在惯性传感器建模和校准之后，可以获得最终的基于四元数的方向估计

用于位置估计的补偿后的加速度^sa_b，t和补偿后的角速度^sω_c，t。

图2给出了世界坐标系(e)、传感器坐标系(s)与摄像机坐标系(c)之间的关系，以及基于单位四元数方向估计的相应变换。

2)单目视觉传感器

本实施例选取基本的针孔相机模型，根据三角形相似性，推导出图像坐标中的二维点与相机坐标中的三维点之间的映射关系，如公式(1)所示：

通过引入齐次坐标，式(1)可以写成矩阵乘法，表示为

其中，s作为非零比例引入。

数字图像的坐标通常以左上角的像素为指定单位，为了描述像素坐标系p中的投影点，必须考虑相机的固有参数。因此，公式(1)可以改写成

u＝k_xx+c_x

v＝k_yy+c_y (3)

其中，k_x和k_y表示x和y方向上的像素数/长度单位([像素/长度])。

结合公式(1)和(3)，(x，y)^T和(u，v)^T的关系可以被描述为

结合公式(2)和(4)，相机的固有标定矩阵K，可表示从相机坐标中的3维点到像素坐标中的相应2维点的映射。

其中，f_x和f_y是焦距，分别由焦距f乘以k_x和k_y得到，在相机建模和标定之后，可以获得两个连续图像之间的baseline(基线)b，以进行位置估计。

(2)自适应帧率的单目视觉跟踪

本方法的目的是更精确地恢复连续两帧图像之间的相对位移，即baseline(基线)，同时又可以降低计算量；然而，当单目相机发生剧烈旋转或者平移时，很容易使得图像模糊而导致错误的baseline(基线)恢复。因此本系统提出了一种可将单目相机的帧率自动调节的算法，即自适应帧率的单目视觉算法。此外，现有的方向估计可以解决基于纯相机平移特性的特征点匹配错误的问题，不仅可提高计算精度又可降低计算量。

1)特征点检测

为了获得基于图像帧之间的对应点的相机自运动，找到一种鲁棒的特征点提取和匹配算法至关重要。在实施例中，使用SIFT算法进行特征点匹配，可以解决由于单目相机帧率低而产生的baseline(基线)较宽的问题，此算法即使在比例、照明和姿态变化下，它也能找到有效的特征点。图4给出了在走廊环境中的连续两帧图像中的检测和匹配的特征点。

提取特征点目的是根据图像帧与帧之间的点对应，获得精确的相机自运动。

2)自适应增益方向滤波器辅助特征异常值抑制方法

在特征点匹配的过程中，会出现无匹配的问题，这将会导致错误的自运动估计。为了提高特征点匹配的准确性，并充分利用现有自适应增益方向滤波器，提出了一种基于旋转补偿后的纯相机平移的异常值抑制算法，用于剔除不合适的对应点。

若有一三维点M，m₁和m₂分别为其在连续两帧图像中所对应的二维点。图3(a)给出它们之间的对极几何关系，其中c1、c2分别为t，t+1时刻相机的中心点，e₁和e₂是极点，l₁和l₂是对应于图像点m₂和m₁的极线。根据Eight point算法可以得到如下的公式：

其中，F是基本矩阵(Fundamental matrix)。图3(b)显示了单目相机的纯平移运动的特性。当单目相机作纯平移运动时，二维点在图像平面上沿直线运动。实际上，所有的线将在同一点交叉，这个点被称为“极点”。根据已得到的匹配特征点，可以计算出相机的运动。

公式(5)中F∈R^3×3，其代数意义就表示对极几何，其秩为2。在相机的纯平移运动下(即K₁＝K₂＝K且R＝I)，F可以表示如下：

其中，K是单目相机的固有参数，b是相机从c1到c2的baseline(基线)，R是相机从c1到c2的旋转矩阵。

但是，实际上单目相机的运动，除了平移运动之外，还有旋转运动。从公式(6)可知，可从F矩阵中恢复相机的旋转分量和平移分量。因此，可将基于现有的方向估计作为已知量，利用纯相机平移的特性消除基于公式(5)和(6)的错误对应关系，如：

其中，

为连续两帧图像之间的相对旋转矩阵。

在实际应用中，由SIFT算法得到的对应点包含噪声，故公式(7)不能满足小误差的条件。因此，需要重新定义SIFT算法中的最小重投影误差。本专利中，根据纯相机平移的特性，利用极线对

和l₂＝[e]_×m₁评估公式(8)中所示的距离d。满足公式(8)的相应特征点，则与对极几何完全一致，否则，将被视为异常点而剔除。

3)辅助单目相机的帧率调整-自适应增益方向滤波器

为了在连续两帧图像之间获得baseline(基线)b，需要单目相机在连续两帧图像之间有明显的运动。因此，本专利选择低帧率作为单目相机的主要帧率；但是，当相机快速运动时，单目相机的低帧率会使图像模糊，从而导致baseline(基线)恢复不正确；为了避免图像模糊，我们需要提高帧率。

考虑到单目相机的旋转会造成严重的图像变形，为了调整单目相机帧率，旋转被认为是单目相机运动的主要因素，而平移被认为是单目相机运动的次要因素，特别是在一些地面车辆的应用。

因此，本实施例提出了一种基于现有的自适应增益方向滤波器的单目相机的帧率调整算法，用于基于连续两帧图像之间的^sω_c，t，^sa_b，t和二维点的对应关系，将单目相机运动分类为三种类型(快，慢和静态运动)。

具体地，

当角速度≤ε_ω，并且加速度≤ε_a时，定义单目相机运动为慢；此时，保持单目相机的预定义帧率；

当角速度≥ε_ω，并且加速度≥ε_a时，定义单目相机运动为快；此时，增加单目相机的帧率；其中，ε_ω和ε_a均为预设的角速度阈值和加速度阈值；

根据连续两帧图像之间的二维点的对应关系来确定是否为静态运动；如果是静态运动，则保持单目相机的预定义帧率。

这使得视觉系统能够以类似于ZUPT算法的方式进行漂移校正，ZUPT算法通常用于行人室内导航。

4)无尺度的单目相机-自运动的baseline(基线)恢复

相机的外部参数可以根据E＝[b]_×R从本质矩阵E中提取，其中[b]_×为baseline(基线)b对应的3×3斜对称矩阵，R为旋转矩阵。E可以通过Eight point算法计算得出。通过使用E中的(9)和(10)可以恢复图3(a)中所示的旋转矩阵R和baseline(基线)b。

其中I是一个3×3的单位矩阵，E^*为E的辅助因子矩阵。B可通过公式(9)得到，b有两个解(b₁和b₂)，可以按如下方式计算：

其中i＝1，2，3，B(i，i)是矩阵B的对角线的最大元素。通过检查重建点在单目相机两个视图前的深度是否为正，可以选择正确的解。

通过前述的异常点剔除和帧率的调整，得到正确匹配点，基于正确的匹配点，得到baseline(基线)。

如我们所提到的，b表示连续两帧图像之间的相对平移，因此，需要根据公式(11)假设世界坐标系与相机坐标系在t＝0时刻相同，计算相机在t＝k时刻的绝对自运动。

T_k＝δT*T_k-1 (11)

其中，T_k，T_k-1和δT分别是相对于地球坐标系下时间t＝k，t＝k-1的齐次变换矩阵。

在单目相机跟踪系统中，由于目标场景的大小与单目相机到场景的距离存在模糊性，因此，无法确定世界坐标系的尺度。相反，在惯性导航系统中，由于加速度计的读数是用绝对公制单位表示的，所以世界坐标系的比例因子是与之相关的。因此，将惯性传感器与单目相机进行融合，可以得到绝对比例因子。

(3)惯性传感器-单目视觉融合的自适应增益方向滤波器/线性卡尔曼滤波器

将低采样率的单目视觉数据与高采样率的惯性数据相结合，目的是获得高采样率的精确位置估计。单目视觉传感器在跟踪过程中会出现尺度信息丢失的问题，而惯性跟踪由于漂移误差的累积，在长期导航中无法提供正确的尺度位置。因此，本实施例设计了一种称为“自适应增益方向滤波器/线性卡尔曼滤波器”的线性估计器，将单目视觉测量值和惯性测量值进行融合(惯性测量值包括加速度，角速度，磁力强度，视觉测量值就是视频录像)，以实现其互补特性。采用多速率融合方法对两种采样速率下的测量数据进行融合。此外，绝对比例因子可以将它们融合在一起。

下面对无需更新方向的自适应增益方向滤波器/线性卡尔曼滤波器进行介绍：

1)状态向量的定义

t＝k时刻的状态向量由单目相机位置

(无标度)、单目相机速度

单目相机加速度

(以米为单位)、绝对比例因子s_k的倒数：λ_k＝1/s_k(其中λ_k可以生成一个低阶多项式)和加速度计偏差b_a，k共同组成；因此状态向量和系统过程噪声表示为：

2)动态模型

假设系统在k时刻有一个匀加速的线性平移，k和k+1之间的时间间隔为T。单目相机的平移可以用一个方程组来建模，随机模型用于估计λ_k和k时刻的白噪声的偏差b_a，k。因此，动态模型定义为：

λ_k+1＝λ_k+nλ

3)测量模型：因为这两个传感器的采样频率不同，所以采用了来自于这两种传感器的测量值：

表示可用的惯性测量值，

表示可用的单目视觉测量值，输出状态的测量值的更新方程为：

y_k＝Hx_k+e_k (12)

其中H_s，k＝(I_3×3 0_3×3 0_3×4 0_3×3)为可用的惯性测量值，H_c，k＝(I_3×3 0_3×3 0_3×40_3×3)为可用的单目视觉测量值。

单目视觉部分的测量输入是单目相机的位置，由于加速度是在相机坐标系中使用的，为了将原始的测量值从传感器坐标系转换为相机坐标系，消除重力^eg＝[0 0 -9.8]^T在世界坐标系中的影响，需要(13)所示的惯性传感器的测量输入转换模型。

其中，

为在传感器坐标系下加速度计在k时刻的原始测量值；运算符

为将单位四元数表示的方向转换为由旋转矩阵表示的方向；

和

可以从初始校准中获得；

为相机坐标系到传感器坐标系的相对方向；

为相机坐标系到传感器坐标系的相对平移；表示k时刻，传感器坐标系s到世界坐标系e下的方向；^eg为世界坐标系下的重力值，e_a，k表示加速度在k时刻的误差。

相比于目前的需要更新方向的扩展卡尔曼滤波器，本实施例中的无需更新方向的自适应增益方向滤波器/线性卡尔曼滤波器，所需要的参数比较少，计算复杂度低，准确性高。

(4)实验验证

1)系统架构：本专利使用的惯性传感器为LPMS-B2惯性传感器，它集成三轴加速度计、三轴陀螺仪、三轴磁力计。对于视觉输入，使用的为USB CMOS彩色自动对焦单目相机(DFK 72AUC02-F)，分辨率为640x480，帧率高达52帧/秒。在本实施例中，使用低帧速率3帧/秒作为主要速率来满足对极几何约束，以获得精确的baseline(基线)，并且根据自适应增益方向滤波器辅助单目相机帧率调整算法来适应性地调整帧率。使用的阈值设为：ε＝0.01，ε_ω＝18°/s，ε_a＝1.5m/s²，ε_c＝5像素。

2)实验装置：首先，在走廊和房间中进行两个不同的实验。为了验证位置估计的准确性，我们使用了一个先锋机器人的二维位置作为标准测量值，其准确度已经得到了验证。例如，在第一个实验中，进行了真实长度为12米的直线测试，先锋机器人的最终结果是12.1393米，如图5(a)-(c)所示。因此，误差精度为大约为1.16％。

其次，图6(b)为xy平面的半圆运动；该测试在家庭环境中的机器人手臂上进行，通过控制遥控器产生半圆运动。图6(a)示出了方向估计的比较结果，可清楚地看到，仅使用单目相机所得到的方向具有明显的漂移，尤其是俯仰角和翻滚角的估计。因此，EKF的估计结果具有较大的漂移，而本专利中的基于自适应增益方向滤波器的KF的精度优于EKF。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.基于可穿戴单目视觉和惯性传感器融合的行人导航方法，其特征在于，包括：

通过惯性传感器获得陀螺仪的测量值、加速度计的测量值和磁力计的测量值；

将上述得到的值输入自适应增益方向滤波器，得到基于四元数的方向估计，用于位置估计的补偿后的加速度和补偿后的角速度；

通过视觉传感器得到从相机坐标中的三维点到图像坐标中的二维点之间的映射；

基于SIFT算法进行特征点提取；利用自适应增益方向滤波器来辅助消除不匹配的特征点；

利用自适应帧率的单目视觉算法恢复连续两帧图像之间的相对位移；

将基于四元数的方向估计、用于位置估计的补偿后的加速度以及连续两帧图像之间的相对位移输入到自适应增益方向滤波器/线性卡尔曼滤波器，最终得到被测目标的方向和位置估计。

2.如权利要求1所述的基于可穿戴单目视觉和惯性传感器融合的行人导航方法，其特征在于，通过视觉传感器得到从相机坐标中的三维点到图像坐标中的二维点之间的映射，具体为：

其中，f_x和f_y是焦距，分别由焦距f乘以k_x和k_y得到，k_x和k_y表示x和y方向上的像素数/长度单位，(c_x,c_y)^T表示相机主点，(x,y)^T表示二维图像点；(X,Y,Z)^T表示三维点。

3.如权利要求1所述的基于可穿戴单目视觉和惯性传感器融合的行人导航方法，其特征在于，利用自适应增益方向滤波器来辅助消除不匹配的特征点，具体为：

其中，m₁和m₂分别为其在连续两帧图像中所对应的二维点，c1、c2分别为t,t+1时刻相机的中心点，e₁和e₂是极点，l₁和l₂是对应于图像点m₂和m₁的极线，其中和l₂＝[e]_×m₁，

为连续两帧图像之间的相对旋转矩阵；[e]_×表示的是极点e所对应的斜对称矩阵；l₁(1,:)表示的是一个3行1列的一个矩阵，l₁(1,:)取的是l₁中第1行的数，l₁(2,:)取的是第2行的数；l₂(1,:)取的是l₂中第1行的数，l₂(2,:)取的是l₂中第2行的数；d表示距离，ε为阈值。

对于满足上述公式的特征点，则与对极几何完全一致，否则，被视为异常点而剔除。

4.如权利要求1所述的基于可穿戴单目视觉和惯性传感器融合的行人导航方法，其特征在于，所述自适应帧率的单目视觉算法具体为：

将单目相机运动分类为快、慢和静态运动三种类型；

当角速度≤ε_ω，并且加速度≤ε_a时，定义单目相机运动为慢；此时，保持单目相机的预定义帧率；

当角速度≥ε_ω，并且加速度≥ε_a时，定义单目相机运动为快；此时，增加单目相机的帧率；其中，ε_ω和ε_a均为预设的角速度阈值和加速度阈值；

根据连续两帧图像之间的二维点的对应关系来确定是否为静态运动；如果是静态运动，则保持单目相机的预定义帧率。

5.如权利要求1所述的基于可穿戴单目视觉和惯性传感器融合的行人导航方法，其特征在于，利用自适应帧率的单目视觉算法恢复连续两帧图像之间的相对位移，具体为：

(b·b)R＝E^*T-b×E

其中，I是一个3×3的单位矩阵，E^*为E的辅助因子矩阵；E为本质矩阵，R为旋转矩阵，b表示连续两帧图像之间的相对位移，b有两个解，可以按如下方式计算：

i＝1，2，3，B(i,i)是矩阵B的对角线的最大元素，矩阵B＝bb^T。

6.如权利要求1所述的基于可穿戴单目视觉和惯性传感器融合的行人导航方法，其特征在于，所述自适应增益方向滤波器/线性卡尔曼滤波器的状态向量和系统过程噪声分别为：

其中，

为单目相机位置，

为单目相机速度，

为单目相机加速度，λ_k＝1/s_k表示绝对比例因子s_k的倒数，b_a,k为加速度计偏差。

7.如权利要求1所述的基于可穿戴单目视觉和惯性传感器融合的行人导航方法，其特征在于，所述自适应增益方向滤波器/线性卡尔曼滤波器的测量模型为：

y_k＝Hx_k+e_k；

其中，采用了来自于这两种传感器的测量值:

表示可用的惯性测量值，表示可用的单目视觉测量值；H_s,k＝(I_3×3 0_3×3 0_3×4 0_3×3)为可用的惯性测量值，H_c,k＝(I_3×3 0_3×3 0_3×4 0_3×3)为可用的单目视觉测量值；e_k代表测量误差。

8.如权利要求1所述的基于可穿戴单目视觉和惯性传感器融合的行人导航方法，其特征在于，所述自适应增益方向滤波器/线性卡尔曼滤波器的测量模型中，惯性传感器的测量输入转换模型为：

其中，

为在传感器坐标系下加速度计在k时刻的原始测量值；运算符

为将单位四元数表示的方向转换为由旋转矩阵表示的方向；

为相机坐标系到传感器坐标系的相对方向；为相机坐标系到传感器坐标系的相对平移；

表示k时刻，传感器坐标系s到世界坐标系e下的方向；^eg为世界坐标系下的重力值，e_a,k表示加速度在k时刻的误差。

9.一种终端设备，其包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，其特征在于，所述指令适于由处理器加载并执行权利要求1-8任一项所述的基于可穿戴单目视觉和惯性传感器融合的行人导航方法。

10.一种计算机可读存储介质，其中存储有多条指令，其特征在于，所述指令适于由终端设备的处理器加载并执行权利要求1-8任一项所述的基于可穿戴单目视觉和惯性传感器融合的行人导航方法。

Images