IMU协助视觉特征点捕获的头盔位姿跟踪系统及方法与流程

imu协助视觉特征点捕获的头盔位姿跟踪系统及方法
技术领域
1.本发明涉及位姿跟踪技术领域，尤其涉及一种imu协助视觉特征点捕获的头盔位姿跟踪系统及方法。

背景技术：

2.目前的，运动载具中的头盔相对位置与姿态跟踪应用，通常使用布置在头盔上的红外灯组与布置在运动载具座舱内的红外相机进行配合，完成头盔位置与姿态计算。在该应用中，精确标定后的高速相机与灯组配合，在相机获取实时图像后经过载具计算机的图像处理，提取出灯特征点信息，经过特征点匹配后通过计算机视觉的pnp方法解算出头盔相对座舱的位置与姿态信息。
3.pnp位姿测量方法的基础是准确的特征点匹配，在头盔慢速运动时，特征点匹配过程可以根据上一帧相机图像匹配结果作为初值进行迭代；但在头盔相对相机运动速度过快或灯组切换时，特征点位置在图象中变化较大，无法利用上一帧数据进行迭代判断。如果当上一帧图像迭代尚未进行完，下一组图像数据就已经到达，此时也无法继续完成特征点匹配，造成头盔跟踪失败。低动态工况的准确测量。
4.为解决头盔位姿跟踪的动态性，通常引入imu作为补充手段，设计卡尔曼滤波器，将视觉测量结果作为观测量，用于修正imu的积分漂移。视觉解算与imu解算延迟基本为固定值，在视觉准确时，该滤波器能获得良好的测量结果。但在头盔位置移动范围过大时，存在视觉测量缺失，造成特征匹配运算量过大，跟踪性能变差的问题。

技术实现要素：

5.鉴于上述的分析，本发明旨在提供一种imu协助视觉特征点捕获的头盔位姿跟踪系统及方法，以实现头部位姿跟踪，并提升跟踪的动态性能。
6.本发明提供的技术方案是：
7.本发明公开了一种imu协助视觉特征点捕获的头盔位姿跟踪系统，包括：相机单元、标志点单元、imu单元和跟踪处理单元；
8.标志点单元，包括设置在头盔不同位置上的多组标志点；每组标志点处于点亮或熄灭状态；
9.相机单元，包括设置在载具座舱内不同位置上的多个相机；每个相机拍摄角度朝向座舱内头盔移动区域范围，使至少一个相机对准一组被点亮标志点进行拍摄；
10.imu单元，包括头盔imu和载具imu，分别测量头盔和载具的imu数据；
11.跟踪处理单元，利用相机单元拍摄图像中被点亮标志点特征进行头盔的视觉测姿；并建立视觉与惯性融合的卡尔曼滤波器，利用imu数据对视觉测姿数据进行滤波处理；根据滤波结果预判出被点亮标点在下一帧拍摄图像中的位置，用于下一帧视觉测姿的快速匹配。
12.进一步地，对所述相机单元、标志点单元和imu单元采用同步控制；具体包括：
13.将相机单元中的各相机进行编号，并将其中一个相机作为主控相机，负责产生同步信号cam sync；其余相机在收到同步信号cam sync后同步拍摄，将带有相机编号的每帧图像发送至跟踪处理单元；
14.将标志点单元的多组标志点进行编号；同步信号cam sync对每组标志点进行点亮控制，并将点亮的标志点的编号信息发送至跟踪处理单元；
15.同步信号cam sync同样发送给imu单元，控制头盔imu和座舱imu同步测量。
16.进一步地，所述跟踪处理单元包括imu差分模块、视觉测姿模块、卡尔曼滤波器和位置预判模块；其中，
17.imu差分模块，用于对头盔imu和载具imu的测量数据进行差分计算，获取头盔相对座舱的加速度和角速度信息；
18.视觉测姿模块，用于对于确定出的在相机视野中出现一组完整的被点亮标志点的图像进行特征点匹配，并在特征点匹配后进行pnp解算，获取头盔相对于座舱的视觉测姿数据实时输出到卡尔曼滤波器；所述视觉测姿数据包括位置和姿态数据；
19.卡尔曼滤波器，用于基于头盔相对载具的加速度和角速度信息，建立起卡尔曼滤波的状态向量，构造传播方程，使用视觉测姿数据作为观测量对滤波器进行更新；将滤波后的头盔相对座舱的位置和姿态信息作为输出；
20.位置预判模块，用于对滤波后的头盔相对座舱的位置和姿态信息，结合从imu 差分模块接收的新的头盔相对座舱的加速度和角速度信息进行预积分，推测标志点的下一帧可能位置。
21.进一步地，所述视觉测姿模块采用视觉单目工作方式，在视觉单目工作方式中，
22.当一组完整的被点亮标志点出现在一个相机视野内时，确认标志点编号并进行特征点匹配，在特征点匹配后进行pnp解算，获取标志点的位置与姿态测量，并将视觉测量结果实时输出，用于后续滤波器处理；
23.当被点亮标志点的运动超出拍摄相机视野时，标志点单元按照同步信号camsync轮番点亮各组标志点；各相机持续判断是否有一组完整的标志点出现在一个相机视野内，如果出现则停止标志点切换，并使用拍摄该组标志点的相机的图像进行特征点匹配与pnp解算。
24.进一步地，pnp解算过程中，采用空间正交迭代算法进行视觉测姿。
25.进一步地，所述卡尔曼滤波器，使用头盔imu和载具imu差分获取的相对加速度与相对角速度构建状态向量构造传播方程；使用视觉观测的位置和姿态测量数据作为观测量对滤波器进行更新；将滤波后的头盔相对座舱的位置和姿态信息作为输出；
26.其中，为座舱imu坐标系下头盔imu的位置、速度、旋转四元数； b
bω
、b
ba
为头盔imu测量角速度与加速度的零偏；b
vω
、b
va
为载具imu测量角速度与加速度的零偏；λ为视觉尺度因数。
27.进一步地，跟踪过程中，点亮标志点或拍摄点亮标志点的相机发生切换情况下的快速匹配过程，包括：
28.1)根据同步信号cam sync，获取灯组或相机在切换时刻的卡尔曼滤波器输出头
盔相对座舱的位姿数据；
29.2)根据所述头盔相对座舱的位姿数据和各组标志点在头盔上的空间位置，得到各组标志点在座舱中的空间三维坐标；
30.3)基于各相机的拍摄角度，进行空间三维坐标到二维坐标的投影，计算出每个相机可拍摄到的标志点在相机拍摄画面二维坐标；同时根据同步信号cam sync确定出的正在点亮的标志点编号，得到点亮标志点在各个相机拍摄画面的计算二维坐标；
31.4)获取切换时刻，各相机实际拍摄图像中点亮标志点在各相机拍摄画面的实际二维坐标，并计算出所述实际二维坐标与对应的计算二维坐标之间的中心距离；当某个相机计算的中心距离小于设定阈值时，则将该相机和该点亮标志点进行特征点匹配，并在特征点匹配后进行pnp解算，获取头盔相对于座舱的视觉测姿数据。
32.进一步地，所述每组标志点中包括多个发光特征点；且每组标志点的多个发光特征点以一定的几何构型布置在头盔上。
33.进一步地，每组标志点的组内发光特征点的布置方式为四面体形或金字塔形；其中，
34.四面体形中，三个特征点位于同一特征平面，中心特征点高于该平面；
35.金字塔形中，四个特征点位于同一特征平面，中心特征点高于该平面。
36.本发明还公开了一种采用如上所述的imu协助视觉特征点捕获的头盔位姿跟踪系统的头盔位姿跟踪方法，包括：
37.步骤s1、对系统中相机单元、标志点单元和imu单元采用同步控制；
38.步骤s2、根据imu单元采集载具、头盔的加速度和角速度，进行惯性差分计算得到头盔相对载具的加速度和角速度信息；
39.步骤s3、根据被点亮标志点信息进行特征匹配，并在特征点匹配后进行pnp解算，获取视觉观测的头盔相对座舱的位置与姿态测量；
40.步骤s4、基于头盔相对载具的加速度和角速度信息，建立起卡尔曼滤波的状态向量，构造传播方程，使用视觉测姿数据作为观测量对滤波器进行更新；将滤波后的头盔相对座舱的位置和姿态信息作为输出；
41.步骤s5、对滤波后的头盔相对座舱的位置和姿态信息，结合惯性差分计算出的新的头盔相对座舱的加速度和角速度信息进行预积分，推测标志点的下一帧可能位置。
42.本发明的可以至少实现以下有益效果之一：
43.本发明提出imu协助视觉特征点捕获的头盔位姿跟踪系统及方法，通过红外特征点布局与相机同步触发配合同时满足了大范围与高精度视觉测量。
44.在引入imu辅助特征点匹配方法后，在等待下一帧图像以及灯组切换时候，可以进行特征点位置预判，减小特征点匹配的运算量；
45.该方法相较于传统相对位姿测量方案，更为有效利用设备同步特性，且布置简便，适用于各种乘用头戴显示系统等工程应用。
附图说明
46.附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
47.图1为本发明实施例中的头盔位姿跟踪系统组成示意框图；
48.图2为本发明实施例中的组内发光特征点布置方式为四面体型示意图；
49.图3为本发明实施例中的组内发光特征点布置方式为金字塔形示意图；
50.图4为本发明实施例中的红外led发光灯珠上方覆盖薄柔光片示意图；
51.图5为本发明实施例中的组间发光特征点交错布置示意图；
52.图6为本发明实施例中的同步方式示意图；
53.图7为本发明实施例中的跟踪处理单元组成连接示意图；
54.图8为本发明实施例中的头盔位姿跟踪方法流程图。
具体实施方式
55.下面结合附图来具体描述本发明的优选实施例，其中，附图构成本技术一部分，并与本发明的实施例一起用于阐释本发明的原理。
56.本实施例公开了一种imu协助视觉特征点捕获的头盔位姿跟踪系统，如图1所示，包括：相机单元、标志点单元、imu单元和跟踪处理单元；
57.标志点单元，包括设置在头盔不同位置上的多组标志点；每组标志点被控处于点亮或熄灭状态；
58.具体的，每组标志点在标志点控制器的控制下处于点亮或熄灭状态；对标志点进行编号为led1、led2、led3、
…
、ledm；
59.相机单元，包括设置在载具座舱内不同位置上的多个相机；每个相机拍摄角度朝向座舱内头盔移动区域范围，使至少一个相机对准一组被点亮标志点进行拍摄；
60.对相机进行编号为cam1、cam2、cam3、
…
、camn；并且相机可采用高速相机，以保证图像的采样频率；
61.imu单元，包括头盔imu和载具imu，分别测量头盔和载具的imu数据；
62.跟踪处理单元，利用相机单元拍摄图像中被点亮标志点特征进行头盔的视觉测姿；并建立视觉与惯性融合的卡尔曼滤波器，利用imu数据对视觉测姿数据进行滤波处理；根据滤波结果预判出被点亮标点在下一帧拍摄图像中的位置，用于下一帧视觉测姿的快速匹配。
63.具体的，所述多组标志点设置在头盔的不同位置上，并在标志点控制器的控制下处于点亮或熄灭状态；标志点编号为led1、led2、led3、
…
、ledm；
64.imu单元包括头盔imu和载具imu以及imu控制器；
65.其中，所述载具imu布置在载具座舱中与座舱固定连接，用于测量运动载具的加速度和角速度数据；
66.优选的，所述载具imu布置在座舱上的其中一个相机上；
67.所述头盔imu布置在头盔上与头盔固定连接，用于测量头盔的加速度和角速度数据。
68.具体的，设置在头盔不同位置上的多组标志点中，每组标志点均包括多个发光特征点；且每组标志点的多个发光特征点以一定的几何构型布置在头盔上；多组标志点的几何构型可以相同也可以不同。
69.优选的，所述标志点中的发光特征点采用红外led发光灯珠，对应的相机单元的
相机为红外相机。
70.发光特征点也可采用主动发光或被动发光两种方式发光；其中，主动式为自发光方式，被动式为相机或环部补光方式。
71.如图2所示，在一种典型的几何构型中，组内发光特征点布置方式为四面体型，其中三个特征点位于同一特征平面，中心特征点高于该平面。
72.如图3所示，在另一种典型的几何构型中，组内发光特征点布置方式为金字塔形，其中四个特征点位于同一特征平面，中心特征点高于该平面。
73.并且，在图2和图3中的两种布置方式中，每个发光特征点的法向均垂直与特征平面，便于相机观察。
74.在优选的方案中，如图4所示，发光特征点的红外led发光灯珠上方覆盖薄柔光片，通过薄柔光片控制灯珠发光均匀性及可视范围。
75.由于头盔的表面积相对较小，为在较小的头盔表面布置更多组的标志点，以提高头盔表面的利用率。在如图5所示的优选方案中，多组的红外led发光灯珠在头盔上采用交错布置的布置方式。
76.优选的方案中，如图6所示，为了便于系统的数据传输和工作，对所述相机单元、标志点单元和imu单元采用同步控制；具体包括：
77.1)将相机单元中的各相机进行编号，并将其中一个相机作为主控相机，负责产生同步信号cam sync；其余相机在收到同步信号cam sync后同步拍摄，将带有相机编号的每帧图像发送至跟踪处理单元；
78.具体的实例中，以编号为cam 1的相机为主控相机，负责产生同步信号camsync，其余相机在收到cam sync后同步拍摄，cam 1、cam 2、cam 3
……
cam n 同步拍摄的图像，通过cam data发送至跟踪处理单元。为减轻跟踪处理单元运算量以及传输速度，也可使相机只发送提取后的红外特征点。
79.2)将标志点单元的多组标志点进行编号；同步信号cam sync对每组标志点进行点亮控制，并将点亮的标志点的编号信息发送至跟踪处理单元；
80.具体的实例中，主控相机产生的同步信号cam sync同样发送给标志点控制器 led controller用于控制led点亮，每个相机与可与任意一组led对应，即cam 1 可以与led 1、led 2、led 3、
…
、led n对应，同理cam 2、cam 3、
…
、cam n。标志点控制器led controller在控制灯组点亮后也将点亮的灯组编号信息发送给跟踪处理单元，用于后续计算。
81.2)同步信号cam sync同样发送给imu单元，控制头盔imu和座舱imu同步测量。
82.具体的实施例中，imu单元接收同步信号cam sync后，可控制两个imu采用同步信号cam sync整倍数的频率控制imu进行采样。例如相机采样频率为120hz，那么imu可以采用960hz的采样频率。imu controller在采集完发送给跟踪处理单元。
83.并且，在开始位置与姿态测量前，分别标定相机单元中个相机cam 1、cam 2、 cam 3、
…
、cam n的内参，并在各个相机安装好后标定外参；以及标定标志点单元中各标志点的三维空间位置坐标。
84.具体的，如图7所示，所述跟踪处理单元包括imu差分模块、视觉测姿模块、卡尔曼滤波器和位置预判模块；其中，
85.imu差分模块，用于对头盔imu和载具imu的测量数据进行差分计算，获取头盔相
中旋转矩阵每一行元素的物理含义为头盔坐标系坐标轴单位向量在相机坐标系下的坐标，平移向量的物理含义为头盔坐标系原点在相机坐标系下的坐标。
103.点pi在归一化平面坐标坐标为[ui,vi,1]
t
，单位为像素，归一化平面坐标和相机坐标系存在内参变换关系
[0104][0105]
其中，f
x
、fy、c
x
、cy为相机内参，单位为像素，k为相机内参矩阵。
[0106]
2)采用空间正交迭代算法计进行视觉测姿；
[0107]
定义为视线投影矩阵，当vi作用于某个向量时，可以将该向量垂直投影到pi上。
[0108]
定义点pi在vi上的投影为qi，则有
[0109]
qi＝vi(rpi+t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0110]
理想情况下物点、像点、相机原点三点满足空间共线性方程，即pi在vi上的投影应当为pi自身
[0111]
rpi+t＝vi(rpi+t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0112]
变形可得目标空间共线性误差为
[0113]ei
＝(i-vi)(rpi+t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0114]
以空间共线行误差的平方和作为目标函数，通过优化目标函数得到r、t的最优估计
[0115][0116]
该目标函数在给定旋转r时，可通过偏导数得到t关于r的最优解：
[0117][0118]
因此对于固定的r，可通过上式得到对应的t。接下来寻r的最优解，对于第k 次迭代时的r估计值r
(k)
，可以得到第k次迭代的t
(k)
，计算得到空间点pi的投影估计
[0119][0120]
k+1次旋转矩阵估计值r
(k+1)
可以通过求解如下函数的最小值解算
[0121][0122]
该式可以看作点集{pi}到点集{qi}绝对定向问题，可采用奇异值分解(svd)的方式求解，步骤如下，定义与为点集的质心，有
[0123][0124]
定义(1/n)m为点集{pi}与点集{qi}的协方差矩阵
[0125][0126]
则使e(r,t)最小的r
*
与t
*
满足
[0127][0128]
对m进行svd分解，即u
t
mv＝σ，此时最优解
[0129]r(k+1)
＝vu
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13) 优于算法具有全局收敛性，对于任意初始旋转矩阵r，重复上述步骤，即可收敛至最优值。收敛的最优值即为，通过式(7)可以得到。
[0130][0131]
具体的，卡尔曼滤波器中变量列表如下：
[0132][0133]
卡尔曼滤波器中，
[0134]
使用头盔imu和载具imu差分获取的相对加速度与相对角速度构建状态向量构造传播方程；使用视觉观测的位置和姿态测量数据作为观测量对滤波器进行更新。
[0135]
其中，为座舱imu坐标系下头盔imu的位置、速度、旋转四元数；b
bω
、b
ba
为头盔imu测量角速度与加速度的零偏；b
vω
、b
va
为载具imu测量角速度与加速度的零偏；λ为视觉尺度因数。
[0136]
不含噪声状态
[0137]
用于表示真实状态x与不含噪声状态的状态误差向量表示为：
[0138][0139]
其中；各项的具体展开如下：
[0140][0141][0142]
具到头盔相对旋转矩阵；
[0165][0166][0167]
i为单位矩阵；
[0168]
噪声转移矩阵
[0169]
则，系统的状态协方差矩阵为
[0170]
基于上述过程，所述更新卡尔曼滤波的误差状态协方差矩阵的过程包括：
[0171]
1)获取头盔imu数据；
[0172]
2)根据头盔运动模型更新状态向量
[0173]
3)更新状态转移矩阵f
x
、更新协方差矩阵fnnf
nt
；
[0174]
4)更新误差状态协方差矩阵
[0175]
适用于本方案的位姿测量卡尔曼滤波的滤波器的观测方程为：
[0176][0177]
其中，误差向量h
p
为位置量测矩阵；误差向量 hq为位置量测矩阵；z
p
、zq为卡尔曼滤波的视觉观测的位置向量和姿态向量；为卡尔曼滤波的视觉观测的位置向量和姿态向量；为卡尔曼滤波估计的位置向量和姿态向量。
[0178]
在观测过程中，
[0179]
1)列写更新部分位置测量模型z
p
；
[0180]
其中，位置测量模型
[0181]
式中，代表标志点相对相机的位移，由视觉测量经过内参变化后得到；为载具坐标系到相机坐标系的转换矩阵，可通过标定得到；代表头盔坐标系在载具坐标系的位移；为头盔imu与载具坐标之间平移向量与旋转矩阵，为滤波器中的状态向量；为头盔相对imu的外参，可通过标定得到；n
p
为量测噪声。
[0182]
则误差向量展开有：
[0183][0184]
展开后忽略二阶项，得到：
[0185]
[0186]
根据观测方程δz
p
＝h
p
δx，位置量测矩阵h
p
写作如下：
[0187][0188]
式中，为位置观测量，为对应叉乘运算矩阵。
[0189]
2)列写更新部分姿态测量模型zq；
[0190]
其中，姿态测量模型
[0191]
则误差向量展开有
[0192][0193]
根据观测方程δzq＝hqδx，旋转量测矩阵hq写作如下：
[0194][0195]
本实施例的更新状态协方差矩阵与状态向量的过程包括：
[0196]
1)计算观测残差
[0197]
2)计算更新矩阵s＝hph
t
+r；
[0198]
3)计算卡尔曼增益k＝ph
t
s-1
；
[0199]
4)计算状态修正量
[0200]
5)计算状态协方差矩阵的递推结果p
←
(i
d-kh)p(i
d-kh)
t
+krk
t
。
[0201]
6)将状态更新量与原本的状态向量叠加后得到更新后的状态向量。
[0202]
位置预判模块，用于对滤波后的头盔相对座舱的位置p和姿态q信息，结合从 imu差分模块接收的新的头盔相对座舱的加速度a和角速度ω信息进行预积分，推测标志点的下一帧可能位置。
[0203]
相机单元利用位置预判模块输出的标志点的下一帧可能位置，进行快速匹配，输出视觉测姿结果。
[0204]
更进一步地，跟踪过程中，点亮标志点或拍摄点亮标志点的相机发生切换情况下的快速匹配过程，包括：
[0205]
1)根据同步信号cam sync，获取灯组或相机在切换时刻的卡尔曼滤波器输出头盔相对座舱的位姿数据；
[0206]
2)根据所述头盔相对座舱的位姿数据和各组标志点在头盔上的空间位置，得到各组标志点在座舱中的空间三维坐标；
[0207]
3)基于各相机的拍摄角度，进行空间三维坐标到二维坐标的投影，计算出每个相机可拍摄到的标志点在相机拍摄画面二维坐标；同时根据同步信号cam sync确定出的正在点亮的标志点编号，得到点亮标志点在各个相机拍摄画面的计算二维坐标；
[0208]
4)获取切换时刻，各相机实际拍摄图像中点亮标志点在各相机拍摄画面的实际二维坐标，并计算出所述实际二维坐标与对应的计算二维坐标之间的中心距离；当某个相机计算的中心距离小于设定阈值时，则将该相机和该点亮标志点进行特征点匹配，并在特征点匹配后进行pnp解算，获取头盔相对于座舱的视觉测姿数据；
[0209]
所述设定阈值为σδt，其中，σ为测量允许误差系数，按照经验值设定；δt为同步信号的间隔时间。
[0210]
如果存在多组正确匹配的特征点时，可采用最接近画面中心、特征点外接圆面积最大的作为视觉测量值，继续进行滤波器更新(计算量小)；也可将多组视觉测量值都作为视觉测量值，继续进行滤波器更新(精度高)。
[0211]
综上所述，本发明实施例的的imu协助视觉特征点捕获的头盔位姿跟踪系统，通过红外特征点布局与相机同步触发配合同时满足了大范围与高精度视觉测量。在引入imu辅助特征点匹配方法后，在等待下一帧图像以及灯组切换时候，可以进行特征点位置预判，减小特征点匹配的运算量；该方法相较于传统相对位姿测量方案，更为有效利用设备同步特性，且布置简便，适用于各种乘用头戴显示系统等工程应用。
[0212]
实施例二
[0213]
本实施例公开了一种采用实施例一所述的imu协助视觉特征点捕获的头盔位姿跟踪系统的头盔位姿跟踪方法，如图8所示，包括以下步骤，
[0214]
步骤s1、对系统中相机单元、标志点单元和imu单元采用同步控制；
[0215]
步骤s2、根据imu单元采集载具、头盔的加速度和角速度，进行惯性差分计算得到头盔相对载具的加速度和角速度信息；
[0216]
步骤s3、根据被点亮标志点信息进行特征匹配，并在特征点匹配后进行pnp解算，获取视觉观测的头盔相对座舱的位置与姿态测量；
[0217]
步骤s4、基于头盔相对载具的加速度和角速度信息，建立起卡尔曼滤波的状态向量，构造传播方程，使用视觉测姿数据作为观测量对滤波器进行更新；将滤波后的头盔相对座舱的位置和姿态信息作为输出；
[0218]
步骤s5、对滤波后的头盔相对座舱的位置和姿态信息，结合惯性差分计算出的新的头盔相对座舱的加速度和角速度信息进行预积分，推测标志点的下一帧可能位置。
[0219]
本实施例具体的技术细节和有益效果与上一实施例中所叙述的内容相同，请参照上一实施例，在此就不一一赘述了。
[0220]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

技术特征：

1.一种imu协助视觉特征点捕获的头盔位姿跟踪系统，其特征在于，包括：相机单元、标志点单元、imu单元和跟踪处理单元；标志点单元，包括设置在头盔不同位置上的多组标志点；每组标志点处于点亮或熄灭状态；相机单元，包括设置在载具座舱内不同位置上的多个相机；每个相机拍摄角度朝向座舱内头盔移动区域范围，使至少一个相机对准一组被点亮标志点进行拍摄；imu单元，包括头盔imu和载具imu，分别测量头盔和载具的imu数据；跟踪处理单元，利用相机单元拍摄图像中被点亮标志点特征进行头盔的视觉测姿；并建立视觉与惯性融合的卡尔曼滤波器，利用imu数据对视觉测姿数据进行滤波处理；根据滤波结果预判出被点亮标点在下一帧拍摄图像中的位置，用于下一帧视觉测姿的快速匹配。2.根据权利要求1所述的盔位姿跟踪系统，其特征在于，对所述相机单元、标志点单元和imu单元采用同步控制；具体包括：将相机单元中的各相机进行编号，并将其中一个相机作为主控相机，负责产生同步信号cam sync；其余相机在收到同步信号cam sync后同步拍摄，将带有相机编号的每帧图像发送至跟踪处理单元；将标志点单元的多组标志点进行编号；同步信号cam sync对每组标志点进行点亮控制，并将点亮的标志点的编号信息发送至跟踪处理单元；同步信号cam sync同样发送给imu单元，控制头盔imu和座舱imu同步测量。3.根据权利要求1所述的盔位姿跟踪系统，其特征在于，所述跟踪处理单元包括imu差分模块、视觉测姿模块、卡尔曼滤波器和位置预判模块；其中，imu差分模块，用于对头盔imu和载具imu的测量数据进行差分计算，获取头盔相对座舱的加速度和角速度信息；视觉测姿模块，用于对于确定出的在相机视野中出现一组完整的被点亮标志点的图像进行特征点匹配，并在特征点匹配后进行pnp解算，获取头盔相对于座舱的视觉测姿数据实时输出到卡尔曼滤波器；所述视觉测姿数据包括位置和姿态数据；卡尔曼滤波器，用于基于头盔相对载具的加速度和角速度信息，建立起卡尔曼滤波的状态向量，构造传播方程，使用视觉测姿数据作为观测量对滤波器进行更新；将滤波后的头盔相对座舱的位置和姿态信息作为输出；位置预判模块，用于对滤波后的头盔相对座舱的位置和姿态信息，结合从imu差分模块接收的新的头盔相对座舱的加速度和角速度信息进行预积分，推测标志点的下一帧可能位置。4.根据权利要求3所述的盔位姿跟踪系统，其特征在于，所述视觉测姿模块采用视觉单目工作方式，在视觉单目工作方式中，当一组完整的被点亮标志点出现在一个相机视野内时，确认标志点编号并进行特征点匹配，在特征点匹配后进行pnp解算，获取标志点的位置与姿态测量，并将视觉测量结果实时输出，用于后续滤波器处理；当被点亮标志点的运动超出拍摄相机视野时，标志点单元按照同步信号cam sync轮番点亮各组标志点；各相机持续判断是否有一组完整的标志点出现在一个相机视野内，如果出现则停止标志点切换，并使用拍摄该组标志点的相机的图像进行特征点匹配与pnp解算。
5.根据权利要求4所述的盔位姿跟踪系统，其特征在于，pnp解算过程中，采用空间正交迭代算法进行视觉测姿。6.根据权利要求3所述的盔位姿跟踪系统，其特征在于，所述卡尔曼滤波器，使用头盔imu和载具imu差分获取的相对加速度与相对角速度构建状态向量imu和载具imu差分获取的相对加速度与相对角速度构建状态向量构造传播方程；使用视觉观测的位置和姿态测量数据作为观测量对滤波器进行更新；将滤波后的头盔相对座舱的位置和姿态信息作为输出；其中，为座舱imu坐标系下头盔imu的位置、速度、旋转四元数；b
bω
、b
ba
为头盔imu测量角速度与加速度的零偏；b
vω
、b
va
为载具imu测量角速度与加速度的零偏；λ为视觉尺度因数。7.根据权利要求6所述的盔位姿跟踪系统，其特征在于，跟踪过程中，点亮标志点或拍摄点亮标志点的相机发生切换情况下的快速匹配过程，包括：1)根据同步信号cam sync，获取灯组或相机在切换时刻的卡尔曼滤波器输出头盔相对座舱的位姿数据；2)根据所述头盔相对座舱的位姿数据和各组标志点在头盔上的空间位置，得到各组标志点在座舱中的空间三维坐标；3)基于各相机的拍摄角度，进行空间三维坐标到二维坐标的投影，计算出每个相机可拍摄到的标志点在相机拍摄画面二维坐标；同时根据同步信号cam sync确定出的正在点亮的标志点编号，得到点亮标志点在各个相机拍摄画面的计算二维坐标；4)获取切换时刻，各相机实际拍摄图像中点亮标志点在各相机拍摄画面的实际二维坐标，并计算出所述实际二维坐标与对应的计算二维坐标之间的中心距离；当某个相机计算的中心距离小于设定阈值时，则将该相机和该点亮标志点进行特征点匹配，并在特征点匹配后进行pnp解算，获取头盔相对于座舱的视觉测姿数据。8.根据权利要求1-7任一项所述的盔位姿跟踪系统，其特征在于，所述每组标志点中包括多个发光特征点；且每组标志点的多个发光特征点以一定的几何构型布置在头盔上。9.根据权利要求7所述的盔位姿跟踪系统，其特征在于，每组标志点的组内发光特征点的布置方式为四面体形或金字塔形；其中，四面体形中，三个特征点位于同一特征平面，中心特征点高于该平面；金字塔形中，四个特征点位于同一特征平面，中心特征点高于该平面。10.一种采用如权利要求1-9所述的imu协助视觉特征点捕获的头盔位姿跟踪系统的头盔位姿跟踪方法，其特征在于，包括：步骤s1、对系统中相机单元、标志点单元和imu单元采用同步控制；步骤s2、根据imu单元采集载具、头盔的加速度和角速度，进行惯性差分计算得到头盔相对载具的加速度和角速度信息；步骤s3、根据被点亮标志点信息进行特征匹配，并在特征点匹配后进行pnp解算，获取视觉观测的头盔相对座舱的位置与姿态测量；步骤s4、基于头盔相对载具的加速度和角速度信息，建立起卡尔曼滤波的状态向量，构造传播方程，使用视觉测姿数据作为观测量对滤波器进行更新；将滤波后的头盔相对座舱
的位置和姿态信息作为输出；步骤s5、对滤波后的头盔相对座舱的位置和姿态信息，结合惯性差分计算出的新的头盔相对座舱的加速度和角速度信息进行预积分，推测标志点的下一帧可能位置。

技术总结

本发明涉及一种IMU协助视觉特征点捕获的头盔位姿跟踪系统及方法；系统包括：相机单元、标志点单元、IMU单元和跟踪处理单元；标志点单元包括设置在头盔不同位置上处于点亮或熄灭状态的多组标志点；相机单元包括设置在载具座舱内的多个相机；每个相机拍摄角度朝向座舱内头盔移动区域范围，使至少一个相机对准一组被点亮标志点进行拍摄；IMU单元包括头盔IMU和载具IMU；跟踪处理单元利用相机单元拍摄图像中被点亮标志点特征进行头盔的视觉测姿；并建立视觉与惯性融合的卡尔曼滤波器，利用IMU数据对视觉测姿数据进行滤波处理；根据滤波结果预判出被点亮标点在下一帧拍摄图像中的位置，用于下一帧视觉测姿的快速匹配。本发明满足了大范围与高精度视觉测量。范围与高精度视觉测量。范围与高精度视觉测量。