基于MSP430单片机的姿态测量系统设计

，，

(1.海军工程大学 a.兵器工程系；b.导航工程系，武汉 430033；2.海军海洋测绘研究所，天津 300061)

随着MEMS技术的成熟，以及对姿态测量中低功耗、低成本的需求，微机械陀螺、加速度计和磁传感器在无人机、智能机器人等领域得到广泛的应用[1]。针对单一传感器测量姿态存在零漂，受环境因素影响大等问题，多传感器融合测姿已成为一种发展趋势，并且随着MEMS传感器的迅速发展以及向各个学科领域的渗透，其各个方面性能如精度、鲁棒性、动态响应等都得到了很大的提高，因此，如何在多MEMS传感器系统中实现数据融合进而进行姿态测量成为主要研究方向。Marins基于四元数扩展的卡尔曼滤波，研究使用MARG传感器进行姿态检测[2]，然而该算法计算量较大，难以在硬件上实现。宋东等人[3]提出了一种基于MEMS加速度计、磁阻传感器的方法，该方法简化了计算量，但无法抑制动态环境下数据中的噪声。Mahony等人[4]利用MEMS器件及互补滤波器建立姿态估计方法，得出互补滤波器截止频率设计方法，但该算法PI参数只能通过试凑，无法实时调整PI参数。针对以上姿态测量方法的缺陷，基于GY-85九轴传感器模块，设计一种以单片机MSP430F5438为核心的姿态检测系统，利用梯度下降法对传感器进行数据融合，弥补单一传感器测姿的不足。

1 硬件组成

设计的系统以MSP430F5438为核心，通过与集成了三轴加速度计ADXL345、三轴地磁传感器HMC5883L以及陀螺仪ITG3205的九轴传感器GY-85进行I2C通信以采集各个传感器的输出数据，并将解算出的各个姿态角通过无线串口发送给上位机。系统设计的原理框图如图1所示。

图1 系统设计原理框图

MSP430F5438是德州仪器近两年推出的MSP43054XX系列单片机中的最高型号，其资源充足，功能强大，非常适用于多传感器的姿态检测。ADXL345是一款超低功耗的三轴加速度计，分辨率高(13位)，测量范围达±16 g，能测量到小于1°的倾斜角度变化[5]。HMC5883L磁场传感器分辨率达0.002 G，最大输出频率达160 Hz。陀螺仪ITG3205内部具有3个整合16位模数转换器，提供陀螺仪同步取样，并带有可选择的数字低通滤波器以及快速模式(400 kHz)的I2C接口。

2 软件流程

系统的主流程图如图2所示。为了便于移植，对所设计程序进行模块化处理，通过调用各个子函数完成所需的功能。程序首先完成MSP430F5438初始化，并对各个传感器的工作模式进行初始化设定，接着对四元数初始化及各个传感器的初始偏移进行校正。随后在中断函数程序中完成数据采集与滤波，并解算出姿态角发送给上位机。

图2 系统流程

3 系统校正

加速度计校正的方法为将传感器水平放置，使z轴在1 g的重力场，x、y轴在0 g的重力场，取一系列样本的平均值，记为x0、y0、z1。这些值即为相对于各轴的偏移量，通过减去这些值以获得实际加速度的值。

(1)

式中：Xacc、Yacc、Zacc为加速度计实际值，Xmea、Ymea、Zmea为加速度计测量值，SZ为加速度计z轴在1 g的重力场下的理想灵敏度。

陀螺仪校正时需采集初始时的一组数据求平均值后作为零位偏移，此后将测量值减去偏移即可。

ωacc=ωmea-ωbias

(2)

式中：ωacc为陀螺仪实际值；ωmea为陀螺仪测量值；ωbias为零位偏移值。

在理想状态下，地磁传感器三轴的输出磁场矢量应构成一个圆心在原点的正球体，而在实验的过程中，地磁传感器受到周围硬磁干扰和软磁干扰，导致该球体变成了一个偏离圆心的椭球体，所以需对地磁传感器进行校正。图3和图4为采用AntMag软件对地磁传感器校正前后的对比示意图。校正后的地磁传感器各轴的零位偏移和均方误差有了明显的改善，如表1所示。

图3 校正前地磁传感器输出

图4 校正后地磁传感器输出

校正X零位偏移Y零位偏移Z零位偏移均方误差校正前0.08660.09380.11930.059校正后0.00080.00180.00330.029

4 姿态解算

4.1 坐标系建立、四元数与欧拉角之间的转换

分别建立导航坐标系N和载体坐标系B，N系采用东北天坐标系，B系固连在载体上，传感器的三轴分别安装在B系的x、y、z轴上。设α、β、γ分别为俯仰角，滚转角、航向角。则由N系到B系的旋转矩阵可表示为

(3)

用四元数Q=q0+q1i+q2j+q3k(|Q|=1)表示该旋转矩阵为

(4)

通过上述2种表示形式，将欧拉角用四元数表示为

(5)

4.2 四元数微分方程求解

根据四元数微分方程定义，对应的矩阵[6]为

采用一阶龙格库塔求解四元数微分方程。

Q(t+h)=Q(t)+hk1

(7)

得到四元数更新方程为

式中：ωx、ωy、ωz为△轴输出角速度；h为采样周期。

4.3 单传感器解算姿态角

ADXL345加速度计在静态或者低动态的情况下利用重力在各轴上的投影分量求解姿态角[7]。

(9)

式中：α、β、γ分别为俯仰角、翻滚角、z轴与重力加速度的夹角。

ITG3205陀螺仪测得绕各个轴的角速度，通过积分便可以得到绕各个轴旋转的角度[8]:

θi=θi-1+ωdt

(10)

式中：θi为i时刻的角度值；θi-1为i-1时刻的角度值；ω为修正后的角速度；dt为积分时间。

HMC5883L地磁传感器通过测得地磁场在其各轴上的投影分量解算出航向角。当地磁传感器处于水平状态时，可通过式(11)对航向角进行检测，而当其处于倾斜状态时，则需通过加速度计所测俯仰角α和翻滚角β代入式(12)，对其进行倾斜补偿后解算出航向角。

γ=arctan(Hy/Hx)

(11)

(12)

式中：Hx、Hy、Hz为地磁传感器各轴所测数据。

4.4 梯度下降法融合解算姿态

通过陀螺仪积分解算姿态，由于受到温度和零漂的影响，长时间使用误差较大[9]。而加速度计不仅对重力，同时对外力产生的加速度十分敏感，动态下噪声明显，且其不能对陀螺仪所测的航向角进行修正，故无法使用单传感器对姿态进行测量。本文基于Madgwick所提出的梯度下降法，将九轴传感器中的加速度计、陀螺仪、地磁传感器数据进行融合修正旋转矩阵，从而得到姿态的近似最优解。

(13)

构造目标函数为

F(Q)=(egx)2+(egy)2+(egz)2+

(emx)2+(emy)2+(emz)2

将四元数旋转矩阵的优化转化为求F(Q)的最小值。

由梯度下降法得到其迭代公式为[10]

(14)

式中：m为迭代步长。

将其与由四元数微分方程得到的四元数Qω(t)相结合，得到最终的梯度下降法的姿态融合公式：

(15)

式中：n为迭代步长；Δt为采样时间；Q(t)为所求姿态四元数。

通过加速度计对地磁传感器进行倾斜补偿后求得的欧拉角，带入式(16)转化为初始四元数，以解决初始对准时梯度下降法收敛过慢的问题。当四元数初始化后，便可利用上述方法解算姿态的近似值。

(16)

5 实验与分析

5.1 静态实验

利用角度分度装置将九轴传感器静止放置水平，同时采集通过加速度计、陀螺仪、梯度下降法解算出的俯仰角，实验结果如图5所示。通过对比可以看出：加速度计存在明显的噪声，陀螺仪存在较明显的偏移，随着时间的增加，累积误差逐渐增大。梯度下降法能有效的抑制加速度计的噪声，俯仰角测量误差大约在±0.3°以内。

图5 俯仰角静止实验

5.2 动态实验

将传感器初始水平，此后绕x轴旋转30°得到的图像如图6所示。由图像看出，梯度下降法能及时跟踪姿态的变化，同时一定程度上抑制了加速度计的噪声和陀螺仪的漂移，所测的俯仰角误差在±1°以内。

图6 俯仰角动态实验

5.3 四元数初始化对比试验

初始时将传感器x轴指向东方，然后旋转90°使其指向北方，分别将前文所提出的对四元数初始化的方法与一般采用的将初始四元数设为(1，0，0，0)所测的航向角进行比较，实验效果如图7、8所示。由实验结果看出，进行初始对准后，梯度下降法初始收敛时间由原来的5 s降到了1 s以内，改善了其姿态角初始时刻不稳的问题，采用该系统测的航向角误差在±2°以内。

图7 未初始对准航向角实验结果

图8 初始对准航向角实验结果

6 结论

基于MSP430F5438与九轴传感器GY- 85搭建姿态测量系统，实现了姿态角的检测。针对地磁传感器的所测数据受周围磁场干扰比较严重的问题，使用AntMag软件对其进行了校正，实验结果表明该软件简单可行，正确有效。另外为解决加速度计测量噪声、陀螺仪漂移误差及加速度计无法对航向角进行修正等问题，引入梯度下降法，在角度分度装置上进行了动静态实验。结果表明，该系统克服了航向角初始难以对准的问题，具有响应快、实时性好的特点，并可应用于平衡小车、无人机姿态检测、惯性导航等领域。

[1] 夏圣，许勇.基于MEMS组合模块的姿态检测系统设计[J].单片机与嵌入式系统应用,2011(4):52- 55.

[2] MARINS J L,YUN X,BACHAMARM E,et al. An extended Kalman filter for quaternion based orientation rst- imation using MARG sensors[C]∥Proceedings of the 2001 IEEE/RSJ I- nternational Conference on Intel- ligent Robots and systems.Maui,Hawaii:IEEE,2001(8):2003- 2011.

[3] 宋东,章侃,王彦文,等.基于MEMS传感器的飞行姿态指示系统[J].测控技术,2009,28(3):1- 4.

[4] MAHONY R,HAMEL T,PFILMLIN J M,et al.Nonlinear complementary filte- rs on the special orthogonal gro- up[J].IEEE trans.on autom atic control,2008,53(5):1203- 1217.

[5] 陈新隽，刘晓平，戴若犁，等.多MEMS传感器的嵌入式姿态测量系统设计[A].单片机与嵌入式系统：应用天地,2011(10):58- 61.

[6] 秦永元.惯性导航[M].北京:科学出版社,2014.

[7] KIMBERLY T.Tilt sensing using linear Accelerometers[J].Freescale Semiconductor Application Note,2007(6):1- 7.

[8] 李伟，何鹏举，高社生.多传感器加权信息融合算法研究[J].西北工业大学学报，2010,28(5):674 - 678.

[9] 毕盛.闵华清.李淳，等.姿态传感器采集测试系统的设计与实现[J].计算机测量与控制，2011,19(7):1562- 1564.

[10] MADGWICK S.An efficient orientation filter for intertial and inertial/magnetic sensor arrays [R].Report xio and University of Bristol(UK),2010.