基于九軸慣性傳感器的人體關節(jié)活動度測量方法研究

高 照，祝小波，寇曉潔

（陜西福音假肢有限責任公司，西安710016）

0 引言

現(xiàn)代醫(yī)學康復領域通過讓被測者做特定動作來測量其關節(jié)活動度，分析測量的結(jié)果后得出康復結(jié)論[1]。傳統(tǒng)關節(jié)活動度測量方法為采用尺規(guī)法測量[2-3]，這種方法需固定關節(jié)另一端在已知角度且只能靜態(tài)地測量關節(jié)的一維活動度；李文浩等[4]利用人體標記點配合攝像頭測量被測者的關節(jié)活動度，該方法可實時測量關節(jié)活動度，但標記點若被阻擋會影響輸出結(jié)果；使用慣性傳感器測量人體關節(jié)活動度是一種無需依賴攝像頭數(shù)量和環(huán)境光照的方法，趙曉皓等[5]提出了使用2 個六軸傳感器分別在關節(jié)兩端進行測量的方案，但2 個傳感器需人為校準偏航角在同一坐標系內(nèi)后方可進行測量，操作較為煩瑣，除此之外，多個電路在分布式場景下同時測量還需要實現(xiàn)同步技術[6]。

鑒于此，本文采用九軸慣性傳感器搭配無線處理器采集肢體的角速度、加速度、磁力原始數(shù)據(jù)，然后對這些數(shù)據(jù)進行校準，接著帶入到Madgwick 算法求出在世界坐標系下的四元數(shù)，通過四元數(shù)計算出無需人為校準偏航角的歐拉角。最后通過將電路布置在關節(jié)兩端的肢體上，求2 個電路輸出的歐拉角差值得到該關節(jié)的活動度，與只測量關節(jié)一端肢體活動度的方法相比，本文方法不僅省去固定關節(jié)一端在已知角度的環(huán)節(jié)，而且能夠減少因關節(jié)固定端肢體運動而造成的誤差。

1 角度計算算法

常用的角度計算算法有以下3 種：（1）對角速度積分可求得響應較快的全姿態(tài)角度值，但該結(jié)果無法計算初始姿態(tài)角度并且受自身噪聲累積影響會導致測量角度漂移；（2）對加速度計的三軸數(shù)據(jù)進行反三角函數(shù)計算可求得俯仰角和橫滾角，但是其結(jié)果有滯后性且無法計算偏航角；（3）對磁力計的三軸數(shù)據(jù)進行反三角函數(shù)計算，可求得穩(wěn)定的全姿態(tài)角度值，但是其結(jié)果也存在滯后性。為了結(jié)合這三者的優(yōu)勢，本文選擇Madgwick 互補算法[7]進行姿態(tài)解算，并在姿態(tài)解算前選取均值濾波算法、幾何校準算法[8-9]分別對角速度計、加速度計和磁力計進行校準。

1.1 Madgwick 算法

式中，Δt 為角速度計采樣間隔。

地磁場具有水平方向和垂直方向的分量，因此磁力計的向量式為=[0，mx，my，mz]，其中mx、my、mz分別是物體三軸磁力歸一化后的值。記地磁向量為b^E=[0，bx，0，bz]，其中bx和bz分別為地磁場水平和垂直方向的分量，其目標函數(shù)可用公式（3）表示：

然而加速度計雖然可以快速計算出橫滾角和俯仰角，但無法求出偏航角；磁力計雖然可以求出全姿態(tài)的歐拉角，但其響應速度遠小于加速度計。因此需要將加速度計和磁力計的姿態(tài)進行融合，得到目標函數(shù)公式（4）。對公式（4）進行梯度下降可得公式（5），從而得到公式（4）中的最優(yōu)解。 在公式（5）中μt為步長，梯度可通過求解目標函數(shù)的雅可比式得到，如公式（6）～（9）所示。

式中，γt為加權(quán)系數(shù)。

然而，若直接使用未校準的九軸慣性傳感器數(shù)據(jù)進行姿態(tài)解算，其結(jié)果和真實姿態(tài)之間將存在很大偏差，因此在姿態(tài)解算前須對九軸慣性傳感器進行校準。

1.2 角速度計校準

校準角速度計的目的是避免由于角速度計誤差在長時間積分后導致姿態(tài)漂移，角速度計誤差來源于傳感器零偏、溫度漂移。校準角速度計的數(shù)學模型為

式中，gyroraw為原始角速度數(shù)據(jù)；gyrooffset為偏移值。當角速度計靜止的時候，原始數(shù)據(jù)減去偏移值應為零，因此可以通過將傳感器靜止放置一段時間后求得角速度計各軸的平均值來作為偏移值，參考設定的放置時間是60 s，則偏移值計算公式為

式中，gyrosum為60 s 內(nèi)該軸角速度的數(shù)據(jù)和；samplesum為60 s 內(nèi)該軸角速度的采樣數(shù)量。

1.3 加速度計校準

在加速度計生產(chǎn)過程中，加速度計易產(chǎn)生零偏和敏感軸不正交，這會使得加速度計線性度漂移，影響俯仰角和橫滾角的準確性。加速度計校準模型[10]如下：

式中，ki為標度因子；Aoffsetx、Aoffsety、Aoffsetz分別為加速度計x、y、z 軸的加速度零偏；datai為歸一化的三軸加速度計值（i∈{x，y，z}）。因為datai和g 是已知的，可知求出ki和Aoffseti需要采集6 個不同的姿態(tài)。本文選用的加速度計幾何校準法流程圖如圖1 所示。具體流程如下：

圖1 加速度計校準流程圖

（1）擺放加速度計，在6 個姿態(tài)采集數(shù)據(jù)。6 個參考姿態(tài)分別與加速度計三軸正反方向重力軸方向一致。

（2）計算零偏。在計算零偏時需與第一步同步進行，在擺放加速度計姿態(tài)過程中對數(shù)據(jù)進行如下處理：

式中，M 為采集次數(shù)；Arawxi、Arawyi、Arawzi分別為加速度計三軸原始數(shù)據(jù)。使用高斯消元法[11]求解（XTX）β=XTY 中的加速度零偏矩陣β 后，記即加速度計x 軸、y 軸、z 軸的零偏Aoffsetx、Aoffsety、Aoffsetz。

（3）提取敏感姿態(tài)數(shù)據(jù)。加速度計三軸中某一軸加速度絕對值越大，則重力軸和該軸夾角越小。記絕對值最大的軸為敏感軸，為了保證計算標度因子準確性，需要求敏感軸與重力軸夾角在一定范圍內(nèi)，可設置剔除夾角閾值為10°，在采集加速度過程中，加速度計的軸向和重力方向夾角若大于10°，則剔除該部分數(shù)據(jù)。

（4）求解標度因子。使用加速度原始值減去第二步求得的零偏后，將公式（13）變換后可得到在不同位置提取的敏感軸原始均值減去零偏后的差值矩陣：

公式（16）中各項計算公式如公式（17）～（19）所示：

在矩陣Acalib中，aαβ、aαγ、aαδ（α∈{x，y，z}，β∈{00，01}，γ∈{02，03}，δ∈{04，05}）表示在不同位置提取敏感姿態(tài)數(shù)據(jù)后敏感軸原始值減去零偏后的均值；KS矩陣為標度因子矩陣；Mx、My、Mz為加速度計重力原始值的2 倍，MS為加速度計重力原始值的6 倍。求解線性方程公式（16）易得KS，最后對KS各元素開根可求得x、y、z 三軸的標度因子。

（5）將上述求得的零偏Aoffsetx、Aoffsety、Aoffsetz和標度因子kx、ky、kz帶入到公式（20）中，即可校準加速度計。

1.4 磁力計校準

因為每一個電路會受到不同軟磁場和硬磁場干擾，這會導致偏航角數(shù)值不一致，所以磁力計校準的目的是將多個關節(jié)活動度測量電路的偏航角統(tǒng)一在世界坐標系內(nèi)。本文基于幾何校準法[8-9]列出磁力計零偏線性方程，使用高斯消元[11]求解磁力計零偏線性方程。校準過程中需將磁力計在空間中繞中心點自由旋轉(zhuǎn)，磁力計的數(shù)據(jù)會組成一個球面[9]，經(jīng)校準后該球面的球心坐標將處于（0，0，0）。以下是數(shù)據(jù)處理方法：

式中，Brawxi、Brawyi、Brawzi分別為磁力計三軸原始數(shù)據(jù)。求解（XTX）β=XTY 中的β 后，記即磁力計x、y、z 軸的零偏Boffsetx、Boffsety、Boffsetz，最后將磁力計各軸原始值和零偏作差即完成校準。

2 關節(jié)活動度測量電路設計

關節(jié)活動度測量電路由無線處理器、九軸慣性傳感器、同步電路、電源管理模塊和喚醒關機電路組成，如圖2 所示。

圖2 關節(jié)活動度測量電路框圖

圖3 為無線處理器電路原理圖。其中無線處理器為ESP32-WROOM-32D 模組，該模組不僅支持藍牙4.2，而且支持Wi-Fi 通信，還具有集成電路總線（inter-integrated circuit，I2C）、串行外設接口（serial peripheral interface，SPI）、通用輸入輸出（general purpose input output，GPIO）接口和通用異步收發(fā)器（universal asynchronous receiver/transmitter，UART）、模擬數(shù)字轉(zhuǎn)換器（analog-to-digital converter，ADC）等[12]。圖3 中C8 和C9 為電源濾波電容；IO34 為GPIO 輸入模式，用于判斷電路是否在充電；IO35 為ADC 模式，用于判斷電池電量；IO13 為GPIO 中斷輸入模式，用于判斷是否讀取九軸慣性傳感器的更新數(shù)據(jù)；IO18和IO19 為I2C 模式，用于與九軸慣性傳感器通信；IO16為GPIO 中斷輸入模式，用于判斷是否進行同步；IO4為GPIO 輸入模式，用于判斷是否開關機。

圖3 無線處理器電路原理圖

圖4 為九軸慣性傳感器電路原理圖。傳感器型號為MPU9250，該傳感器集成角速度計、加速度計和磁力計，通信方式為I2C 和SPI，MPU9250 可以通過I2C配置為更新數(shù)據(jù)后INT 引腳輸出脈沖信號[13]的模式。

圖4 九軸慣性傳感器電路原理圖

圖5 為同步電路原理圖。其中R3 為充電保險電阻，其功能為過流保護；P1 是一種USB 插座，該插座的引腳4 與引腳5 不連接，其功能是將外部信號的地和關節(jié)活動度測量電路的地連接，還將P1 的引腳4 連接到無線處理器的IO4 引腳，由于外部電路已經(jīng)和關節(jié)活動度電路共地，因此IO4 可以接收外部電信號。多個測量電路可用同步裝置[14]同時輸入一個電平信號，無線處理器可被觸發(fā)GPIO 中斷，其在中斷服務中可清零內(nèi)部時間戳，若計算機此時開始接收數(shù)據(jù)包，并且由于網(wǎng)絡擁擠導致數(shù)據(jù)包接收順序錯誤，可根據(jù)數(shù)據(jù)包上帶有的時間戳重新對數(shù)據(jù)排序就能解決多個設備協(xié)作沖突的問題。其中數(shù)據(jù)包格式包含幀頭、數(shù)據(jù)包長度、設備編號、數(shù)據(jù)值和時間戳。

圖5 同步電路原理圖

電源管理模塊由電源管理芯片電路和穩(wěn)壓電路組成，其電路原理圖如圖6、7所示。電源管理芯片型號為LTC4067EDE，其中引腳4、5和P1 的VCC_IN 連接，當充電器件P1 的引腳1 有5 V 電源接入時，LTC4067EDE 的引腳10 可給外接電池充電，同時使得其引腳2 變?yōu)榈碗娖酵ㄖ獰o線處理器此時為充電狀態(tài)。R7、R8 和R10 組成的分壓電路對鋰電池電壓進行分壓，鋰電池分壓后的低壓節(jié)點SIGNAL_POWER_AD 和無線處理器主引腳IO35相連，無線處理器使用模數(shù)轉(zhuǎn)換功能可實現(xiàn)電池電量的采集。穩(wěn)壓芯片型號為TPS73133，其中引腳1以及引腳3 與電源管理芯片電路的VOUT 連接，分別用于給TPS73133 輸入電能和使能芯片；磁珠L1和電容C3 組成濾波電路，使TPS73133 引腳5 輸出的電源紋波降低。

圖6 電源管理芯片電路原理圖

如圖8 所示，喚醒關機電路包含上拉電阻R17、微動按鍵P15 和濾波電容C17，當微動按鍵按下后KEY 節(jié)點為低電平，無線處理器根據(jù)KEY 節(jié)點電平狀態(tài)與持續(xù)時長實現(xiàn)喚醒與關機功能。

圖7 穩(wěn)壓電路原理圖

圖8 喚醒關機電路原理圖

整個電路可通過Wi-Fi 或藍牙和計算機進行交互，計算機需配備無線路由器或藍牙適配器。圖9 為關節(jié)活動度測量電路電路板實物圖，電路板的尺寸為30 mm×35 mm（長×寬）。

圖9 關節(jié)活動度測量電路電路板實物圖

3 校準結(jié)果和同步功能測試結(jié)果

3.1 角速度計校準結(jié)果

圖10、11 為角速度計校準前后自身的噪聲變化對歐拉角的影響。由圖10 可看出，校準前角速度計的噪聲長時間積分導致歐拉角漂移。由圖11 可看出，校準后角速度計噪聲降低，從而減少了歐拉角的漂移誤差。

圖11 校準后的角速度和歐拉角波形

3.2 加速度計校準結(jié)果

圖12（a）、（b）為加速度計校準前的原始值，軸向絕對值近似8 000，圖12（c）、（d）為加速度計經(jīng)過校準后的原始值，軸向絕對值近似5 000，乘以最小有效位（1/4 096）g（g 為重力加速度）進行校準后，加速度計真實值更接近1g。對比表1 和表2 中6 個姿態(tài)下的俯仰角、橫滾角及三軸加速度值，可知校準前后加速度計所測得的角度與關節(jié)量角器的測量結(jié)果相比，誤差明顯減少，分別為±2°～±7°與±1°。因此加速度計的校準提高了歐拉角精度。

表1 校準前的加速度、歐拉角測量結(jié)果

表2 校準后的加速度、歐拉角測量結(jié)果

圖12 加速度計校準前后原始值

3.3 磁力計校準結(jié)果

由圖13 可以看出，磁力計校準前磁力球面圓心坐標為（-0.1，-0.2，0），進行校準后磁力球面的球心回到（0，0，0），這意味著幾何校準算法消除了軟硬磁場的干擾。

在校準了磁力計后，開始驗證引入磁力計是否會使得偏航角一致。在驗證過程中需保證2 個關節(jié)活動度測量電路的姿態(tài)一致，再通過這2 個電路輸出的角度數(shù)據(jù)判斷誤差，其一致性測試結(jié)果見表3。由表3 可知在4 個姿態(tài)下2 個電路的俯仰角和橫滾角誤差為±1°，偏航角誤差能達到±1°，說明引入了磁力計使得偏航角得到了統(tǒng)一，而且沒有降低俯仰角和橫滾角精度。

表3 一致性測試結(jié)果 單位：（°）

3.4 同步功能測試

開啟2 個關節(jié)活動度測量電路向計算機發(fā)送數(shù)據(jù)，未同步的數(shù)據(jù)包接收示意圖如圖14（a）所示，可以看出同一時間點下，2個電路的數(shù)據(jù)由于內(nèi)部時間戳（TS）不一致，上位機無法辨識在現(xiàn)實環(huán)境中2 個數(shù)據(jù)是否為同時測量的，因此無法進行數(shù)據(jù)分析。從圖14（b）同步后的數(shù)據(jù)包接收示意圖可以看出，內(nèi)部時間戳一致時可以對數(shù)據(jù)進行協(xié)同分析。另外圖14（b）中還模擬了一種由于數(shù)據(jù)堵塞導致電路2 沒有及時處理t2時間段的數(shù)據(jù)包的場景，但由于時間戳（TS）是統(tǒng)一的，因此在t3時刻接收來自t2的數(shù)據(jù)包后，仍可通過計算機將2 個電路的數(shù)據(jù)按時間戳重新排列求差，進而獲取關節(jié)活動度。

圖14 數(shù)據(jù)包接收示意圖

4 頸椎活動度測量實驗

為了使測量結(jié)果不失一般性，使用關節(jié)活動度測量電路測量一名志愿者的頸椎活動度。將測量電路分別佩戴在志愿者額頭及胸部中央，開始測量前對2 個電路進行同步操作，然后引導志愿者頭部左右側(cè)屈、前后屈伸、左右旋轉(zhuǎn)，測量電路通過Wi-Fi 將頭部、胸骨體歐拉角發(fā)送到計算機，使用MATLAB 將頭部歐拉角減去胸骨體歐拉角獲得頸椎關節(jié)活動度。實驗結(jié)果表明頸椎活動度符合人體特征。

4.1 頸椎前后屈伸活動度

頸椎前后屈伸活動度波形如圖15 所示。其中圖15（a）中俯仰角為興趣角度，-77.33°、-156.20°分別表示頭部前屈、后仰角度。從圖15（b）中可知，頸椎前后屈伸過程中身體無法保持完全靜止，會隨著頸部活動向同側(cè)傾斜，產(chǎn)生誤差，對應的角度為-113.70°和-116.00°。當用頭部歐拉角減去胸骨體歐拉角后，其余2 個角度基線降低至0°，俯仰角分別為36.37°和-40.20°，而頸椎前后屈伸范圍[15]是35°～45°，因此該結(jié)果符合人體關節(jié)活動度特征。

圖15 頸椎前后屈伸活動度波形

4.2 頸椎左右側(cè)屈活動度

頸椎左右側(cè)屈活動度波形如圖16 所示。由于測量方式和4.1 章節(jié)類似，只是興趣角度改為橫滾角，因此使用相同的處理方法，用頭部橫滾角減去胸骨體橫滾角，最終得到頸椎左右側(cè)屈活動度分別為30.37°和-30.04°，該結(jié)果也符合頸椎左右側(cè)屈特征（0°～45°）[15]。

圖16 頸椎左右側(cè)屈活動度波形

4.3 頸椎左右旋轉(zhuǎn)活動度

在測量頸椎左右旋轉(zhuǎn)活動度過程中，為了規(guī)避萬向節(jié)死鎖問題，需改變電路擺放方向，所以圖17 中頭部和胸骨體的初始俯仰角和橫滾角為0°。最終，頸椎左右旋轉(zhuǎn)活動度剔除誤差后的結(jié)果分別為62.60°和-60.14°，該結(jié)果同樣符合頸椎左右旋轉(zhuǎn)活動度特征（60°～80°）[14]。

圖17 頸部左右旋轉(zhuǎn)活動度波形

5 結(jié)語

本文提出了一種基于九軸慣性傳感器的人體關節(jié)活動度的測量方法，并針對該測量方法提出了相應的角度計算算法和電路設計方案。其中角度計算算法使用均值濾波算法和幾何校準算法對傳感器進行校準，然后再使用Madgwick 算法計算姿態(tài)角度，保證輸出的歐拉角在同一坐標系內(nèi)且精度為±1°。提出的電路測量方案具備無線傳輸功能和硬件同步功能，使電路可布置在關節(jié)兩端協(xié)同測量。另外，在頸椎活動度測量實驗中利用關節(jié)兩端肢體角度差值來反映關節(jié)活動狀況，可降低因關節(jié)另一端運動引起的誤差，驗證了測量方法的準確性和有效性。

但本研究也存在一些不足之處：（1）在測量過程中由于歐拉角存在萬向節(jié)死鎖的問題，要根據(jù)測量情況調(diào)整電路擺放位置，后續(xù)研究可以尋求新的角度計算方法。（2）應用實驗只使用2 個采集電路測量了頸椎活動度，后續(xù)研究可以利用更多傳感器同時采集多個肢體活動度并加入神經(jīng)網(wǎng)絡算法、支持向量機等算法來得到輔助性的診斷意見。