基于MEMS的精確計步算法的設計與實現

葉繼超， 關勝曉

(中國科學技術大學 自動化系，安徽 合肥 230026)

0 引 言

微機電系統(micro-electro-mechanical system,MEMS)加速度傳感器具有體積小、測量精度高、功耗低等優點[1]，廣泛應用于可穿戴設備中，文獻[2]使用MEMS設計了一種可以在空間中使用的鼠標，文獻[3]使用MEMS實現了空間定位等功能。文獻[4,5]分別將MEMS放置于腰間和小腿處進行了步長距離估測，但兩種方式對使用者穿戴均造成不便。文獻[6]采用了行人的身高與步頻估算步長，文獻[7]提出了一種基于模糊邏輯的非線性步長估計方法，以步頻、身高、體重作為邏輯系統輸入變量設計模糊邏輯控制器,得到可變的步長估計系數,從而實現了步長動態估算，但以上2種方法測量的步長精確度有限。將MEMS置于腳部，利用壓力傳感器[9]、地面反作用力[10]、開關傳感器[8]或者腳移動模式[11]等方式計算步長，盡管計算精度良好，但對測試條件要求比較嚴格且實驗設備成本高，很難在生活中廣泛應用。

本文設計了一種新型步長測量系統，采用MPU6050作為加速度傳感器[12]、CC2541藍牙模塊負責對數據進行采集和上傳，系統成本低廉。MPU6050體積小，易于平放置鞋底，避免了穿戴不方便。采用C#作為程序語言進行了上位機設計，結合數據處理、坐標變換、脈沖提取以及積分等方法實現了對任意大小步長距離的計算。

1 硬件設計

1.1 前期準備

因為系統需要將MPU6050傳感器模塊水平置于鞋底，且需要固定，所以用到的實驗工具主要有：一雙底足夠厚且軟的運動鞋、工具刀、鑷子、膠槍及膠棒等。

1.2 硬件設計

系統采用MPU6050加速度計模塊，模塊內部對原始的加速度和角速度進行濾波處理，并通過卡爾曼濾波估測出最優的角度值，故其能夠輸出穩定的三軸加速度、角速度和角度值，系統選擇加速度量程為(-16～16)gn，角度量程為-180°～180°。

采用了2塊CC2541藍牙模塊，分別用于下位機端和上位機端，實現了數據的無線傳輸。MPU6050加速度計鑲嵌在鞋底，藍牙模塊置于鞋面上以保證數據可靠傳輸。

2 軟件及算法設計

C#程序語言上位機的工作流程如圖1。

軟件分為4部分：接收、計算并顯示數據變化曲線、坐標變換、波形規范及脈沖提取算法、計算單步步長距離。

圖1 C#上位機軟件工作流程

1)接收、計算并顯示數據變化曲線

下位機發送的數據以數據幀的形式體現，每11個字節的數據為一幀，故上位機接收的數據首先需要進行幀校驗，幀校驗公式為

Data[10]=(Data[0]+Data[1]+Data[2]+Data[3]+Data[4]+Data[5]+Data[6]+Data[7]+Data[8]+

Data[9])& 0XFF

(1)

式中Data為接收到的數據，每一幀的最后一個字節為校驗值，如果前10個字節的總和與0XFF邏輯與運算后等于第11個字節的值，則該幀數據準確。

對得到的數據進行計算可求得加速度、角度等參數

a[0]=long(((short)(Data[3]<<8|Data[2]))×4.79)，a[1]=long(((short)(Data[5]<<8|Data[4]))×4.79),a[2]=long(((short)(Data[7]<<8|Data[6]))×4.79),angle[0]=long((short)(Data[3]<<8|Data[2])×5.49,angle[1]=long((short)(Data[5]<<8|Data[4])×5.49),angle[2]=long((short)(Data[7]<<8|Data[6]×5.49)

(2)

式中a[0]，a[1]，a[2]分別為x，y，z軸加速度；angle[0]，angle[1]，angle[2]為繞x，y，z軸的旋轉角度。為了保證數據的精度，此時計算出來的加速度較實際的加速度值擴大了10 000倍，角度比實際的角度值擴大了1 000倍。

為了確保對數據變化實時顯示，系統中選用了C#中的ZedGraph控件及多線程機制，圖2為上位機顯示的3個軸向的加速度的變化曲線。

圖2 三軸加速度變化曲線

2)坐標變換

(1)坐標變換模型

圖3中，規定船前進的方向為z軸，此時橫滾相當于繞z軸旋轉φ角，俯仰相當于繞y軸旋轉θ角，偏轉相當于繞x軸旋轉ψ角，對于旋轉順序，作如下規定

RPY(φ,θ,ψ)=Rot(z,φ)Rot(y,θ)Rot(x,ψ)

(3)

式中RPY為橫滾、俯仰、偏轉3種旋轉的組合變換，其中

(4)

圖3 船體的橫滾、俯仰和偏轉

(2)坐標變換模型在系統中的應用

取MPU6050所在的坐標系為S坐標系，大地所在的坐標系為G坐標系，則MPU6050在空中平移和翻轉時實際是其相對于G坐標系發生了平移和翻轉運動，圖4為這一坐標變換關系。

圖4 加速度傳感器坐標變換示意

圖4中OgXgYgZg坐標系代表G坐標系，其原點Og代表鞋離地前MPU6050垂直投影到地面上的點，取人體前進方向為Yg，橫移方向為Xg，垂直于地面的方向為Zg。OSXSYSZS坐標系代表S坐標系，在此坐標系中，規定鞋頭的方向為YS，鞋內側的方向為XS，垂直于鞋底的方向為ZS。

根據RPY坐標變換原理，可以將S坐標系下的傳感器測得的加速度轉換成G坐標系下的加速度。圖5為經過RPY坐標變換之后G坐標系下的加速度變化曲線。

圖5 G坐標系下的加速度變化曲線(規范前)

圖中的脈沖代表人體向前邁出了一步，兩個脈沖之間的部分代表加速度計所在的腳站立不動。

3)波形規范及脈沖提取算法

經過坐標變換的加速度曲線可以看出：在人體原地不動時，加速度的輸出并不為零，如果對此時的加速度進行二次積分求取位移，得到的步長將不為零，有悖于實際。繼續觀察經過坐標變換之后的加速度顯示曲線可知，如果僅僅計算一步的距離，只需要對行進過程的加速度進行二次積分即可，而靜止時的加速度不應該作二次積分運算。

針對加速度曲線變化規律，設計了一種波形規范與脈沖提取的算法，該算法在一個新線程中獨立循環運行，首先，設置了一個開關變量button，如果button為true，則認為人體處于步行狀態；如果button為false，則認為人體處于靜止狀態。然后，設置進入步行狀態的標志位flag_in和靜止狀態的標志位flag_out，當進入步行狀態時，flag_in=true， flag_out=false， button=true。當進入靜止狀態時flag_in=false， flag_out=true，button=false。設置ay為前行方向的加速度，并記錄ay的4個連續的歷史值為ay_his[0]～ay_his[3]，進入步行狀態的條件為ay-ay_his[0]>delta，其中delta為一個閾值，根據經驗設置，圖6為算法的程序流程。

圖6 波形規范算法流程

圖中的button_his代表了button變量上一次的歷史值。首次運行該線程時將button初始化為true，flag_in初始化為false。

實驗表明：算法能夠有效地提取人體行走時的整個脈沖波形，并記錄行走狀態下的加速度值。經過規范之后的加速度曲線顯示如圖7所示。

圖7 波形規范之后的加速度變化曲線

與圖5對比，在步行狀態下圖7的數據與圖5一致，保證了采集的數據均為最原始、最準確的數據，而在靜止狀態下，圖7的加速度值為零，消除了靜態誤差。

4)計算單步步長距離

加速度經過二次積分可以求取位移，圖8為速度—時間變化曲線。速度與加速度之間存在著如下的關系

vt=vt0+a(t)t

圖8 速度—時間曲線

如果從時間t0開始采樣，到時刻t結束，則在這段連續的時間域中，對于位移s(t)、速度v(t)和加速度a(t)，存在

(5)

式中v(t0)為系統在t0時刻的瞬時速度;s(t0)為系統從0～t0時刻的累積位移。

由于MPU6050屬于數字傳感器，其輸出的數據為數字離散值，所以,依據高等數學知識可以將速度—時間曲線分解為若干個直角梯形，當t=t0時，s(t)=0，有

(tn-tn-1)

(6)

令t1-t0=t2-t1=…=tn-tn-1=Δt，其中，Δt為MPU6050的采樣時間間隔，當n>1時，有

(7)

上式可用于在連續時間域中計算s(t)，但如果在離散域中計算s(t)，應使用

(8)

式中v[k]為k時刻的速度值。當n>1時，有

…+a[n-1])Δt

(9)

…+v[n-1])Δt

(10)

將式(9)、式(10)合并[12]，用a[n]表示s[n]，有

s[n]=n·v[0]·Δt+[(n-1)·a[1]+(n-2)·

a[n])·(Δt)2

(11)

可知，只要已知系統的初始速度v[0]和各個時刻的加速度即可求出目標物體的軸向移動距離。考慮到在實際系統中計算的是單步距離，因此，實際上在邁出一步之前的初始速度為零，即v[0]等于零，將式(11)進一步化簡為

s[n]=[(n-1)·a[1]+(n-2)·a[2]+…+

(12)

3 實驗測試結果

為了保證實驗的準確性，排除其他因素的干擾，實驗環境選擇了一條長直樓道，且地面盡量水平以減少傳感器傾斜帶來的誤差影響。人體徑直向前行走，對行走過程中的每一步的步長進行測試，測試結果如表1。

表1 系統對單步步長的測試結果

實際位移值為使用軟尺測得，測試結果表明：系統能將單步步長的測量誤差控制在3.4 %以內。

4 結 論

研究了一種新型的計步器，系統對于單步的測量誤差能控制在3.4 %以內，實現了良好的單步步長測量效果。另外，系統還可以通過記錄行走過程中的有效脈沖數計算行走的步數，累加步長值計算總的行程，從而為穿戴設備提供了一種新型的計步及測距原理。

參考文獻:

[1] 劉 宇，鞠文斌，劉羽熙.MEMS加速度傳感器計量檢測技術的研究進展[J].計測技術，2010,30(4):5-8.

[2] 周 獲.基于MEMS技術的無線空中鼠標的研究[D].廈門：華僑大學,2013.

[3] 董 威.六軸MEMS傳感器空間定位設計與實現[J].計算機技術與發展,2014(7):250-252.

[4] 任凱天,劉 昱,汪少初.基于腰部移動模型的行走步長計算方案[J].電子測量與儀器學報,2012,26(8):711-715.

[5] 任凱天.基于慣性傳感系統的人員行進分析及位移計算[D].天津：天津大學,2012.

[6] 劉 宇,周 帆,李云梅,等.基于人體多方位運動的三維自主導航定位算法[J].中國慣性技術學報,2016,24(4):449-453.

[7] 賀鋒濤,趙勝利,周廣平,等.基于模糊邏輯的室內導航步長估計方法研究[J].電子技術應用,2016,42(11):59-61.

[8] Zijlstra W,Rutgers A,Hof A L,et al.Voluntary and involuntary adaptation of walking to temporal and spatial constraints[J].Gait & Posture,1995,3(1):13-18.

[9] Zijlstra W,Rutgers F A W,Weerden T W.Voluntary and involuntary adaptation of gait in Parkinson’s disease[J].Gait and Postrue,1998,7:53-63.

[10] Zijlstra W,Dietz V.Adaptability of the human stride cycle during split-belt walking[J].Gait & Posture,1995,3(4):250-257.

[11] Huitema R B,Hof A L,Postema K.Ultrasonic motion analysis system—measurement of temporal and spatial gait parame-ters[J].Journal of Biomechanics,2002,35(6):837-842.

[12] 胡三慶.基于MEMS加速度傳感器的空間運動軌跡追蹤系統設計與實現[D].武漢：華中科技大學,2009.