引言

航向姿态解算装置能够测量航向角、俯仰角和横滚角等姿态信息。高精度惯性导航传感器件，如机械陀螺仪及光纤陀螺仪等，造价较高，不利于在实际应用中推广。MEMS（Micro Electro Mechanical System，微机电系统）技术是近年来发展的新兴技术，它将机械装置与集成电路集成于一个芯片中，因此，与传统的机电系统相比，具有体积小、功耗低和成本低的优点。

不足之处是MEMS角速率陀螺仪存在零点漂移和误差，在惯性导航解算中会产生累积误差，精度难以满足实际应用的要求。磁场计和加速度计能分别测量出重力地磁场和加速度两个不相关的三维矢量，可应用于装置的姿态观测矢量来校准陀螺仪。

基于四元数的扩展卡尔曼滤波可以结合这几种传感器的特点，以陀螺仪测量的角速率作为预测更新，以重力加速度和磁场观测更新，得到更高精度的航向姿态，可应用于实际跳伞人员训练过程中的姿态等信息的实时监测。

1 硬件结构

MEMS器件的AHRS（Attitude and Heading Reference System）硬件基本组成如下：三轴角速度陀螺仪和三轴加速度计MPU6050、三轴磁阻传感器HMC5883L、DSP处理器TMS320C6713和STM32系列处理器STM32F103T8。航向姿态参考系统的硬件结构框图如图1所示。

图1 系统结构图

1.1 6自由度传感器MPU6050

本装置采用组合了16位A/D转换器的三轴陀螺仪和三轴加速度计的MPU6050。MPU6050为整合性6轴运动处理组件，相较于多组件方案，有效减少组合陀螺仪与加速器的轴间误差问题，减少了大量的包装空间。MPU6050的角速度全格感测范围为±250°/s、±500°/s、±1000°/s与±2000°/s，可准确追踪快速与慢速动作，并且用户可用程序控制的加速器全格感测范围为±2g、±4g ±8g与±16g。产品传输可通过最高至400 kHz的I2C总线。

1.2 磁阻传感器HMC5883L

HMC5883L包括最先进的高分辨率HMC118X系列磁阻传感器，并附带霍尼韦尔专利的集成电路，包括放大器、自动消磁驱动器、偏差校准、能使罗盘精度控制在1°~2°的12位模数转换器、简易的I2C系列总线接口。HMC5883L采用霍尼韦尔各向异性磁阻(AMR)技术。

这些各向异性传感器具有在轴向高灵敏度和线性高精度的特点。传感器具有的对正交轴的低灵敏度的固相结构能用于测量地球磁场的方向和大小。霍尼韦尔的磁传感器在低磁场传感器行业中是灵敏度极高和可靠性较好的传感器。

1.3 DSP处理器的选择

本文是在单片机处理能力不能满足姿态解算的速度要求的情况下，选择具有强大数据处理器能力的DSP芯片作为另一处理器进行数据的解算。因此，选用一个具有高性能运算能力的处理芯片至关重要。

TMS320C6713是TI公司推出的一种高速的浮点数字信号处理芯片，它每周期可以执行8条32位指令，支持32/64位数据，具有最高225 MHz的运行速度和1 800 MIPS（百万次定点运算/s）或者1 350 MIPS（百万次浮点运算/s）的处理能力，16位EHPI接口可以连接各种处理器。TMS320C6713的以上性能技术指标达到了姿态解算速度和精度两方面的要求。

1.4 STM32处理器的选择

主控制芯片选用了STM32F103T8处理器。该处理器以ARM 32位CortexM3为内核，具有64 KB的闪存存储器。20 KB的运行内存、7通道的DMA、7个定时器，结合外部8 MHz晶振和STM32F103T8内部的PLL，控制器可以运行在72 MHz的主频上。对于姿态解算这种需要大量数学运算的程序，更快的处理速度可以完成更多的解算优化。

传感器通过I2C总线接口与STM32F103T8连接，同时传感器的数据中断引脚与STM32F103T8的I/O相连。使得传感器在完成ADC转换后，STM32F103T8在第一时间读取最新的数据，快速响应姿态的变化。这样的连接使得控制器拥有最大的主动权，可最快地获取各传感器的状态和转换结果。

以上芯片通信控制关系如图2所示。

图2 通信控制关系

2 四维扩展卡尔曼滤波算法

扩展卡尔曼滤波算法是一套由计算机实现的实时递推算法，所处理的对象是随机信号，利用系统噪声和观测噪声的统计特性，以系统的观测量作为滤波器的输入，以所要求的估计值（系统状态变量）作为滤波器的输出，滤波器的输入和输出由时间更新和观测更新算法联系在一起，根据系统的状态方程和观测方程估计出所需要处理的信号。

AHRS扩展卡尔曼滤波算法的状态变量采用思维四元数，与采用欧拉角相比，避免了采用欧拉角计算时涉及的大量三角函数运算，保证了更新速率和实时性，同时不存在采用欧拉角运算出现的奇异性。欧拉角与四元数的转换关系式如下所示：

四元数微分方程如下所示：

四元数姿态矩阵微分方程只要分解4个微分方程，比方向余弦姿态矩阵微分方程减少了大量的运算，便于微处理器的编程实现，提高整体的运行速度和精度。

2.1 时间更新

系统的状态方程如下所示：

其中,状态变量为四元数X=q0 q1 q2 q3,Wk-1为四维过程噪声。矩阵A可以根据陀螺仪测得的三轴角速率ωX ωY ωZT得到，如下所示：

其中，Δt为两次时间预测更新所经过的时间。状态变量的时间更新如下所示：

协方差矩阵Pk预测如下所示：其中，Q为四维过程激励噪声协方差。

2.2 时间更新

AHRS的观测更新是通过本体系坐标上的重力加速度和地磁场的参考矢量旋转至导航坐标系上，再与加速度和磁场传感器比较，得到观测变量的残余。由本体系转换至导航系的转移矩阵由四元数可以表示如下：

三维参考向量v转移至导航系中可由观测方程表示:

当重力加速度观测更新时参考向量v等于重力加速度参考矢量 ，可设置为当平台精致水平放置时，加速度计测量得到的三维矢量为：

当磁场观测更新是v等于磁场参考矢量,可设置为当平台水平放置且航向指向正北时，磁阻计测量得到的三维矢量为：

卡尔曼增益矩阵Kk如下所示：

其中R阵为三维观测噪声协方差矩阵；H是h对X求偏导的雅克比矩阵。

观测更新：

当重力加速度观测更新得到Zk为加速度时，传感器测量得到的三维矢量Zk=XYZT。当磁场观测更新时Zk为磁阻传感器，测量得到的三维矢量Zk=mXmYmZT。协方差更新：

3 软件设计

AHRS的软件设计主要分为：

① 初始化。包括处理器初始化设置和传感器初始化，后者包括初始化传感器的更新速率、量程等。

② 初始化基于四元数的卡尔曼滤波的相关矩阵。根据传感器的特点设置激励噪声协方差矩阵Q，设为对角元素为0、1的思维对角方阵。

③ 若成功读取陀螺仪数据，则进行基于四元数的卡尔曼滤波的时间更新。

④ 采集加速度传感器和磁阻传感器的数据。若读取成功则进行观测更新，加速度观测更新与磁场观测更新算法差别在于观测方差R，可根据两种传感器的置信度设置相应的值。航向姿态参考系的程序流程如图3所示。

图3 软件流程图

4 上位机检测实验

4.1 动态检测效果

为了便于对系统的性能进行测试，利用VS2008环境，采用C#语言编写上位机软件，分别以动态波形的形式和数据文本的方式进行记录。

根据其接收数据的协议要求，将解算后的姿态数据转化为符合上位机的格式，通过STM32F103T8的串口2发送至上位机。测试软件选择相应的串口采集数据，以图表及3D模型的形式展现所检测得到的姿态信息。

通信协议格式如表1所列。

实际测试效果图如图4、图5所示。

图4 上位机测试效果图

图5 上位机软件测试3D效果图

表1 通信协议格式

测试过程中，上位机检测效果良好，尤其在3D模型视图中，该模型可以很好地跟踪检测平台的三维动作。表明该姿态解算系统可以快速准确地检测平台的航向角、俯仰角和横滚角信息。

注意，在剧烈的大幅度动作后，若检测平台突然处于静止状态，由于卡尔曼滤波估计算法的影响，需要短暂时间才能解算输出真实的三维姿态信息。

4.2 静态检测效果

其中，α、β、γ分别为由转台读到的航向角、俯仰角和横滚角；α′、β′、γ′分别为由本装置检测得到的航向角、俯仰角和横滚角。静态检测结果如表2所列。

表2 静态检测结果

结语

STM32F103T8单片机具备强大的接口控制功能，并结合TMS320C6713强大的数据处理能力，基于四元数的扩展卡尔曼滤波算法的AHRS具有更新速率高、实时性好、价格低廉等特点，可广泛应用于无人机的姿态检测、跳伞训练人员的姿态监测，以及手机、平板电脑等消费类电子产品，甚至能够满足一些机器人对姿态控制的精确测量要求。