基于多MEMS传感器的姿态测量系统
加入技术交流群
2.3 加速度计、电子罗盘与I2C接口
MC9S08QE8内带的高速I2C模块拥有多主机操作、可编程从机地址、中断驱动的逐字节数据发送、支持广播模式和10位寻址等特点，总线在最大负荷下可达到100kbps的速度。系统中，加速度计、电子罗盘芯片与MC9S08QE8 I2C模块的接口如图4所示。图中ADXL345的CS引脚用来控制选择I2C还是SPI通信协议，电平为高表示采用I2C协议，而SDA和SCL引脚分别连接到MC9S08QE8的I2C总线引脚上。电子罗盘HMC5843支持双电压工作，其中引脚VDD表示内核电压，引脚VDDIO表示外部I/O电压，本系统中采用单电压模式，即内核电压与外部I/O电压相同。
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部分的效果，可以说明本设计达到了良好的效果，能比较正确地测量物体的姿态信息。
结语
评论