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

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

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

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

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

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

微機電系統傳感器；擴展卡爾曼濾波；關節角度測量；動作捕捉

0 引言

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

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

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

1 動作捕捉系統

表1 MEMS傳感器具體參數

圖1 傳感器節點內部傳感器類型說明

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

2 關節角解算

2.1 坐標系定義

圖2 人體肢體坐標系與傳感器節點坐標系

2.2 初始姿態獲取

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

2.3 基于二級EKF的姿態更新

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

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

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

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

式中

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

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

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

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

2.4 關節角解算

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

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

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

3 實驗與結果分析

3.1 姿態更新實驗

圖5 靜態時2種算法解算各軸姿態角對比

圖6 動態時2種算法解算各軸姿態角對比

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

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

3.2 關節角解算實驗

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

圖7 關節角解算實驗

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

由圖8可以發現：二級擴展卡爾曼濾波方法解算出的角度相對于傳統的誤差四元數卡爾曼濾波方法解算出的角度更接近每次桿臂旋轉的角度，相對誤差更小；二級擴展卡爾曼濾波相對于傳統的誤差四元數擴展卡爾曼濾波在靜止階段下解算姿態穩定性更高。表3所示為6次重復實驗中關節計算值的統計結果。

表3 關節計算值統計結果 (°)

4 結束語

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

[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] 仇森. 人體步態分析的多傳感器數據融合研究[D]. 大連: 大連理工大學, 2016.

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

[8] 劉玉燾. 基于可穿戴式傳感器的人體動作捕獲與識別研究[D]. 哈爾濱: 哈爾濱工業大學, 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] 李啟雷, 金文光, 耿衛東. 基于無線慣性傳感器的人體動作捕獲方法[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] 王建東, 劉云輝, 宋寶泉, 等. 行人導航系統設計與IMU模塊數據預處理[J]. 電測與儀表, 2006, 43(11): 19-22.

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

[16] 龍湘林. 慣性式人體上肢運動姿態捕捉系統研究[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傳感器的肘關節角度精確測量[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—)，男，吉林長春人，博士，副研究員，碩士研究生導師，研究方向為激光陀螺及慣性導航系統。