阅读说明：本技术 一种单目视觉定位的imu辅助跟踪方法及装置 (Monocular vision positioning IMU (inertial measurement unit) auxiliary tracking method and device ) 是由 白立建 李骊 于 2019-10-16 设计创作，主要内容包括：本发明提供了一种单目视觉定位的IMU辅助跟踪方法及装置,在视觉系统初始化完成且IMU在数据稳定时初始化完成后,获取视觉定位结果和IMU定位结果；在视觉跟踪正常且视觉定位时间与IMU定位时间一致的情况下,依据所述视觉定位结果对所述IMU定位结果进行校正；在视觉跟踪失败的情况下,利用所述IMU定位结果辅助视觉定位。视觉跟踪正常时,通过视觉定位结果对IMU定位结果进行校正,提高IMU定位定姿的精度,视觉跟踪失败时,通过利用IMU定位结果辅助视觉定位,提高动态跟踪定位精度。(The invention provides an IMU (inertial measurement unit) auxiliary tracking method and device for monocular vision positioning, wherein a vision positioning result and an IMU positioning result are obtained after initialization of a vision system is completed and the initialization is completed when data of the IMU is stable; under the condition that the visual tracking is normal and the visual positioning time is consistent with the IMU positioning time, correcting the IMU positioning result according to the visual positioning result; and in the case of visual tracking failure, assisting visual positioning by utilizing the IMU positioning result. When the visual tracking is normal, the IMU positioning result is corrected through the visual positioning result, the IMU positioning and attitude determining precision is improved, and when the visual tracking fails, the dynamic tracking and positioning precision is improved by assisting the visual positioning through the IMU positioning result.)

一种单目视觉定位的IMU辅助跟踪方法及装置

技术领域

本发明涉及视觉定位技术领域，更具体的，涉及一种单目视觉定位的IMU辅助跟踪方法及装置。

背景技术

传统的单目视觉导航是根据一帧一帧的图像进行定位跟踪，受图片更新频率的限制，在快速运动时易出现丢帧和跟踪失败现象，从而无法快速精确进行定位定姿。IMU(Inertia Measurement Unit，惯性测量单元)能敏感测量载体在惯性空间中的角速度和加速度，通过积分进行定位定姿，故可在快速移动视觉丢失的情况下采用IMU进行位置跟踪。

如何利用IMU辅助跟踪得到准确的定位结果成为本领域亟待解决的技术问题。

发明内容

有鉴于此，本发明提出了一种单目视觉定位的IMU辅助跟踪方法，将视觉和IMU信息进行快速融合，解决了快速移动时视觉无法高精度定位定姿的问题。

为了实现上述发明目的，本发明提供的具体技术方案如下：

一种单目视觉定位的IMU辅助跟踪方法，包括：

在视觉系统初始化完成且IMU在数据稳定时初始化完成后，获取视觉定位结果和IMU定位结果；

在视觉跟踪正常且视觉定位时间与IMU定位时间一致的情况下，依据所述视觉定位结果对所述IMU定位结果进行校正；

在视觉跟踪失败的情况下，利用所述IMU定位结果辅助视觉定位。

可选的，所述方法还包括：

建立视觉坐标系，完成视觉系统的初始化。

可选的，在视觉系统初始化完成之后，所述方法还包括：

在IMU数据稳定时计算IMU到导航坐标系的姿态转移矩阵；

计算IMU到所述视觉坐标系的姿态转移矩阵；

根据IMU到所述导航坐标系的姿态转移矩阵和IMU到所述视觉坐标系的姿态转移矩阵，计算所述视觉坐标系到所述导航坐标系的姿态转移矩阵，完成IMU的初始化。

可选的，在所述计算IMU到所述视觉坐标系的姿态转移矩阵之后，所述方法还包括：

将IMU到所述导航坐标系的Z轴欧拉角设置为IMU到所述视觉坐标系的姿态转移矩阵的Z轴欧拉角。

可选的，在所述计算所述视觉坐标系到所述导航坐标系的姿态转移矩阵之后，所述方法还包括：

在IMU数据稳定时多次计算所述视觉坐标系到所述导航坐标系的姿态转移矩阵，并对计算得到的多个所述视觉坐标系到所述导航坐标系的姿态转移矩阵取平均值，得到准确的所述视觉坐标系到所述导航坐标系的姿态转移矩阵。

可选的，所述利用所述IMU定位结果辅助视觉定位，包括：

获取IMU测量得到的加速度和陀螺仪数据；

基于所述陀螺仪数据对IMU到所述视觉坐标系的姿态转移矩阵进行更新；

基于准确的所述视觉坐标系到所述导航坐标系的姿态转移矩阵与更新后的IMU到所述视觉坐标系的姿态转移矩阵，计算IMU到所述导航坐标系的姿态转移矩阵；

基于所述加速度计算所述导航坐标系中的位移矢量；

依据所述导航坐标系中的位移矢量和准确的所述视觉坐标系到所述导航坐标系的姿态转移矩阵，计算所述视觉坐标系中的位移矢量。

一种单目视觉定位的IMU辅助跟踪装置，包括：

定位结果获取单元，用于在视觉系统初始化完成且IMU在数据稳定时初始化完成后，获取视觉定位结果和IMU定位结果；

定位结果校正单元，用于在视觉跟踪正常且视觉定位时间与IMU定位时间一致的情况下，依据所述视觉定位结果对所述IMU定位结果进行校正；

辅助视觉定位单元，用于在视觉跟踪失败的情况下，利用所述IMU定位结果辅助视觉定位。

可选的，所述装置还包括：

视觉初始化单元，用于建立视觉坐标系，完成视觉系统的初始化。

可选的，所述装置还包括：

IMU初始化单元，用于在IMU数据稳定时计算IMU到导航坐标系的姿态转移矩阵；计算IMU到所述视觉坐标系的姿态转移矩阵；根据IMU到所述导航坐标系的姿态转移矩阵和IMU到所述视觉坐标系的姿态转移矩阵，计算所述视觉坐标系到所述导航坐标系的姿态转移矩阵，完成IMU的初始化。

可选的，所述装置还包括：

Z轴欧拉角设置单元，用于将IMU到所述导航坐标系的Z轴欧拉角设置为IMU到所述视觉坐标系的姿态转移矩阵的Z轴欧拉角。

可选的，所述IMU初始化单元，还用于在IMU数据稳定时多次计算所述视觉坐标系到所述导航坐标系的姿态转移矩阵，并对计算得到的多个所述视觉坐标系到所述导航坐标系的姿态转移矩阵取平均值，得到准确的所述视觉坐标系到所述导航坐标系的姿态转移矩阵。

可选的，所述辅助视觉定位单元，具体用于：

获取IMU测量得到的加速度和陀螺仪数据；

基于所述陀螺仪数据对IMU到所述视觉坐标系的姿态转移矩阵进行更新；

基于准确的所述视觉坐标系到所述导航坐标系的姿态转移矩阵与更新后的IMU到所述视觉坐标系的姿态转移矩阵，计算IMU到所述导航坐标系的姿态转移矩阵；

基于所述加速度计算所述导航坐标系中的位移矢量；

依据所述导航坐标系中的位移矢量和准确的所述视觉坐标系到所述导航坐标系的姿态转移矩阵，计算所述视觉坐标系中的位移矢量。

相对于现有技术，本发明的有益效果如下：

本发明公开的一种单目视觉定位的IMU辅助跟踪方法，在视觉跟踪正常且视觉定位时间与IMU定位时间一致的情况下，依据视觉定位结果对IMU定位结果进行校正，提高IMU定位定姿精度，在视觉跟踪失败的情况下，根据IMU和视觉系统的测量特点将视觉定位结果和IMU定位结果进行融合，利用IMU定位结果辅助视觉定位，提高动态跟踪定位精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明实施例公开的一种单目视觉定位的IMU辅助跟踪方法的流程示意图；

图2为本发明实施例公开的一种单目视觉定位的IMU辅助跟踪装置的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本实施例公开了一种单目视觉定位的IMU辅助跟踪方法，可以应用于移动终端如智能手机、平板电脑等，请参阅图1，该方法包括以下步骤：

S101：在视觉系统初始化完成且IMU在数据稳定时初始化完成后，获取视觉定位结果和IMU定位结果；

在移动设备开机后，首先要对视觉系统和IMU进行初始化。

其中，对视觉系统的初始化包括建立视觉坐标系。

对IMU进行初始化需要在IMU数据稳定时，具体的，可以通过判断IMU数据与IMU静止状态的数据的差值是否小于阈值来判定IMU数据是否稳定。

IMU初始化包括在IMU数据稳定时计算IMU到导航坐标系的姿态转移矩阵

计算IMU到视觉坐标系的姿态转移矩阵

并根据IMU到导航坐标系的姿态转移矩阵

和IMU到视觉坐标系的姿态转移矩阵

计算视觉坐标系到导航坐标系的姿态转移矩阵其中，

的计算方式如下：

其中，计算IMU到导航坐标系的姿态转移矩阵

的方法如下：

传统POS初始环境为静止或微幅晃动的环境，可采用传统解析式的方法进行粗对准。绝大部分使用的惯导系统都是选用与地球相固联的参考坐标系^[73]，所以借助惯性仪表测量两个在空间不共线的矢量：重力矢量g和地球自转角速率矢量ω_ie，实现自对准。常用的粗对准算法包括：①利用g、ω_ie和构造的g×ω_ie计算姿态矩阵；②利用g、构造的g×ω_ie和构造的(g×ω_ie)×g计算姿态矩阵；③利用惯性器件输出的直接计算法。在进行初始对准时，当地的经度λ和纬度L是已知的，重力加速度g和地球自转角速度ω_ie在地理坐标系的分量都是确定的，可表示为以下两个矢量：

捷联矩阵

中包含9个未知元素，为了求解全部9个元素，需要构造9个方程，意味着需要3个三维向量，因此取载体坐标系中任意不共线的矢量通过捷联矩阵

变换到导航坐标系中有：

则有：

因为捷联矩阵是正交矩阵，故

可以写成：

重力加速度和地球自转加速度为已知量，它们的叉乘也是已知量。

因此可令d₁＝g，d₂＝g×ω_ie，d₃＝(g×ω_ie)×g，或者是令d₁＝g，d₂＝ω_ie，d₃＝g×ω_ie，则有：

或者：

为了提高

的精度，可求取多次

取平均值，但由于初始化根据求解的姿态角中Z轴欧拉角与IMU中的陀螺仪的Z轴欧拉角不一致，需要在计算之前，将IMU到视觉坐标系的姿态转移矩阵的Z轴欧拉角设置为IMU的陀螺仪的Z轴欧拉角，在此基础上，在IMU数据稳定时多次计算视觉坐标系到导航坐标系的姿态转移矩阵

并对计算得到的多个取平均值，得到准确的

完成IMU的初始化。

还需要说明的是，实时采集IMU数据，当IMU未进行移动时，重新对IMU进行初始化，得到为常数矩阵。

S102：在视觉跟踪正常且视觉定位时间与IMU定位时间一致的情况下，依据所述视觉定位结果对所述IMU定位结果进行校正；

由于视觉跟踪更新频率较慢，IMU更新频率较快，因此，只有在视觉定位时间与IMU定位时间一致的情况下，才能保证理想状态下视觉定位结果与IMU定位结果一致，视觉定位时间与IMU定位时间一致时，依据视觉定位结果对IMU定位结果进行校正才能得到准确的定位数据。

S103：在视觉跟踪失败的情况下，利用所述IMU定位结果辅助视觉定位。

视觉跟踪失败，如视觉系统发生丢帧，此时视觉定位结果是不准确的，需要利用IMU定位结果辅助视觉定位。

获取IMU测量得到的加速度

和陀螺仪数据。

首先，基于陀螺仪数据对IMU到视觉坐标系的姿态转移矩阵

进行更新，将IMU到导航坐标系的Z轴欧拉角设置为IMU到所述视觉坐标系的姿态转移矩阵的Z轴欧拉角，使姿态角中Z轴欧拉角与IMU中的陀螺仪的Z轴欧拉角一致。

然后基于准确的视觉坐标系到导航坐标系的姿态转移矩阵与更新后的IMU到视觉坐标系的姿态转移矩阵

计算IMU到导航坐标系的姿态转移矩阵

然后基于及速度计算导航坐标系中的位移矢量。

具体的，利用IMU到导航坐标系的姿态转移矩阵和加速度计测量出的加速度

计算导航坐标系中的加速度

由于加速度计测量出的加速度

为绝对加速度，绝对加速度中包含了IMU相对于惯性系的所有加速度，其中包括重力加速度等有害加速度，所以需要消除导航坐标系中的加速度

中的有害加速度。

其中，消除导航坐标系中的加速度中的有害加速度首先需要确定重力加速度与视觉坐标系的关系，本实施例提供两种确定重力加速度与视觉坐标系的关系的方法：

方法1：求解IMU到导航坐标系的姿态转移矩阵

已知同时刻的IMU到视觉坐标系的姿态转矩阵

就可求得

重力加速度与导航坐标系的Z轴重合，这就可计算出视觉坐标系下IMU的位移变化。

方法2：基于VINS系统初始化步骤中求取重力在视觉坐标系的分量，从而去除有害加速度求解视觉坐标系下IMU的位移变化。

对消除有害加速度后的加速度进行积分处理和位置计算，得到所述导航坐标系中的位移矢量，具体的，对消除有害加速度后的加速度进行积分处理得到导航坐标系中的速度矢量Vⁿ，对速度矢量Vⁿ进行位置计算，得到导航坐标系中的位移矢量Tⁿ。

依据导航坐标系中的位移矢量Tⁿ和准确的视觉坐标系到导航坐标系的姿态转移矩阵

计算视觉坐标系中的位移矢量T^cam。

本实施例公开的一种单目视觉定位的IMU辅助跟踪方法，在视觉跟踪正常且视觉定位时间与IMU定位时间一致的情况下，依据视觉定位结果对IMU定位结果进行校正，提高IMU定位定姿精度，在视觉跟踪失败的情况下，根据IMU和视觉系统的测量特点将视觉定位结果和IMU定位结果进行融合，利用IMU定位结果辅助视觉定位，提高动态跟踪定位精度。

基于上述实施例公开的一种单目视觉定位的IMU辅助跟踪方法，本实施例对应公开了一种单目视觉定位的IMU辅助跟踪装置，请参阅图2，该装置包括：

定位结果获取单元201，用于在视觉系统初始化完成且IMU在数据稳定时初始化完成后，获取视觉定位结果和IMU定位结果；

定位结果校正单元202，用于在视觉跟踪正常且视觉定位时间与IMU定位时间一致的情况下，依据所述视觉定位结果对所述IMU定位结果进行校正；

辅助视觉定位单元203，用于在视觉跟踪失败的情况下，利用所述IMU定位结果辅助视觉定位。

可选的，所述装置还包括：

视觉初始化单元，用于建立视觉坐标系，完成视觉系统的初始化。

可选的，所述装置还包括：

IMU初始化单元，用于在IMU数据稳定时计算IMU到导航坐标系的姿态转移矩阵；计算IMU到所述视觉坐标系的姿态转移矩阵；根据IMU到所述导航坐标系的姿态转移矩阵和IMU到所述视觉坐标系的姿态转移矩阵，计算所述视觉坐标系到所述导航坐标系的姿态转移矩阵，完成IMU的初始化。

可选的，所述装置还包括：

Z轴欧拉角设置单元，用于将IMU到所述导航坐标系的Z轴欧拉角设置为IMU到所述视觉坐标系的姿态转移矩阵的Z轴欧拉角。

可选的，所述IMU初始化单元，还用于在IMU数据稳定时多次计算所述视觉坐标系到所述导航坐标系的姿态转移矩阵，并对计算得到的多个所述视觉坐标系到所述导航坐标系的姿态转移矩阵取平均值，得到准确的所述视觉坐标系到所述导航坐标系的姿态转移矩阵。

可选的，所述辅助视觉定位单元，具体用于：

获取IMU测量得到的加速度和陀螺仪数据；

基于所述陀螺仪数据对IMU到所述视觉坐标系的姿态转移矩阵进行更新；

基于准确的所述视觉坐标系到所述导航坐标系的姿态转移矩阵与更新后的IMU到所述视觉坐标系的姿态转移矩阵，计算IMU到所述导航坐标系的姿态转移矩阵；

基于所述加速度计算所述导航坐标系中的位移矢量；

依据所述导航坐标系中的位移矢量和准确的所述视觉坐标系到所述导航坐标系的姿态转移矩阵，计算所述视觉坐标系中的位移矢量。

本实施例公开的一种单目视觉定位的IMU辅助跟踪装置，在视觉跟踪正常且视觉定位时间与IMU定位时间一致的情况下，依据视觉定位结果对IMU定位结果进行校正，提高IMU定位定姿精度，在视觉跟踪失败的情况下，根据IMU和视觉系统的测量特点将视觉定位结果和IMU定位结果进行融合，利用IMU定位结果辅助视觉定位，提高动态跟踪定位精度。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。