基于多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傳感器的應用范圍，也延伸了姿態測量系統的應用領域。實驗演示表明系統性能和使用性都比較好，可以應用于消費電子與一般工業的姿態測量與物體穩定控制的應用中。