一種基于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 結論

　　本文完成了基于無線傳感器網絡的角度監測系統的設計，克服了已有監測裝置中傳感器單一的弊端，滿足對空間物體三維姿態的實時、高精度、無線監測的要求。系統使用三軸地磁傳感器對水平面內的角速度值進行修正，使用三軸加速度計對垂直面內的角速度值進行修正，從而精確測量空間物體三維姿態。