本發(fā)明屬于可穿戴傳感器領(lǐng)域，具體涉及一種可穿戴式運(yùn)動(dòng)傳感器及其抗磁場(chǎng)干擾的方法。

背景技術(shù)：

隨著微機(jī)電系統(tǒng)(MEMS)技術(shù)的發(fā)展，基于MEMS的慣性傳感器和磁力計(jì)以成本低、體積小、重量輕的優(yōu)點(diǎn)被廣泛地應(yīng)用在人體運(yùn)動(dòng)分析領(lǐng)域，如手勢(shì)識(shí)別，關(guān)節(jié)運(yùn)動(dòng)學(xué)分析，日?；顒?dòng)監(jiān)測(cè)。一般地，一個(gè)典型的運(yùn)動(dòng)傳感器包括三軸加速度計(jì)、三軸陀螺儀和三軸磁力計(jì)，對(duì)于人體運(yùn)動(dòng)分析來說，準(zhǔn)確地測(cè)量身體部位的姿態(tài)非常關(guān)鍵。為了準(zhǔn)確地獲取身體部位的姿態(tài)，通過融合算法進(jìn)行多傳感器信息融合是常用的方法，比較常用的融合算法有擴(kuò)展卡爾曼濾波法、梯度下降法、互補(bǔ)濾波法等。通過這些算法，可以提高估算姿態(tài)的精確度。然而，估算的精度仍然容易受到外部環(huán)境的干擾，特別是環(huán)境中的磁場(chǎng)干擾。因?yàn)榈卮艌?chǎng)非常微弱，日常環(huán)境中的建筑物，電磁設(shè)備，電腦，手機(jī)等都會(huì)產(chǎn)生較大的磁場(chǎng)干擾。由磁場(chǎng)干擾引起的偏航角的均方根誤差會(huì)達(dá)到15.4°(Yadav et al.在磁場(chǎng)干擾條件下通過AHRS精確估計(jì)姿態(tài)Sensors 14 2014)。為了解決磁場(chǎng)干擾對(duì)估算精度的影響，有些方法通過設(shè)定了一個(gè)磁場(chǎng)強(qiáng)度閾值，超過閾值的磁場(chǎng)被認(rèn)為是無效磁場(chǎng)，但是閾值的調(diào)整是一個(gè)非常繁瑣的過程，而且很難找到一個(gè)非常合適的閾值。又如申請(qǐng)?zhí)枮镃N201310431846.6的發(fā)明專利中公開了一種運(yùn)動(dòng)慣性追蹤系統(tǒng)，該系統(tǒng)通過移除磁力計(jì)模塊來回避磁場(chǎng)干擾對(duì)精度的影響的問題。然而，減少磁力計(jì)模塊后，無法獲得絕對(duì)偏航角，且偏航角會(huì)存在漂移誤差。而申請(qǐng)?zhí)枮镃N201510666248.6的發(fā)明專利中公開了一種基于微慣性傳感器的室內(nèi)定位方法，該方法具有靜動(dòng)態(tài)測(cè)量數(shù)據(jù)辨識(shí)模塊，能夠針對(duì)靜態(tài)和動(dòng)態(tài)分別采取不同的方法估算姿態(tài)角，但是該方法沒有針對(duì)磁場(chǎng)干擾進(jìn)行特別的優(yōu)化，在環(huán)境中有磁場(chǎng)干擾時(shí)，仍然會(huì)引入較大的估算誤差。在有磁場(chǎng)干擾條件下精確估算姿態(tài)角對(duì)人體運(yùn)動(dòng)分析具有重要的意義，有必要提出一種針對(duì)外界磁干擾進(jìn)行特別優(yōu)化的方法，使得運(yùn)動(dòng)傳感器的姿態(tài)估計(jì)具有較強(qiáng)的抗磁場(chǎng)干擾能力。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于解決現(xiàn)有技術(shù)中抗磁場(chǎng)干擾能力弱的問題，并提供一種可穿戴式運(yùn)動(dòng)傳感器及其抗磁場(chǎng)干擾的方法，解決了運(yùn)動(dòng)傳感器在外部有磁場(chǎng)干擾時(shí)精度降低的技術(shù)問題。本發(fā)明為解決技術(shù)問題，所采用的具體技術(shù)方案如下：一種可穿戴式運(yùn)動(dòng)傳感器，包括鋰電池、電源管理模塊、MCU模塊、傳感器模塊和狀態(tài)指示模塊，所述鋰電池與電源管理模塊相連，所述電源管理模塊、傳感器模塊以及狀態(tài)指示模塊均與MCU模塊相連。

進(jìn)一步的，所述MCU模塊上集成有WIFI。

進(jìn)一步的，所述傳感器模塊由加速度計(jì)、陀螺儀以及磁力計(jì)組成。

本發(fā)明的另一目的是提供一種利用可穿戴式運(yùn)動(dòng)傳感器的抗磁場(chǎng)干擾的方法，包括以下步驟：實(shí)時(shí)測(cè)量傳感器模塊的加速度、角速度和磁場(chǎng)信息，根據(jù)加速度和角速度信息進(jìn)行靜態(tài)判斷，如果為靜態(tài)，則保持當(dāng)前姿態(tài)不變，如果為非靜態(tài)，則進(jìn)行外部磁場(chǎng)干擾程度計(jì)算，根據(jù)磁場(chǎng)干擾程度計(jì)算出融合加速度和角速度的6軸算法以及融合加速度、角速度和磁場(chǎng)的9軸算法的權(quán)重，加權(quán)后得到傳感器的當(dāng)前姿態(tài)，

式中：為估算的傳感器在t時(shí)刻的姿態(tài)(四元數(shù)形式)，為6軸算法得到的姿態(tài)，為9軸算法得到的姿態(tài)，λ和1-λ分別為6軸算法和9軸算法的權(quán)重。

進(jìn)一步的，所述靜態(tài)判斷具體步驟如下：進(jìn)行靜態(tài)檢測(cè)時(shí)，設(shè)置了加速度靜態(tài)判斷條件和角速度判斷條件，只有同時(shí)滿足這兩個(gè)條件時(shí)才判定當(dāng)前傳感器處于靜態(tài)。

加速度的靜態(tài)判斷條件可描述為在一定的時(shí)間段內(nèi)，3軸方向上的加速度變化幅度小于設(shè)定的閾值，可以表示為：

式中：表示傳感器在t時(shí)刻X軸方向的加速度，t₀表示一個(gè)可調(diào)的時(shí)間間隔，是t-t₀時(shí)刻X軸方向的加速度，th_a是設(shè)定的靜態(tài)判斷的閾值，Y軸，Z軸方向的加速度靜態(tài)判斷條件與X軸的條件相同，X、Y、Z軸的加速度靜止判斷條件是“與”的關(guān)系。

角速度數(shù)據(jù)判斷的條件可描述為3軸的角速度必須分別小于一個(gè)設(shè)定的閾值，可表示為：

式中：ω_xω_yω_z分別是3軸的角速度，th_gyro是設(shè)定的角速度靜止判斷閾值，X,Y,Z軸的角速度靜止判斷條件是“與”的關(guān)系。

進(jìn)一步的，所述6軸算法的權(quán)重λ的求解過程如下：

將測(cè)得的磁場(chǎng)強(qiáng)度和磁傾角與地磁場(chǎng)進(jìn)行比較，從而確定干擾的程度，其計(jì)算公式可表示為：

λ₁＝|||mag||-m₀|/m₀ifλ₁＞1，λ₁＝1

λ₂＝|θ_dip-θ₀|/th_dip ifλ₂＞1，λ₂＝1

λ＝(λ₁+λ₂)/2

式中：||mag||為當(dāng)前測(cè)得的磁場(chǎng)大小，θ_dip為當(dāng)前測(cè)得的磁傾角，m₀和θ₀分別是地磁場(chǎng)大小和磁傾角，th_dip為設(shè)定的最大磁傾角誤差，λ₁是通過磁場(chǎng)大小計(jì)算的磁場(chǎng)干擾程度，λ₂是根據(jù)磁傾角計(jì)算的干擾程度，最后的權(quán)重λ取λ₁、λ₂的平均值。

本發(fā)明相對(duì)于現(xiàn)有技術(shù)而言，其有益效果是：

1、使用本發(fā)明計(jì)算姿態(tài)角，在靜態(tài)情況下，可以使傳感器的角度估算精度對(duì)任意強(qiáng)度和時(shí)長(zhǎng)的磁場(chǎng)干擾免疫。

2、在動(dòng)態(tài)情況下，能夠明顯降低外部磁場(chǎng)干擾對(duì)偏航角估算精度的影響。

3、本發(fā)明不依賴于具體的融合算法，通用性好，可以附加在常用的姿態(tài)估算算法上，幫助其增強(qiáng)抗磁場(chǎng)干擾能力。

4、本發(fā)明硬件結(jié)構(gòu)簡(jiǎn)單，使用元器件少，能夠明顯地減小電路板體積和降低成本。

附圖說明

圖1為本發(fā)明中運(yùn)動(dòng)傳感器結(jié)構(gòu)示意圖；

圖2為本發(fā)明中抗磁場(chǎng)干擾方法的結(jié)構(gòu)圖；

圖3為本發(fā)明中加速度靜態(tài)判斷參數(shù)確定示意圖；

圖4為本發(fā)明中靜態(tài)抗磁場(chǎng)干擾驗(yàn)證方法示意圖；

圖5為本發(fā)明中靜態(tài)抗磁場(chǎng)干擾驗(yàn)證結(jié)果圖；

圖6為本發(fā)明中動(dòng)態(tài)抗磁場(chǎng)干擾驗(yàn)證方法示意圖；

圖7為本發(fā)明中動(dòng)態(tài)抗磁場(chǎng)干擾驗(yàn)證時(shí)外部磁場(chǎng)強(qiáng)度圖；

圖8為本發(fā)明中動(dòng)態(tài)抗磁場(chǎng)干擾驗(yàn)證相對(duì)歐拉角誤差圖；

圖9為本發(fā)明中動(dòng)態(tài)抗磁場(chǎng)干擾驗(yàn)證相對(duì)歐拉角均方根誤差統(tǒng)計(jì)圖。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明進(jìn)行進(jìn)一步說明，因便于更好地理解。本發(fā)明中的技術(shù)特征在不相互沖突的前提下，均可進(jìn)行相互組合，不構(gòu)成限制。

本發(fā)明中所涉及的部分名詞含義如下：

姿態(tài)角也稱為歐拉角，是一種直觀的姿態(tài)表示方法，本發(fā)明涉及的歐拉角為ZYX旋轉(zhuǎn)順序的歐拉角，其中繞Z軸旋轉(zhuǎn)為偏航角，繞Y軸旋轉(zhuǎn)為俯仰角，繞X軸旋轉(zhuǎn)為滾轉(zhuǎn)角。

四元數(shù)是姿態(tài)表示的另一種方法，可以理解為繞一個(gè)單位向量旋轉(zhuǎn)一個(gè)角度，四元數(shù)表示法可以避免歐拉角表示法存在的奇異性問題，一個(gè)四元數(shù)可以表示為：

其中，e＝[e_xe_ye_z]表示旋轉(zhuǎn)軸，θ表示矢量繞旋轉(zhuǎn)軸旋轉(zhuǎn)的角度。

6軸算法是指僅通過融合三軸加速度、三軸角速度信息來估算姿態(tài)角的多傳感器信息融合算法。

9軸算法是指通過融合三軸加速度、三軸角速度和三軸磁場(chǎng)信息來估算姿態(tài)角的多傳感器信息融合算法。

本發(fā)明使用一個(gè)帶有加速度計(jì)、陀螺儀和磁力計(jì)的運(yùn)動(dòng)傳感器以及用于該設(shè)備的抗磁場(chǎng)干擾的方法，實(shí)時(shí)估算傳感器當(dāng)前的姿態(tài)。本發(fā)明具體的實(shí)施過程如下：

1)準(zhǔn)備工作：

圖1為本發(fā)明中運(yùn)動(dòng)傳感器系統(tǒng)模塊結(jié)構(gòu)圖，包括鋰電池、電源管理模塊、MCU模塊、傳感器模塊和狀態(tài)指示模塊，所述鋰電池與電源管理模塊相連，所述電源管理模塊、傳感器模塊以及狀態(tài)指示模塊均與MCU模塊相連。本發(fā)明的抗磁場(chǎng)干擾的方法通過編程的方式實(shí)現(xiàn)在運(yùn)動(dòng)傳感器中，進(jìn)行姿態(tài)角的實(shí)時(shí)估算，所述的MCU模塊上集成有WiFi模塊，本實(shí)施例選擇為TI公司的CC3200芯片，但不限于此；所述所述傳感器模塊由加速度計(jì)、陀螺儀以及磁力計(jì)組成，本實(shí)施例選擇InvenSense公司的9軸傳感器一體化傳感器MPU9250，但不限于此。所述運(yùn)動(dòng)傳感器在使用之前，需進(jìn)行磁力計(jì)校準(zhǔn)，以去掉固定的磁場(chǎng)干擾。運(yùn)動(dòng)傳感器在使用過程中，采樣頻率為200Hz。

2)靜態(tài)判斷與磁場(chǎng)干擾程度計(jì)算

圖2所示為本發(fā)明中抗磁場(chǎng)干擾方法結(jié)構(gòu)框圖，運(yùn)動(dòng)傳感器實(shí)時(shí)采集當(dāng)前的加速度，角速度和磁場(chǎng)信息，首先根據(jù)加速度和角速度進(jìn)行靜止?fàn)顟B(tài)檢測(cè)，如果判斷為靜態(tài)的，則保持當(dāng)前姿態(tài)不變，如果非靜態(tài)，則進(jìn)行外部磁場(chǎng)干擾程度計(jì)算。進(jìn)行靜態(tài)檢測(cè)時(shí)，設(shè)置了加速度靜態(tài)判斷條件和角速度判斷條件，只有同時(shí)滿足這兩個(gè)條件時(shí)才判定當(dāng)前傳感器處于靜態(tài)。

加速度的靜態(tài)判斷條件可描述為在一定的時(shí)間段內(nèi)，3軸方向上的加速度變化幅度小于設(shè)定的閾值，可以表示為：

式中：表示傳感器在t時(shí)刻X軸方向的加速度，t₀表示一個(gè)可調(diào)的時(shí)間間隔，是t-t₀時(shí)刻X軸方向的加速度，th_a是設(shè)定的靜態(tài)判斷的閾值，Y軸，Z軸方向的加速度靜態(tài)判斷條件與X軸的條件相同，X、Y、Z軸的加速度靜止判斷條件是“與”的關(guān)系。圖3所示為加速度靜態(tài)判斷閾值選擇示意圖，選取的th_a應(yīng)該比加速度計(jì)在靜態(tài)時(shí)的峰峰值稍大，t₀決定了實(shí)際靜態(tài)到判定靜態(tài)的延遲，與在本實(shí)施例中，t₀選取為0.5s，th_a選取為0.04g。

角速度數(shù)據(jù)判斷的條件可描述為3軸的角速度必須分別小于一個(gè)設(shè)定的閾值，可表示為：

式中：ω_xω_yω_z分別是3軸的角速度，th_gyro是設(shè)定的角速度靜止判斷閾值，X，Y，Z軸的角速度靜止判斷條件是“與”的關(guān)系。

同樣地，設(shè)定的th_gyro應(yīng)該比陀螺儀在靜態(tài)時(shí)的峰峰值稍大，在本實(shí)施例中，th_gyro選取為3°/s。

在計(jì)算磁場(chǎng)干擾程度時(shí)，將測(cè)得的磁場(chǎng)強(qiáng)度和磁傾角與地磁場(chǎng)進(jìn)行比較，從而確定干擾的程度，其計(jì)算公式可表示為：

λ₁＝|||mag||-m₀|/m₀ifλ₁＞1，λ₁＝1

λ₂＝|θ_dip-θ₀|/th_dip ifλ₂＞1，λ₂＝1

λ＝(λ₁+λ₂)/2

式中：||mag||為當(dāng)前測(cè)得的磁場(chǎng)大小，θ_dip為當(dāng)前測(cè)得的磁傾角，m₀和θ₀分別是地磁場(chǎng)大小和磁傾角，他們的值在磁力計(jì)校準(zhǔn)的過程中確定。th_dip為設(shè)定的最大磁傾角誤差，λ₁是通過磁場(chǎng)大小計(jì)算磁場(chǎng)干擾程度，λ₂是根據(jù)磁傾角計(jì)算的干擾程度，最后的權(quán)重λ取λ₁、λ₂的平均值。

在本實(shí)施例中，磁場(chǎng)干擾程度計(jì)算的參數(shù)確定為：m₀＝0.46Gs，θ₀＝40.6°，th_dip＝20°，實(shí)時(shí)的磁傾角通過公式θ_dip＝arccos(A(q)g·h/||h||)計(jì)算，式中，A(q)為當(dāng)前姿態(tài)的旋轉(zhuǎn)矩陣形式，g為重力加速度，h為測(cè)得的磁場(chǎng)。

3)傳感器姿態(tài)角的計(jì)算

如圖2所示，在本發(fā)明所述方法中，同時(shí)進(jìn)行融合加速度、角速度信息的6軸算法和融合加速度、角速度磁場(chǎng)信息的9軸算法，本實(shí)施例的6軸算法和9軸算法均為基于的梯度下降法的融合算法。在傳感器姿態(tài)求解過程中，當(dāng)傳感器處于靜態(tài)時(shí)，直接以上一次估算的姿態(tài)作為本次姿態(tài)。當(dāng)傳感器處于動(dòng)態(tài)時(shí)，根據(jù)計(jì)算得到的干擾程度權(quán)重λ和1-λ分別應(yīng)用于6軸算法和9軸算法，得到本次的姿態(tài)，可表示為：

式中：為估算的傳感器在t時(shí)刻的姿態(tài)(四元數(shù)形式)，為6軸算法得到的姿態(tài)，為9軸算法得到的姿態(tài)。

4)靜態(tài)抗磁場(chǎng)干擾驗(yàn)證

圖4為本發(fā)明中靜態(tài)抗磁場(chǎng)干擾驗(yàn)證方法示意圖，在本實(shí)驗(yàn)中，將傳感器靜止放置在沒有磁場(chǎng)干擾的平板上，將一個(gè)圓形的永磁鐵來回靠近運(yùn)動(dòng)傳感器，以此方式來模擬外界的磁場(chǎng)干擾，采集這段時(shí)間的加速度、角速度和磁力計(jì)信息，同時(shí)用本發(fā)明所述的方法和原始的9軸算法估算傳感器當(dāng)前的姿態(tài)角，通過比較兩種方法得到的姿態(tài)角來確定本發(fā)明的靜態(tài)抗磁場(chǎng)干擾效果。

5)動(dòng)態(tài)抗磁場(chǎng)干擾驗(yàn)證

動(dòng)態(tài)抗磁場(chǎng)干擾驗(yàn)證實(shí)驗(yàn)在一個(gè)3軸儀器轉(zhuǎn)臺(tái)上進(jìn)行，該3軸轉(zhuǎn)臺(tái)具有XYZ三個(gè)旋轉(zhuǎn)自由度，與歐拉角姿態(tài)表示法的XYZ軸一一對(duì)應(yīng)，轉(zhuǎn)臺(tái)的每個(gè)旋轉(zhuǎn)軸配有電機(jī)和編碼器，電機(jī)用于提供旋轉(zhuǎn)動(dòng)力，編碼器用于測(cè)量每個(gè)軸的旋轉(zhuǎn)角度，因編碼器的精度高，于是將測(cè)得的3軸角度作為標(biāo)準(zhǔn)值與估算的姿態(tài)角進(jìn)行比較，從而可得到估算的姿態(tài)角的精度。

圖6為本實(shí)施例的動(dòng)態(tài)抗磁場(chǎng)干擾驗(yàn)證方法示意圖，左下角為儀器轉(zhuǎn)臺(tái)的坐標(biāo)系，將運(yùn)動(dòng)傳感器固定在X軸框架上。X軸框架上安裝有磁場(chǎng)干擾模擬裝置，包括兩端封閉的方形塑料管和一個(gè)圓形永磁鐵。實(shí)驗(yàn)進(jìn)行時(shí)，XYZ軸同時(shí)轉(zhuǎn)動(dòng)，圓形永磁鐵會(huì)在重力作用下來回滑動(dòng)，永磁鐵與運(yùn)動(dòng)傳感器的距離也隨之變化，從而產(chǎn)生變化的磁場(chǎng)干擾，以此方式模擬外界的磁場(chǎng)干擾。在此條件下，同時(shí)使用本發(fā)明的方法、原始的9軸算法、原始的6軸算法分別估算傳感器的姿態(tài)角，與轉(zhuǎn)臺(tái)提供的標(biāo)準(zhǔn)姿態(tài)角進(jìn)行比較，得到相對(duì)歐拉角誤差，通過比較3種算法的估算誤差，確定本發(fā)明的動(dòng)態(tài)抗磁場(chǎng)干擾效果。

6)抗磁場(chǎng)干擾的效果

在本實(shí)施例中進(jìn)行了靜態(tài)抗磁場(chǎng)干擾實(shí)驗(yàn)和動(dòng)態(tài)抗干擾實(shí)驗(yàn)，圖5為靜態(tài)抗干擾實(shí)驗(yàn)結(jié)果，可以看到，本發(fā)明估算的姿態(tài)角完全不受外界磁場(chǎng)干擾影響，在強(qiáng)磁場(chǎng)干擾下，仍然保持不變，符合實(shí)際情況，而原始的9軸算法的偏航角偏差則達(dá)到了50°。本實(shí)施例中，動(dòng)態(tài)抗干擾實(shí)驗(yàn)共進(jìn)行了10次，圖7為其中一次動(dòng)態(tài)抗磁場(chǎng)干擾實(shí)驗(yàn)時(shí)外部磁場(chǎng)強(qiáng)度圖，由圖可見，磁場(chǎng)干擾周期性地出現(xiàn)。圖8為本次實(shí)驗(yàn)三種對(duì)比方法估算的相對(duì)歐拉角誤差圖，圖9為全部的10次實(shí)驗(yàn)得到的相對(duì)歐拉角均方根誤差圖，由圖8和圖9可見，本發(fā)明所述的方法得到的相對(duì)歐拉角誤差明顯小于原始的6軸算法和9軸算法。

由此可見，本發(fā)明無論在靜態(tài)還是在動(dòng)態(tài)條件下，均提高了估算姿態(tài)角的精度。

以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，有關(guān)技術(shù)領(lǐng)域的普通技術(shù)人員，在不脫離本發(fā)明的精神和范圍的情況下，還可以做出各種變化和變型。如選擇的6軸算法和9軸算法也可以使用基于擴(kuò)展卡爾曼濾波或互補(bǔ)濾波的融合算法。由此可見，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。