基于多MEMS传感器的姿态测量系统

3 软件设计与测试

系统的软件设计是整个系统的重要组成部分。依据上述硬件电路的设计原理与功能要求，软件中首先要完成MC9S08QE8的初始化，对各种MEMS传感器的工作模式进行设定；然后获取三轴陀螺仪、加速度计、电子罗盘的实时信号，并根据姿态计算算法计算姿态角，最终把姿态角信息通过串口传送到上位机中进行测试与演示，嵌入式姿态测量系统软件流程如图5所示。


3．1 MC9S08QE8的初始化

MC9S08QE8的初始化函数主要实现对系统时钟、端口及各个使用的功能模块进行初始化，如ADC模块、SCI模块、I2C模块。初始化函数Sys_init_fun(void)如下：


3．2 传感器工作模式的设定

各种MEMS传感器工作模式的设定中，陀螺仪涉及的均为模拟信号，不用对其具体工作模式进行设定。加速度计ADXL345丰富的功能是通过配置对应的寄存器来实现的，通过对应的寄存器可以选择数据格式、FIFO工作模式、数字通信模式、节电模式、中断使能以及修正各轴偏差等。POWER_CTL寄存器用来设定供电模式，与BW_RATE寄存器配合，可设定数据传输速率，如果要进一步降低功耗，可将BW_RATE寄存器中的LOW_POWER位置位，进入低功耗模式。而DATA_FORMAT寄存器用来设定数据格式与加速度计的量程，FIFO_CTL寄存器用来设置缓存器具体的工作模式，如Bypass、FIFO、Stream、Trigger。最后OFSX、OFSY、OFSZ三个用来存放初始化时标定的X、Y、Z轴的偏移量，以便对数据进行修正。

电子罗盘HMC5843配置相对简单，主要有3个寄存器，通过配置寄存器A可以设定数据传输速率和测量模式，寄存器B用来设置设备的增益大小，而通过模式寄存器用来设置设备的工作模式。

3．3 姿态计算

典型的姿态解算方法有方向余弦矩阵求解法、四元数法、旋转矢量法等，本系统采用William Premerlani andPaul Bizard的DCM算法，DCM算法框图如图6所示。三轴陀螺仪的输入信号通过运动学方程计算出方向余弦矩阵，三轴加速度计信号与三轴电子罗盘信号结合PI反馈控制对陀螺仪信号进行修正。

其中算法由kinematics_and_normalization(t_vector*gyro，t_matrix*dcm)函数实现：

上述计算得到的实时姿态角数据通过串口传送到上位机，上位机中通过编写的python演示程序对下位机姿态测量模块的运动姿态进行跟踪与显示，演示效果如图7所示。每幅图中包含3个部分：第1部分(左上角)中红、绿、蓝3种指针指向分别代表横滚角(roll)、俯仰角(pitch)与航向角(yaw)的大小，第2部分(左下角)显示模块实时运动姿态，第3部分(右边)用于显示姿态角信息。左图为物体静止不动下的演示效果，右图表示在运动过程中物体的姿态运动效果，通过对比与分析2个图及各图对应3部分的效果，可以说明本设计达到了良好的效果，能比较正确地测量物体的姿态信息。


结语

当前，各种消费电子设备内部一般含有三轴加速度计和电子罗盘，如智能手机、平板电脑等，但加速度计动态性能相比陀螺仪逊色很多，而陀螺仪的增加可以提升系统整体的动态与静态性能。本文设计的嵌入式姿态测量系统，采用多MEMS传感器组合方式，拓展了MEMS传感器的应用范围，也延伸了姿态测量系统的应用领域。实验演示表明系统性能和使用性都比较好，可以应用于消费电子与一般工业的姿态测量与物体稳定控制的应用中。