CN104280022A - 视觉辅助惯性测量单元的数字头盔显示设备跟踪系统 - Google Patents

Abstract

本发明公开了一种基于视觉辅助惯性测量单元的数字头盔跟踪系统，结合惯性测量单元和视觉测量各自的优点，自主、实时地实现头盔姿态的跟踪。本发明的新颖之处在于利用视觉辅助的方法，修正惯性测量随时间而不断积累的误差，实现对数字头盔显示器姿态和位置的实时跟踪计算。本发明的方法考虑了测量过程中的噪声影响，利用卡尔曼滤波算法进行头盔姿态的估计，尽可能地减小噪声对测量结果的影响。本发明具有自主性好，实时性好，计算量小，成本低，质量轻，负荷小，使用方便，精度较高等优点。

Description

视觉辅助惯性测量单元的数字头盔显示设备跟踪系统

技术领域

本发明与计算机视觉和惯性测量相关，属于视觉测量和组合导航研究领域，建立的头盔显示器跟踪系统具有轻质、舒适、快响应、高精度等特点，其中采用的是一种视觉辅助惯性测量单元的导航方法，能够实时、可靠地实现对头盔姿态的精确跟踪。

背景技术

头盔跟踪系统通过跟踪飞行员头部运动，实现武器系统的瞄准，在实际飞行任务中，飞行员不仅要时刻查看仪器面板数据变化，同时还要关注舱外打击目标的位置，视线瞄准目标，因此通过飞行员视线方向变化控制瞄准系统瞄准目标的方法是非常有意义的，飞行员视线方向体现在飞行员所带头盔的姿态变化，利用传感器获取头部姿态，从而得到视线方向，转化成控制信号，控制瞄准系统。

头盔跟踪系统应用在头盔瞄准具中已有很长一段时间，传统的瞄准方式主要有机电式、电磁式和光电式：机电式是通过连杆装置和电磁离合器将头盔与座舱顶部连接起来，由连杆两端的测角器测量头部转动的角度；电磁式是通过头盔和座舱内的磁敏感器和磁辐射器，利用磁场方向的偏差获得头部相对于机身正方向的角度，电磁设备的安装会使得飞行员头部负荷大，且易对舱内其他设备产生电磁干扰；光电式是通过座舱内的红外接收机接收头盔两侧红外二极管发出的红外信号，与基准信号比对，由偏差计算头部转动的角度，虽然这种方法较为先进，也改进了前两种方法的一些缺点，但是光电式的瞄准系统结构复杂，安装调试难度大，也为头盔瞄准器的使用造成一定的不便。

随着计算机视觉的发展，越来越多的学者将目光转向了视觉测量，其用于头盔瞄准具的应用价值也逐渐体现，视觉测量由于其成本低、质量轻、可靠性高、使用方便、技术成熟等优点，大大减轻飞行员的负担和安装调试的难度，因此，许多学者对基于视觉测量、图像处理技术的头盔瞄准具进行了大量的仿真与研究，但是视觉图像的处理需要占用很大的计算空间，处理速度较慢，若要满足视线对准的快速性与实时性，必须要求机载计算机具有很大的存储空间和很高的计算效率。

惯性测量单元基于惯性测量技术，是非常成熟的测量手段，已广泛应用于各类飞行器和舰船，它具有不依赖外界信息、不向外界辐射能量、不受干扰、隐蔽性好、能连续不断地提供数据等特点，是一种完全自主性、实时性的测量、导航方法，但惯性测量单元的测量误差随时间的不断增加而积累，再加上自身的漂移特性，不能满足长时间的精度要求，本发明的方法就是基于视觉辅助惯性测量单元的方法，利用成熟的惯性测量单元测量技术，加以视觉测量的辅助，以一定的时间间隔修正惯性测量的误差，以达到精确跟踪头盔姿态的目的。

发明内容

利用惯性测量单元而发展起来的各项技术凭借其自主性、实时性等强大优点，保证了惯性测量在数字头盔姿态跟踪方面的可行性；视觉测量技术的发展也为数字头盔的姿态跟踪提供了技术支持，其精度较高的优势在保证姿态跟踪自主性的基础上，弥补惯性测量单元误差随时间积累的缺点，辅助惯性测量单元实现对数字头盔姿态的实时跟踪；本发明设计一种视觉辅助惯性测量单元的数字头盔跟踪系统，在头盔顶部选定若干个标志/特征点进行标定，相机安装在头盔顶部正上方的座舱内，对正下方进行拍照；惯性测量单元安装在飞行员的头盔上，通过建立一种导航滤波方法实现对头盔姿态的计算。

本发明的基本原理如下。

一、坐标系统的建立

为方便参数的表示和转换，建立惯性坐标系、相机坐标系、像素坐标系、头盔初始坐标系、头部转动坐标系等五个坐标系；惯性测量单元的测量值在惯性坐标系下表示；相机测量到的像点坐标在像素坐标系下表示；头盔初始坐标系以头盔中心为坐标原点，以平视前方的视线方向为x轴，以向上指向相机的方向为z轴，y轴与x轴、z轴构成右手坐标系，且不随头部转动而转动；头部转动坐标系随头部一起转动，以头盔最底部中心（颈部）为坐标原点，平视前方时的三轴方向与头盔初始坐标系三轴方向一致。这五个坐标系之间，存在一定的转换关系。

 二、惯性测量单元模型

惯性测量单元（陀螺仪和加速度计）的测量值为头盔在惯性空间内的转动角速度和非引力加速度                                                沿头盔三个转轴方向的分量，而和则表示头盔在惯性空间内真实的、不含噪声的转动角速度和加速度，由于惯性测量单元存在漂移，且在测量的过程中受到随机噪声的干扰，因此实际测量值和与真实值和有如下关系，

                    (1)

其中，为头部转动坐标系相对于头盔初始坐标系的姿态四元数对应的旋转矩阵，为重力加速度，和分别为陀螺仪和加速度计的零均值测量高斯白噪声，和分别为陀螺仪和加速度计的漂移

                           (2)

和分别为陀螺仪和加速度计的零均值驱动高斯白噪声。

    三、视觉测量模型

头盔顶部的特征点在头部转动坐标系中的三维坐标是固定且已知的，由此可以得到相机到特征点的位置矢量在相机坐标系下的分量表示

                    (3)

其中，为相机在头盔初始坐标系下的位置矢量，为头部转动坐标系在头盔初始坐标系下的位置矢量，为相机坐标系相对于头部转动坐标系的旋转四元数对应的旋转矩阵，利用相机的成像透射原理以及特征点坐标在像素坐标系下的投影关系，可得到测量坐标矢量

              (4)

其中，为头盔顶部特征点投影到像素坐标系中的二维坐标，为特征检测过程中产生的误差，测量坐标的估计值可由当前状态估计求得，已知位置特征点在相机坐标系下的三维坐标估计值为

                    (5)

由此可得到估计测量矢量

                    (6)

是位置特征点在相机坐标系下的三维坐标下的估计值。

    四、头盔惯性姿态参数的解算

本发明利用欧拉参数来描述头盔的姿态，欧拉参数由四个元素组成

                 (7)

这四个元素并不是独立的，满足下面归一化的约束条件：

                     (8)

用欧拉参数表示姿态运动学方程，

              (9)

或                     (10)

或                              (11a)

                         (11b)

其中，“”表示四元数的乘法，四元数乘法定义为

，为头部转动坐标系相对于头盔初始坐标系的转动角速度（在系下的分量）；为头部转动坐标系相对于头盔初始坐标系的姿态四元数，为的标量部分，为的矢量部分，为在系下的坐标方阵，

.

可由惯性测量单元的测量值求得

            (12)

其中，是惯性测量单元实时测得的头盔转动角速度，为头部转动坐标系相对于头盔初始坐标系的姿态四元数对应的旋转矩阵

        (13)

为头盔初始坐标系在实际飞行过程中的转动角速度，基于本发明以地面仿真模拟器为背景对象，可以简化为，则

                         (14)

因此，求解式(9)或(10)或(11)来完成头盔姿态的解算工作，将微分方程整理为

                    (15)

其中，为在头部转动坐标系下的坐标方阵，利用四级四阶龙格库塔法求解上式

                  (16)

即可得到头部转动坐标系相对头盔初始坐标系的姿态四元数，头部转动坐标系相对头盔初始坐标系的位置和速度均不变，将系统的微分方程组整理为

                  (17)

其中，.由于和为随机噪声，无法预知其实际值，因此在对实际状态的估计中，均假设为。由此可得，

                  (18)

其中，。

五、卡尔曼滤波算法

在姿态参数估计过程中利用了如下信息：系统方程、观测方程、白噪声激励的统计特性、量测误差的统计特性，本发明建立的离散型算法如下：

设时刻的估计状态受系统噪声驱动，状态方程

                       (19)

描述了噪声的驱动机理。的量测与满足线性关系，即观测方程

                        (20)

其中，为时刻的估计状态，为时刻至时刻的一步转移阵，为系统噪声驱动阵，为系统噪声，为量测阵，为量测噪声，和满足

            (21)

其中，为系统噪声方差阵，为量测噪声方差阵，如果被估计状态满足式(19)，量测量满足式(20)，系统噪声和量测噪声满足式(21)，系统噪声方差阵非负定，量测噪声方差阵正定，时刻的量测为，则的估计按下述方程求解：

状态一步预测

                        (22a)

一步预测均方误差

                  (22b)

卡尔曼增益

               (22c)

状态估计

                (22d)

估计均方误差

        (22e)

式(22)即为离散型卡尔曼滤波基本方程。只要给定初值和，根据时刻的量测，就可递推计算得到时刻的状态估计。

扩展卡尔曼滤波包括两个步骤：一是时间更新，利用状态转移模型和观测方程，将探测器的状态和误差协方差阵观测值从上个时刻推测到此时刻；二是测量更新，即用一组新的测量信息对估计的探测器状态进行处理，这两个步骤交替进行，不断计算出新的滤波估计值，本发明采用扩展卡尔曼滤波的算法过程如下：

    在使用基本方程之前，对系统方程和量测方程做离散化和线性化处理，考虑一般化的连续非线性系统，线性化的状态方程可写为

               (23)

观测方程为

                        (24)

其中，为零均值系统噪声向量，有，，为系统噪声方差阵，为零均值观测噪声向量，有，设定系统的状态向量为，其中包含头部转动坐标系相对头盔初始坐标系的姿态四元数的矢量部分和陀螺仪的漂移，状态向量为

                           (25)

由式(11)和(18)可将状态方程可表示为

           (26)

所以，状态转移矩阵可由式(26)对状态向量求雅可比矩阵得到，

                  (27)

                   (28a)

                  (28b)

其中，，设系统噪声向量为

                        (29)

其中，为陀螺仪的零均值测量高斯白噪声，其方差阵为，为陀螺仪的零均值驱动高斯白噪声，方差阵为。则系统噪声方差阵为

                     (30)

系统噪声输入矩阵为

             (31)

因此，将系统离散化后得到的状态转移矩阵为

                       (32)

当滤波周期较短时，可看作常阵，即时，

                  (33)

根据扩展卡尔曼滤波的线性化方法，对上式舍去二阶及以上项，只保留到一阶项，可得到状态转移矩阵为

                         (34)

其中，，系统噪声输入阵为

                          (35)

系统噪声方差阵为，

       (36)

其中，。当滤波周期较短时，，由此系统噪声方差阵可简化为

                   (37)

量测阵可由量测方程对状态向量求雅可比矩阵得到，

                     (38)

式中，

                (39)

                      (40)

当有个已知位置特征点时，量测阵可以表示成以下形式，

                      (41)

其中，，().

卡尔曼滤波的应用需给定初始化条件和，即

                  (42)

在估计过程中，利用时刻的估计值，并根据时刻的量测量，即可递推得到时刻的状态估计。

 本发明技术效果：

    本发明与传统的机电式、电磁式和光电式等头盔姿态跟踪方法相比，克服了传统方法的缺点，使头盔瞄准具具有质量轻、成本低、使用方便、安全性高、噪声小、干扰小等优良特性；本发明在基于视觉的头盔跟踪系统的基础上加以改进，以惯性测量单元为主，视觉相机为辅，进行精确的头盔转动姿态的获取，达到对头盔姿态跟踪的目的；本发明采用的视觉辅助惯性测量单元的方法，不仅保证了算法的自主性和实时性，同时提高了测量精度，利用姿态估计的滤波方法，对头盔的姿态进行估计，以实现对头盔姿态的获取。

附图说明：

图1为本发明使用的坐标系空间位置示意图；

图2为本发明给出的视觉辅助惯性测量单元的算法流程图；

图3为本发明实例计算中得到的姿态估计误差曲线图。

具体实施方式，本实施方法结合图1至图3对本发明进行具体介绍，本发明用到的五个坐标系如图1所示。

本发明的方法为视觉辅助惯性测量单元的测量方法，此方法以惯性测量为基础，辅助以视觉测量，视觉测量的修正方式为每经过一固定时间，修正惯性测量误差一次，本发明算法流程如图2所示。

首先，对系统的初始参数进行设定，包括系统设备参数（如头盔的初始位置、头盔大小、头盔顶部特征点个数及在头部转动坐标系下的坐标、相机的具体参数与具体位置等）和算法初始参数（如状态向量和其方差阵的初值、惯性测量单元与视觉相机的测量误差方差阵等）。

其次，获取惯性测量单元和视觉相机的测量数据，惯性测量单元测量得到头盔在惯性空间内的沿头盔本体三轴方向的角速度，视觉相机测量头盔顶部特征点在像素坐标系下的二维坐标。

最后，以头部转动坐标系相对头盔初始坐标系的姿态四元数和惯性测量单元的陀螺仪漂移为状态向量，运用扩展卡尔曼滤波算法估计姿态信息，由卡尔曼滤波生成状态误差估计，采用迭代的思想对状态向量进行更新，更新完毕的状态向量中即包含头盔在此时的姿态，起到对头盔姿态的跟踪效果，姿态估计算法的实现过程如下：

1. 迭代起始

迭代过程从一步预测估计开始，即

                          (43)

2. 迭代流程

设此次迭代为第次迭代，

1）  根据式(6)，可以由求出观测坐标估计值；

2）  根据式(38)及当前估计状态，能够得到量测阵；

3）  构造像素坐标系内二维坐标的残差

                          (44)

并通过下式计算出残差的协方差阵

                    (45)

其中，一步估计预测方差阵为

                       (46)

4）  计算卡尔曼增益

                    (47)

5）  利用卡尔曼增益、残差和量测阵计算修正误差（设初始误差）

            (48)

6）  此时可以通过下列公式得到下一轮迭代的状态估计，返回步骤a，

继续状态误差的迭代更新，直至迭代结束

a.       四元数的更新通过下式计算得到

                      (49)

b.      状态估计向量中的其他状态量（陀螺仪漂移量）的更新方法由下式提供，仅作简单的代数求和即可

                  (50)

3. 迭代结束

    此时刻的IEKF误差迭代更新完成后，当前估计状态方差阵通过下式来更新

                      (51)

由迭代过程可以看出，IEKF的第一次迭代等同于常规的扩展卡尔曼滤波，通过上述的卡尔曼滤波方法，利用视觉信息修正惯性测量的误差，辅助惯性测量单元获得精确的头盔姿态。

本发明给出了具体仿真实例的结果如图3所示，可以看出本发明使用的视觉辅助惯性测量单元的头盔姿态跟踪系统具有较好的收敛性，在设备测量噪声未知的情况下，能够较好地排除噪声的干扰，获得较精确地头盔的姿态，形成了一种具有很强实用性的数字头盔跟踪方法。

Claims (5)

1.本发明权利要求一种视觉辅助惯性测量单元的数字头盔跟踪系统，这套系统的特征包括：数字头盔跟踪系统在头盔顶部选定若干个标志/特征点进行标定，视觉相机安装在头盔顶部正上方的座舱内，对正下方的数字头盔进行拍照，惯性测量单元安装在飞行员的头盔上，惯性测量单元测量头盔在运动中的角速度信息，视觉相机测量头盔顶部已知特征点的坐标，惯性测量单元不间断地对头盔姿态进行测量，视觉相机每隔一固定的时间间隔对视野内出现的头盔顶部特征点进行拍照，经过特征提取、匹配后，获得特征点坐标值，对此时的惯性测量误差予以修正。

2.本发明权利要求一种惯性测量单元和视觉测量的模型建立方法

    惯性测量单元（陀螺仪和加速度计）的测量值为头盔在惯性空间内的转动角速度                                                和非引力加速度沿头盔三个转轴方向的分量，而和则表示头盔在惯性空间内真实的、不含噪声的转动角速度和加速度，由于惯性测量单元存在漂移，且在测量的过程中受到随机噪声的干扰，因此实际测量值和为

                    (1)

其中，为头部转动坐标系相对于头盔初始坐标系的姿态四元数对应的旋转矩阵，为重力加速度，和分别为陀螺仪和加速度计的零均值测量高斯白噪声，和分别为陀螺仪和加速度计的漂移，头盔顶部的特征点在头部转动坐标系中的三维坐标是固定且已知的，由此可以得到相机到特征点的位置矢量在相机坐标系下的分量表示

                    (2)

其中，为相机在头盔初始坐标系下的位置矢量，为头部转动坐标系在头盔初始坐标系下的位置矢量，为相机坐标系相对于头部转动坐标系的旋转四元数对应的旋转矩阵，利用相机的成像透射原理以及特征点坐标在像素坐标系下的投影关系，可得到测量坐标矢量

              (3)

为头盔顶部特征点投影到像素坐标系中的二维坐标，为特征检测过程中产生的误差。

3.本发明权利要求一种头盔姿态运动表示方法

利用姿态四元数来描述头盔的姿态，并满足归一化的约束条件，则姿态运动学方程表示为

              (4)

或                              (5a)

                         (5b)

其中，，为头部转动坐标系相对于头盔初始坐标系的转动角速度（在系下的分量），可由惯性测量单元的测量值求得；为头部转动坐标系相对于头盔初始坐标系的姿态四元数，为的标量部分，为的矢量部分，为的反对称阵，利用四级四阶龙格库塔法求解式(4)或(5)姿态运动学方程，即可得到头部转动坐标系相对头盔初始坐标系的姿态四元数，头部转动坐标系相对头盔初始坐标系的位置和速度均不变，将系统的微分方程组整理为

                  (6)

其中，，为在头部转动坐标系下的坐标方阵。

4.本发明权利要求一种卡尔曼滤波算法以实现对姿态的计算

    估计过程中利用了如下信息：系统方程、观测方程、白噪声激励的统计特性、量测误差的统计特性，本发明建立的离散型卡尔曼滤波算法如下：

设时刻的估计状态受系统噪声驱动，状态方程

                       (7)

描述了噪声的驱动机理，的量测与满足线性关系，即观测方程

                        (8)

其中，为时刻的估计状态，为时刻至时刻的一步转移阵，为系统噪声驱动阵，为系统噪声，为量测阵，为量测噪声

和满足

            (9)

其中，为系统噪声方差阵，为量测噪声方差阵

如果被估计状态满足式(7)，量测量满足式(8)，系统噪声和量测噪声满足式(9)，系统噪声方差阵非负定，量测噪声方差阵正定，时刻的量测为，则的估计按下述方程求解：

状态一步预测

                        (10a)

一步预测均方误差

                  (10b)

卡尔曼增益

               (10c)

状态估计

                (10d)

估计均方误差

        (10e)

式(10)即为离散型卡尔曼滤波基本方程，只要给定初值和，根据时刻的量测，就可递推计算得到时刻的状态估计。

5.本发明权利要求一种扩展卡尔曼滤波算法以实现对姿态的计算

    权利要求4给出的卡尔曼滤波只能用于线性随机系统的状态估计，权利要求5要求的扩展卡尔曼滤波是对非线性系统方程进行线性化近似后，再利用普通卡尔曼滤波方法进行滤波估计的算法，在使用离散型卡尔曼滤波基本方程之前，对系统方程和量测方程做离散化和线性化处理，考虑一般化的连续非线性系统，线性化的状态方程可写为

               (11)

观测方程为

                        (12)

其中，为零均值系统噪声向量，有，，为系统噪声方差阵，为零均值观测噪声向量，有.

设定系统的状态向量为，其中包含头部转动坐标系相对头盔初始坐标系的姿态四元数的矢量部分和陀螺仪的漂移，状态向量为

                           (13)

由式(5)和(6)可将状态方程可表示为

           (14)

所以，状态转移矩阵可由式(14)对状态向量求雅可比矩阵得到，

                  (15)

式中，

                   (16a)

                  (16b)

其中，；

    设系统噪声向量为

                        (17)

                     (18)

其中，为陀螺仪的零均值测量高斯白噪声的方差阵，为陀螺仪的零均值驱动高斯白噪声的方差阵；系统噪声输入矩阵为

             (19)

将系统离散化后得到状态转移矩阵，

                     (20)

考虑到滤波周期较短， 可看作常阵，且根据扩展卡尔曼滤波的线性化方法，舍去二阶及以上项，可得到状态转移矩阵为

                         (21)

其中，，系统噪声输入阵为

                          (22)

系统噪声方差阵为

       (23)

其中，，当滤波周期较短时，，由此系统噪声方差阵可简化为

                   (24)

量测阵可由量测方程对状态向量求雅可比矩阵得到，

                     (25)

式中

                (26)

                      (27)

当有个已知位置特征点时，量测阵可以表示成以下形式，

                (28)

其中，，().

卡尔曼滤波的应用需给定初始化条件和，即

                  (29)

在估计过程中，利用时刻的估计值，并根据时刻的量测量，即可递推得到时刻的状态估计。