基于自适应滤波的头部姿态跟踪方法研究(通讯)

基于自适应滤波的头部姿态跟踪方法研究(通讯)

                刘世良^1，2，  仇振安³，  丁全心²，  张劲锋²

    （1．光电控制技术重点实验室，河南洛阳471000；2．中国航空工业集团公司洛阳电光设备研究所，河南洛阳471000；3．陆航驻洛阳地区军事代表机构，河南洛阳471000）

摘要：针对视觉式头部定位系统存在着噪声统计特性掌握不充分的问题，设计了一种新的基于自适应滤波的头部姿态跟踪方法，其综合了视觉测量算法和自适应滤波算法。在获取n对2D/3D（图像点／空间点）匹配对的基础上，先利用正交迭代算法( 01)解算头部姿态，然后应用自适应滤波器估计精度更高的头部姿态，最后针对带有时变噪声且噪声统计特性掌握不充分的测量值，应用头部姿态跟踪方法进行了仿真测试，结果显示该方法的测量精度有很大提高，并且验证了所设计的头部姿态跟踪方法是合理且有效的。

关键词：头部定位系统；姿态跟踪；正交迭代算法；自适应滤波

中图分类号：V243.6   文章编号：1671  - 637X( 2016) 04 - 0033 - 04

0引言

    头部姿态跟踪就是跟踪测量头部相对于世界坐标系的3个姿态角度。头部姿态跟踪是虚拟现实和增强现实的核心技术之一，目前广泛应用的头部姿态跟踪方式有电磁式、光电式和计算机视觉式。与电磁式、光电式相比，视觉式头部定位技术在解决头部姿态跟踪定位时，干扰小、测量误差小、运算速度快、稳定可靠，因而得到了广泛的应用。基于视觉式的头部姿态定位技术根据传感器（摄像机）安装位置分为外置传感器视觉式和盔载传感器视觉式两大类。两者基本原理相似，区别在于特征点的选取及其相应的图像处理部分不同。基于视觉式的头部姿态定位技术是根据空间中n个控制点的世界坐标与其在单幅图像中的坐标之间的对应关系，通过求解PnP问题完成头部姿态的跟踪、定位。求解PnP问题的方法可分为线性算法和非线性迭代算法两大类。经典的线性算法有直接线性变换法(DLT)和EPnP算法，虽然两者计算速度很快，但是易受噪声干扰且精度不高；非线性迭代算法通过对某种目标函数进行最优化迭代，可以获取相对精度很高的数值解。经典的非线性迭代算法有Leven-berg-Marquardt提出的最大似然估计(LM)算法和正交迭代( OI)算法。OI算法精度略次于LM算法，但是速度却远快于LM算法。通过PNP问题的求解可以得到各个时刻头部相对于飞机坐标系的头部姿态角。

    图像处理环节引入的图像噪声最终会带人PNP算法解算的头部姿态角误差中，而且头部姿态角误差会随着测量角度和测量环境的变化而变化。目前对这些综合误差因素造成的头部姿态角测量值的噪声统计特性掌握还不充分。而且目前视觉式头部定位技术只能求解当前时刻的头部姿态角，不能预测和跟踪下一时刻的头部姿态，对于拍摄死角引起的头部姿态测量丢失问题无能为力。针对视觉测量中存在的非线性和离散化等特点，通过建立合适的运动模型，应用扩展卡尔曼滤波( EKF)对头部运动的姿态进行估计，而对于测量噪声和系统噪声统计特性掌握不充分和时变性，EKF就不再适用了，滤波效果明显下降。本文在EKF的基础上引入自适应滤波进行最优化估计，获取了更高精度的头部姿态角。

1方法描述

    本文跳过相机标定、特征识别和特征匹配等前期图像处理工作，直接利用前期图像处理的结果，即在获取n对2D/3D（图像点／空间点）坐标点匹配对——目标特征点在二维图像坐标系和三维世界坐标系中的坐标点对的基础上进行算法研究。图1所示为2 D/3D坐标点匹配对在相机坐标系的示意图。

为摄像机内参数；R为摄像机相对于世界坐标系的旋转矩阵；T为平移向量。如图2所示，在获取2D/3D点对后，把匹配点对代入OI算法中解算出3个头部姿态角：方位角、俯仰角和横滚角，最后由自适应滤波算法估计出最优头部姿态。

2  头部姿态求解算法

    1998年LU提出的全局收敛的OI算法以目标空间共线性误差为目标函数，利用最小二乘原理进行迭代优化，可以得到高精度的数值解且具有抗噪性强和实时性

则有

3基于自适应滤波的头部姿态估计

    头部运动一般是非线性的，其运动轨迹很难用具体的运动函数来表达。由于头部运动具有惯性和连续性，所以头部当前运动状态和过去运动状态是密切相关的。假设头部运动可以模拟为匀速转动的运动模型来逼近实际的头部机动运动过程，并且引入系统状态噪声W_k来补偿匀速运动模型与头部实际机动运动中带有的无规则加速度的误差，它为零均值的高斯噪声，且其方差阵为con（W_k）=Q _k。假设V为观测噪声，其反映了图像处理误差和正交迭代算法误差等综合误差因素造成的观测值的偏差。设它为零均值的高斯噪声，且其方差阵为con(V_K)=R _k。系统状态方程和测量方程为

3.1  EKF

    针对头部运动的非线性和OI算法获取的观测值离散化的特点，应用方法比较成熟的EKF来估计头部位姿，其滤波方程为

3.2  自适应滤波算法

    目前对于系统噪声W_k和姿态观测噪声V_K的统计特性掌握不充分，而且随着环境变化，观测噪声是时变噪声。使用传统的EKF进行头部姿态估计和预测，因为不能合适选取QK和RK，将导致估计精度的下降。对于这种情况，引入自适应滤波对系统状态噪声和测量噪声进行统计更新，从而进一步提高头部位姿的估计和预测精度。在K时刻，利用测量值对预测值进行修正后，前K个头部姿态状态信息可以对系统状态噪声和测量噪声统计信息进行更新。引入状态估计差值

着K值的增大，新信息利用率会越来越小。为了使自适应滤波更有效地利用新信息，需要固定新信息的利用效率，更改式(17)为式(18)可使新信息的利用率不随着时间的积累而衰弱，同时过去很久的信息会随时间的更新而衰弱。

    自适应滤波利用估计残差来对统计不充分且时变的系统状态噪声和测量噪声统计参数Q_K和R_K进行实时修正更新，以提高头部姿态的估计精度。自适应滤波只需在EKF的基础上加入噪声估计器：式(16)、式(18)、式(19)和式(20)即可。

4仿真及分析

系统的仿真流程如图3所示。

利用Matlab2012a对上述算法进行仿真验证。在给出摄像机内参数K、目标特征点在世界坐标系中的坐标和运动模型的前提下，先根据运动模型推算出头部姿态角并转化成头部姿态参数R和T，然后由式(1)计算出目标特征点在图像坐标系中理论的2D坐标，最后在2D坐标中添加相应的干扰噪声进行仿真实验。

4.1  PnP算法仿真比较

 由于系统在摄像机标定、图像畸变校正、特征中心坐标提取等图像处理环节会引入随机误差，根据PnP问题的特点，这些随机误差最终会累积在图像特征点的2D坐标中，因此在2D图像特征点坐标中添加均值为0、均方差以步长0.1像素，在0.1～2像素的范围内增长的随机高斯图像噪声；随机给定旋转矩阵和平移向量，每个步长进行2000次随机实验。以3个头部姿态角均方差为例对DLT算法、EPnP算法和OI算法进行仿真测试，仿真结果如图4所示。

    从仿真结果来看，3种算法解算的头部姿态角误差均方差都随着图像特征点噪声的增加而线性增大。而且非线性的OI算法性能优于线性的DLT算法和EPnP算法。由此可以看出，非线性的OI算法对于解算头部姿态角来说，性能更好，可以为后面的自适应头部姿态估计提供更加精确的测量值。

4.2滤波算法仿真比较

    在4.1节的仿真基础上，利用OI算法解算得到每一时刻的头部姿态测量值。由于系统在图像处理环节和OJ算法解算头部姿态环节中会引入误差，目前对这些综合误差的统计并不充分，而且随着环境因素的改变，还会引入时变的测量噪声，所以在仿真时应在图像坐标中加入适当的时变噪声。假没头部运动的初始位置为方位角45⁰，俯仰角30⁰，横滚角5⁰，初始角速度分别为0. 015(⁰)/s，0.01(⁰)/s，0.009(⁰)/s，测量周期设为T= 20 ms，相关时间30 min，分段添加时变噪声。仿真结果如图5所示。

  从图5可以明显看出，经过2种滤波方法处理后，头部姿态角的精度都有所提高，3个姿态角的误差范围与校正前相比明显减小。EKF虽然能够改善头部姿态角的误差范围，但是对于未充分统计的噪声，其估计效果明显不如自适应滤波；对于加入时变噪声的头部姿态角测量值，EKF的滤波精度随着测量精度的变差而降低，而自适应滤波却能很好地适应变化了的噪声。滤波初期的前50 s内，自适应滤波的滤波精度和EKF相当，随着对噪声方差阵Q_K和R_K的统计更新，自适应滤波开始优化，50 s以后能够完全适应时变的噪声并进行更精确的滤波。自适应滤波的滤波精度可以达到0.1⁰以内，对于加入时变噪声的观测值能够快速稳定地跟踪。

5结论

 本文提出的基于自适应滤波的头部姿态跟踪方法，首次针对视觉式的头部姿态定位技术中带有时变且未充分统计噪声的测量值进行了自适应滤波估计研究。经仿真验证，该算法合理有效并且获得了比EKF更高的精度。本文对提高传统的计算机视觉头部姿态定位方法的精度，提供了一种可行、有效的理论方案依据。对于本文提出的自适应滤波算法需要较长时间才能达到稳态的不足，在未来的研究中将根据实际应用需要，逐步对自适应滤波算法进行改进，在保证实时性的前提下缩短达到稳态所需要的时间，并提高跟踪的精度。