基于MEMS傳感器的肘關節(jié)角度精確測量

董壯壯，魏文量，田凱文，程廣新，李 鼎，于旭東

基于MEMS傳感器的肘關節(jié)角度精確測量

董壯壯1，魏文量2，田凱文1，程廣新1，李 鼎1，于旭東1

（1. 國防科技大學前沿交叉學科學院，長沙 410073；2. 廈門高精度自主導航實驗室，福建廈門 361100）

針對基于微機電系統(tǒng)（MEMS）傳感器監(jiān)測關節(jié)康復效果的實際需求，提出一種自適應二級擴展卡爾曼濾波（EKF）算法。該算法將觀測過程分為二級，分別更新水平姿態(tài)角與航向角，減少外界磁干擾對水平姿態(tài)角的影響，設計加速度自適應算子與磁力值自適應算子，提高算法的抗干擾能力。實驗結果證明，二級擴展卡爾曼濾波算法具有較高的靜態(tài)和動態(tài)精度，接近市面主流慣性動作捕捉系統(tǒng)的姿態(tài)解算精度，解算出的關節(jié)角均方根誤差（RMSE）小于0.5°，能夠滿足人體肘關節(jié)康復監(jiān)測、動作捕獲的需求。

微機電系統(tǒng)傳感器；擴展卡爾曼濾波；關節(jié)角度測量；動作捕捉

0 引言

隨著近年來體育運動的逐漸流行，運動損傷的發(fā)生率也在增加。人體動作捕捉系統(tǒng)可通過穩(wěn)定、重復的方式對受損關節(jié)運動幅度進行評估，因此在醫(yī)療診斷和康復中逐漸普及[1-2]。人體動作捕捉的方案主要有電磁式、機電式、光學式[3]、慣性式等。電磁式、光學式、機電式等動作捕捉方案存在易丟失信號，對環(huán)境要求嚴格等問題，適用場景較少?；谖C電系統(tǒng)（micro electro mechanical systems, MEMS）的慣性傳感器通過記錄和提取人體運動時的重要參數(shù)，可實現(xiàn)佩戴人在自由生活條件下實時、連續(xù)的關節(jié)健康記錄和運動監(jiān)測[4]。慣性傳感器測量系統(tǒng)被證明適合代替?zhèn)鹘y(tǒng)的基于光學的人體運動跟蹤系統(tǒng)；但是隨著使用時間延長，慣性傳感器會因加速度和角速度的積分產(chǎn)生累積漂移[5]，且目前MEMS陀螺儀無法敏感地球自轉，影響正常使用。為了解決這些問題，國內(nèi)外研究者們提出了眾多解決方法。

文獻[6]提出一種基于梯度下降算法的最優(yōu)四元數(shù)估計方法，通過傳感器特征層融合，減少線性加速度疊加重力加速度的影響。文獻[7]采用一種擴展卡爾曼濾波方法，結合有限狀態(tài)機和零速檢測算法，解算出的膝關節(jié)角度與光學系統(tǒng)相比具有較高精度。文獻[8]提出一種自適應四元數(shù)間接卡爾曼濾波的融合算法，在線性加速度疊加和外界磁場異常時也能保持較高精度。文獻[9]提出一種結合慣性傳感器和磁力計的關節(jié)角度測量方法，避免信號的數(shù)值積分，允許長時間估計而沒有角估計漂移，取得良好效果。文獻[10]將用于機器人手臂控制的運動學模型與無跡卡爾曼濾波算法結合，估算估計人體關節(jié)角度，精度較高。文獻[11]采用一種基于級聯(lián)卡爾曼濾波的傳感器融合算法，將可穿戴慣性傳感器與超寬帶定位系統(tǒng)融合，同時利用關節(jié)解剖約束條件，在下肢動作捕捉和三維軌跡跟蹤中同樣取得了良好的效果。

綜上所述，本文為了監(jiān)測肘關節(jié)康復效果，根據(jù)慣性動作捕捉系統(tǒng)的多傳感器數(shù)據(jù)融合特點，提出一種二級擴展卡爾曼濾波（extend Kalman filter, EKF）算法，采用四元數(shù)更新，避免采用歐拉角更新時的萬向節(jié)鎖問題，針對外界磁場干擾和線性加速度疊加重力加速度問題，采用自適應算子進行抑制，分別利用加速度計和磁力計進行觀測，利用二級濾波將水平姿態(tài)角和航向角分開解算，減小計算量，提高系統(tǒng)的魯棒性。

1 動作捕捉系統(tǒng)

表1 MEMS傳感器具體參數(shù)

圖1 傳感器節(jié)點內(nèi)部傳感器類型說明

傳感器節(jié)點內(nèi)包含三軸陀螺儀和三軸加速度計組成的傳感器ICM-20602與三軸磁力計傳感器QMC5883L。進行動作捕捉時，用戶首先將傳感器節(jié)點綁定到身體上的各個運動部位，當用戶運動時，這些傳感器獲取用戶肢體段的數(shù)據(jù)，無線傳輸協(xié)議將獲取的數(shù)據(jù)以幀的形式發(fā)送到電腦端進行姿態(tài)解算[12]。

2 關節(jié)角解算

2.1 坐標系定義

圖2 人體肢體坐標系與傳感器節(jié)點坐標系

2.2 初始姿態(tài)獲取

加速度計測量的是其所附著的載體的比力，在靜止或者接近勻速運動的時候，加速度計測量的重力在各軸的分量為

2.3 基于二級EKF的姿態(tài)更新

標準卡爾曼濾波是一種線性最優(yōu)化方法，只能對線性系統(tǒng)濾波，擴展卡爾曼濾波是利用泰勒級數(shù)展開將非線性系統(tǒng)線性化，然后利用標準卡爾曼濾波方法對信號進行濾波[15]，而人體運動過程往往是非線性的，所以本文采用擴展卡爾曼濾波。

設擴展卡爾曼濾波離散時間狀態(tài)空間非線性模型為

為增加系統(tǒng)的魯棒性，將觀測過程分為兩級，即將加速度計和磁力計的測量值的歸一化值分別作為一級和二級觀測量，為減少線性加速度疊加重力加速度對加速度計數(shù)據(jù)的影響，在一級濾波中引入自適應加速度觀測噪聲誤差協(xié)方差算子[16]，在二級濾波中引入自適應磁力值觀測噪聲誤差協(xié)方差算子，減少外界磁干擾對磁力計數(shù)值的影響，則式（5）更新后為

圖3 二級擴展卡爾曼濾波流程

式中

由四元數(shù)表示的導航坐標系到傳感器坐標系為

一級濾波時觀測量為加速度計輸出的歸一化值，則一級濾波的觀測方程為

同理，二級濾波時觀測量為磁力計輸出的歸一化值，其觀測方程為

在人體運動過程中，線性加速度疊加重力加速度、外界磁場干擾對姿態(tài)解算造成影響，為降低誤差，減少卡爾曼濾波器對當前量測值的影響，將自適應算子代入，得觀測噪聲誤差協(xié)方差矩陣為：

2.4 關節(jié)角解算

將傳感器節(jié)點綁定在人體肢體上時，存在傳感器節(jié)點的敏感軸與待測部位軸向不一致的現(xiàn)象，因此無法直接得到肢體坐標系與參考坐標系之間的旋轉矩陣[17]。圖4所示為傳感器坐標系與人體肢體坐標系不一致的情況。因此初始時刻須進行傳感器坐標系與肢體坐標系之間的標定。標定分為標定動作和標定姿態(tài)，本文選擇姿態(tài)T-pose進行初始標定。T-pose姿態(tài)下，設定人體各關節(jié)角為零，即此時代表關節(jié)角大小的四元數(shù)為[1000]，被測試者以T-pose姿態(tài)靜止站立5 s，傳感器收集數(shù)據(jù)，根據(jù)加速度計和磁力計計算得各傳感器的初始姿態(tài)，初始時刻人體關節(jié)角計算方程為

圖4 傳感器坐標系與肢體坐標系的不一致情況

根據(jù)肘關節(jié)的臨床定義[18]，本文中肘關節(jié)為具有2自由度的關節(jié)，分別為屈伸角度和內(nèi)外旋角度，其中屈伸角度為繞冠狀軸旋轉的角度，內(nèi)外旋角度為繞垂直軸旋轉的角度。為降低肘關節(jié)活動過程中對傳感器綁定位置的影響，上臂的傳感器節(jié)點綁定在上臂的下方的外側，前臂的傳感器節(jié)點綁定在前臂的下方的外側，靠近腕關節(jié)處。

3 實驗與結果分析

3.1 姿態(tài)更新實驗

圖5 靜態(tài)時2種算法解算各軸姿態(tài)角對比

圖6 動態(tài)時2種算法解算各軸姿態(tài)角對比

表2 靜態(tài)、動態(tài)姿態(tài)更新時的RMSE (°)

由圖5、圖6及表2可以發(fā)現(xiàn)，本文所提算法相對于傳統(tǒng)的誤差四元數(shù)卡爾曼濾波算法穩(wěn)定性更高，動態(tài)性能較好，在靜態(tài)精度、動態(tài)精度上接近市面主流動作捕捉系統(tǒng)精度，能夠滿足人體動作捕捉系統(tǒng)需求。

3.2 關節(jié)角解算實驗

選擇磁場干擾小的環(huán)境，將小臂傳感器綁定在連接步進電機轉軸的桿臂上，上臂傳感器靜止放置在水平面上，如圖7所示。使用STM32單片機輸出PWM波驅動步進電機旋轉，控制器選擇 20000細分，每次脈沖旋轉0.00036°，其精度為0.075°。首先靜止10 s，每旋轉30°，靜止20 s，逐步旋轉至120°，重復步驟，多次實驗。圖8所示為其中一次實驗中，誤差四元數(shù)卡爾曼濾波算法解算的關節(jié)角與本文提出算法解算關節(jié)角的對比。

圖7 關節(jié)角解算實驗

圖8 2種算法角度解算對比

由圖8可以發(fā)現(xiàn)：二級擴展卡爾曼濾波方法解算出的角度相對于傳統(tǒng)的誤差四元數(shù)卡爾曼濾波方法解算出的角度更接近每次桿臂旋轉的角度，相對誤差更?。欢墧U展卡爾曼濾波相對于傳統(tǒng)的誤差四元數(shù)擴展卡爾曼濾波在靜止階段下解算姿態(tài)穩(wěn)定性更高。表3所示為6次重復實驗中關節(jié)計算值的統(tǒng)計結果。

表3 關節(jié)計算值統(tǒng)計結果 (°)

4 結束語

本文針對肘關節(jié)康復監(jiān)測關節(jié)活動度的實際需求，慣性動作捕捉系統(tǒng)的多傳感器數(shù)據(jù)融合特點，提出一種二級擴展卡爾曼濾波算法，通過靜態(tài)和動態(tài)實驗以及關節(jié)角實驗，結果表明，本文算法靜態(tài)、動態(tài)精度均較高，能夠滿足肘部關節(jié)康復監(jiān)測關節(jié)活動度要求，為進一步研究基于慣性動作捕捉系統(tǒng)的人體上肢動作識別提供參考。

[1] CALLEJAS C M, GUTIERREZ R M, HERNANDEZ A. Joint amplitude MEMS based measurement platform for low cost and high accessibility telerehabilitation: elbow case study[J]. Journal of Bodywork & Movement Therapies, 2017, 21(3): 574-581.

[2] ABBASI K R, NIKFARJAM A. A miniature sensor system for precise hand position monitoring[J]. IEEE Sensors Journal, 2018, 18(6): 2577-2584.

[3] 黃波士, 陳福民. 人體運動捕捉及運動控制的研究[J]. 計算機工程與應用, 2005(7): 60-63.

[4] BRANDON O, JEAN F D, KATHERINE B. A simple low-cost wearable sensor for long-term ambulatory monitoring of knee joint kinematics[J]. IEEE Transactions on Bio-medical Engineering, 2020, 67(12): 3483-3490.

[5] H MARTIN S, DANIEL R, PETER H V. Ambulatory human motion tracking by fusion of inertial and magnetic sensing with adaptive actuation[J]. Medical & Biological Engineering & Computing, 2010, 48(1): 27-37.

[6] 仇森. 人體步態(tài)分析的多傳感器數(shù)據(jù)融合研究[D]. 大連: 大連理工大學, 2016.

[7] 白桂峰. 基于MEMS慣性傳感器的人體步態(tài)分析關鍵技術研究[D]. 太原: 中北大學, 2020.

[8] 劉玉燾. 基于可穿戴式傳感器的人體動作捕獲與識別研究[D]. 哈爾濱: 哈爾濱工業(yè)大學, 2020.

[9] ALVAREZ D, ALVAREZ J C, GONZALEZ R C, et al. Upper limb joint angle measurement in occupational health[J]. Computer Methods in Biomechanics Biomed Engin, 2016, 19(2): 159-170.

[10] MAHMOUD E, SEAN P, JAMES M. Joint angle tracking with inertial sensors[C]//The Institute of Electrical and Electronic Engineers (IEEE). Proceedings of the 30th Annual International IEEE Engineering in Medicine and Biology Society. Palms Springs: IEEE, 2008: 1068-1071[2022-06-23].

[11] ZIHAJEHZADEH S, YOON P K, KANG B, et al. UWB-aided inertial motion capture for lower body 3-D dynamic activity and trajectory tracking[J]. IEEE Transactions on Instrumentation and Measurement, 2015, 64(12): 3577-3587.

[12] 李啟雷, 金文光, 耿衛(wèi)東. 基于無線慣性傳感器的人體動作捕獲方法[J]. 浙江大學學報(工學版), 2012, 46(2): 280-285.

[13] NEMEC D, JANOTA A, HRUBOS M, et al. Intelligent real-time MEMS sensor fusion and calibration[J]. IEEE Sensors Journal, 2016, 16(19): 7150-7160.

[14] 王建東, 劉云輝, 宋寶泉, 等. 行人導航系統(tǒng)設計與IMU模塊數(shù)據(jù)預處理[J]. 電測與儀表, 2006, 43(11): 19-22.

[15] 嚴恭敏, 翁浚. 捷聯(lián)慣導算法與組合導航原理[M]. 西安: 西北工業(yè)大學出版社, 2019: 170-172.

[16] 龍湘林. 慣性式人體上肢運動姿態(tài)捕捉系統(tǒng)研究[D]. 長沙: 湖南大學, 2017.

[17] 張鋆豪, 何百岳, 楊旭升, 等. 基于可穿戴式慣性傳感器的人體運動跟蹤方法綜述[J]. 自動化學報, 2019, 45(8): 1439-1454.

[18] WU G, VAN DER HELM F C, VEEGER H E, et al. ISB recommendation on definitions of joint coordinate systems of various joints for the reporting of human joint motion: part II shoulder, elbow, wrist and hand [J]. Journal of Biomechanics, 2005, 38(5): 981-992.

Precise measurement of elbow angle based on MEMS sensors

DONG Zhuangzhuang1, WEI Wenliang2, TIAN Kaiwen1, CHENG Guangxin1, LI Ding1, YU Xudong1

（1.College of Advanced Interdisciplinary Studies, National University of Defense Technology, Changsha 410073, China;2. Xiamen High Precision Autonomous Navigation Laboratory, Xiamen, Fujian 361100, China）

Aiming at the actual demand of monitoring joint rehabilitation effect based on micro-electro-mechanical system (MEMS) sensor, an adaptive two-stage extended Kalman filter (EKF) algorithm is presented. The algorithm divides the observation process into two stages and updates horizontal attitude angle and heading angle respectively, reducing the influence of external magnetic interference on the horizontal attitude angle. Acceleration adaptive operator and magnetic value adaptive operator are designed to improve the anti-interference ability of the algorithm. Experimental results show that the two-stage extended Kalman filter algorithm has high static and dynamic accuracy, close to the attitude calculation accuracy of the mainstream inertial motion capture system in the market, and the root mean squared error (RMSE) of the joint angle is less than 0.5°, which meets the needs of human elbow rehabilitation monitoring and motion capture.

micro electro mechanical system sensors; extended Kalman filter; joint angle measurement; motion capture

TP391.41;V249.32

A

2095-4999(2022)06-0129-07

董壯壯，魏文量，田凱文，等. 基于MEMS傳感器的肘關節(jié)角度精確測量[J]. 導航定位學報, 2022, 10(6): 129-135.（DONG Zhuangzhuang, WEI Wenliang, TIAN Kaiwen, et al. Precise measurement of elbow angle based on MEMS sensors[J]. Journal of Navigation and Positioning, 2022, 10(6): 129-135.）

10.16547/j.cnki.10-1096.20220617.

2022-06-24

國家自然科學基金面上項目（62173335）。

董壯壯(1994—)，男，江蘇連云港人，碩士研究生，研究方向為光電慣性技術。

于旭東(1982—)，男，吉林長春人，博士，副研究員，碩士研究生導師，研究方向為激光陀螺及慣性導航系統(tǒng)。