三轴矢量数据融合的视觉跟踪惯性稳定系统设计

江潇，何波贤，郑铁山，岳明

（92724部队，青岛266108）

引 言

在三轴视觉跟踪惯性稳定控制系统中，保持视觉稳定的关键技术在于隔离因外界因素引起的抖动。惯性稳定系统的核心是检测角运动敏感器件三轴陀螺，对转台姿态扰动十分敏感，一旦发生扰动立刻反馈到控制系统[1]。在实际系统的测量中，由于三轴陀螺本身存在零点漂移误差以及随机误差，采用多传感器信息融合技术处理惯性稳定姿态信号，把三轴陀螺、加速度计、磁极三个不相干的三维矢量进行互补融合，致力提高惯性稳定系统测量精度[2]。

1 系统框架设计

三轴视觉跟踪稳定平台的姿态测量系统主要由三轴陀螺、三轴加速度计、三轴磁计、温度传感器和STM32系列微处理器组成。MPU-6050集成了三轴陀螺、三轴加速度计、温度传感器，HMC5883是三轴磁阻传感器。姿态测量系统框架结构如图1所示。

2 基于四元数卡尔曼滤波器设计

图1 姿态测量系统框架结构

三轴陀螺的主要测量载体是三维角速度，三轴加速度计的主要测量载体三维重力加速度，三轴磁计用来测量三维地磁强度。其中三轴加速计和三轴磁阻传感器提供了两个不相关的三维矢量，并且这些矢量可以作为观测矩阵来校正因为三轴陀螺引起的误差[3]。四元数卡尔曼滤波器结合这三种传感器的特点，进行数据互补融合，以三轴陀螺输出的角速度为状态更新，以重力加速度和磁场强度作为观测方程的更新，估计出精度高、稳定性强的姿态角[4]。

2.1 状态量随时间更新矩阵

基于四元数的卡尔曼滤波器是以四元数为状态量，所以设状态参数为：

卡尔曼滤波器状态方程是基于三轴陀螺四元数输出进行设计，建立四元数的状态方程为四元数的微分方程：

采用基于定时采样的毕卡三阶逼近法将此四元数微分方程展开，已知四元数的毕卡三阶逼近法形式为：

Δθy、Δθz同理可求，其中：Δθx、Δθy、Δθz为 三 轴 陀 螺 在[k，k＋1]采样时间内的角增量。

基于四元数的微分方程矩阵形式为：

将式（3）代入四元数的微分方程矩阵，得到状态变量随时间更新的矩阵为：

2.2 观测方程更新

把三轴加速度计以及三轴磁计作为卡尔曼滤波器观测方程的输入，那么观测方程的更新就是基于加速度计和磁计数据的更新，也就是把三轴加速度计和三轴磁计的本体坐标系上的重力加速度和地磁场的参考矢量，通过四元数转移矩阵转移到当前坐标系上进行处理。其中四元数转移矩阵为[5]：

假设三维参考向量V＝[VxVyVz]转移到旋转当前坐标系中可由观测方程表示：

由于上式是非线性方程，所以对h进行求取雅可比矩阵，也就是h对X偏导的雅可比矩阵：

当重力加速度（相对加速度静止水平放置时）观测更新时有：

当磁场观测（相对磁阻传感器静止水平放置并航向指向正北时）更新时有：

把式（8）、式（9）代入式（7），得到基于三轴加速度计以及三轴磁计的观测方程：

式中：α为磁倾角。

2.3 定姿算法

观测方程进行了离散线性化处理后，得到了状态更新的方程，并且系统的过程噪音Wk-1和系统的量测噪声Vk满足上述的要求，则卡尔曼滤波方程满足以下求解过程。

预测状态：

状态估计：

由于式（12）并不是离散的，必须采用以下离散连续的公式进行代替：

用陀螺或者加速度计以及磁计来计算姿态角，陀螺的特性就是动态性能比较好，但是较长时间内存在零点漂移，使误差结果放大；而加速度计以及磁计静态性能较好，但是运动时就会引起较大的噪音，既不能全依靠陀螺计算的值，也不可以完全依靠加速度计和磁计计算出来的数值，只能利用传感器的互补信息进行相互补偿，用陀螺计算出来的姿态角，以及加速度计和磁计计算出来的姿态角进行作差，所以f（X（k））就是加速计和磁计计算出来的角度，也就是三轴加速度计和磁计测量出来的数值。HkX（k）就是陀螺在上一次误差四元数计算出来的欧拉角。

滤波增益：

预测误差方差阵：

估计误差协方差：

卡尔曼滤波是一种递推的过程，该算法充分利用陀螺输出的角速度信号以及加速度计、磁计信号互补特性[6]。当运动时，以陀螺的动态特性为主；静止时，以加速度计、磁计的静态特性为主。权衡参数主要由增益矩阵决定，得到对状态输出的最优估计，增益矩阵的特性主要依据当前更新估计以及前次状态系统估计的统计特性得到，减小了系统计算量。

3 程序设计

基于四元数卡尔曼滤波器设计的三轴视觉跟踪惯性稳定系统程序流程。主要的处理步骤如下：

①系统的初始化主要包括主控STM32、传感器MPU6050、HMC5883初始化。

②首先在三轴惯性系统保持静止时，通过采集3 200组数据进行零位自动标定，同时完成卡尔曼滤波器误差协方差矩阵P、噪声协方差矩阵Q、方差强度矩阵R的初始化；然后通过采集三轴加速度以及三轴磁场强度计算出系统的初始姿态；再把东—北—天坐标系下的磁场强度初始化；最后，把欧拉角形式的初始化姿态转化成四元数形式，实现四元数卡尔曼滤波器的初始化。

③采集三轴陀螺10个数据，取平均值，然后对四元数的卡尔曼滤波器随时间预测更新；然后采集三轴加速度以及三轴磁场强度的数值，对卡尔曼滤波器的观测矩阵进行更新。

④当加速度过大时，不是进行数据融合，而是进行误差协方差矩阵P更新；当判断加速度在合适的范围时，进行卡尔曼数据融合。

⑤完成卡尔曼更新后，通过UD分解确保误差协方差P不发散，最后把四元数转换成欧拉角形式。

程序流程如图2所示。

图2 程序流程图

4 对比实验

为了检测这种算法输出的姿态精度，利用一款工业级的微垂直陀螺系统进行对比实验。微垂直陀螺系统是一款高性能陀螺系统，基于三轴陀螺仪和三轴加速度计设计的，采用自适应卡尔曼数据融合算法，以100 Hz更新速率输出载体的惯性运动信息（三轴角速度、三轴加速度）、最优姿态角（横滚角和俯仰角）和温度等参数，并对传感器的安装误差、轴间正交误差和温度误差进行有效补偿。

实验主要采用三轴摇摆台进行测试，首先把陀螺系统安装在三轴摇摆台的内框上，如图3所示，然后对转台进行转动，通过转台上的1024光栅码盘计算出转台转过的角度并传递到上位机，上位机同时采集陀螺和码盘的数据进行对比。

图3 三轴摇摆台

本实验主要采用俯仰角的数据进行对比。图4为微垂直陀螺系统输出的俯仰角以及码盘输出的角度，它的优点就是跟踪性能比较好，但是波峰相差比较大，最大的幅度差在20°左右，主要原因就是自适应卡尔曼滤波器的参数没有进行优化，把波形的高通部分当作噪音来处理，所以该类型的陀螺系统性能并不是很好，存在很多改进的可能。

图4 微垂直陀螺数据

由图5可知，基于四元数卡尔曼滤波器设计的三轴陀螺系统的性能更具有优势，角度误差在±2°范围，还增加了三轴磁计融合陀螺方位角信息，可满足三轴跟踪转台全方位测量要求。

结 语

采用四元数扩展卡尔曼滤波算法把三轴陀螺、加速度、磁阻传感器的误差特性进行相互拟合、相互补偿，不仅提高了姿态解算的精度，而且更新速率快、实时性强。目前，该测试系统已用于视觉目标跟踪系统进行测试，满足了三轴视觉跟踪稳定系统对姿态控制测量的需求。

图5 陀螺数据

[1]辛哲奎，方勇纯，张雪波.小型无人机地面目标跟踪系统机载云台自适应跟踪控制[J].控制理论与应用，2010，27（8）：1002-1003.

[2]吕漫丽，孙灵芳.多传感器信息融合技术[J].自动化技术与应用，2008，2（2）：79-80.

[3]秦永元.惯性导航[M].2版.北京：科学出版社，2014.

[4]秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航原理[M].2版.西安：西北工业大学出版社，2012.

[5]徐梓皓，周召发，孙立江.基于卡尔曼滤波器的姿态角测量系统设计[J].电子测量技术，2014，37（1）：1-2.

[6]RuanJianS-guo，LinJiaS-jun.Analysis on stability of digital protectional gorithms[J].Proceedings of 2005 IEEE International conference on information acquisition，2005：313-317.