结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统的制作方法

1.本发明涉及相对位姿测量和卡尔曼滤波技术领域，尤其涉及一种结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统。


背景技术：

2.随着现代装备的智能化升级，在运动的车辆或飞行器中头戴显示功能需获取头盔佩戴者的姿态位姿信息。这类装备需要获取运动载具中操控人员与运动载具实时准确的相对姿态变化，将乘员头部动作与装备操控结合，实现人机一体的自然交互方式，从而实现装备快速协同、信息互联等功能，提升战场效率。相对位姿测量相比于惯性系下绝对姿态测量存在的主要差别为：其一，在载具相对惯性系存在未知运动的条件下，被测目标的单一惯性信息也不可作为直接参考，需要同载具的惯性信息差分处理，通过转换到载具坐标系下进行相对姿态解算；其二，卫星导航与惯性导航这种常用的组合导航手段不再可用，需要使用卫星导航、视觉导航、雷达导航等多种导航方式分别对运动载具上与头盔上两个imu的零偏分别进行修正。
3.在目前常用的相对位姿测量手段中，惯性与视觉组合是一种可靠的相对姿态测量方式。惯性测姿的短时精度较高，长时积分会产生累积误差，而视觉测姿虽然易受外部近似特征干扰及光线变化影响，但特征准确时精度稳定，在测姿精度上与惯性测量形成互补。从测量频率上，惯性信息可实现高达1khz采样的实时测量，图像信息通常为60hz或120hz，且存在稳定解算延迟，惯性与视觉的融合可实现与纯惯性相同的高频率输出。惯性与视觉的组合可充分利用惯性测量采样周期短、短时精度高的特性，并发挥视觉测量可直接获得无累积误差相对姿态的特点。
4.现有的视觉观测与惯性组合的形式，缺少足够的观测信息，且无法同时抑制两个惯性器件漂移，导致导航系统积分发散。


技术实现要素：

5.鉴于上述的分析，本发明旨在提供一种结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统，利用北斗导航和视觉进行辅助，提升头部测姿结果的准确性。
6.本发明提供的技术方案是：
7.本发明公开了一种结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统，包括：北斗导航单元、座舱imu、头盔imu、双视觉辅助单元和测量单元；
8.所述座舱imu与运动载具固连，头盔imu与载具内人员头盔固连，分别用于获取运动载具和人员头部的运动信息；
9.所述双视觉辅助单元布置于运动载具内，用于通过双视觉方式获取头盔相对于座舱的视觉位姿数据；
10.所述北斗导航单元布置于运动载具上，利用北斗数据获得运动载具的导航结果；
11.所述测量单元，用于根据导航结果对座舱imu零偏进行修正；通过座舱和头盔的双
imu差分获得的载体坐标系中头盔相对座舱的惯性位姿数据；以视觉位姿数据为外部观测量对惯性位姿数据进行卡尔曼滤波，输出滤波后相对位姿数据。
12.进一步地，所述测量单元包括座舱imu零偏校正模块、双imu差分模块和位姿滤波模块；
13.所述座舱imu零偏校正模块与所述北斗导航单元和座舱imu连接，用于根据导航结果对座舱imu零偏进行修正；
14.所述双imu差分模块与座舱imu和头盔imu连接；接收座舱imu和头盔imu的惯性测量数据进行双imu差分运算得到的包括载体坐标系下头盔相对座舱的位置、速度和旋转四元数在内的惯性位姿数据；
15.所述位姿滤波模块与座舱imu零偏校正模块、双imu差分模块和双视觉辅助单元分别连接，以双视觉辅助单元输出的视觉位姿数据为外部观测量对包括惯性位姿数据和修正后座舱imu零偏在内的状态向量进行卡尔曼滤波，输出滤波后相对位姿数据。
16.进一步地，所述北斗导航单元为载波相位差分的北斗导航系统；其北斗卫星接收天线布置于运动载具外，接收卫星信号；处理器布置于座舱内，用于测量出运动载具在地理坐标系中的导航结果输出到所述座舱imu零偏校正模块。
17.进一步地，所述座舱imu零偏校正模块内的零偏进行修正的方式包括：
18.方式一、构建运动载具在地理坐标系中的运动状态方程，建立座舱imu的卡尔曼滤波器，利用北斗导航单元测量的数据生成运动载具在地理坐标系中的观测量，对载具在地理坐标系中的运动状态方程中包括的角速度与加速度的零偏进行更新，得到修正后的座舱imu的角速度与加速度的零偏；
19.方式二、构建运动载具和北斗导航单元深组合状态方程，建立组合后的卡尔曼滤波器，将座舱imu介入到北斗导航的卫星捕获环路中，滤波后获得修正后的座舱imu的角速度与加速度的零偏。
20.进一步地，双视觉辅助单元包括第一视觉模块和第二视觉模块；
21.所述第一视觉模块包括第一相机模和第一标志点；
22.所述第一相机模组安装于座舱内正对头盔佩戴人员供其使用的操纵台上，并与座舱imu固连；与第一相机模组配合的第一标志点布置于头盔上；第一相机模组通过测量第一标志点获得头盔相对座舱的第一视觉位姿数据；
23.所述第二视觉模块包括第二相机模和第二标志点；
24.所述第二相机模组安装于头盔上，并与头盔imu固连；与第二相机模组配合的第二标志点布置于正对头盔佩戴人员供其使用的操纵台上；第二相机模组通过测量第二标志点后，进行坐标系转换获得头盔相对座舱的第二视觉位姿数据。
25.进一步地，所述第二视觉位姿数据有输出时，采用第二视觉位姿数据为卡尔曼滤波的外部观测量；当所述第二视觉位姿数据无输出时，采用第一视觉位姿数据为卡尔曼滤波的外部观测量。
26.进一步地，所述第一标志点在头盔上的布局，满足在头盔设定的运动范围内，使所述第一标志点处于第一相机模组的视觉范围内；并且，所述第一标志点的数量大于等于4；
27.所述第二标志点在操纵台上的布局，保证在固定角度内，使所述第二标志点处于第二相机模组的视觉范围内；所述第二标志点的数量大于等于4。
28.进一步地，所述第一标志点和第二标志点使用红外发光点的方式或红外发光二维码的方式；所述第一相机模组和第二相机模组采用单目、双目或结构光方式。
29.进一步地，所述双imu差分模块中计算的惯性位姿数据中的旋转四元数采用角速度差分法进行求解；
30.在角速度差分法中，通过对当前时刻头盔与运动载具的相对角速度进行积分得到头盔与运动载具的旋转四元数。
31.进一步地，所述角速度差分法，包括：
32.获取头盔坐标系到运动载具坐标系的相对旋转矩阵；
33.根据头盔与运动载具的imu获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量；
34.使用当前时刻相对角增量计算相对旋转四元数得到头盔相对于运动载具的位姿数据。
35.本发明的可以至少实现以下有益效果之一：
36.本发明提出的一种结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统，通过北斗导航提升了座舱imu的零偏准确性，通过双视觉辅助头盔与座舱的相对位姿关系，对进行头盔与座舱的双imu惯性积分差分的位姿测量结果进行修正，得到准确的位姿测量结果。这种方法充分利用惯性测姿采样频率高、短时精度高，卫星导航和视觉测姿无累积误差的特性，实现高精度的位姿追踪。该方法相较于传统相对位姿测量方案，更为有效调用各种导航方式特性，且布置简便，适用于乘用车辆、战斗机、武装直升机中头戴显示系统等工程应用。
附图说明
37.附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
38.图1为本发明实施例中的觉辅助惯性差分位姿测量系统组成框图；
39.图2为本发明实施例中的运动载具中姿态测量的坐标系示意图；
40.图3为本发明实施例中的测量单元的组成和连接示意框图；
41.图4为本发明实施例中的车辆内的典型座舱布置方式图。
具体实施方式
42.下面结合附图来具体描述本发明的优选实施例，其中，附图构成本技术一部分，并与本发明的实施例一起用于阐释本发明的原理。
43.本实施例以车辆作为运动载具，公开的一种结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统，如图1所示，包括北斗导航单元、座舱imu、头盔imu、双视觉辅助单元和测量单元；
44.所述座舱imu与运动载具固连，头盔imu与载具内人员头盔固连，分别用于获取运动载具和人员头部的运动信息；
45.所述双视觉辅助单元布置于运动载具内，用于通过双视觉方式获取头盔相对于座舱的视觉位姿数据；
46.所述北斗导航单元布置于运动载具上，利用北斗数据获得运动载具的导航结果；
47.所述测量单元，用于根据导航结果对座舱imu零偏进行修正；通过座舱imu和头盔imu的双imu差分获得的载体坐标系中头盔相对座舱的惯性位姿数据；以视觉位姿数据为外部观测量对惯性位姿数据进行卡尔曼滤波，输出滤波后相对位姿数据。
48.运动车辆中姿态测量的坐标系示意图如图2所示，
49.其中，如图3所示，所述测量单元包括座舱imu零偏校正模块、双imu差分模块和位姿滤波模块；
50.所述座舱imu零偏校正模块与所述北斗导航单元和座舱imu连接，用于根据导航结果对座舱imu零偏进行修正；
51.所述双imu差分模块与座舱imu和头盔imu连接；接收座舱imu和头盔imu的惯性测量数据进行双imu差分运算得到的载体坐标系下头盔相对座舱的位置、速度、旋转四元数；
52.所述位姿滤波模块与座舱imu零偏校正模块、双imu差分模块和双视觉辅助单元分别连接，以双视觉辅助单元输出的视觉位姿数据为外部观测量对惯性位姿数据进行卡尔曼滤波，输出滤波后相对位姿数据。
53.具体的，所述北斗导航单元为载波相位差分的北斗导航系统；即为rtk系统，基于载波相位观测值的实时动态定位，实时地提供运动载具在地理坐标系中的三维定位结果，并达到厘米级精度。
54.北斗卫星接收天线布置于运动载具外，接收卫星信号；处理器布置于座舱内，用于测量出运动载具在地理坐标系中的导航结果输出到所述座舱imu零偏校正模块。
55.具体的，所述座舱imu零偏校正模块内的零偏进行修正的方式包括：
56.方式一、构建运动载具在地理坐标系中的运动状态方程，建立座舱imu的卡尔曼滤波器，利用北斗导航单元测量的数据生成运动载具在地理坐标系中的观测量，对载具在地理坐标系中的运动状态方程中包括的角速度与加速度的零偏进行更新，得到修正后的座舱imu的角速度与加速度的零偏；
57.所述座舱imu的卡尔曼滤波器的状态向量可包括座舱imu测量的运动载具的三维位置、速度、姿态和imu的角速度与加速度的零偏；观测向量可为位置偏差及姿态偏差。以此进行卡尔曼滤波，实现imu的角速度与加速度的零偏的修正。
58.方式二、构建运动载具和北斗导航单元深组合状态方程，建立组合后的卡尔曼滤波器，将座舱imu介入到北斗导航的卫星捕获环路中，滤波后获得修正后的座舱imu的角速度与加速度的零偏。
59.所述组合后的卡尔曼滤波器中状态向量可包括由北斗导航单元输出和座舱imu输出混合的三维位置、速度、姿态数据以及imu的角速度与加速度的零偏；其中，混合的三维位置、速度、姿态数据可以根据卫星导航和imu测量的不同优势来选取对应的数据组成。观测向量可为位置偏差及姿态偏差。以此进行卡尔曼滤波，实现座舱位置、速度、姿态数据的精确输出和座舱imu的角速度与加速度的零偏的修正。
60.具体的，双视觉辅助单元包括第一视觉模块和第二视觉模块；
61.所述第一视觉模块包括第一相机模和第一标志点；
62.所述第一相机模组安装于座舱内正对头盔佩戴人员供其使用的操纵台上，并与座舱imu固连；与第一相机模组配合的第一标志点布置于头盔上；第一相机模组通过测量第一
标志点获得头盔相对座舱的第一视觉位姿数据。
63.所述第一标志点在头盔上的布局，满足在头盔设定的运动范围内，使所述第一标志点处于第一相机模组的视觉范围内；并且，所述第一标志点的数量大于等于4；
64.所述第一视觉模块的优势在于，头盔带动第一标志点运动时，第一标志点始终不会超出相机视野，视觉测量系统能够全时段工作。
65.所述第二视觉模块包括第二相机模和第二标志点；
66.所述第二相机模组安装于头盔上，并与头盔imu固连；与第二相机模组配合的第二标志点布置于正对头盔佩戴人员供其使用的操纵台上；第二相机模组通过测量第二标志点后，进行坐标系转换获得头盔相对座舱的第二视觉位姿数据。
67.所述第二标志点在操纵台上的布局，保证在固定角度内，使所述第二标志点处于第二相机模组的视觉范围内，例如，安装在佩戴头盔的座舱乘员面前的显示设备的周边边框上，使乘员在观察显示信息时所述第二标志点处于第二相机模组的视觉范围内。所述第二标志点的数量大于等于4。
68.所述第二视觉模块的优势在于，所述第二标志点可以在相机视野内占有更大面积，因此视觉相对测量精度也更高，同时这种布置方式还带来了另外一种优势，即多套测量系统的多个相机可共享同一套标志点靶标，保证各套系统工作互不干扰。但缺点在于，在头盔运动过程中，一旦标志点靶标超出相机视野，视觉测量将无法工作。
69.优选的，所述第一标志点和第二标志点使用红外发光点的方式或红外发光二维码的方式；
70.所述第一相机模组和第二相机模组采用单目、双目或结构光方式。只要能准确捕获红外标志点的图像信息即可。
71.更优选的，所述第一标志点和第二标志点采用同频闪烁的方式与对应的相机模组同步。通过图像处理提取红外点位置，避免外部自然光红外波段的光线干扰。
72.本实施例中的双视觉辅助单元中的一种具体的视觉位姿数据综合的方式包括：在所述第二视觉位姿数据有输出时，采用第二视觉位姿数据为卡尔曼滤波的外部观测量；当所述第二视觉位姿数据无输出时，采用第一视觉位姿数据为卡尔曼滤波的外部观测量。以此方式，在保证了测量精度的同时保证了较大的测量范围。
73.并且，在进行布置时，使所述座舱imu与第一相机模组固连，所述头盔imu与第二相机模组固连，通过上述固连可以减少标定工作量。
74.具体的，车辆内的典型座舱布置方式如图4所示。
75.具体的，双imu差分模块中，进行双imu差分运算得到的载体坐标系下头盔相对座舱的位置、速度和旋转四元数；其中，载体坐标系下头盔相对座舱的位置和速度可采用现有的技术方法获得，在此就不赘述。
76.所述旋转四元数采用角速度差分法进行求解；
77.在角速度差分法中，通过对当前时刻头盔与运动载具的相对角速度进行积分得到头盔与运动载具的相对位姿。
78.进一步地，所述角速度差分法，包括：
79.1)获取头盔坐标系到运动载具坐标系的相对旋转矩阵r
rel
；
80.头盔坐标系到运动载具坐标系的相对旋转矩阵r
rel
可通过对头盔和运动载具的标
定获得。
81.2)根据头盔与运动载具的imu获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量；
82.根据头盔与运动载具的imu获取角速度信息ωh
(t)
、ω
p(t)
与t
k-1
时刻相对旋转矩阵计算当前时刻t的相对角增量为：
[0083][0084]
3)使用当前时刻相对角增量计算相对旋转四元数得到头盔相对于运动载具的位姿数据。
[0085]
用当前时刻相对角增量计算相对旋转四元数为
[0086][0087]
相对旋转四元数即为旋转四元数q
h-p
。
[0088]
具体的，位姿滤波模块中，卡尔曼滤波中的状态向量
[0089]
x＝[p
h-p
，v
h-p
，q
h-p
，b
ha
,b
pa
，b
hω
，b
pω
]
t
；
[0090]
x中p
h-p
、v
h-p
、q
h-p
定义为双imu差分获得的载体坐标系下头盔相对运动载具的位置、速度、旋转四元数；b
hω
、b
ha
为头盔imu测量角速度与加速度的零偏；b
pω
、b
pa
为修正后的座舱imu的角速度与加速度的零偏。
[0091]
在忽略杆臂效应时，真实情况下状态方程可以用下式表示：
[0092][0093][0094][0095][0096][0097][0098][0099]
式中，为相对旋转矩阵；ah、b
ha
和n
ha
为头盔imu加速度、加速度零偏和加速度噪声；a
p
、b
pa
、n
pa
为座舱imu加速度、加速度零偏和加速度噪声；ωh、b
hω
、n
hω
为头盔imu角速度、角速度零偏和角速度噪声，ω
p
、b
pω
、n
pω
为座舱imu角速度、角速度零偏和角速度噪声；为头盔imu加速度零偏噪声和角速度零偏噪声；为头盔imu加速度零偏噪声和角速度零偏噪声；为座舱imu加速度零偏噪声和角速度零偏噪声。
[0100]
为不含误差的状态向量；
[0101][0102]
定义为不含误差的载体坐标系下头盔相对载体的位置、速度、旋转四元数；为不含误差的头盔imu测量角速度与加速度的零偏；b
pω
、b
pa
为不含误差的座舱imu测量角速度与加速度的零偏；
[0103]
则用于表示真实状态x与不含噪声状态的状态误差向量δx为：
[0104][0105]
其中各项的具体展开如下：
[0106][0107][0108][0109][0110]
根据建立当前时刻误差状态方程δx与下一时刻状态误差的递推关系
[0111][0112][0113][0114][0115][0116][0117][0118]
δt为当前时刻到下一时刻的时间差。
[0119]
得到，系统状态误差递推关系即状态误差方程为：
[0120][0121]fx
为状态转移矩阵；fn为噪声转移矩阵；
[0122][0123][0124]
u＝[ah，ωh，a
p
，ω
p
]
t
；
[0125]ah
、ωh、a
p
、和ω
p
分别为头盔imu加速度与角速度输出、座舱imu的加速度与角速度输出；
[0126]
n为状态噪声向量，
[0127][0128][0129][0130][0131][0132]
其中，和分别为头盔imu加速度噪声、座舱imu加速度噪声、头盔imu角速度噪声、座舱imu角速度噪声，头盔imu加速度零偏噪声、座舱imu加速度零偏噪声、头盔imu角速度零偏噪声和座舱imu角速度零偏噪声的方差。
[0133]
进一步地，
[0134]
所述状态转移矩阵
[0135]
其中，为不含误差的头盔到运动载具相对旋转矩阵；
[0136][0137][0138]
i为单位矩阵；
[0139]
噪声转移矩阵
[0140]
则，系统的状态协方差矩阵为
[0141]
基于上述过程，所述更新卡尔曼滤波的误差状态协方差矩阵的过程包括：
[0142]
1)获取头盔、座舱的imu数据；
[0143]
2)根据头盔与座舱运动模型更新状态向量
[0144]
3)更新状态转移矩阵f
x
、更新协方差矩阵fnnf
nt
；
[0145]
4)更新误差状态协方差矩阵
[0146]
适用于本方案的位姿测量卡尔曼滤波的滤波器的观测方程为：
[0147][0148]
其中，误差向量h
p
为位置量测矩阵；误差向量hq为位置量测矩阵；z
p
、zq为卡尔曼滤波的外部观测量的位置向量和速度向量；为卡尔曼滤波估计的位置向量和速度向量。
[0149]
在观测过程中，
[0150]
1)列写更新部分位置测量模型zp；
[0151]
其中，位置测量模型
[0152]
式中，p
c-t
代表相机观测的位移，由视觉测量经过内参变化后得到；p
h-p
、为头盔相对座舱的平移向量与旋转矩阵；p
c-h
为相机相对头盔的位置观测量，可通过标定得到；n
p
为量测噪声。
[0153]
则误差向量展开有：
[0154][0155]
展开后忽略二阶项，得到：
[0156][0157]
根据观测方程δz
p
＝h
p
δx，位置量测矩阵h
p
写作如下：
[0158][0159]
式中，p
c-h
为位置观测量，[p
c-h
]
×
为对应叉乘运算矩阵。
[0160]
2)列写更新部分角度测量模型zq；
[0161]
其中，角度测量模型
[0162]
则误差向量展开有
[0163][0164]
根据观测方程δzq＝hqδx，旋转量测矩阵hq写作如下
[0165][0166]
本实施例的更新状态协方差矩阵与状态向量的过程包括：
[0167]
1)计算观测残差
[0168]
2)计算更新矩阵s＝hph
t
+r；
[0169]
3)计算卡尔曼增益k＝ph
t
s-1
；
[0170]
4)计算状态修正量
[0171]
5)计算状态协方差矩阵的递推结果p
←
(i
d-kh)p(i
d-kh)
t
+krk
t
。
[0172]
6)将状态更新量与原本的状态向量叠加后得到更新后的状态向量。
[0173]
在本实施例的一个优选方案中，还包括了对双视觉辅助单元的第一或第二视觉位姿数据进行筛选的方法，以第二视觉位姿数据的筛选为例，具体的筛选过程如下：
[0174]
双imu差分通过积分过程以较高采样频率获取变换矩阵t
imu
，同时第二视觉位姿数据以较低速率获取变换矩阵t
cam
。记连续几帧第二视觉位姿数据更新得到的变换矩阵与(依次递推)，选取差分后imu历史测姿信息中与第二视觉位姿数据获取时刻最接近的几帧惯性测姿变换矩阵与(依次递推)。则计算得到第二视觉位姿数据得到的相邻两帧相对位姿变换矩阵相邻两帧最近时刻的惯性测量相对姿态变换矩阵
[0175]
为求取相邻帧时间内视觉测量与惯性测量的相对偏差，定义在李代数上的对数映射的二范数dk[0176][0177]
计算k时刻前一定时间内，n组有效相对变换dk均方根偏差基准rmsdk。
[0178][0179]
判定，若新一帧视觉测量与惯性测量偏差dk∈[-2rmsdk，2rmsdk]，则认为第二视觉位姿数据可用于对滤波器状态进行更新，同时该dk计入均方根偏差基准的更新。否则认为是第二视觉位姿数据无效，丢弃不用，该dk也不计入均方根偏差基准。通过对视觉位姿数据，可以提高测量精度。
[0180]
综上所述，本实施例的结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统，通过惯性差分获取头盔相对于座舱中的相对位姿，通过北斗导航接收机对座舱imu进行零偏修正，再通过分别布置于头盔、座舱的两组相机模组，获取头盔与运动载具的相对位姿关系，对进行惯性积分差分的位姿测量结果进行修正，得到准确的位姿测量结果。这种方法充分利用惯性测姿采样频率高、短时精度高，视觉测姿无累积误差的特性，通过引入卫导观测抑制了座舱imu漂移，通过运动载具上设置的一组相机实现对头盔的大范围位姿追踪，通过头盔上设置的一组相机实现高精度的位姿追踪。该方法相较于传统相对位姿测量方案，更为有效调用各种导航方式特性，且布置简便，适用于乘用车辆、战斗机、武装直升机中头戴显示系统等工程应用。
[0181]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。