張劍鋒，周澤波,2

穿戴式導(dǎo)航傳感器在步行康復(fù)訓(xùn)練監(jiān)控中的應(yīng)用

張劍鋒1，周澤波1,2

(1.電子科技大學(xué) 航空航天學(xué)院，成都 611731；2.中國科學(xué)院 測量與地球物理研究所，武漢 430077)

針對步行康復(fù)訓(xùn)練中的實(shí)時監(jiān)控問題，開發(fā)了低成本導(dǎo)航傳感器的硬件平臺，設(shè)計了一種計算步行距離的零速修正算法，給出了本地坐標(biāo)系下的導(dǎo)航解算公式，并論述了三組零速檢測閾值聯(lián)合判決條件。最后通過實(shí)驗(yàn)驗(yàn)證了本文所提出算法的正確性和實(shí)用性，可滿足步行康復(fù)訓(xùn)練監(jiān)控的應(yīng)用需求。

步行康復(fù)訓(xùn)練；步行距離；導(dǎo)航傳感器；零速修正

0 引言

受中樞神經(jīng)系統(tǒng)受損導(dǎo)致的關(guān)節(jié)功能活動障礙，會不同程度的影響偏癱病人的步行能力，如文獻(xiàn)[1]所述，主要表現(xiàn)為無法步行或步態(tài)異常，嚴(yán)重影響病人的日常生活。不正確的康復(fù)訓(xùn)練方法不僅無法達(dá)到康復(fù)效果，還可能引發(fā)其他的并發(fā)癥或加重異常運(yùn)動模式。文獻(xiàn)[2]論述，在步行康復(fù)訓(xùn)練中，監(jiān)控并分析患者步距、步長和步速等信息對康復(fù)治療師掌握病情有重要參考價值?；颊叩目祻?fù)訓(xùn)練通常在室內(nèi)進(jìn)行，采用室內(nèi)導(dǎo)航定位傳感技術(shù)來獲取相關(guān)狀態(tài)信息。目前，無線定位(WIFI，藍(lán)牙，ZigBee等)、射頻識別(RFID)、計算機(jī)視覺等多種導(dǎo)航傳感器已廣泛應(yīng)用于室內(nèi)導(dǎo)航定位，文獻(xiàn)[3]分別對各種室內(nèi)定位技術(shù)進(jìn)行了論述，但這些技術(shù)無法反應(yīng)患者的詳細(xì)步態(tài)信息。穿戴式傳感器(以慣性器件為主)可提供更多的細(xì)節(jié)信息，因此是實(shí)現(xiàn)人體運(yùn)動捕獲與跟蹤的主流方案，如文獻(xiàn)[4]中介紹的多種利用慣性技術(shù)的運(yùn)動捕獲方案。文獻(xiàn)[5]通過可穿戴傳感器檢測脛骨垂直狀態(tài)，將行人的行進(jìn)分為若干個單步，可以實(shí)現(xiàn)較大概率下得到較小的距離誤差，但由于偏癱病人的肢體大多呈現(xiàn)屈曲狀態(tài)，即無法檢測到脛骨垂直狀態(tài)，故這種方法有一定局限性。文獻(xiàn)[6]利用固定在腳上的慣性傳感器實(shí)現(xiàn)行人導(dǎo)航，并設(shè)計了三個零速狀態(tài)檢測器，包括加速度方差檢測，加速度能量檢測和角速率幅值檢測。文獻(xiàn)[7]在此基礎(chǔ)上，針對每一步行走的停頓設(shè)計算法對運(yùn)算結(jié)果做平滑，雖然實(shí)時性略受影響，但導(dǎo)航結(jié)果更加連續(xù)和準(zhǔn)確。

本文開發(fā)了一種低成本可穿戴的慣性傳感器硬件平臺。在此基礎(chǔ)上，建立可穿戴傳感器的導(dǎo)航數(shù)學(xué)模型，設(shè)計了一種適用于康復(fù)訓(xùn)練的零速修正(zero-velocity update，ZUPT)算法進(jìn)行步速、姿態(tài)以及位置的實(shí)時修正。最后采用實(shí)驗(yàn)驗(yàn)證本文所開發(fā)穿戴式導(dǎo)航傳感器可滿足患者康復(fù)信息監(jiān)控精度要求，為步行訓(xùn)練分析提供重要參考數(shù)據(jù)。

1 穿戴式硬件平臺及導(dǎo)航數(shù)學(xué)模型

1.1 硬件平臺

本文所開發(fā)的穿戴式硬件平臺如圖1所示。硬件平臺由三軸微機(jī)械(micro-electro-mechanical system，MEMS)加速度計、三軸MEMS陀螺儀、三軸磁力計，嵌入式微處理器與無線通信模塊以及電源管理模塊組成。陀螺儀和加速度計采用InvenSense公司的MPU6050芯片，陀螺儀的角速度測量范圍設(shè)置為±2 000°/s，加速度計的測量范圍為±39.2 m/s2，采樣頻率為1 kHz。磁力計采用霍尼韋爾公司的HMC5883L，測量范圍設(shè)置為±1.3 mT，數(shù)據(jù)輸出頻率為75 Hz。嵌入式微處理器是基于ARM Cortex-M4內(nèi)核的STM32F405主控芯片，工作頻率為168 MHz，通過集成電路總線(inter-integrated circuit，IIC)以100 Hz頻率讀取加速度計/陀螺儀數(shù)據(jù)，并以75 Hz讀取磁力計數(shù)據(jù)?？紤]到導(dǎo)航計算涉及浮點(diǎn)運(yùn)算，我們使用了STM32F405浮點(diǎn)運(yùn)算單元。無線通信模塊采用HF-LPB100串口WIFI模塊，設(shè)置為無線接入點(diǎn)模式，由上位機(jī)連接WIFI模塊的服務(wù)集標(biāo)識符加入網(wǎng)絡(luò)獲取數(shù)據(jù)。傳感器原始數(shù)據(jù)及導(dǎo)航解算結(jié)果通過STM32F405的串行接口發(fā)送到WIFI模塊，再通過用戶數(shù)據(jù)報協(xié)議(user datagram protocol，UDP)方式發(fā)送到上位機(jī)。硬件平臺使用的是600 mAh聚合物鋰電池，可以連續(xù)為系統(tǒng)供電超過4 h。此外，為了實(shí)時監(jiān)控電源電量，采用了德州儀器公司生產(chǎn)的BQ24401電源管理芯片，嵌入式微處理器可通過IIC總線實(shí)時讀取剩余電池電量。

圖1 用于康復(fù)訓(xùn)練的穿戴式傳感器硬件平臺

2.2 導(dǎo)航傳感器建模

需要說明的是，慣性測量單元(inertial measurement unit，IMU)零偏是影響監(jiān)控精度的關(guān)鍵因素，文獻(xiàn)[8]論述了零偏對位置誤差的影響與時間的高階次冪成正比。IMU零偏主要包括固定零偏和時變零漂。其中，固定零偏可通過文獻(xiàn)[9]介紹的現(xiàn)場標(biāo)定算法校正，但時變零漂的存在仍然會極大的降低導(dǎo)航精度。為此，本文通過實(shí)際數(shù)據(jù)的零偏特性分析，繪制了IMU的自相關(guān)曲線，如圖2所示。

圖2 IMU自相關(guān)函數(shù)曲線(左：加速度計，右：陀螺儀)

從圖2中不難看出，低成本IMU的時間相關(guān)特性并不顯著(相關(guān)時間小到可被忽略)，傳感器的零漂則可視為隨機(jī)游走過程有

(1)

式(1)中，ba、bg分別為加速度計和陀螺儀的零偏，na、ng表示加速度計和陀螺儀零偏的隨機(jī)噪聲。

2 基于ZUPT的導(dǎo)航估計

2.1 算法結(jié)構(gòu)

本文所采用的導(dǎo)航算法流程如下圖3所示。

為保證數(shù)據(jù)的同步和相位線性特性，傳感器原始數(shù)據(jù)分別通過4階前置低通有限脈沖響應(yīng)(finite impulse response，F(xiàn)IR)濾波器。然后，將頻域?yàn)V波后的傳感器送入ZUPT導(dǎo)航解算模塊以獲得實(shí)時的位置、速度與姿態(tài)信息。值得注意的是，在每次導(dǎo)航解算完成后，都需重新估計速度和姿態(tài)誤差量以及其估計誤差的協(xié)方差矩陣。

圖3 導(dǎo)航算法結(jié)構(gòu)

2.2 IMU力學(xué)編排

本文選擇當(dāng)?shù)厮阶鴺?biāo)系(East-North-Up，ENU)為導(dǎo)航計算坐標(biāo)系(記為n系)。下面給出歷元k的位置、速度與姿態(tài)的導(dǎo)航更新方程。

位置更新

(2)

速度更新

vk+1=vk+akΔt

(3)

姿態(tài)更新

(4)

式(4)中，v、r分別表示速度、位置矢量；w為角速率；bf為載體系下的比力矢量；q=[q1q2q3q4]T為姿態(tài)四元數(shù)。以上導(dǎo)航解算通過對時間積分遞推獲得當(dāng)前位置、速度與姿態(tài)，然而積分過程中必然引入累積誤差，為此建立如下PSI角動態(tài)誤差方程

(5)

式(5)中，δv為速度誤差；Ψ為姿態(tài)角誤差；bδf為加速度計零偏；bδw為陀螺儀零偏。由于低成本MEMS陀螺零偏不穩(wěn)定性較大，故在PSI角誤差方程中忽略了地球自轉(zhuǎn)等信號。

導(dǎo)航狀態(tài)估計顧及偏癱病人的低步頻特點(diǎn)，每步間的停頓時間相對較長，導(dǎo)航狀態(tài)估計采用間接校正的ZUPT建立如下12維誤差狀態(tài)方程

(6)

式(6)中，

FvΨ為n系三軸比力矢量構(gòu)成的反對稱矩陣

ZUPT所構(gòu)造的觀測方程為

Zk=Hxk+ε

(7)

2.3ZUPT檢測判決條件

結(jié)合患者的步行特點(diǎn)，本文采用如下三個判決條件聯(lián)合檢測以確定是否進(jìn)行ZUPT修正：

判決條件1：加速度量測幅值

(8)

判決條件2：角速率量測幅值

(9)

判決條件3：磁力計量測方差

(10)

當(dāng)滿足聯(lián)合檢測條件

c=c1c2c3=0

(11)

判定當(dāng)前步行為持續(xù)移動狀態(tài)，此時則采用式(12) ～ 式(15)僅對誤差量和估計誤差協(xié)方差矩陣做先驗(yàn)估計，

Φk+1=I12×12+FΔt

(12)

(13)

(14)

(15)

當(dāng)滿足聯(lián)合檢測條件

c=c1c2c3=1

(16)

判定當(dāng)前步行狀態(tài)為零速，執(zhí)行卡爾曼濾波式(12)～式(19)，獲得狀態(tài)的后驗(yàn)估計并反饋修正當(dāng)前導(dǎo)航狀態(tài)參數(shù)。

(17)

(18)

(19)

3 實(shí)驗(yàn)及分析

3.1 零速檢測閾值參數(shù)選取

為合理確定ZUPT檢測判決條件的閾值先驗(yàn)參數(shù)，離線采集了動態(tài)實(shí)測數(shù)據(jù)。加速度、角速率、磁力計量測幅值序列如圖4所示。檢測窗口長度設(shè)置為4個時間單元(0.01s)，即每4單位進(jìn)行一次綜合分析。

圖4 ZUPT判決閾值測試

3.2 初始對準(zhǔn)與導(dǎo)航性能評估

固定穿戴式傳感器于腳背上方，上電后將上位機(jī)連接傳感器WIFI網(wǎng)絡(luò)。待網(wǎng)絡(luò)連接成功，上位機(jī)提示開始步行康復(fù)訓(xùn)練并實(shí)時采集、存儲數(shù)據(jù)。在步行開始前，需在站立位保持腳部短時間靜止，以完成傳感器初始化和IMU初始對準(zhǔn)工作，即在腳部靜止?fàn)顟B(tài)下，每單位時間進(jìn)行ZUPT修正直至位置、速度與姿態(tài)收斂(如圖5所示)。

圖5 IMU初始對準(zhǔn)結(jié)果

圖5中，數(shù)據(jù)的采樣頻率為100Hz，傳感器上電之后，共設(shè)置了2s的初始化時間，共采集了200組數(shù)據(jù)?？梢钥吹剑疚脑O(shè)計的算法可以在0.2s內(nèi)完成IMU的初始對準(zhǔn)。上位機(jī)收到傳感器數(shù)據(jù)判斷初始化完畢后，提示患者開始康復(fù)訓(xùn)練。監(jiān)控結(jié)果如圖6所示。

圖6 患者平面步行軌跡

步行康復(fù)訓(xùn)練監(jiān)控的結(jié)果如表1，結(jié)果基本符合偏癱患者實(shí)際情況。從表1中可以看到，患者步行較為吃力，由于平均步長達(dá)到了0.57m，但步行平均速度很小，說明患者行動較為遲緩，且訓(xùn)練過程中停頓較多。

表1 步行康復(fù)訓(xùn)練監(jiān)控結(jié)果

4 結(jié)束語

本文開發(fā)了一種穿戴的低成本傳感器平臺，對其中的消費(fèi)級導(dǎo)航傳感器進(jìn)行了數(shù)學(xué)建模，提出了適用于患者康復(fù)訓(xùn)練的ZUPT算法及其靜態(tài)檢測條件，實(shí)驗(yàn)測試結(jié)果表明該系統(tǒng)可滿足步行康復(fù)訓(xùn)練監(jiān)控的基礎(chǔ)數(shù)據(jù)需求。由于低成本慣性元件系統(tǒng)誤差不穩(wěn)定，且ZUPT無法永久消除位置的積分誤差，極大的制約了系統(tǒng)性能，暫時無法滿足大范圍、長時間的導(dǎo)航需求。因此，后續(xù)工作將引入其他的室內(nèi)導(dǎo)航定位傳感器進(jìn)行外部輔助，以提高步行康復(fù)訓(xùn)練監(jiān)控系統(tǒng)的精度和可靠性。

[1] 甕長水.腦卒中患者步行功能障礙評價[J].中國臨床康復(fù)，2002，6(13)：1869-1871.

[2] 甕長水，畢勝，謝遠(yuǎn)見，等.腦卒中偏癱患者步行速度與步長和步頻的關(guān)系[J].中國臨床康復(fù)，2003，7(7)：1108-1109.

[3] MAUTZ R.Indoor Positioning Technologies[D].Zurich：ETH Zurich，2012.

[4] LUINGE H J.Inertial Sensing of Human Movement[D].Enschede：University of Twente，2002.

[5] 汪少初，劉開華，劉昱.脛骨捷聯(lián)慣性導(dǎo)航的人員行進(jìn)距離估計[J].天津大學(xué)學(xué)報：自然科學(xué)與工程技術(shù)版，2014，47(8)：719-727.

[6] SKOG I，HANDEL P，NILSSON J O，et al.Zero-velocity Detection- An Algorithm Evaluation[J].IEEE Transactions on Biomedical Engineering，2010，57(11)：2657-2666.

[7] COLOMAR D S，NILSSON J，HANDEL P.Smoothing for ZUPT-aided INSs[C]//Proceedings of 2012 International Conference on Indoor Positioning and Indoor Navigation (IPIN 2012).Sydney：[s.n.]，2012：1-5.

[8] EL-SHEIMY N.Inertial Techniques and INS/DGPS Integration[M].Calgary：University of Calgary，2006.

[9] SYED Z F，AGGARWAL P，GOODALL C，et al.A New Multi-position Calibration Method for MEMS Inertial Navigation Systems[J].Measurement Science and Technology，2007(8)：1897-1907.

[10]YUN Xiao-ping，BACHMANN E R，MCGREE R B.A Simplified Quaternion-based Algorithm for Orientation Estimation from Earth Gravity and Magnetic Field Measurements[J].IEEE Transactions on Instrumentation and Measurement，2008，57(3)：638-650.

Research on Walking Rehabilitation Training Monitoring with Wearable Low-cost Navigation Sensors

ZHANGJian-feng1,ZHOUZe-bo1，2

(1.School of Aeronautics & Astronautics，University of Electronic Science and Technology of China，Chengdu 611731，China； 2.Institute of Geodesy and Geophysics，Chinese Academy of Sciences，Wuhan 430077，China)

With the requirements of real-time monitoring in walking rehabilitation training for stroke patients，a low-cost navigation hardware platform is developed.A zero-velocity update (ZUPT) algorithm is used to estimate the walking distance in real time.The navigation estimation formula are derived in the local frame and three joint-judgment conditions for ZUPT are constructed.Real experiment is carried out to demonstrate the efficiency of our proposed navigation algorithm.The result shows the developed navigation system can adequately fulfill the walking rehabilitation training requirement.

walking rehabilitation training；walking distance；navigation sensor；zero-velocity update

張劍鋒，周澤波.穿戴式導(dǎo)航傳感器在步行康復(fù)訓(xùn)練監(jiān)控中的應(yīng)用[J].導(dǎo)航定位學(xué)報,2015,3(3):126-131.(ZHANG Jian-feng,ZHOU Ze-bo.Research on Walking Rehabilitation Training Monitoring with Wearable Low-cost Navigation Sensors[J].Journal of Navigation and Positioning,2015,3(3):126-131.)

10.16547/j.cnki.10-1096.20150325.

2015-05-18

大地測量與地球動力學(xué)國家重點(diǎn)實(shí)驗(yàn)室開放基金(SKLGED 2014-3-3-E)。

張劍鋒(1990—)，男，四川內(nèi)江人，工程師，主要從事慣性導(dǎo)航和組合導(dǎo)航算法研發(fā)工作。

TP24

A

2095-4999(2015)-03-0126-06