视觉辅助惯性测量单元的数字头盔显示设备跟踪系统的制作方法
【专利摘要】本发明公开了一种基于视觉辅助惯性测量单元的数字头盔跟踪系统，结合惯性测量单元和视觉测量各自的优点，自主、实时地实现头盔姿态的跟踪。本发明的新颖之处在于利用视觉辅助的方法，修正惯性测量随时间而不断积累的误差，实现对数字头盔显示器姿态和位置的实时跟踪计算。本发明的方法考虑了测量过程中的噪声影响，利用卡尔曼滤波算法进行头盔姿态的估计，尽可能地减小噪声对测量结果的影响。本发明具有自主性好，实时性好，计算量小，成本低，质量轻，负荷小，使用方便，精度较高等优点。
【专利说明】视觉辅助惯性测量单元的数字头盔显示设备跟踪系统
【技术领域】
[0001] 本发明与计算机视觉和惯性测量相关，属于视觉测量和组合导航研究领域，建立 的头盔显示器跟踪系统具有轻质、舒适、快响应、高精度等特点，其中采用的是一种视觉辅 助惯性测量单元的导航方法，能够实时、可靠地实现对头盔姿态的精确跟踪。
【背景技术】
[0002] 头盔跟踪系统通过跟踪飞行员头部运动，实现武器系统的瞄准，在实际飞行任务 中，飞行员不仅要时刻查看仪器面板数据变化，同时还要关注舱外打击目标的位置，视线瞄 准目标，因此通过飞行员视线方向变化控制瞄准系统瞄准目标的方法是非常有意义的，飞 行员视线方向体现在飞行员所带头盔的姿态变化，利用传感器获取头部姿态，从而得到视 线方向，转化成控制信号，控制瞄准系统。
[0003] 头盔跟踪系统应用在头盔瞄准具中已有很长一段时间，传统的瞄准方式主要有机 电式、电磁式和光电式：机电式是通过连杆装置和电磁离合器将头盔与座舱顶部连接起来， 由连杆两端的测角器测量头部转动的角度；电磁式是通过头盔和座舱内的磁敏感器和磁辐 射器，利用磁场方向的偏差获得头部相对于机身正方向的角度，电磁设备的安装会使得飞 行员头部负荷大，且易对舱内其他设备产生电磁干扰；光电式是通过座舱内的红外接收机 接收头盔两侧红外二极管发出的红外信号，与基准信号比对，由偏差计算头部转动的角度， 虽然这种方法较为先进，也改进了前两种方法的一些缺点，但是光电式的瞄准系统结构复 杂，安装调试难度大，也为头盔瞄准器的使用造成一定的不便。
[0004] 随着计算机视觉的发展，越来越多的学者将目光转向了视觉测量，其用于头盔瞄 准具的应用价值也逐渐体现，视觉测量由于其成本低、质量轻、可靠性高、使用方便、技术成 熟等优点，大大减轻飞行员的负担和安装调试的难度，因此，许多学者对基于视觉测量、图 像处理技术的头盔瞄准具进行了大量的仿真与研究，但是视觉图像的处理需要占用很大的 计算空间，处理速度较慢，若要满足视线对准的快速性与实时性，必须要求机载计算机具有 很大的存储空间和很高的计算效率。
[0005] 惯性测量单元基于惯性测量技术，是非常成熟的测量手段，已广泛应用于各类飞 行器和舰船，它具有不依赖外界信息、不向外界辐射能量、不受干扰、隐蔽性好、能连续不断 地提供数据等特点，是一种完全自主性、实时性的测量、导航方法，但惯性测量单元的测量 误差随时间的不断增加而积累，再加上自身的漂移特性，不能满足长时间的精度要求，本发 明的方法就是基于视觉辅助惯性测量单元的方法，利用成熟的惯性测量单元测量技术，力口 以视觉测量的辅助，以一定的时间间隔修正惯性测量的误差，以达到精确跟踪头盔姿态的 目的。
【发明内容】
[0006] 利用惯性测量单元而发展起来的各项技术凭借其自主性、实时性等强大优点，保 证了惯性测量在数字头盔姿态跟踪方面的可行性；视觉测量技术的发展也为数字头盔的姿 态跟踪提供了技术支持，其精度较高的优势在保证姿态跟踪自主性的基础上，弥补惯性测 量单元误差随时间积累的缺点，辅助惯性测量单元实现对数字头盔姿态的实时跟踪；本发 明设计一种视觉辅助惯性测量单元的数字头盔跟踪系统，在头盔顶部选定若干个标志/特 征点进行标定，相机安装在头盔顶部正上方的座舱内，对正下方进行拍照；惯性测量单元安 装在飞行员的头盔上，通过建立一种导航滤波方法实现对头盔姿态的计算。
[0007] 本发明的基本原理如下。
[0008] 一、坐标系统的建立 为方便参数的表示和转换，建立惯性坐标系、相机坐标系、像素坐标系、头盔初始坐标 系、头部转动坐标系等五个坐标系；惯性测量单元的测量值在惯性坐标系下表示；相机测 量到的像点坐标在像素坐标系下表示；头盔初始坐标系以头盔中心为坐标原点，以平视前 方的视线方向为X轴，以向上指向相机的方向为Z轴，y轴与X轴、z轴构成右手坐标系，且 不随头部转动而转动；头部转动坐标系随头部一起转动，以头盔最底部中心(颈部）为坐标 原点，平视前方时的三轴方向与头盔初始坐标系三轴方向一致。这五个坐标系之间，存在一 定的转换关系。
[0009] 二、惯性测量单元模型 惯性测量单元(陀螺仪和加速度计）的测量值为头盔在惯性空间内的转动角速度和非 引力加速度\沿头盔三个转轴方向的分量，而《和a则表示头盔在惯性空间内真实的、不 含噪声的转动角速度和加速度，由于惯性测量单元存在漂移，且在测量的过程中受到随机 噪声的干扰，因此实际测量值和 aj?与真实值ω和a有如下关系，
【权利要求】
1. 本发明权利要求一种视觉辅助惯性测量单元的数字头盔跟踪系统，这套系统的特征 包括：数字头盔跟踪系统在头盔顶部选定若干个标志/特征点进行标定，视觉相机安装在 头盔顶部正上方的座舱内，对正下方的数字头盔进行拍照，惯性测量单元安装在飞行员的 头盔上，惯性测量单元测量头盔在运动中的角速度信息，视觉相机测量头盔顶部已知特征 点的坐标，惯性测量单元不间断地对头盔姿态进行测量，视觉相机每隔一固定的时间间隔 对视野内出现的头盔顶部特征点进行拍照，经过特征提取、匹配后，获得特征点坐标值，对 此时的惯性测量误差予以修正。
2. 本发明权利要求一种惯性测量单元和视觉测量的模型建立方法 惯性测量单元(陀螺仪和加速度计）的测量值为头盔在惯性空间内的转动角速度 和非引力加速度\沿头盔三个转轴方向的分量，而m和》则表示头盔在惯性空间内真 实的、不含噪声的转动角速度和加速度，由于惯性测量单元存在漂移，且在测量的过程中受 到随机噪声的干扰，因此实际测量值和&为
其中，为头部转动坐标系$相对于头盔初始坐标系G的姿态四元数对应的旋转矩 阵，g"5为重力加速度，\和na分别为陀螺仪和加速度计的零均值测量高斯白噪声，1^和 分别为陀螺仪和加速度计的漂移，头盔顶部的特征点在头部转动坐标系S中的三维坐 标Pf是固定且已知的，由此可以得到相机到特征点的位置矢量Pf在相机坐标系下的分量 表完
其中，P?为相机在头盔初始坐标系G下的位置矢量，Pi为头部转动坐标系S在头盔 初始坐标系G下的位置矢量，为相机坐标系e相对于头部转动坐标系j的旋转四元数 对应的旋转矩阵，利用相机的成像透射原理以及特征点坐标在像素坐标系下的投影关系， 可得到测量坐标矢量z
z为头盔顶部特征点4投影到像素坐标系中的二维坐标，为特征检测过程中产生的 误差。
3. 本发明权利要求一种头盔姿态运动表示方法 利用姿态四元数来描述头盔的姿态，并满足归一化的约束条件，则姿态运动学方程表 示为
始坐标系G的转动角速度(在S系下的分量)，可由惯性测量单元的测量值求得；=[分〇 ft分2分3]为头部转动坐标系S相对于头盗初始坐标系G的姿态四兀数，分〇为 的标量部分，+ 夬为？js的矢量部分，F为5的反对称阵，利用四级四阶龙格 库塔法求解式（4)或（5)姿态运动学方程，即可得到头部转动坐标系S相对头盔初始坐标 系G的姿态四元数，头部转动坐标系S相对头盔初始坐标系G的位置和速度均不变，将系 统的微分方程组整理为
4.本发明权利要求一种卡尔曼滤波算法以实现对姿态的计算 估计过程中利用了如下信息：系统方程、观测方程、白噪声激励的统计特性、量测误差 的统计特性，本发明建立的离散型卡尔曼滤波算法如下： 设％+1时刻的估计状态受系统噪声％驱动，状态方程 描述了噪声的驱动机理，
的量测与4+r满足线性关系，即观测方程
其中，为％时刻的估计状态，为％时刻至时刻的一步转移阵，为系统噪 声驱动阵，％为系统噪声，为量测阵，G为量测噪声
其中，为系统噪声方差阵，A为量测噪声方差阵 如果被估计状态1^!满足式（7)，量测量4+1满足式（8)，系统噪声％和量测噪声^满 足式（9)，系统噪声方差阵a非负定，量测噪声方差阵為.正定，先+1时刻的量测为&+1，则 义+1的估计ffc+1按下述方程求解：
式（10)即为离散型卡尔曼滤波基本方程，只要给定初值i〇和马，根据fc + l时刻的量 测4+1，就可递推计算得到i + 1时刻的状态估计。
5.本发明权利要求一种扩展卡尔曼滤波算法以实现对姿态的计算 权利要求4给出的卡尔曼滤波只能用于线性随机系统的状态估计，权利要求5要求的 扩展卡尔曼滤波是对非线性系统方程进行线性化近似后，再利用普通卡尔曼滤波方法进行 滤波估计的算法，在使用离散型卡尔曼滤波基本方程之前，对系统方程和量测方程做离散 化和线性化处理，考虑一般化的连续非线性系统，线性化的状态方程可写为
| 声 设定系统的状态向量为x，其中包含头部转动坐标系S相对头盔初始坐标系G的姿态 四元数的矢量部分ft 和陀螺仪的漂移状态向量为
由式（5)和（6)可将状态方程可表示为
所以，状态转移矩阵可由式（14)对状态向量x求雅可比矩阵得到，
其中，Qs(<)为陀螺仪的零均值测量高斯白噪声的方差阵，为陀螺仪的零均值驱 动高斯白噪声的方差阵；系统噪声输入矩阵为
将系统离散化后得到状态转移矩阵，
考虑到滤波周期T较短，可看作常阵，且根据扩展卡尔曼滤波的线性化方法，舍 去二阶及以上项，可得到状态转移矩阵为
其中，7 = ^4-^，系统噪声输入阵为
系统噪声方差阵为
其中，=5[w(l)w(if],当滤波周期^较短时，G〇G⑷= Gfc 由此 系统噪声方差阵可简化为
量测阵H可由量测方程对状态向量求雅可比矩阵得到，
式中
当有*个已知位置特征点时，量测阵H可以表示成以下形式，
在估计过程中，利用t时刻的估计值,并根据t+1时刻的量测量Zw,即可递推得 至ij先+1时刻的状态估计i^+1。
【文档编号】A42B3/04GK104280022SQ201310295855
【公开日】2015年1月14日 申请日期:2013年7月13日 优先权日:2013年7月13日
【发明者】张泽旭, 刘蕾, 郑博 申请人:哈尔滨点石仿真科技有限公司