基于多MEMS傳感器的姿態(tài)測(cè)量系統(tǒng)

3 軟件設(shè)計(jì)與測(cè)試

系統(tǒng)的軟件設(shè)計(jì)是整個(gè)系統(tǒng)的重要組成部分。依據(jù)上述硬件電路的設(shè)計(jì)原理與功能要求，軟件中首先要完成MC9S08QE8的初始化，對(duì)各種MEMS傳感器的工作模式進(jìn)行設(shè)定;然后獲取三軸陀螺儀、加速度計(jì)、電子羅盤(pán)的實(shí)時(shí)信號(hào)，并根據(jù)姿態(tài)計(jì)算算法計(jì)算姿態(tài)角，最終把姿態(tài)角信息通過(guò)串口傳送到上位機(jī)中進(jìn)行測(cè)試與演示，嵌入式姿態(tài)測(cè)量系統(tǒng)軟件流程如圖5所示。

3.1 MC9S08QE8的初始化

MC9S08QE8的初始化函數(shù)主要實(shí)現(xiàn)對(duì)系統(tǒng)時(shí)鐘、端口及各個(gè)使用的功能模塊進(jìn)行初始化，如ADC模塊、SCI模塊、I2C模塊。初始化函數(shù)Sys_init_fun(void)如下：

3.2 傳感器工作模式的設(shè)定

各種MEMS傳感器工作模式的設(shè)定中，陀螺儀涉及的均為模擬信號(hào)，不用對(duì)其具體工作模式進(jìn)行設(shè)定。加速度計(jì)ADXL345豐富的功能是通過(guò)配置對(duì)應(yīng)的寄存器來(lái)實(shí)現(xiàn)的，通過(guò)對(duì)應(yīng)的寄存器可以選擇數(shù)據(jù)格式、FIFO工作模式、數(shù)字通信模式、節(jié)電模式、中斷使能以及修正各軸偏差等。POWER_CTL寄存器用來(lái)設(shè)定供電模式，與BW_RATE寄存器配合，可設(shè)定數(shù)據(jù)傳輸速率，如果要進(jìn)一步降低功耗，可將BW_RATE寄存器中的LOW_POWER位置位，進(jìn)入低功耗模式。而DATA_FORMAT寄存器用來(lái)設(shè)定數(shù)據(jù)格式與加速度計(jì)的量程，F(xiàn)IFO_CTL寄存器用來(lái)設(shè)置緩存器具體的工作模式，如Bypass、FIFO、Stream、Trigger。最后OFSX、OFSY、OFSZ三個(gè)用來(lái)存放初始化時(shí)標(biāo)定的X、Y、Z軸的偏移量，以便對(duì)數(shù)據(jù)進(jìn)行修正。

電子羅盤(pán)HMC5843配置相對(duì)簡(jiǎn)單，主要有3個(gè)寄存器，通過(guò)配置寄存器A可以設(shè)定數(shù)據(jù)傳輸速率和測(cè)量模式，寄存器B用來(lái)設(shè)置設(shè)備的增益大小，而通過(guò)模式寄存器用來(lái)設(shè)置設(shè)備的工作模式。

3.3 姿態(tài)計(jì)算

典型的姿態(tài)解算方法有方向余弦矩陣求解法、四元數(shù)法、旋轉(zhuǎn)矢量法等，本系統(tǒng)采用William Premerlani andPaul Bizard的DCM算法，DCM算法框圖如圖6所示。三軸陀螺儀的輸入信號(hào)通過(guò)運(yùn)動(dòng)學(xué)方程計(jì)算出方向余弦矩陣，三軸加速度計(jì)信號(hào)與三軸電子羅盤(pán)信號(hào)結(jié)合PI反饋控制對(duì)陀螺儀信號(hào)進(jìn)行修正。

其中算法由kinematics_and_normalization(t_vector*gyro，t_matrix*dcm)函數(shù)實(shí)現(xiàn)：

上述計(jì)算得到的實(shí)時(shí)姿態(tài)角數(shù)據(jù)通過(guò)串口傳送到上位機(jī)，上位機(jī)中通過(guò)編寫(xiě)的python演示程序?qū)ο挛粰C(jī)姿態(tài)測(cè)量模塊的運(yùn)動(dòng)姿態(tài)進(jìn)行跟蹤與顯示，演示效果如圖7所示。每幅圖中包含3個(gè)部分：第1部分(左上角)中紅、綠、藍(lán)3種指針指向分別代表橫滾角(roll)、俯仰角(pitch)與航向角(yaw)的大小，第2部分(左下角)顯示模塊實(shí)時(shí)運(yùn)動(dòng)姿態(tài)，第3部分(右邊)用于顯示姿態(tài)角信息。左圖為物體靜止不動(dòng)下的演示效果，右圖表示在運(yùn)動(dòng)過(guò)程中物體的姿態(tài)運(yùn)動(dòng)效果，通過(guò)對(duì)比與分析2個(gè)圖及各圖對(duì)應(yīng)3部分的效果，可以說(shuō)明本設(shè)計(jì)達(dá)到了良好的效果，能比較正確地測(cè)量物體的姿態(tài)信息。

結(jié)語(yǔ)

當(dāng)前，各種消費(fèi)電子設(shè)備內(nèi)部一般含有三軸加速度計(jì)和電子羅盤(pán)，如智能手機(jī)、平板電腦等，但加速度計(jì)動(dòng)態(tài)性能相比陀螺儀遜色很多，而陀螺儀的增加可以提升系統(tǒng)整體的動(dòng)態(tài)與靜態(tài)性能。本文設(shè)計(jì)的嵌入式姿態(tài)測(cè)量系統(tǒng)，采用多MEMS傳感器組合方式，拓展了MEMS傳感器的應(yīng)用范圍，也延伸了姿態(tài)測(cè)量系統(tǒng)的應(yīng)用領(lǐng)域。實(shí)驗(yàn)演示表明系統(tǒng)性能和使用性都比較好，可以應(yīng)用于消費(fèi)電子與一般工業(yè)的姿態(tài)測(cè)量與物體穩(wěn)定控制的應(yīng)用中。