基于IMU和Mahony算法的人體手臂關(guān)節(jié)角度姿態(tài)測量系統(tǒng)研究

楊光耀 夏振華 肖一帆 楊澤瑞 呂元

關(guān)鍵詞：IMU；手臂關(guān)節(jié)測量；Mahony算法；跟蹤模擬

1 緒論

1.1 研究背景和意義

人體關(guān)節(jié)角度檢測在康復(fù)醫(yī)療、體育運動、實時監(jiān)護(hù)等領(lǐng)域都非常有應(yīng)用價值。在康復(fù)醫(yī)療領(lǐng)域，對關(guān)節(jié)極限角度進(jìn)行檢測，可以為醫(yī)生的臨床診斷、病人的康復(fù)效果評估提供重要參數(shù)；在體育運動領(lǐng)域，對關(guān)節(jié)運動規(guī)律捕捉分析，可以為運動員的訓(xùn)練方法提供科學(xué)指導(dǎo);在實時監(jiān)護(hù)領(lǐng)域，獲取關(guān)節(jié)運動信息可以為老年人、康復(fù)期患者的跌倒風(fēng)險評估、步態(tài)檢測等提供重要依據(jù)[1]。

目前關(guān)節(jié)姿態(tài)角測量方法主要有光學(xué)傳感器測量、三維影像測試分析系統(tǒng)、MEMS慣性傳感器測量等。光學(xué)傳感器測量結(jié)果精度較高，然而在動作捕獲系統(tǒng)中較少采用這類傳感器進(jìn)行測量，且成本較高，低成本設(shè)備難以達(dá)到測量精度[2]。三維影像測量需要利用攝像機(jī)搭建攝像測量系統(tǒng)，利用OpenCV和嵌入式設(shè)備進(jìn)行分析，數(shù)據(jù)處理復(fù)雜，硬件成本較高。而MEMS技術(shù)近幾年得到了快速發(fā)展，且成本低廉，性價比較高。但是MEMS器件，精度相對較差，同時陀螺儀、加速度計、地磁計單個傳感器無法得到滿意的姿態(tài)角信息，所以需要一些融合算法，進(jìn)行姿態(tài)估計。

本文提出了一種基于MEMS 九軸傳感器，采用Mahony姿態(tài)解算算法作為數(shù)據(jù)濾波器的人體手臂關(guān)節(jié)角度姿態(tài)測量系統(tǒng)，算法中以陀螺儀輸出角速度為主，加速度計和磁力計的即時輸出值對陀螺儀進(jìn)行修正，最終獲得三個姿態(tài)角，并以姿態(tài)角作為手臂關(guān)節(jié)運動判斷標(biāo)準(zhǔn)和三維模型跟蹤可視化，最后通過實驗證明了該系統(tǒng)的可行性和準(zhǔn)確性。

2 手臂關(guān)節(jié)測量系統(tǒng)

本文設(shè)計的系統(tǒng)主要分為兩個部分：1） 兩個分別安裝在人體上臂和下臂的基于MEMS慣性傳感器和MCU的手臂運動檢測模塊，用于獲取人體手臂運動過程中的姿態(tài)數(shù)據(jù)；2） 基于PC的上位機(jī)軟件，用于接收測姿態(tài)角數(shù)據(jù)和跟蹤顯示手臂的運動狀態(tài)。

2.1 MCU 硬件設(shè)計

整體硬件框圖如圖1所示。本系統(tǒng)硬件上由兩塊相同的測量節(jié)點構(gòu)成，節(jié)點原理圖、PCB 使用同一Al?tium工程，MCU主控芯片使用STM32F401，六軸陀螺儀、加速度計選用ICM20602，磁力計選用AK8975。MEMS 慣性傳感器作為外圍芯片與主控芯片STM32F4通過SPI協(xié)議進(jìn)行數(shù)據(jù)讀寫，從節(jié)點姿態(tài)數(shù)據(jù)通過串口1發(fā)送匯總至主節(jié)點。

2.2 MCU 軟件設(shè)計

如圖2所示，首先進(jìn)行外設(shè)及慣性傳感器初始化，主從節(jié)點時序同步后main.c 建立任務(wù)調(diào)度器并進(jìn)入循環(huán)執(zhí)行，任務(wù)調(diào)度中主要實現(xiàn)了傳感器數(shù)據(jù)的獲取、預(yù)處理及姿態(tài)解算，從機(jī)以一定頻率發(fā)送姿態(tài)數(shù)據(jù)，主機(jī)對pitch、roll和yaw姿態(tài)角匯總并計算關(guān)節(jié)角度，通過串口轉(zhuǎn)Wi-Fi發(fā)送至上位機(jī)進(jìn)行展示。

2.3 UDP 上位機(jī)

上位機(jī)使用Qt 開發(fā)。上位機(jī)使用UDP和串口讀取接收到的數(shù)據(jù)，測量模塊解算得到最終姿態(tài)角。QCustomPlot 繪制的“Pitch 波形”展示了主從節(jié)點的姿態(tài)角原始數(shù)據(jù)和濾波數(shù)據(jù)對比圖。上位機(jī)的圖形界面見圖3。

2.4 手臂模型載入與控制

模型的開發(fā)基于OpenGL和obj三維模型載入和重繪方式。obj文件通過文本行的形式指定頂點（v）、法線（vn）、紋理坐標(biāo)（vt）和面（f）等信息，因此本文通過篩選關(guān)鍵字來獲得模型的各種數(shù)據(jù)。最終三維手臂模型導(dǎo)入效果如圖4所示。

OpenGL進(jìn)行模型和視圖變換，主要涉及三種變化：translate（），模型平移；rotate（），模型逆時針旋轉(zhuǎn)；scale（num） ，模型縮放。

3 Mahony 姿態(tài)解算算法

本文不直接使用加速度和磁場強(qiáng)度信息進(jìn)行姿態(tài)解算，通過設(shè)計Mahony 濾波器，使用加速度和磁場強(qiáng)度信息修正機(jī)體角速度，算法流程如圖5所示。

Mahony 算法主要實現(xiàn)數(shù)據(jù)融合，算法用PI 控制器對重力加速度、磁力偏向的誤差量對陀螺儀數(shù)據(jù)進(jìn)行校正[3]。

3.1 加速度和角速度誤差

慣性系下的重力加速度在地球表面上任一點方向指向豎直向下，大小為g（≈ 9.81）。MEMS慣性傳感器測量得到的物理量是相對于慣性系的，因此需要將慣性系下坐標(biāo)GR 轉(zhuǎn)換到載體坐標(biāo)系。只需要左乘慣性系到載體坐標(biāo)系坐標(biāo)變換矩陣Gb R，得到理論重力加速度v?：

另外，還可以通過加速度計測量出實際重力加速度向量vˉ?？紤]到向量外積模的大小與向量夾角呈正相關(guān)，故通過計算外積來得到向量方向差值θ：

在進(jìn)行叉乘運算前，應(yīng)先將理論向量v?和實際向量vˉ做單位化處理，故有：

角速度誤差的求取本質(zhì)和加速度誤差一致，只是需要注意的是地磁的大小與方向并非固定不變，且磁場向量還需通過GbR變換到地理坐標(biāo)系中，經(jīng)過一定處理后，再通過GbR變換回機(jī)體坐標(biāo)系，從而得到理論地磁向量。最終計算得到誤差向量em[4] 。

3.2 誤差補(bǔ)償

根據(jù)四元數(shù)計算歐拉角可得到測量節(jié)點姿態(tài)。至此，主從機(jī)都實現(xiàn)了姿態(tài)解算的過程。

3.3 算法驗證

開始實驗前，模塊需要上電后在測量節(jié)點初始化過程中盡量將模塊置于水平面，這個過程包括兩塊測量節(jié)點進(jìn)行偏移量校正和主從機(jī)的時間同步。并且由于OpenGL控制模型旋轉(zhuǎn)角度的范圍是0°～360°，因此上位機(jī)需要將得到姿態(tài)角數(shù)值增加180°。

3.3.1 靜態(tài)實驗

將測量模塊靜止放置在水平平臺，則俯仰角、橫滾角和偏航角均為180°。由圖6可見yaw角誤差會隨著時間累積，靜態(tài)特性較差，而pitch和roll角原始波形較為平穩(wěn)，靜態(tài)特性良好。在經(jīng)過Mahony算法互補(bǔ)濾波之后，各個姿態(tài)角精度很高，誤差在0. 5°之內(nèi)，并且沒有發(fā)生角度偏移現(xiàn)象。

3.3.2 動態(tài)實驗

在室外進(jìn)行時長30s的手臂動態(tài)實驗，動作包括單一姿態(tài)角變化和混合姿態(tài)角變化。如圖7所示，Mahony算法解算的姿態(tài)角輸出平滑，濾波效果更好，動態(tài)誤差小。手臂變換運動軌跡時姿態(tài)角快速變化時，姿態(tài)角曲線光滑，不存在“尖峰”問題，可快速響應(yīng)姿態(tài)的變化。

3.3.3 手臂跟蹤動態(tài)實驗

將姿態(tài)測量單元固定于靠近腕關(guān)節(jié)和肘關(guān)節(jié)3cm 處，保持身體其他部位不動，肩關(guān)節(jié)位置可近似看成固定，作為位置原點，此時IMU測量單元的測量精確程度是最高的[5]。圖8為手臂姿態(tài)跟蹤展示。

測量時發(fā)現(xiàn)測量節(jié)點pitch 接近90°時，測量誤差逐漸增大，如表1所示。測量誤差源于三方面，一是靜態(tài)測量需要先將測量節(jié)點安裝在手臂上，再進(jìn)行關(guān)節(jié)尺測量，測量角度很容易出現(xiàn)誤差。二是手臂的皮膚不平整也可能造成載體坐標(biāo)系出現(xiàn)偏移從而影響測量結(jié)果。三是pitch測量值在接近90°時會達(dá)到極值。

由于yaw姿態(tài)角和pitch姿態(tài)角本質(zhì)上都是肘關(guān)節(jié)和肩關(guān)節(jié)的屈伸運動，并且由于yaw不存在極值問題，因此在動態(tài)測量的表現(xiàn)上要優(yōu)于pitch角。根據(jù)圖8 手臂三維模型的運動看出基本和手臂姿態(tài)保持一致

4 總結(jié)

本課題設(shè)計了一種基于IMU和Mahony的雙節(jié)點手臂姿態(tài)測量系統(tǒng)，通過實驗對算法進(jìn)行驗證，結(jié)果表明本文Mahony融合算法能夠有效融合多個傳感器數(shù)據(jù)，可以滿足手臂關(guān)節(jié)姿態(tài)高精度解算的任務(wù)需求。而且，系統(tǒng)還可以進(jìn)一步優(yōu)化：可將傳感器替換成集成九軸MEMS慣性傳感器可在一定程度減小因焊接過程中可能存在的安裝移位造成的坐標(biāo)系軸的偏移。

MEMS慣性傳感器進(jìn)行偏移量的計算與校正的算法仍然可優(yōu)化，通過得到精確的偏移量減小偏移誤差可使測量結(jié)果更精確。如果增加氣壓計則可基于本課題設(shè)計的關(guān)節(jié)姿態(tài)測量模塊進(jìn)行高度測定，本模塊作為一個節(jié)點進(jìn)而建立一套可實現(xiàn)全身動作捕捉的測量系統(tǒng)。