一种基于HMC5843和Zigbee的三维姿态监测系统设计

　　3 系统软件设计

　　3.1 主程序设计

　　系统上电完成初始化后，读取监测终端发送的控制指令，如果为开始工作，则读取三轴加速度传感器测量出的空间物体三维加速度值ax、ay、az，然后进入加速度处理子程序对其进行运算处理，得出空间物体的滚转角ψ、俯仰角θ。继续读取三轴地磁传感器测量出的空间物体三维地磁感应强度分量mx、my、mz，结合加速度传感器对其进行修正，通过地磁处理子程序得出横摆角γ的值。最终通过Zigbee无线通信模块将数据传输至监测终端进行数据的实时显示与存储;如果为工作停止，则停止以上监测数据的采集、处理。系统主程序流程图如图5所示。

　　3.2 加速度数据处理子程序

　　滚转角ψ、俯仰角θ和横摆角γ分别是绕x轴、y轴和z轴的旋转角度，三轴加速度传感器测量出空间物体三维加速度值分别为ax、ay、az，考虑到反正弦函数的值域为

　　，由三角函数关系可知：

　　3.3 地磁数据修正子程序

　　三轴地磁传感器测量出空间物体三维地磁感应强度分量分别为mx、my、mz，在没有外界磁场干扰的情况下，我们近似认为地磁的方向是地理的由南向北水平方向的，由于空间物体在绕轴、轴方向上有倾角，要将其分解到水平面上，结合加速度传感器对三维地磁感应强度分量进行修正，得出在轴方向和轴方向上的地磁感应强度分量m_x和m_y如下：

　　m—x=mxcosθ-mysinψsinθ-mzcosψsinθ (3)

　　m_y=mycosψ-mxsinψsinθ-mzsinψcosθ (4)

　　3.4 地磁数据处理子程序

　　由地磁数据修正子程序得出的在轴方向和轴方向上的地磁感应强度分量分别为m_x和m_y，考虑到反正弦函数的值域为

　　，则横摆角的取值如下：

　　4 结论

　　本文完成了基于无线传感器网络的角度监测系统的设计，克服了已有监测装置中传感器单一的弊端，满足对空间物体三维姿态的实时、高精度、无线监测的要求。系统使用三轴地磁传感器对水平面内的角速度值进行修正，使用三轴加速度计对垂直面内的角速度值进行修正，从而精确测量空间物体三维姿态。