iNEMO模塊的特點(diǎn)及用其實(shí)現(xiàn)人體姿態(tài)檢測(cè)系統(tǒng)的設(shè)計(jì)

近年來(lái)，MEMS技術(shù)的發(fā)展開(kāi)辟了一個(gè)全新的技術(shù)領(lǐng)域和產(chǎn)業(yè)，采用MEMS技術(shù)制作的微傳感器、微執(zhí)行器、電力電子器件等在航空、航天、汽車(chē)、生物醫(yī)學(xué)、軍事等領(lǐng)域中都有著十分廣闊的應(yīng)用前景，同時(shí)人體姿態(tài)檢測(cè)和信息融合技術(shù)在人體醫(yī)學(xué)工程、健康監(jiān)護(hù)等領(lǐng)域的研究也逐漸開(kāi)展起來(lái)。

目前人體姿態(tài)檢測(cè)的主要手段有圖像分析和加速度分析兩種，圖像分析的算法通過(guò)攝像捕捉人體運(yùn)動(dòng)姿態(tài)，經(jīng)過(guò)一定的圖像處理技術(shù)確定人體的姿態(tài)，該方法需要在人體運(yùn)動(dòng)的區(qū)域安裝攝像頭，價(jià)格昂貴、具有一定的局限性；傳統(tǒng)的加速度算法采用SVM （Support Vector Machine） 算法對(duì)數(shù)據(jù)進(jìn)行預(yù)處理，然后用KFD（Kernel Fisher Discriminant）算法和k-NN（Nearest Neighbour）算法進(jìn)行精確判定，該方法計(jì)算量大、編程復(fù)雜。

本系統(tǒng)提出的算法主要是在加速度計(jì)、陀螺儀、磁力計(jì)采集人體運(yùn)動(dòng)姿態(tài)信息的基礎(chǔ)上，運(yùn)用擴(kuò)展卡爾曼濾波將數(shù)據(jù)進(jìn)行融合，得到人體腰部和腿部的角度信息，然后通過(guò)大量實(shí)驗(yàn)建立與姿態(tài)的對(duì)應(yīng)關(guān)系，采用這種方式可以利用加速度計(jì)與磁力計(jì)克服單獨(dú)采用陀螺儀引起的姿態(tài)角發(fā)散，另外利用陀螺儀可以克服由于振動(dòng)對(duì)于加速度計(jì)的影響與由于軟硬鐵磁對(duì)于磁力計(jì)的影響。

1 系統(tǒng)整體設(shè)計(jì)方案

系統(tǒng)分為上位機(jī)子系統(tǒng)和下位機(jī)子系統(tǒng)，下位機(jī)子系統(tǒng)由電源模塊、2個(gè)iNEMO模塊、GPRS模塊和主控板組成，其中電源模塊給整個(gè)系統(tǒng)提供3.3V和5V的工作電壓，兩個(gè)iNEMO模塊分別固定在人體的腰部和腿部，完成對(duì)加速度計(jì)、磁力計(jì)、陀螺儀的信息采集，然后通過(guò)串口發(fā)送到主控板，主控板進(jìn)行卡爾曼濾波融合出腰部和腿部的角度，然后根據(jù)腰部和腿部的角度值完成姿態(tài)的檢測(cè)，最后GPRS模塊通過(guò)socket協(xié)議將姿態(tài)信息打包傳送到上位機(jī)，上位機(jī)實(shí)現(xiàn)遠(yuǎn)程監(jiān)控。

圖1 系統(tǒng)整體設(shè)計(jì)方案

2數(shù)據(jù)融合原理

在慣性導(dǎo)航領(lǐng)域，求取姿態(tài)角的數(shù)學(xué)表達(dá)式叫做方向余弦矩陣，用于表示方向余弦矩陣有兩種方式：歐拉角與四元數(shù)。歐拉角的優(yōu)點(diǎn)是比較直觀，缺點(diǎn)是在俯仰角為正負(fù)90°時(shí)系統(tǒng)存在不穩(wěn)定奇點(diǎn)。四元數(shù)的優(yōu)點(diǎn)是當(dāng)俯仰角為正負(fù)90°時(shí)系統(tǒng)不受影響，缺點(diǎn)是不直觀，下面是它們之間的相互轉(zhuǎn)換公式。

歐拉角轉(zhuǎn)換成四元數(shù)

四元數(shù)轉(zhuǎn)換成歐拉角

在程序中使用的是四元數(shù)，由于四元數(shù)不能直觀的表示輸出的角度，所以首先根據(jù)陀螺儀求取四元數(shù)，再將四元數(shù)轉(zhuǎn)換成姿態(tài)角。根據(jù)運(yùn)動(dòng)體安裝的三軸陀螺儀，可以得到在運(yùn)動(dòng)體坐標(biāo)系下的三軸角速度（ wx，wy，wz），用三軸角速度更新四元數(shù)

然后利用加速度計(jì)的信息，采用卡爾曼濾波的方法來(lái)對(duì)四元數(shù)進(jìn)行修正，設(shè)系統(tǒng)的狀態(tài)空間方程為

其中，X（k）是k時(shí)刻的系統(tǒng)狀態(tài)，U（k）是k時(shí)刻對(duì)系統(tǒng)的控制量。A和B是系統(tǒng)參數(shù)。Z（k）是k時(shí)刻的測(cè)量值，H是測(cè)量系統(tǒng)的參數(shù)。W（k）和V（k）分別表示過(guò)程和測(cè)量的噪聲。

為了提高慣性導(dǎo)航儀的啟動(dòng)速度，需要利用歐拉角求取初始的四元數(shù)，首先根據(jù)運(yùn)動(dòng)體上三軸加速度計(jì)輸出的三軸加速度（ fx，fy，fz），求取俯仰角θ與橫滾角φ

隨后讀取磁力計(jì)輸出的三軸磁場(chǎng)強(qiáng)度，然后用加速度計(jì)對(duì)磁力計(jì)進(jìn)行傾斜補(bǔ)償

根據(jù)傾斜補(bǔ)償后的磁力計(jì)輸出，可以求得偏航角為

利用公式（3-5）和（3-7）可以完成對(duì)四元數(shù)的初始化工作。

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

3.1 iNEMO模塊簡(jiǎn)介

iNEMO慣性導(dǎo)航模塊的基本原理如圖2所示，利用MEMS傳感器和主控芯片STM32F103RET6提供動(dòng)靜態(tài)方向和慣性測(cè)量功能。集成3個(gè)（4種）意法半導(dǎo)體傳感器：3軸陀螺儀（L3GD20）、3軸加速度計(jì)+3軸磁力計(jì)（LSM303DLHC）以及氣壓傳感器（LPS331AP），iNEMO傳感器平臺(tái)主控制器通過(guò)I2C總線(xiàn)與各傳感器通信。iNEMO傳感器平臺(tái)可以通過(guò)串口向外界提供各傳感器原始數(shù)據(jù)，還可以提供各傳感器經(jīng)AHRS算法融合后的橫滾、偏航及俯仰角度。

圖2 iNEMO慣性導(dǎo)航模塊框圖

3.2主控制器電路設(shè)計(jì)

圖3為系統(tǒng)主控板電路圖，其中處理器采用了STM32F103VCT6，STM32系列單片機(jī)以Cortex-M3為內(nèi)核，時(shí)鐘頻率可達(dá)到72M Hz，具有豐富的片內(nèi)外設(shè)，高性能、低成本、低功耗的特點(diǎn)使其成為32位產(chǎn)品用戶(hù)的最佳選擇。主控板拓展了多個(gè)串行接口，和iNEMO腿部模塊、iNEMO腰部模塊和GPRS模塊進(jìn)行串口通信。穩(wěn)壓芯片選用了LM2576D2T-5，其最大輸入電壓為45V，可穩(wěn)定輸出5V電壓，再經(jīng)過(guò)REG1117-3.3的穩(wěn)壓電路，產(chǎn)生3.3V工作電壓，當(dāng)電流異常導(dǎo)致溫度過(guò)高時(shí)，保險(xiǎn)絲F1、F2可以自身熔斷切斷電流，起到保護(hù)電路的作用。另外主控電路還包括復(fù)位電路、晶振電路、電容濾波電路以及JTAG下載接口等。

圖3主控板電路圖

3.3 GPRS電路設(shè)計(jì)

圖4為GPRS電路圖，主要包括MCU、SIM300模塊、SIM卡卡槽、穩(wěn)壓電路四部分。MCU同樣采用了STM32F103VCT6，圍繞它設(shè)計(jì)了單片機(jī)最小系統(tǒng)，包括晶振電路、復(fù)位電路、電容濾波電路等，MCU通過(guò)串口向SIM300發(fā)送控制指令以及接收SIM300返回的數(shù)據(jù)；SIM300模塊與PCB板的連接方式為60引腳的板板連接器，全部引腳從該連接器引出，SIM300的網(wǎng)絡(luò)狀態(tài)指示燈引腳可根據(jù)模塊的網(wǎng)絡(luò)狀態(tài)輸出不同頻率的電壓脈沖，方便觀測(cè)模塊是否正常工作，對(duì)于不需要用的音頻接口、LCD接口等，對(duì)應(yīng)的引腳懸空即可；穩(wěn)壓芯片選擇方面，可以使用開(kāi)關(guān)型穩(wěn)壓芯片或者LDO線(xiàn)性穩(wěn)壓芯片，開(kāi)關(guān)型穩(wěn)壓芯片轉(zhuǎn)換效率高，但是需要的外圍器件較多，且多為功率器件，占用空間比較大，為了節(jié)省PCB空間，設(shè)計(jì)中采用MIC5219-3.3BM5穩(wěn)壓芯片作為SIM300的電源調(diào)理芯片。

圖4 GPRS電路圖

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

4.1系統(tǒng)軟件整體流程圖

圖5所示為系統(tǒng)軟件整體流程圖，首先對(duì)STM32進(jìn)行初始化配置，包括時(shí)鐘電路RCC高速時(shí)鐘和低速時(shí)鐘的起振、通用引腳GPIO輸出輸出模式的配置、串口USART波特率的設(shè)置，然后將SIM300設(shè)置為透?jìng)髂Ｊ?，進(jìn)行網(wǎng)絡(luò)連接（具體過(guò)程可參考3.3），成功連接網(wǎng)絡(luò)之后，配置iNEMO模塊各傳感器的分辨率、字節(jié)對(duì)齊方式、讀取頻率等，然后分別讀取腰部和腿部iNEMO模塊的加速度、陀螺儀、磁力計(jì)原始數(shù)據(jù)，獲得原始數(shù)據(jù)之后根據(jù)分別率的設(shè)置計(jì)算磁力計(jì)、陀螺儀、加速度的大小，最后通過(guò)擴(kuò)展卡爾曼濾波融合得到腰部的俯仰角PITCH、腿部的偏航角YAW，結(jié)合兩個(gè)角度信息通過(guò)查表法進(jìn)行姿態(tài)判斷，設(shè)置相應(yīng)的標(biāo)志位，通過(guò)GPRS將姿態(tài)標(biāo)志位打包后發(fā)送到遠(yuǎn)程監(jiān)控端。

圖5系統(tǒng)軟件整體流程圖

4.2姿態(tài)檢測(cè)

如圖6所示，iNEMO模塊中L3GD20、LSM303DLHC以及LPS331AP通過(guò)I2C串行總線(xiàn)與單片機(jī)進(jìn)行通信，I2C總線(xiàn)兩根雙向信號(hào)線(xiàn)一根是數(shù)據(jù)線(xiàn)SDA，另一根是時(shí)鐘線(xiàn)SCL，通過(guò)上拉電阻接到正電源VDD，每個(gè)接到I2C總線(xiàn)上的器件都有唯一的地址，主機(jī)發(fā)送地址時(shí)，總線(xiàn)上的每個(gè)從機(jī)都將7位地址碼與自己的地址進(jìn)行比較，如果相同，則認(rèn)為自己正被主機(jī)尋址。

圖6 I2C串行總線(xiàn)示意圖

iNEMO各個(gè)傳感器的地址碼宏定義如下：

#define ACC_ADDRESS 0x32 //加速度計(jì)地址碼

#define MAG_ADDRESS 0x3C //磁力計(jì)地址碼

#define GYRO_ADDRESS 0xD6 //陀螺儀地址碼

采集到傳感器的數(shù)據(jù)之后，首先利用三軸陀螺儀信號(hào)，采用四元數(shù)的姿態(tài)表達(dá)式，積分求得姿態(tài)角，然后采用三軸加速度計(jì)和三軸磁力計(jì)，利用大地磁場(chǎng)和重力磁場(chǎng)在地理坐標(biāo)系和運(yùn)動(dòng)坐標(biāo)系之間的方向余弦進(jìn)行絕對(duì)角度的解算，然后采用卡爾曼濾波進(jìn)行前三者數(shù)據(jù)的融合，得到腰部的俯仰角PITCH、腿部偏航角YAW.通過(guò)大量實(shí)驗(yàn)建立姿態(tài)角與人體姿態(tài)的對(duì)應(yīng)表如下：

表1姿態(tài)與YAW、PITCH的對(duì)應(yīng)表

4.3 GPRS發(fā)送

SIM300參數(shù)的設(shè)定、TCP/ IP連接的建立以及數(shù)據(jù)的傳輸都是通過(guò)AT指令來(lái)實(shí)現(xiàn)的，AT指令是終端設(shè)備與PC應(yīng)用之間的通信方式，一般由終端設(shè)備（如PC、控制板等）向終端適配器（如GPRS模塊等）發(fā)送。AT指令以AT開(kāi)頭，以回車(chē)作為結(jié)尾；每條指令是否成功執(zhí)行都有相應(yīng)的響應(yīng)返回。SIM300的TCP/IP功能支持普通和透?jìng)鲀煞N模式，本文采用的是透?jìng)髂Ｊ?，一旦建立連接模塊就進(jìn)入數(shù)據(jù)模式。

SIM300透?jìng)髂Ｊ降呐渲么笾路譃橐韵滤牟剑?/p>

（1）匹配波特率，發(fā)送“AT”指令后，延時(shí)一段時(shí)間發(fā)送“ATE0”。

（2）注冊(cè)網(wǎng)絡(luò)，發(fā)送“AT+CREG？”，若返回“OK”，則網(wǎng)絡(luò)注冊(cè)成功。

（3）設(shè)置透?jìng)髂Ｊ剑l(fā)送“AT + CIPMODE=1”，若返回“OK”，則設(shè)置成功。

（4）連接網(wǎng)絡(luò)，發(fā)送“AT+CIPSTART=”TCP“，”219.236.xxx.xxx“，”7000“”，其219.236.xxx.xxx為PC的IP地址，7000為端口號(hào)。連接成功返回：CONNECT OK

5系統(tǒng)測(cè)試

為了驗(yàn)證系統(tǒng)姿態(tài)檢測(cè)功能的可靠性，選取了不同的人分別佩戴該系統(tǒng)做了大量的實(shí)驗(yàn)，完成了對(duì)站立、坐下、行走、彎腰、躺下、趴倒的識(shí)別率的測(cè)試。從測(cè)試結(jié)果表2可以看出，大部分動(dòng)作均可以準(zhǔn)確無(wú)誤的識(shí)別出來(lái)，行走、彎腰兩個(gè)動(dòng)作出現(xiàn)了誤識(shí)別，這是由于不同的人佩戴該系統(tǒng)時(shí)，各個(gè)姿態(tài)對(duì)應(yīng)的角度閾值會(huì)有少許的差別，但是百分之九十五以上的識(shí)別率足夠滿(mǎn)足用戶(hù)的要求。

表2測(cè)試結(jié)果

結(jié)束語(yǔ)

人體姿態(tài)檢測(cè)裝置實(shí)現(xiàn)了對(duì)人體姿態(tài)的檢測(cè)，可以很好地應(yīng)用在對(duì)老人的監(jiān)護(hù)、體感游戲等領(lǐng)域。數(shù)字式傳感器的應(yīng)用簡(jiǎn)化了硬件電路，GPRS模塊的網(wǎng)絡(luò)傳輸突破了射頻傳輸距離短的缺陷，擴(kuò)大了傳輸距離，整個(gè)系統(tǒng)體積小巧方便、應(yīng)用性強(qiáng)。