视觉及惯性混合位姿跟踪方法、系统、头盔及存储介质与流程

1.本发明属于位姿跟踪技术领域，具体地涉及一种视觉及惯性混合位姿跟踪方法、系统、头盔及存储介质。


背景技术：

2.传统头部跟踪实现的技术手段有多种方法，可分为电磁法、光电法、超声波法等。电磁法基本原理是建立一个特定磁场区域，利用磁场感应器来获取磁场信息，根据所得信息来计算出感应器所在位置的坐标系与参考系之间的关系参数；缺点是易受到磁性金属、电磁场级地磁的干扰，稳定性不好。超声波法以脉冲时间为基础的超声波测距技术，在超声波跟踪器中，所有能够影响声速的因素都会影响系统的性能，且发射器和接收器之间不能有遮挡，而较大的延迟则进一步限制其适用范围。光电法利用光电探测器对一定辐射源进行位置和角度测量，进而求出被测物体的姿态、位置参数；其具有抗电磁能力强，一旦位置固定，其精度可以保证；缺点是存在遮挡问题。
3.现有技术的运动载具中的头盔位姿跟踪应用主流方法有两种：1、惯性测量算法，其根据陀螺仪的角速度、加速度计的加速度以及磁传感器的磁强度计算目标的姿态角，具有刷新频率较高以及测量延迟较低的优点且跟踪时不依赖于外部标志点，在出现遮挡问题时可在较短时间继续进行位置和姿态跟踪，具有短时高精度的优点；但其测量误差会随时间的不断增加而积累，再加上自身的漂移特性，不能满足长时间的精度要求。2、视觉测量算法，其采用布置在头盔上的红外灯组与布置在运动载具座舱内的红外相机进行配合，完成头盔位姿计算。通过精确标定后的高速相机与灯组配合，在相机获取实时图像后经过载具计算机的图像处理，提取出灯特征点信息，经过特征点匹配后通过计算机视觉的pnp方法解算出头盔相对座舱的位姿信息，具有误差无累积性的优点。但是这种视觉模式建立于准确的特征点匹配基础上，仅适用于慢速运动的运动载具，在特征点位置在图像中变化较大就难以完成特征点匹配而跟踪失败。
4.因此，如何将视觉测量算法的误差无累积与惯性测量算法的短时高精度相结合应用于运动载具中头盔的位姿跟踪，以解决因运动载具具有多变速度、颠簸和振动等因素导致头盔的头部位姿跟踪的精度低、实时性差的技术问题，是本领域技术人员亟待解决的课题。


技术实现要素：

5.为了解决上述技术问题，本发明提供了一种视觉及惯性混合位姿跟踪方法、系统、头盔及存储介质，提高头盔的头部位姿跟踪在运动载具具有多变速度、颠簸和振动等因素下的精度及实时性。
6.第一方面，本技术提供一种视觉及惯性混合位姿跟踪方法，包括：采用惯性跟踪模式的惯性捷联位姿解算采集的imu数据得到头部位姿的惯性跟踪数据；其中，所述imu数据包括来自运动载具及头盔上的imu数据；
采用图像跟踪模式的图像位姿解算采集的图像数据得到头部位姿的图像跟踪数据；其中，所述图像数据为头盔上led标识点阵列的数据；通过互补式融合模式的数据融合算法将所述惯性跟踪数据及所述图像跟踪数据进行数据融合，得到头部位姿的组合跟踪数据；其中，所述数据融合算法为改进自适应卡尔曼滤波；创建基于头部位姿跟踪的预制判定策略；根据所述预制判定策略，从所述惯性跟踪模式、所述图像跟踪模式、及所述互补式融合模式中确定预制组合参数所对应的最优跟踪模式；其中，所述预制组合参数包括精度、速度和稳定性；将所述最优跟踪模式获取的跟踪数据通过与其对应的滤波算法处理，输出当前头部位姿的跟踪信息。
7.较佳地，所述采用惯性跟踪模式的惯性捷联位姿解算采集的imu数据得到头部位姿的惯性跟踪数据的步骤具体包括：选取运动载具作为惯性跟踪测量的基准；基于所述基准，通过安装在所述运动载具及所述头盔上的imu分别获取所述运动载具及所述头盔相对于惯性系的惯性测量数据；其中，所述惯性测量数据包括角速度和加速度；通过惯性捷联位姿解算所述惯性测量数据求解头部位姿的惯性跟踪数据。
8.较佳地，所述采用图像跟踪模式的图像位姿解算采集的图像数据得到头部位姿的图像跟踪数据的步骤具体包括：基于图像传感器获取头盔上led标识点的图像测量数据；根据所述图像测量数据提取所述led标识点的质心；利用标定的所述图像传感器的内参数对所述质心进行畸变校正，得到无畸变质心坐标；采用图像位姿解算所述无畸变质心坐标、标定的所述led标识点的内参数及外参数和标定的所述图像传感器的外参数，得到头部位姿的图像跟踪数据。
9.较佳地，所述通过互补式融合模式的数据融合算法将所述惯性跟踪数据及所述图像跟踪数据进行数据融合，得到头部位姿的组合跟踪数据的步骤具体包括：基于运动载具及头盔相对位姿关系建立基于相对位姿误差状态的运动学方程，并推导出卡尔曼滤波的滤波状态方程；利用图像位姿解算的相对位姿结果作为观测信息建立线性观测方程；根据所述线性观测方程及所述滤波状态方程计算滤波相对误差值，并根据所述滤波相对误差值选取适配的滤波算法作为数据融合算法；将所述惯性跟踪数据及所述图像跟踪数据通过所述数据融合算法进行数据融合，得到头部位姿的组合跟踪数据。
10.较佳地，所述滤波算法为sage-husa自适应卡尔曼滤波算法、扩展卡尔曼滤波、强跟踪卡尔曼滤波算法中的一种。
11.较佳地，所述预制判定策略具体包括：当所述惯性捷联位姿解算有效，而所述图像位姿解算无效且时间为第一时长时，
针对所述数据融合算法进行时间更新，将所述组合跟踪数据通过时间更新后的所述数据融合算法处理，输出当前头部位姿的跟踪信息；当所述惯性捷联位姿解算有效，而所述图像位姿解算无效且时间为第二时长时，其中所述第二时长大于所述第一时长，将所述组合跟踪数据进行惯性修正滤波算法处理，输出当前头部位姿的跟踪信息；当所述图像位姿解算有效，而所述惯性捷联位姿解算无效且时间为第一时长时，针对所述数据融合算法进行观测更新，将所述组合跟踪数据通过观测更新后的所述数据融合算法处理，输出当前头部位姿的跟踪信息；当所述图像位姿解算有效，而所述惯性捷联位姿解算无效且时间为第二时长时，将所述组合跟踪数据进行图像跟踪滤波算法处理，输出当前头部位姿的跟踪信息。
12.较佳地，所述将所述组合跟踪数据进行惯性修正滤波算法处理的步骤具体包括：以所述惯性捷联位姿解算为状态方程，利用倾角仪原理获取三轴加速度以解算出重力矢量；基于倾角仪计算合理性判定，自适应输出所述重力矢量的观测误差协方差；根据所述观测误差协方差以使组合跟踪数据在卡尔曼滤波框架下进行预测校正。
13.第二方面，本技术还提供了一种视觉及惯性混合位姿跟踪系统，包括：惯性解算模块，用于采用惯性跟踪模式的惯性捷联位姿解算采集的imu数据得到头部位姿的惯性跟踪数据；其中，所述imu数据包括来自运动载具及头盔上的imu数据；图像解算模块，用于采用图像跟踪模式的图像位姿解算采集的图像数据得到头部位姿的图像跟踪数据；其中，所述图像数据为头盔上led标识点阵列的数据；数据融合模块，用于通过互补式融合模式的数据融合算法将所述惯性跟踪数据及所述图像跟踪数据进行数据融合，得到头部位姿的组合跟踪数据；其中，所述数据融合算法为改进自适应卡尔曼滤波；策略创建模块，用于创建基于头部位姿跟踪的预制判定策略；择优确定模块，用于根据所述预制判定策略，从所述惯性跟踪模式、所述图像跟踪模式、及所述互补式融合模式中确定预制组合参数所对应的最优跟踪模式；其中，所述预制组合参数包括精度、速度和稳定性；滤波处理模块，用于将所述最优跟踪模式获取的跟踪数据通过与其对应的滤波算法处理，输出当前头部位姿的跟踪信息。
14.较佳地，所述惯性解算模块具体包括：基准单元，用于选取运动载具作为惯性跟踪测量的基准；第一获取单元，用于基于所述基准，通过安装在所述运动载具及所述头盔上的imu分别获取所述运动载具及所述头盔相对于惯性系的惯性测量数据；其中，所述惯性测量数据包括角速度和加速度；第一解算单元，用于通过惯性捷联位姿解算所述惯性测量数据求解头部位姿的惯性跟踪数据。
15.较佳地，所述图像解算模块具体包括：第二获取单元，用于基于图像传感器获取头盔上led标识点的图像测量数据；提取单元，用于根据所述图像测量数据提取所述led标识点的质心；
畸变校正单元，用于利用标定的所述图像传感器的内参数对所述质心进行畸变校正，得到无畸变质心坐标；第二解算单元，采用图像位姿解算所述无畸变质心坐标、标定的所述led标识点的内参数及外参数和标定的所述图像传感器的外参数，得到头部位姿的图像跟踪数据。
16.较佳地，所述数据融合模块具体包括：推导单元，用于基于运动载具及头盔相对位姿关系建立基于相对位姿误差状态的运动学方程，并推导出卡尔曼滤波的滤波状态方程；建立单元，用于利用图像位姿解算的相对位姿结果作为观测信息建立线性观测方程；选取单元，用于根据所述线性观测方程及所述滤波状态方程计算滤波相对误差值，并根据所述滤波相对误差值选取适配的滤波算法作为数据融合算法；数据融合单元，用于将所述惯性跟踪数据及所述图像跟踪数据通过所述数据融合算法进行数据融合，得到头部位姿的组合跟踪数据。
17.第三方面，本技术实施例提供了一种头盔，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如第一方面所述的视觉及惯性混合位姿跟踪方法。
18.第四方面，本技术实施例提供了一种存储介质，其上存储有计算机程序，该程序被处理器执行时实现如第一方面所述的视觉及惯性混合位姿跟踪方法。
19.相比现有技术，本发明的有益效果为：基于人在稳定凝视观察和快速搜索目标时，其头部具有不同的运动状态，而在整个使用过程中，头部运动呈现较强的随机性，可能时而稳定时而大角速度运动，本技术通过自适应滤波估计以适应图像跟踪观测值的精度变化，根据观测值噪声的大小自适应设置观测噪声方差的大小，从而调整对惯性和图像跟踪结果的利用权重。诸如当图像跟踪的观测噪声变得较大时，则观测噪声方差阵随之增大，此时滤波观测更新过程立即增大惯性跟踪的位姿信息利用权重，而利用图像观测修正的权重变小。当图像观测噪声变小时，观测噪声方差阵随之减小，此时观测更新过程立即降低惯性跟踪信息的利用权重，而利用图像观测修正的权重变大，以此达到自适应调节惯性和图像跟踪信息利用权重、达到图像和惯性数据最优化融合的组合滤波效果，消除解算误差，提高跟踪精度，减小解算延时。
附图说明
20.为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
21.图1为本发明实施例1提供的视觉及惯性混合位姿跟踪方法的流程图；图2是本发明实施例2提供的与实施例1方法对应的视觉及惯性混合位姿跟踪系统结构框图；图3是本发明实施例3提供的头盔的硬件结构示意图。
22.附图标记说明：
10-惯性解算模块、11-基准单元、12-第一获取单元、13-第一解算单元；20-图像解算模块、21-第二获取单元、22-提取单元、23-畸变校正单元、24-第二解算单元；30-数据融合模块、31-推导单元、32-建立单元、33-选取单元、34-数据融合单元；40-策略创建模块；50-择优确定模块；60-滤波处理模块；70-总线、71-处理器、72-存储器、73-通信接口。
具体实施方式
23.为实现智能头盔头部姿态和位置的准确测量跟踪，针对位姿跟踪系统中存在车体运动惯性测量单元、头部惯性测量单元和图像测量传感器等多传感器测量数据的情况，根据头部相对车体姿态是否存在图像和惯性冗余测量的条件，设计了有冗余测量时的高精度位姿跟踪与无冗余测量情况下位姿跟踪方案。为获得头部相对车体的位姿跟踪信息，冗余测量是指头部惯性测量+车体惯性测量与图像相对姿态测量之间的冗余，也即是能够利用头部、车体在惯性空间中的不同位姿间接获得两者之间的相对位姿，图像测量则是对两者之间相对位姿的直接测量。在这种情况下，本技术能够有效进行图像和惯性两种不同传感器的测量数据融合方法，将图像测量误差无累积性与惯性测量短时高精度相结合，获得优于二者任意单一测量精度的融合测量结果。
24.实施例1具体而言，图1所示为本实施例所提供的一种视觉及惯性混合位姿跟踪方法的流程示意图。
25.如图1所示，本实施例的视觉及惯性混合位姿跟踪方法包括以下步骤：s101，采用惯性跟踪模式的惯性捷联位姿解算采集的imu数据得到头部位姿的惯性跟踪数据。
26.其中，所述imu数据包括来自运动载具及头盔上的imu数据。
27.具体地，本实施例中在头部跟踪测量中选取运动载具-车体作为跟踪测量的基准，其相对于地球坐标系也存在相对运动，无法使用单个惯性传感器完成头部相对于车体的位姿测量，需要两个或者更多惯性传感器进行差分解算。在车体和头盔上分别放置imu，分别测量各自相对于地球惯性坐标系的位姿变化，组合形成惯性跟踪测量系统，完成头部和车体之间的相对位姿跟踪测量。
28.进一步地，本实施例的步骤s101具体包括：s1011，选取运动载具作为惯性跟踪测量的基准。
29.具体地，为便于描述和计算，首先定义一系列笛卡尔参考坐标系，每一个坐标系都是正交的右手系。其中，惯性坐标系（i系）是以原点位于地球中心，坐标轴相对于恒星无转动，轴向定义为oxi，oyi，ozi。其中ozi轴的方向与地球极轴的方向一致（假定极轴方向保持不变）。地球坐标系（e系）是以原点位于地球中心，坐标轴与地球固连，轴向定义为oxe，oye，oze。其中oze沿地球极轴方向，oxe轴沿格林尼治子午面和地球赤道的交线。地球坐标系相对于惯性坐标系绕ozi轴以角速度ωe转动。地理坐标系（g系）取当地地理坐标系，原点位于
车体重心，坐标系三个轴指向为：xg轴指向东，yg轴指向北，zg轴指向当地垂线方向。导航坐标系（n系）取当地地理坐标系，原点位于车体重心，当与地理坐标系重合时，xn坐标轴指向东，yn坐标轴指向北，zn坐标轴指向当地垂线方向（向上）。车体坐标系（b系）取原点位于车体重心，xb轴指向车体前方，yb轴沿车体指向左，zb轴为按右手定则沿车体的方位轴指向上方。头部坐标系（h系）取人体的两眼连线中心为坐标原点，以视线正前方方向为xh轴，以人体正常坐姿的从右到左的方向为yh轴，zh轴按右手定则沿头部方位轴指向正上方。本实施例中，根据根据imu跟踪测量的基本原理，需要分析由两个imu组成的惯性差分跟踪测量系统的系统方程和测量方程，因此选取运动载具（车体坐标系）作为惯性跟踪测量的基准。
30.s1012，基于所述基准，通过安装在所述运动载具及所述头盔上的imu分别获取所述运动载具及所述头盔相对于惯性系的惯性测量数据；其中，所述惯性测量数据包括角速度和加速度。
31.具体地，为了便于精确描述车体、头部的位姿信息，引入了相关的导航参数。诸如：1、位置、速度参数；由于头部始终位于车体中，故忽略车体与头部的杆臂效应，认为车体与头部位置相同，用经度λ、纬度l、高度h、东向速度ve、北向速度vn、天向速度v
μ
、描述。2、姿态参数；引入方位角、俯仰角和横滚角来描述头部与车体的姿态信息。3、地球几何常量；使用惯性装置进行姿态测量，需要对地球的形状作一些假设，通常把地球视为一个参考椭球，本算法使用wgs-84坐标系所选定的参考椭球。所涉及到的地球几何常量如下表。
32.s1013，通过惯性捷联位姿解算所述惯性测量数据求解头部位姿的惯性跟踪数据。
33.具体地，姿态有多种参数化表示方法，如方向余弦阵、欧拉轴角、欧拉角和单位四元数等。本实施例采用单位四元数的表示方法，和方向余弦阵或欧拉角相比，四元数有几个显著优点：用四元数计算时不用三角函数，计算速度快；四元数不存在奇点；如果用方向余弦阵计算两次转动，需要81次乘法和加法，而用四元数计算，只需要16次乘法和加法，所以它的计算量最小。本实施例采用四元数q=[q1，q2，q3，q4]
t
来表示追踪器相对于目标器的姿态，四元数可用欧拉轴角表示为qv=[q1，q2，q3]
t
=esin(α/2)，q4=cos(α/2)，其中e为旋转轴的方向向量，α为旋转角。由于四元数q利用四维向量表示三个自由度，因此存在一个冗余参数，必须满足一个归一化约束如下：四元数q对应的方向余弦阵a(q)可将目标坐标系中的向量变换到追踪坐标系中，可表示如下：，其中
，式中，i3×3表示3阶单位矩阵，[q
13
]表示向量q
13
的叉乘矩阵。
[0034]
s102，采用图像跟踪模式的图像位姿解算采集的图像数据得到头部位姿的图像跟踪数据。
[0035]
具体地，所述图像数据为头盔上led标识点阵列的数据。本实施例中从图像传感器成像理论出发，构建跟踪测量特征点的成像模型，通过图像位姿解算算法完成头部位姿跟踪测量。现有的图像跟踪测量包括图像传感器标定、跟踪特征点空间坐标测量、图像位姿解算算法、图像跟踪滤波与系统全局校准等方法和技术。
[0036]
进一步地，本实施例的步骤s102具体包括：s1021，基于图像传感器获取头盔上led标识点的图像测量数据。
[0037]
s1022，根据所述图像测量数据提取所述led标识点的质心。
[0038]
s1023，利用标定的所述图像传感器的内参数对所述质心进行畸变校正，得到无畸变质心坐标。
[0039]
s1024，采用图像位姿解算所述无畸变质心坐标、标定的所述led标识点的内参数及外参数和标定的所述图像传感器的外参数，得到头部位姿的图像跟踪数据。
[0040]
s103，通过互补式融合模式的数据融合算法将所述惯性跟踪数据及所述图像跟踪数据进行数据融合，得到头部位姿的组合跟踪数据。
[0041]
其中，所述数据融合算法为改进自适应卡尔曼滤波，其中，改进自适应卡尔曼滤波为采用互补式融合模式的数据融合，具体采用航位推测法的方式。
[0042]
具体地，从多传感器数据融合的角度，可将组合跟踪方法分为互补式、竞争式和协作式传感器融合三类；其中，互补式传感器融合是组合或者融合相互独立的不同传感器的不完整测量数据，以获得一个更完整的系统状态估计。对不同传感器不完整测量数据的不同融合处理方式导致四类不同的互补式融合，包括自由度组合/切换、单次单限制、间接滤波和航位推测法。本实施例采用航位推测法，该方法所使用的滤波策略不直接作用于跟踪器测量误差，其关键之处是在滤波器融合结构中保留状态过程模型的输入信号，且以惯性实际测量作为此输入信号，利用滤波器内部的预测校正机制实现视觉测量对惯性测量的漂浮补偿。
[0043]
进一步地，本实施例的步骤s103具体包括：s1031，基于运动载具及头盔相对位姿关系建立基于相对位姿误差状态的运动学方程，并推导出卡尔曼滤波的滤波状态方程。
[0044]
s1032，利用图像位姿解算的相对位姿结果作为观测信息建立线性观测方程。
[0045]
s1033，根据所述线性观测方程及所述滤波状态方程计算滤波相对误差值，并根据所述滤波相对误差值选取适配的滤波算法作为数据融合算法。
[0046]
具体地，滤波算法为sage-husa自适应卡尔曼滤波算法、扩展卡尔曼滤波、强跟踪卡尔曼滤波算法中的一种。本实施例中，滤波相对误差值具体如下：
，式中，γ表示滤波相对误差值，表示新息序列的平方和，表示新息序列的方差矩阵；其中，新息序列具体为，z
k+1
表示k+1时刻的观测值，h
k+1
表示k+1时刻的观测矩阵，表示k时刻与k+1时刻之间时段的状态估计值。
[0047]
该滤波相对误差值的具体计算过程为：当滤波器发散时，滤波算法的误差协方差阵无界，这时实际的估计误差往往比理论预计的误差大很多倍。根据这一特点，利用新息序列的性质来构造滤波算法的收敛性判据。由于为新息序列的平方和，包含了实际估计误差的信息，而理论预测误差的信息可通过新息序列的方差来描述，具体为：，因此，可定义上述的滤波相对误差。
[0048]
进一步地，然后根据该误差值的取值范围选择合适的滤波算法，即有如下规则：时，采用sage-husa自适应卡尔曼滤波算法；，采用扩展卡尔曼滤波；，采用强跟踪卡尔曼滤波算法。其中，γ
min
表示滤波相对误差值的最小值，γ
max
表示滤波相对误差值的最大值。
[0049]
s1034，将所述惯性跟踪数据及所述图像跟踪数据通过所述数据融合算法进行数据融合，得到头部位姿的组合跟踪数据。
[0050]
其中，本技术所涉及的数据融合算法包括sage-husa自适应卡尔曼滤波算法、强跟踪卡尔曼滤波算法、扩展卡尔曼滤波。具体地，sage-husa自适应卡尔曼滤波算法涉及到时间更新算法、观测更新算法、时变噪声估计算法。强跟踪卡尔曼滤波算法涉及到时间更新算法、观测更新算法。扩展卡尔曼滤波涉及到时间更新算法、观测更新算法。诸如：卡尔曼滤波主要是采用预测和更新两个过程：预测过程主要是根据机器人运动模型，对机器人的下一个位置进行预测。系统误差是在更新的（例如传感器测量误差非常大，这样导致卡尔曼增益k值变小），这样测量值对于预测值的修正作用就会降低，因为测量值没有对预测值起到足够的修正作用，这也就导致系统误差在累计增加。同样，如果传感器足够精确，对预测量起到有效的修正，这是的系统误差就会降低。
[0051]
进一步地，拓展卡尔曼滤波主要是为了解决预测方程和测量方程可能不是线性方程，如果是非线性方程，只能使用泰勒公式进行求导；多个传感器的融合过程其实是每个测量值与系统模型预测值进行融合，融合结果形成新的系统模型预测值，所有导航子系统各进行一次拓展卡尔曼滤波过程，则整个组合导航系统完成一次组合导航系统，系统模型的最新预测值即为整个组合导航系统的输出的最优估计结果。
[0052]
s104，创建基于头部位姿跟踪的预制判定策略。
[0053]
具体地，所述预制判定策略具体包括：当所述惯性捷联位姿解算有效，而所述图像位姿解算无效且时间为第一时长时，针对所述数据融合算法进行时间更新，将所述组合跟踪数据通过时间更新后的所述数据融
合算法处理，输出当前头部位姿的跟踪信息；当所述惯性捷联位姿解算有效，而所述图像位姿解算无效且时间为第二时长时，其中所述第二时长大于所述第一时长，将所述组合跟踪数据进行惯性修正滤波算法处理，输出当前头部位姿的跟踪信息；当所述图像位姿解算有效，而所述惯性捷联位姿解算无效且时间为第一时长时，针对所述数据融合算法进行观测更新，将所述组合跟踪数据通过观测更新后的所述数据融合算法处理，输出当前头部位姿的跟踪信息；当所述图像位姿解算有效，而所述惯性捷联位姿解算无效且时间为第二时长时，将所述组合跟踪数据进行图像跟踪滤波算法处理，输出当前头部位姿的跟踪信息。
[0054]
进一步地，所述将所述组合跟踪数据进行惯性修正滤波算法处理的步骤具体包括：以所述惯性捷联位姿解算为状态方程，利用倾角仪原理获取三轴加速度以解算出重力矢量；基于倾角仪计算合理性判定，自适应输出所述重力矢量的观测误差协方差；根据所述观测误差协方差以使组合跟踪数据在卡尔曼滤波框架下进行预测校正。
[0055]
s105，根据所述预制判定策略，从所述惯性跟踪模式、所述图像跟踪模式、及所述互补式融合模式中确定预制组合参数所对应的最优跟踪模式。
[0056]
具体地，所述预制组合参数包括精度、速度和稳定性。本实施例中可以适应图像跟踪观测值的精度变化，根据观测值噪声的大小，自适应设置观测噪声方差的大小，从而调整对惯性和图像跟踪结果的利用权重。具体实践中，当图像跟踪的观测噪声变得较大时，则观测噪声方差阵随之增大，此时滤波观测更新过程立即增大惯性跟踪的位姿信息利用权重，而利用图像观测修正的权重变小。当图像观测噪声变小时，观测噪声方差阵随之减小，此时观测更新过程立即降低惯性跟踪信息的利用权重，而利用图像观测修正的权重变大，以此达到自适应调节惯性和图像跟踪信息利用权重。
[0057]
s106，将所述最优跟踪模式获取的跟踪数据通过与其对应的滤波算法处理，输出当前头部位姿的跟踪信息。
[0058]
具体地，根据图像传感器和惯性传感器的可用性，确定图像跟踪状态、惯性跟踪状态及组合跟踪状态，选择头部位姿的最优估计方案，以实时输出最高精度的头部位姿信息，避免因图像或惯性传感器故障导致无法准确快速连续进行头部跟踪。
[0059]
综上所述，本实施例利用惯性捷联位姿解算和图像位姿解算的位姿测量数据，通过组合滤波算法实现图像传感器和惯性传感器的数据融合，获得头部位姿的图像和惯性组合跟踪信息；然后通过头部位姿跟踪方案判定模块，从图像跟踪、惯性跟踪与组合跟踪三种跟踪方案中选择和确定精度、速度和稳定性最优的跟踪方案，最后将该最优方案的跟踪测量结果作为头部位姿跟踪的最终输出。
[0060]
实施例2本实施例提供了与实施例1所述方法相对应的系统的结构框图。图2是根据本实施例的视觉及惯性混合位姿跟踪系统的结构框图，如图2所示，该系统包括：惯性解算模块10，用于采用惯性跟踪模式的惯性捷联位姿解算采集的imu数据得到头部位姿的惯性跟踪数据；其中，所述imu数据包括来自运动载具及头盔上的imu数据。
[0061]
图像解算模块20，用于采用图像跟踪模式的图像位姿解算采集的图像数据得到头部位姿的图像跟踪数据；其中，所述图像数据为头盔上led标识点阵列的数据。
[0062]
数据融合模块30，用于通过互补式融合模式的数据融合算法将所述惯性跟踪数据及所述图像跟踪数据进行数据融合，得到头部位姿的组合跟踪数据；其中，所述数据融合算法为改进自适应卡尔曼滤波。
[0063]
策略创建模块40，用于创建基于头部位姿跟踪的预制判定策略；其中，所述预制判定策略具体包括：当所述惯性捷联位姿解算有效，而所述图像位姿解算无效且时间为第一时长时，针对所述数据融合算法进行时间更新，将所述组合跟踪数据通过时间更新后的所述数据融合算法处理，输出当前头部位姿的跟踪信息。
[0064]
当所述惯性捷联位姿解算有效，而所述图像位姿解算无效且时间为第二时长时，其中所述第二时长大于所述第一时长，将所述组合跟踪数据进行惯性修正滤波算法处理，输出当前头部位姿的跟踪信息；其中，所述将所述组合跟踪数据进行惯性修正滤波算法处理的步骤具体包括：以所述惯性捷联位姿解算为状态方程，利用倾角仪原理获取三轴加速度以解算出重力矢量；基于倾角仪计算合理性判定，自适应输出所述重力矢量的观测误差协方差；根据所述观测误差协方差以使组合跟踪数据在卡尔曼滤波框架下进行预测校正。
[0065]
当所述图像位姿解算有效，而所述惯性捷联位姿解算无效且时间为第一时长时，针对所述数据融合算法进行观测更新，将所述组合跟踪数据通过观测更新后的所述数据融合算法处理，输出当前头部位姿的跟踪信息。
[0066]
当所述图像位姿解算有效，而所述惯性捷联位姿解算无效且时间为第二时长时，将所述组合跟踪数据进行图像跟踪滤波算法处理，输出当前头部位姿的跟踪信息。
[0067]
择优确定模块50，用于根据所述预制判定策略，从所述惯性跟踪模式、所述图像跟踪模式、及所述互补式融合模式中确定预制组合参数所对应的最优跟踪模式；其中，所述预制组合参数包括精度、速度和稳定性；滤波处理模块60，用于将所述最优跟踪模式获取的跟踪数据通过与其对应的滤波算法处理，输出当前头部位姿的跟踪信息。
[0068]
进一步地，所述惯性解算模块10具体包括：基准单元11，用于选取运动载具作为惯性跟踪测量的基准；第一获取单元12，用于基于所述基准，通过安装在所述运动载具及所述头盔上的imu分别获取所述运动载具及所述头盔相对于惯性系的惯性测量数据；其中，所述惯性测量数据包括角速度和加速度；第一解算单元13，用于通过惯性捷联位姿解算所述惯性测量数据求解头部位姿的惯性跟踪数据。
[0069]
进一步地，所述图像解算模块20具体包括：第二获取单元21，用于基于图像传感器获取头盔上led标识点的图像测量数据；提取单元22，用于根据所述图像测量数据提取所述led标识点的质心；畸变校正单元23，用于利用标定的所述图像传感器的内参数对所述质心进行畸变
access memory，简称为edodram）、同步动态随机存取内存（synchronous dynamic random-access memory，简称sdram）等。
[0076]
存储器72可以用来存储或者缓存需要处理和/或通信使用的各种数据文件，以及处理器71所执行的可能的计算机程序指令。
[0077]
处理器71通过读取并执行存储器72中存储的计算机程序指令，以实现上述实施例1的视觉及惯性混合位姿跟踪方法。
[0078]
在其中一些实施例中，头盔还可包括通信接口73和总线70。其中，如图3所示，处理器71、存储器72、通信接口73通过总线70连接并完成相互间的通信。
[0079]
通信接口73用于实现本技术中各模块、装置、单元和/或设备之间的通信。通信接口73还可以实现与其他部件例如：外接设备、图像/数据采集设备、数据库、外部存储以及图像/数据处理工作站等之间进行数据通信。
[0080]
总线70包括硬件、软件或两者，将设备的部件彼此耦接在一起。总线70包括但不限于以下至少之一：数据总线（data bus）、地址总线（address bus）、控制总线（control bus）、扩展总线（expansion bus）、局部总线（local bus）。举例来说而非限制，总线70可包括图形加速接口（accelerated graphics port，简称为agp）或其他图形总线、增强工业标准架构（extended industry standard architecture，简称为eisa）总线、前端总线（front side bus，简称为fsb）、超传输（hyper transport，简称为ht）互连、工业标准架构（industry standard architecture，简称为isa）总线、无线带宽（infiniband）互连、低引脚数（low pin count，简称为lpc）总线、存储器总线、微信道架构（micro channel architecture，简称为mca）总线、外围组件互连（peripheral component interconnect，简称为pci）总线、pci-express（pci-x）总线、串行高级技术附件（serial advanced technology attachment，简称为sata）总线、视频电子标准协会局部（video electronics standards association local bus，简称为vlb）总线或其他合适的总线或者两个或更多个以上这些的组合。在合适的情况下，总线70可包括一个或多个总线。尽管本技术描述和示出了特定的总线，但本技术考虑任何合适的总线或互连。
[0081]
该头盔可以获取到视觉及惯性混合位姿跟踪系统，执行本实施例1的视觉及惯性混合位姿跟踪方法。
[0082]
另外，结合上述实施例1的视觉及惯性混合位姿跟踪方法，本技术可提供一种存储介质来实现。该存储介质上存储有计算机程序指令；该计算机程序指令被处理器执行时实现上述实施例1的视觉及惯性混合位姿跟踪方法。
[0083]
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
[0084]
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种视觉及惯性混合位姿跟踪方法，其特征在于，包括：采用惯性跟踪模式的惯性捷联位姿解算采集的imu数据得到头部位姿的惯性跟踪数据；其中，所述imu数据包括来自运动载具及头盔上的imu数据；采用图像跟踪模式的图像位姿解算采集的图像数据得到头部位姿的图像跟踪数据；其中，所述图像数据为头盔上led标识点阵列的数据；通过互补式融合模式的数据融合算法将所述惯性跟踪数据及所述图像跟踪数据进行数据融合，得到头部位姿的组合跟踪数据；其中，所述数据融合算法为改进自适应卡尔曼滤波；创建基于头部位姿跟踪的预制判定策略；根据所述预制判定策略，从所述惯性跟踪模式、所述图像跟踪模式、及所述互补式融合模式中确定预制组合参数所对应的最优跟踪模式；其中，所述预制组合参数包括精度、速度和稳定性；将所述最优跟踪模式获取的跟踪数据通过与其对应的滤波算法处理，输出当前头部位姿的跟踪信息。2.根据权利要求1所述的视觉及惯性混合位姿跟踪方法，其特征在于，所述采用惯性跟踪模式的惯性捷联位姿解算采集的imu数据得到头部位姿的惯性跟踪数据的步骤具体包括：选取运动载具作为惯性跟踪测量的基准；基于所述基准，通过安装在所述运动载具及所述头盔上的imu分别获取所述运动载具及所述头盔相对于惯性系的惯性测量数据；其中，所述惯性测量数据包括角速度和加速度；通过惯性捷联位姿解算所述惯性测量数据求解头部位姿的惯性跟踪数据。3.根据权利要求1所述的视觉及惯性混合位姿跟踪方法，其特征在于，所述采用图像跟踪模式的图像位姿解算采集的图像数据得到头部位姿的图像跟踪数据的步骤具体包括：基于图像传感器获取头盔上led标识点的图像测量数据；根据所述图像测量数据提取所述led标识点的质心；利用标定的所述图像传感器的内参数对所述质心进行畸变校正，得到无畸变质心坐标；采用图像位姿解算所述无畸变质心坐标、标定的所述led标识点的内参数及外参数和标定的所述图像传感器的外参数，得到头部位姿的图像跟踪数据。4.根据权利要求1所述的视觉及惯性混合位姿跟踪方法，其特征在于，所述通过互补式融合模式的数据融合算法将所述惯性跟踪数据及所述图像跟踪数据进行数据融合，得到头部位姿的组合跟踪数据的步骤具体包括：基于运动载具及头盔相对位姿关系建立基于相对位姿误差状态的运动学方程，并推导出卡尔曼滤波的滤波状态方程；利用图像位姿解算的相对位姿结果作为观测信息建立线性观测方程；根据所述线性观测方程及所述滤波状态方程计算滤波相对误差值，并根据所述滤波相对误差值选取适配的滤波算法作为数据融合算法；将所述惯性跟踪数据及所述图像跟踪数据通过所述数据融合算法进行数据融合，得到头部位姿的组合跟踪数据。
5.根据权利要求4所述的视觉及惯性混合位姿跟踪方法，其特征在于，所述滤波算法为sage-husa自适应卡尔曼滤波算法、扩展卡尔曼滤波、强跟踪卡尔曼滤波算法中的一种。6.根据权利要求1所述的视觉及惯性混合位姿跟踪方法，其特征在于，所述预制判定策略具体包括：当所述惯性捷联位姿解算有效，而所述图像位姿解算无效且时间为第一时长时，针对所述数据融合算法进行时间更新，将所述组合跟踪数据通过时间更新后的所述数据融合算法处理，输出当前头部位姿的跟踪信息；当所述惯性捷联位姿解算有效，而所述图像位姿解算无效且时间为第二时长时，其中所述第二时长大于所述第一时长，将所述组合跟踪数据进行惯性修正滤波算法处理，输出当前头部位姿的跟踪信息；当所述图像位姿解算有效，而所述惯性捷联位姿解算无效且时间为第一时长时，针对所述数据融合算法进行观测更新，将所述组合跟踪数据通过观测更新后的所述数据融合算法处理，输出当前头部位姿的跟踪信息；当所述图像位姿解算有效，而所述惯性捷联位姿解算无效且时间为第二时长时，将所述组合跟踪数据进行图像跟踪滤波算法处理，输出当前头部位姿的跟踪信息。7.根据权利要求6所述的视觉及惯性混合位姿跟踪方法，其特征在于，所述将所述组合跟踪数据进行惯性修正滤波算法处理的步骤具体包括：以所述惯性捷联位姿解算为状态方程，利用倾角仪原理获取三轴加速度以解算出重力矢量；基于倾角仪计算合理性判定，自适应输出所述重力矢量的观测误差协方差；根据所述观测误差协方差以使组合跟踪数据在卡尔曼滤波框架下进行预测校正。8.一种视觉及惯性混合位姿跟踪系统，其特征在于，包括：惯性解算模块，用于采用惯性跟踪模式的惯性捷联位姿解算采集的imu数据得到头部位姿的惯性跟踪数据；其中，所述imu数据包括来自运动载具及头盔上的imu数据；图像解算模块，用于采用图像跟踪模式的图像位姿解算采集的图像数据得到头部位姿的图像跟踪数据；其中，所述图像数据为头盔上led标识点阵列的数据；数据融合模块，用于通过互补式融合模式的数据融合算法将所述惯性跟踪数据及所述图像跟踪数据进行数据融合，得到头部位姿的组合跟踪数据；其中，所述数据融合算法为改进自适应卡尔曼滤波；策略创建模块，用于创建基于头部位姿跟踪的预制判定策略；择优确定模块，用于根据所述预制判定策略，从所述惯性跟踪模式、所述图像跟踪模式、及所述互补式融合模式中确定预制组合参数所对应的最优跟踪模式；其中，所述预制组合参数包括精度、速度和稳定性；滤波处理模块，用于将所述最优跟踪模式获取的跟踪数据通过与其对应的滤波算法处理，输出当前头部位姿的跟踪信息。9.一种头盔，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1～7中任一项所述的视觉及惯性混合位姿跟踪方法。10.一种存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现
如权利要求1～7中任一项所述的视觉及惯性混合位姿跟踪方法。

技术总结
本发明提供了一种视觉及惯性混合位姿跟踪方法、系统、头盔及存储介质，属于位姿跟踪技术领域。该方法包括采用惯性捷联位姿解算采集的IMU数据得到惯性跟踪数据；采用图像位姿解算采集的图像数据得到图像跟踪数据；通过数据融合算法将惯性跟踪数据及图像跟踪数据进行数据融合，得到组合跟踪数据；创建基于头部位姿跟踪的预制判定策略；根据预制判定策略，从惯性跟踪模式、图像跟踪模式、及互补式融合模式中确定预制组合参数所对应的最优跟踪模式；将最优跟踪模式获取的跟踪数据通过与其对应的滤波算法处理，输出当前头部位姿的跟踪信息。通过本申请，可以提高头盔的头部位姿跟踪在运动载具具有多变速度、颠簸和振动等因素下的精度及实时性。的精度及实时性。的精度及实时性。


技术研发人员：刘征 黄昱田 林宇球 雷军政 袁聪 高明焱
受保护的技术使用者：江西联创电声有限公司
技术研发日：2023.07.21
技术公布日：2023/8/28