基于DSP和STM32的航姿参考系统设计

北京信息科技大学 信息获取与检测实验室，北京 100101

一、引言

姿态测量能够为导航和控制提供精确的参考信息，近年来随着MEMS惯性传感器的快速发展，民用领域对姿态测量的使用也越来越广泛，如机器人、无人机、船舶导航、无人驾驶、三维游戏以及跳伞训练的监测等领域。而在姿态的测量中，航姿参考系统（Attitude and Heading Reference System, AHRS）便显得尤为重要[1]。

AHRS包括多个轴向传感器，能够为飞行器提供航向、俯仰和横滚信息，这类系统用来为飞行器提供准确可靠的姿态与航行信息。航姿参考系统包括MEMS三轴陀螺仪、加速度计和磁强计。而AHRS与惯性测量单元（Inertial Measurement Unit, IMU）的区别在于，AHRS包含了嵌入式的姿态数据解算单元与航向信息，IMU仅仅提供传感器数据，并不具有提供准确可靠的姿态数据的功能。故航姿参考系统具有重要的研究价值和广泛的应用前景。

低成本的AHRS是综合利用了微处理器、MEMS传感器和各种姿态解算算法，在相对较小的成本、功耗和体积内计算并提供三轴姿态以及校正的三轴加速度、角速率和地磁场等信息。与基于机械、激光、光纤等陀螺的AHRS相比，低成本AHRS的MEMS传感器有更大的正交误差、测量误差或噪声，因此其姿态解算精度更加依赖于多传感器融合算法。

本文依托实验室在研项目——“基于先验约束信息的航姿参考系统滤波算法研究”，在基于DSP和STM32单片机的基础上，采用多传感器数据融合进行了卡尔曼滤波作为航姿解算单元，设计并完成了航姿参考系统的核心部分。实现其小型化、低功耗和低成本的设计要求，并可应用于对精度要求不是很高的民用领域，如工业和商业无人机、可穿戴设备对姿态的实时监测等。

二、硬件结构

基于MEMS技术的传感器和姿态解算技术极大促进了低成本小型化AHRS技术的发展。硬件结构中，在基于STM32为处理器的同时，以DSP处理器为核心，采用低成本、体积小、低功耗和重量轻的MPU6050惯性传感器和HMC588L磁强计构成测量单元，构建航姿参考系统的硬件平台。

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

1、惯性测量单元MPU6050

惯性测量单元MPU6050具有低成本、功耗低、体积小、重量轻等优点，由于MPU6050单元能够以数字输出6轴或9轴的旋转矩阵、四元数、欧拉角格式的融合演算数据，对姿态的解算更为方便，大大简化航姿系统的硬件设计，同时减小安装、调试的难度和工作量。MPU6050一个片上1024 byte的FIFO有助于降低系统功耗，角速度全格感测范围为±250o/s、±500o/s、±1000o/s与±2000o/s，可准确追踪快速与慢速动作，并且用户可程式控制的加速器全格感测范围为±2g、±4g、±8g与 ±16g[2]。

2、磁传感器HMC5883L

霍尼韦尔HMC5883L是一种表面贴装高集成弱磁传感器芯片，带有数字接口，应用于低成本罗盘和磁场检测领域。HMC5883L包括先进的高分辨率HMC118X系列磁阻传感器，附带霍尼韦尔专利集成电路包括放大器、自动消磁驱动器、偏差校准、能使罗盘精度控制在1o～2o的12位ADC。可在2.16V～3.6V低电压工作，超低功耗仅为100μA，此外，还有着简易的I2C系列总线接口。综上所述，我们选用HMC5883L作为测量航姿信息的辅助模块。

3、DSP数字信号处理器TMS320F28335

TMS320F28335是TI公司的一款浮点DSP控制器。与以往定点DSP相比，该器件具有精度高、成本低、功耗小、性能高、外设集成度高、数据以及程序存储量大，A/D转换更精确快速等优点。

TMS320F28335具有150MHz的高速处理能力，具备32位浮点处理单元，6个DMA通道支持ADC、McBSP和EMIF，多达18路的PWM输出，其中6路为TI特有的更高精度的PWM输出（HRPWM），12位16通道ADC。得益于其浮点运算单元，用户可快速编写控制算法而无需在处理小数操作上耗费过多的时间和精力，与前代DSP相比，平均性能提高50%，并与定点C28x控制器软件兼容，从而简化软件开发，缩短开发周期，降低开发成本。并且，TMS320F28335具有低功耗和省电模式，支持IDLE（空闲）、STANDBY（待机）、HALT（暂停）模式。以上优点，满足了姿态解算速度、较高精度和低成本的要求。

4、STM32处理器的选择

主控制芯片STM32F系列属于中低端的32位ARM微控制器，该系列芯片是意法半导体（ST）公司出品，其内核是Cortex-M3。该系列芯片按片内Flash的大小可分为三大类：小容量（16K和32K）、中容量（64K和128K）、大容量（256K、384K和512K）。我们这里选用了STM32F103T8处理器，该处理器以ARM32位Cortex-M3为内核，具有64kb的闪存存储器，20kb的运行内存，有着睡眠、停机和待机三种省电模式。对于姿态解算更加快速便捷，从而能可以完成更多的解算优化。

综上所述，选用了最为合理的器件来构建航姿参考系统的硬件平台，从而使系统更加低功耗、小型化，其电路连接设计如图2所示。

三、基于四元数的姿态解算法

姿态解算算法是捷联惯导系统算法的核心，通过陀螺仪和加速度计的测量值实时地计算姿态矩阵，从而可以根据姿态矩阵获得运动载体的姿态和航向信息。目前常用的姿态解算方法有欧拉角法、方向余弦法、四元数法和等效旋转矢量法，本文对四元数法加以解释[3-4]。

1、惯导坐标系

航姿系统为了得到比较精准的位置信息，需要提供载体相对于某一坐标系的位置、姿态信息和速度等信息。载体的姿态往往是利用载体坐标系和地理坐标系的位置关系来反映，惯性导航通常用航向角ψ、俯仰角θ、横滚角γ来表示无人机的三个姿态角，如图3所示。

航向角ψ：航向角定义为飞行器绕Z轴转动的角度，定义绕Z轴按右手定则转动为正方向，定义域为-180º～+180º；

俯仰角θ：俯仰角定义为飞行器绕X轴旋转的角度，定义绕X轴按右手定则转动为正方向，定义域为 -90º ～ +90º；

横滚角γ：横滚角定义为飞行器绕Y轴旋转的角度，定义绕Y轴按右手定则转动为正方向，定义域为 -180º ～ +180º。

2、基于四元数的姿态解算算法

四元数是由四个元构成的数，其形式为：

其中，q0、q1、q2、q3是实数；i、j、k表示 3个虚数单位。

四元数可用于描述刚体相对于某个坐标系的定点转动，标量部分表示为转角一半的余弦值，矢量部分保存旋转顺轴、旋转方向和转动角度信息。可表示为：

其中，uR—表示旋转顺轴和旋转方向；θ—表示转动的角度。

当只关心载体坐标系相对导航坐标系的角位置关系时，可认为载体坐标系是由导航坐标系一次等效旋转形成，并无中间多次基本旋转，而四元数包含了等效旋转的全部信息，因此可用来描述刚体的定点转动。

四元数可确定出载体坐标系系到导航坐标系的坐标变换矩阵：

设量测噪声也为可加性噪声，其统计特性为Vk～N(0,Rk)，即量测方程为：

求解的姿态四元数代入到坐标变换矩阵的四元数表达式中，即可解算出姿态角：

四、软件设计

TMS320F28335处理器获得原始传感器数据后，首先对初始位置的航姿信息进行姿态解算。根据得到的姿态四元数的值，利用卡尔曼滤波进行数据融合，计算出最终姿态四元数并反推出姿态角。并且由于卡尔曼滤波数据融合对陀螺进行了补偿，抑制了其漂移，进而得到了输出稳定的航姿信息。

工作的整个软件流程可以按图4来进行，外设初始化单元、传感器初始化、原始数据处理、姿态角输出。

五、航姿参考系统的验证

完成了航姿参考系统的硬件和软件的设计，将进行其性能的精度及稳定性的测试。航姿参考系统的试验包括：静态实验和动态试验，主要是用来测试系统的性能，即在相应条件下，得出航向角ψ、俯仰角θ、横滚角γ与理论值的误差，并且验证其是否在合理的范围之内。

为了满足实际应用需求，本文选用SGT320E型三轴多功能转台（如图5所示）进行航姿参考系统的误差标定、误差补偿以及性能测试等。

1、航姿系统的静态实验

测试系统的静态性能，就是验证静态条件下，测试航向角、俯仰角和横滚角与理论值的误差。将航姿参考系统固定于转台X轴平面上，并且调平，此时能保证系统的Z轴与X、Y轴垂直，也就是保证系统的俯仰角和横滚角的理论值为0。稳定后，通过上位机输出的姿态信息，生成如图6所示的姿态角。

我们对姿态角和航向角数据进行误差分析，采集100个航姿系统输出的姿态数据并和理论值进行对比，得到俯仰角、横滚角和航向角的均值误差和均方差误差，如表1所示。

表1 航姿系统的静态误差分析

由图6及表1所示可知，横滚角和俯仰角的数据在0.2o以内变化，航向角的数据在0.3o以内变化，数值的波动性不大，数据表明，航姿参考系统的静态性能良好。

2、航姿系统的动态实验

测试系统的动态性能，就是利用三轴转台模拟飞行器在平飞、转弯、升降等动态环境下，航姿参考系统输出的信息，我们把输出的姿态角信息与理论值进行对比，它们之间的差值即为误差，从而验证系统的动态性能。

将航姿参考系统固定于转台X轴平面上，并且调节初始航向角ψ=0o、俯仰角θ=30o、横滚角γ=0o，并且使转台绕Z轴以1o/s的速度转动，X轴和Y轴保持初始值。通过上位机输出的姿态信息，在此动态环境下，通过上位机输出姿态信息，图7(a)为俯仰角的姿态角。同理，得出在此动态环境下的横滚角和航向角，在此就不再赘述。

在动态条件下，取1000个俯仰角采样点，与理论值进行误差仿真生成如图7(b)所示。由图可知，动态情况下，俯仰角的角度误差控制在0.5o以内，同理我们得出横滚角和航向角的误差控制在0.4o和0.6o。我们对姿态角和航向角数据进行误差分析，采集100个航姿系统输出的姿态数据并和理论值进行对比，得到俯仰角、横滚角和航向角的均值误差和均方差误差，如表2所示。

表2 航姿系统的动态误差分析

由以上图标数据可知，在动态环境下姿态角的输出能够实时地反映姿态角的信息。故实验数据表明：航姿参考系统整体动态性能良好。

综上所述，该航姿参考系统能够满足对姿态的实时精确测量，实现了设计小型化、低功耗和低成本的设计要求，可应用于民用等领域，满足了本文的设计要求。

六、结语

航姿参考系统能够实时精确的获得姿态信息是系统运行的根本保障。本文首先介绍了选取硬件器材的依据，采用了精度较高的MEMS器件与STM32单片机，并且结合了DSP高速处理数据的能力，构建了航姿参考系统的硬件平台。其次，提出了硬件软件的设计流程，将得到的原始数据经过卡尔曼滤波处理，输出了更为准确的姿态信息。最后，通过三轴转台进行了数据的分析和验证。结果表明，本文设计的航姿参考系统能够实时准确的输出稳定的姿态数据，满足了系统对姿态控制的所需要求。因此，可以广泛地运用于生活民用领域。