基于微惯性器件的头部运动综合测量方法研究
2018-04-19万航雁张求知张伊慧张劲峰刘世良张宝尚
万航雁， 张求知， 张伊慧， 张劲峰， 刘世良， 张宝尚， 陈 君
(1.光电控制技术重点实验室，河南 洛阳 471000; 2.中国航空工业集团公司洛阳电光设备研究所，河南 洛阳 471000)
0 引言
空间姿态测量技术是针对空间两个相对运动物体，将其中一个作为观察者，另一个作为目标，测量两者在运动过程中的相对位姿关系的过程。应用于航空领域的头盔空间姿态测量技术，可以在飞机运动、头部摆动的情况下，准确获得头部相对于飞机的姿态信息。主要可以分为以下几种[1]：机械连杆法是最早的实现头盔位姿测量的方法，通过在盔体上固连一套机械连杆系统，在理论上实现了头盔位姿的测量，但是在一定程度上限制了飞行员的头部运动，影响紧急逃生安全；电磁场法是通过计算固连在盔体上接收信号的三轴正交的环形天线内的磁场变化得到头盔位姿变化，这种方法容易受到座舱周围金属的干扰[2]，但是测量角度大，不易受到阳光干扰；此外还有视觉测量方法与惯性测量方法等。这些方法各有其优缺点，单一的测量方法往往会受到器件本身和算法的限制。为此，考虑利用数据融合技术[3-4]的同时使用惯性测量与视觉测量方法对头盔姿态进行跟踪与分析处理，期望得到更优的性能。
1 综合测量方法
基于MIMU的头盔空间位姿综合测量方法流程如图1所示。MIMU固定在头盔盔体上，输出头盔坐标系(OXvYvZv)相对于地球惯性坐标系(OXiYiZi)的角速率。飞机上的惯性器件具有较高的精度，输出飞机坐标系(OXbYbZb)相对于地球惯性坐标系的角速率。
图1 系统流程图Fig.1 System flow diagram
1.1 惯性测量系统
(1)
(2)
(3)
式中，Cr，Cθ分别是横滚角、俯仰角对应的旋转矩阵。整理后可得
(4)
利用一阶欧拉积分[5]，可求解头盔相对于飞机的姿态角。
1.2 基于MIMU头盔瞄准线综合测量系统
采用基于MIMU的惯性测量与视觉测量综合的方法，可以在很大程度上弥补单一测量方法的不足。仅依靠MIMU的惯性测量虽然能够提供较高的测量速度和全向的测量范围，但是无法获得位姿的初始值，且MIMU器件存在零漂、误差会随时间积累等原因，导致惯性测量无法作为独立的可信赖系统测量运动平台上头盔的姿态[6-8]。虽然视觉测量方法测量精度较高，但存在测量范围不足、测量短时失效、抗环境干扰能力差等缺点。采用综合的测量方法[9-11]可克服单一测量方法的缺点。
1.2.1零偏估计原理
陀螺仪零偏的存在会使解算结果出现极大的偏差，故需要准确估计零偏并跟踪零偏的变化，来保证姿态解算的精度。为了准确估计陀螺仪零偏，根据姿态解算模型建立状态方程，通过状态扩维的方法利用卡尔曼滤波实时标定MIMU陀螺仪零偏，可以对陀螺仪零偏进行有效的估计。
(5)
(6)
取状态变量X为
(7)
式中：ψ，θ，γ表示头部运动的姿态角；ex，ey，ez表示实时标定的MIMU零偏。由于通过卡尔曼滤波得到的零偏会随时间围绕真值上下抖动，为了得到更精确的零偏，取方差阈值σ,对超过阈值的零偏进行平滑处理。
1.2.2惯性测量和视觉测量的数据融合
采用卡尔曼滤波进行相对姿态的自适应数据融合，通过对MIMU与飞机间相对转动关系的计算，建立相对姿态角变化的数学模型，获得相对姿态角递推函数关系作为状态方程；以视觉测量得到的相对姿态角作为观测信息，建立观测方程。从而可以获得完整的滤波模型，实现围绕头盔、飞机间相对姿态角测量的数据融合，达到抑制惯性测量误差、提高输出精度的目的。
取状态变量(如式(7)所示)，建立滤波模型
(8)
tk+1时刻的最优估计为
(9)
协方差为
P(k+1|k+1)=P(k+1|k)-P(k+1|k)HT(k+1)·[H(k+1)P(k+1|k)HT(k+1)+R(k+1)]-1H(k+1)P(k+1|k)
(10)
卡尔曼滤波增益矩阵为
K(k+1)=P(k+1|k)HT(k+1)[H(k+1)P(k+1)·HT(k+1)+R(k+1)]-1。
(11)
本文算法中，观测噪声由视觉测量系统的噪声决定，系统噪声由陀螺仪角速率随机游走的大小决定。仿真流程如图2所示，可以得到头盔的实时姿态角。
图2 计算流程图Fig.2 Flow chart of calculation
2 系统测试与分析
为验证本文算法，进行系统测试试验。由于受到有关试验条件限制，对综合测量系统的测试进行了简化。
测试环境由STIM300、转台、数据采集和处理计算机组成。测试方法和有关条件如下：
1) 飞机的运动假设为匀速直线运动，假设飞机上安装的惯性系统具有足够高的测量精度，飞机坐标系和转台坐标系一致；
2) 将MIMU固定在转台上，以转台输出的数据作为头盔姿态的理想值；
3) 由头盔姿态的理想值，根据已有的视觉测量系统的统计特性叠加噪声，获得视觉系统的测量结果；
4) 用MIMU的测量值，根据本文算法，进行解算获得头盔测量值并与转台位置比较，获得MIMU的测量结果。
运行惯性测量系统，让转台在各个方向分别转过一定的角度，MIMU与转台同时进行数据采集。将采集得到的数据进行解算处理。方位、俯仰、横滚3个方向上的测试结果如表1所示。
表1 测试结果统计
部分测量结果和误差见图3。
图3 不同测量方式对比Fig.3 The comparison of different pose measurements
图3a是静态解算误差，静态情况下解算误差稳定且误差均在0.35以内。图3b是在动态情况下，因为安装误差，解算误差稍有波动；转台运动过程中，误差出现短时变大的情况，转台运动暂停，误差迅速收敛；结合表1可以看出，总体精度与视觉测量观测的误差0.5相比，仍有提高。图3b～图3d是将视觉测量值、综合解算值与真实值进行对比，可以看出，视觉测量相对姿态角的噪声较大，而融合算法能够更好地跟踪盔体的动态特性，相对姿态解算结果围绕真值上下浮动，浮动幅度较视觉测量有了明显的降低，也未出现惯性测量误差随时间漂移的情况。图3f是3种方式的解算误差对比，与两种单一测量方法相比，综合测量方法误差幅值小、稳定性高。
由于不同的MIMU陀螺仪性能差异，零偏大小、测量精度也各不相同，其中，零偏是影响MIMU精度的主要因素。为验证本文算法对陀螺仪零偏的有效抑制作用，假设陀螺仪三轴零偏初值均为0，在转台运动中，记录陀螺仪零偏数据，如图4所示。
可见，转台运转前，MIMU零偏不为零。因此在滤波初始阶段，零偏瞬时值有较大波动，由于卡尔曼滤波的在线标定作用，零偏值迅速收敛。2 s以后能够完全适应MIMU的运动，在整个运动过程中都保持对MIMU零偏的有效抑制。
图4 MIMU零偏统计Fig.4 Statistics data of the MIMU bias
3 结束语
本文提出了一种基于MIMU的惯性测量与视觉测量融合的运动物体相对运动平台的空间位姿测量方法。通过测量数据的融合，以视觉测量数据作为观测值，对惯性测量数据进行校正。初步测试表明，本文提出的算法综合了两种不同测量方式各自的优点，实现了更高精度、更快速度的测量，为将来的工程应用奠定了理论基础。在初步测试中发现，在观测失效的情况下，解算精度稍有降低。今后的研究工作将集中在MIMU的安装校准、运动平台测量性能对综合测量结果的影响、观测失效情况下如何保证测量精度等方面，希望能够得到稳定的、高性能的综合测量方法。
[1]丁全心. 机载瞄准显示系统[M]. 北京：航空工业出版社,2015.
[2]张求知,刘建业,王立伟.电磁式跟踪器磁场畸变的姿态校正技术研究[J].南京航空航天大学学报，2007，39(6):711-715.
[3]刘延新,刘世良,丁全心,等.基于数据融合的座舱头部姿态跟踪方法研究[J].电光与控制,2016，23(8):65-69.
[4]陈靖,王涌天,刘越,等.适用于户外增强现实系统的混合跟踪定位算法[J].计算机辅助设计与图形学学报,2010，22(2):204-209.
[5]MURPHY-CHUTORIAN E,TRIVEDI M M.Head pose estimation and augmented reality tracking:an integrated driver awareness[J].IEEE Transactions on Intelligent Transportation Systems,2010,11(2):300-311.
[6]罗斌,王涌天,刘越.高精度鲁棒的座舱头部姿态跟踪器研究[J].北京理工大学学报,2011,31(3):367-372.
[7]罗斌，王涌天，刘越.双目光学跟踪器姿态精度的预测和估计研究[J].仪器仪表学报，2010，31(1)：194-200.
[8]LI P，DUAN G R，SONG S M.Design of relative measurement system for docking and separation platform based on MIMU/CCD[C]//Proceedings of the 30th Chinese Control Conference，2011:5063-5067.
[9]罗斌,王涌天,刘越.光学头部姿态跟踪的多传感器数据融合研究[J].自动化学报,2010，36(9)：1239-1249.
[10]孙长库,徐怀远,张宝尚,等.基于IMU与单目视觉融合的姿态测量方法研究[J].天津大学学报：自然科学与工程技术版，2017，50(3)：313-320.
[11]田雪怡,李一兵,李志刚.航迹融合算法在多传感器融合中的应用[J].计算机仿真,2012,29(1):53-56.
