基于IMU和Mahony算法的人體手臂關節角度姿態測量系統研究

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

關鍵詞：IMU；手臂關節測量；Mahony算法；跟蹤模擬

1 緒論

1.1 研究背景和意義

人體關節角度檢測在康復醫療、體育運動、實時監護等領域都非常有應用價值。在康復醫療領域，對關節極限角度進行檢測，可以為醫生的臨床診斷、病人的康復效果評估提供重要參數；在體育運動領域，對關節運動規律捕捉分析，可以為運動員的訓練方法提供科學指導;在實時監護領域，獲取關節運動信息可以為老年人、康復期患者的跌倒風險評估、步態檢測等提供重要依據[1]。

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

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

2 手臂關節測量系統

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

2.1 MCU 硬件設計

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

2.2 MCU 軟件設計

如圖2所示，首先進行外設及慣性傳感器初始化，主從節點時序同步后main.c 建立任務調度器并進入循環執行，任務調度中主要實現了傳感器數據的獲取、預處理及姿態解算，從機以一定頻率發送姿態數據，主機對pitch、roll和yaw姿態角匯總并計算關節角度，通過串口轉Wi-Fi發送至上位機進行展示。

2.3 UDP 上位機

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

2.4 手臂模型載入與控制

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

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

3 Mahony 姿態解算算法

本文不直接使用加速度和磁場強度信息進行姿態解算，通過設計Mahony 濾波器，使用加速度和磁場強度信息修正機體角速度，算法流程如圖5所示。

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

3.1 加速度和角速度誤差

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

另外，還可以通過加速度計測量出實際重力加速度向量vˉ。考慮到向量外積模的大小與向量夾角呈正相關，故通過計算外積來得到向量方向差值θ：

在進行叉乘運算前，應先將理論向量v?和實際向量vˉ做單位化處理，故有：

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

3.2 誤差補償

根據四元數計算歐拉角可得到測量節點姿態。至此，主從機都實現了姿態解算的過程。

3.3 算法驗證

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

3.3.1 靜態實驗

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

3.3.2 動態實驗

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

3.3.3 手臂跟蹤動態實驗

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

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

由于yaw姿態角和pitch姿態角本質上都是肘關節和肩關節的屈伸運動，并且由于yaw不存在極值問題，因此在動態測量的表現上要優于pitch角。根據圖8 手臂三維模型的運動看出基本和手臂姿態保持一致

4 總結

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

MEMS慣性傳感器進行偏移量的計算與校正的算法仍然可優化，通過得到精確的偏移量減小偏移誤差可使測量結果更精確。如果增加氣壓計則可基于本課題設計的關節姿態測量模塊進行高度測定，本模塊作為一個節點進而建立一套可實現全身動作捕捉的測量系統。