一種基于HMC5843和Zigbee的三維姿態(tài)監(jiān)測(cè)系統(tǒng)設(shè)計(jì)

　　3 系統(tǒng)軟件設(shè)計(jì)

　　3.1 主程序設(shè)計(jì)

　　系統(tǒng)上電完成初始化后，讀取監(jiān)測(cè)終端發(fā)送的控制指令，如果為開始工作，則讀取三軸加速度傳感器測(cè)量出的空間物體三維加速度值ax、ay、az，然后進(jìn)入加速度處理子程序?qū)ζ溥M(jìn)行運(yùn)算處理，得出空間物體的滾轉(zhuǎn)角ψ、俯仰角θ。繼續(xù)讀取三軸地磁傳感器測(cè)量出的空間物體三維地磁感應(yīng)強(qiáng)度分量mx、my、mz，結(jié)合加速度傳感器對(duì)其進(jìn)行修正，通過地磁處理子程序得出橫擺角γ的值。最終通過Zigbee無線通信模塊將數(shù)據(jù)傳輸至監(jiān)測(cè)終端進(jìn)行數(shù)據(jù)的實(shí)時(shí)顯示與存儲(chǔ);如果為工作停止，則停止以上監(jiān)測(cè)數(shù)據(jù)的采集、處理。系統(tǒng)主程序流程圖如圖5所示。

　　3.2 加速度數(shù)據(jù)處理子程序

　　滾轉(zhuǎn)角ψ、俯仰角θ和橫擺角γ分別是繞x軸、y軸和z軸的旋轉(zhuǎn)角度，三軸加速度傳感器測(cè)量出空間物體三維加速度值分別為ax、ay、az，考慮到反正弦函數(shù)的值域?yàn)?/p>

　　，由三角函數(shù)關(guān)系可知：

　　3.3 地磁數(shù)據(jù)修正子程序

　　三軸地磁傳感器測(cè)量出空間物體三維地磁感應(yīng)強(qiáng)度分量分別為mx、my、mz，在沒有外界磁場(chǎng)干擾的情況下，我們近似認(rèn)為地磁的方向是地理的由南向北水平方向的，由于空間物體在繞軸、軸方向上有傾角，要將其分解到水平面上，結(jié)合加速度傳感器對(duì)三維地磁感應(yīng)強(qiáng)度分量進(jìn)行修正，得出在軸方向和軸方向上的地磁感應(yīng)強(qiáng)度分量m_x和m_y如下：

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

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

　　3.4 地磁數(shù)據(jù)處理子程序

　　由地磁數(shù)據(jù)修正子程序得出的在軸方向和軸方向上的地磁感應(yīng)強(qiáng)度分量分別為m_x和m_y，考慮到反正弦函數(shù)的值域?yàn)?/p>

　　，則橫擺角的取值如下：

　　4 結(jié)論

　　本文完成了基于無線傳感器網(wǎng)絡(luò)的角度監(jiān)測(cè)系統(tǒng)的設(shè)計(jì)，克服了已有監(jiān)測(cè)裝置中傳感器單一的弊端，滿足對(duì)空間物體三維姿態(tài)的實(shí)時(shí)、高精度、無線監(jiān)測(cè)的要求。系統(tǒng)使用三軸地磁傳感器對(duì)水平面內(nèi)的角速度值進(jìn)行修正，使用三軸加速度計(jì)對(duì)垂直面內(nèi)的角速度值進(jìn)行修正，從而精確測(cè)量空間物體三維姿態(tài)。