一種基于蟻群算法誤差修正的人體手臂運動跟蹤方法
【專利摘要】一種基于蟻群算法誤差修正的人體手臂運動跟蹤方法，采用蟻群算法對加速度計靜態(tài)誤差進行校正，補償加速度計輸出對系統(tǒng)精度的影響，采用卡爾曼濾波算法進行姿態(tài)解算，估計手臂關(guān)節(jié)姿態(tài)，通過空間三維變換，得到以肩關(guān)節(jié)為中心節(jié)點的手臂運動位置。引入了運動幾何約束模型，對上下臂的收縮/伸展和下臂的翻轉(zhuǎn)進行了角度約束，肘關(guān)節(jié)的內(nèi)轉(zhuǎn)角被限定在很小的角度，這些約束對手臂運動過程中由于抖動和肌肉收縮變形產(chǎn)生的漂移進行補償，使人體動畫模型真實的展現(xiàn)了手臂的實時運動。
【專利說明】一種基于蟻群算法誤差修正的人體手臂運動跟蹤方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種姿態(tài)跟蹤方法，尤其是一種基于蟻群算法誤差修正的人體手臂運 動跟蹤方法。

【背景技術(shù)】
[0002] 姿態(tài)跟蹤系統(tǒng)在國防和國民經(jīng)濟中發(fā)揮著越來越重要的作用。通過對人體姿態(tài)的 實時捕捉測量及數(shù)據(jù)的處理和分析，可以有效地實現(xiàn)諸如三維步態(tài)分析、人工假肢輔助制 作和矯正、脊柱彎曲矯正測量等輔助診斷和治療以及運動訓練等。
[0003] 慣性運動跟蹤系統(tǒng)主要由MEMS(MicroelectromechanicalSystems，縮寫為MEMS) 加速度計、陀螺儀和磁力計組成。由于MEMS傳感器的制造原因和使用方法，MEMS加速度計 的靜態(tài)輸出存在一定的測量誤差，限制了其使用效果。目前，工程中使用較為普遍的靜態(tài)修 正方法是最大值最小值法，該方法的缺點是僅能估計標度因子誤差和零位偏差，而且容易 受到隨機誤差的干擾。另一種較為常用的修正方法是和陀螺儀組成慣性測量單元進行測 試，具體的有組合標定補償方法，該方法主要是依靠旋轉(zhuǎn)平臺提供的參考數(shù)據(jù)進行誤差修 正，可以同時校準靜態(tài)和動態(tài)誤差，此方法模型完善、修正效果精度高，但是在修正過程中， 需要多次操作旋轉(zhuǎn)平臺，修正過程復雜繁瑣，而且旋轉(zhuǎn)平臺的造價也超出了普通用戶的承 受范圍。
[0004] 已知，人體運動具有一定的幾何約束，引入這些約束可以實現(xiàn)更好的運動跟蹤效 果。目前常用的關(guān)節(jié)鏈接的骨骼模型跟蹤人體姿態(tài)的方法，通過坐標系間的變換得到手臂 肘關(guān)節(jié)和腕關(guān)節(jié)間空間位置，通過約束關(guān)系運用拉格朗日最優(yōu)化方法得到參考肩關(guān)節(jié)點的 位置。該方法定位了關(guān)節(jié)位置，但在手臂的翻轉(zhuǎn)時不能得到相應的翻轉(zhuǎn)角度。


【發(fā)明內(nèi)容】

[0005] 本發(fā)明目的在于提供一種可提高姿態(tài)信息精度、并能改善人體慣性運動跟蹤中精 度差與運動漂移問題的基于蟻群算法誤差修正的人體手臂運動跟蹤方法。
[0006] 為實現(xiàn)上述目的，本發(fā)明所述跟蹤方法包括如下步驟：
[0007] (1)將三軸MEMS加速度計、三軸陀螺儀、三軸磁力計和微控制器通過外圍電路焊 接于電路板上組成慣性傳感單元，并保證加速度計的正常操作和數(shù)據(jù)輸出；將慣性傳感單 元固定于可繞空間一點360度旋轉(zhuǎn)的轉(zhuǎn)動平臺上，保證電路板能夠在空間的三個軸向上自 由旋轉(zhuǎn)；
[0008] (2)通過可360度旋轉(zhuǎn)的轉(zhuǎn)動平臺，以手動或者自動方式驅(qū)動慣性傳感單元在空 間旋轉(zhuǎn)，且旋轉(zhuǎn)速度小于10° /s，保證慣性傳感單元的旋轉(zhuǎn)軌跡覆蓋軌跡球面的全部分 布，同時記錄慣性傳感單元的實際輸出數(shù)據(jù)，建立數(shù)據(jù)庫；
[0009] (3)將采集的數(shù)據(jù)進行預處理，去除錯誤數(shù)據(jù)，建立慣性傳感單元誤差模型，設計 加速度計的誤差估計算法；
[0010] (4)對誤差模型中慣性傳感單元的輸出數(shù)據(jù)進行分析，采用蟻群算法對誤差模型 進行參數(shù)擬合，對誤差模型進行靜態(tài)修正，設置加速度的誤差修正模型，設計加速度計的 誤差估計算法，得到最佳的誤差參數(shù)；
[0011] (5)采用卡爾曼濾波算法進行姿態(tài)解算，估計手臂關(guān)節(jié)位置；
[0012] (6)分析手臂姿態(tài)的運動規(guī)律，提出肘關(guān)節(jié)在手臂運動過程中的幾何約束條件，建 立腕、肘關(guān)節(jié)的空間坐標系，得出手臂的關(guān)節(jié)位置估計；建立關(guān)節(jié)伸展、收縮的平面約束，補 償漂移誤差；
[0013] (7)將慣性傳感單元固定在人體手臂上，對人體手臂運動姿態(tài)進行跟蹤。
[0014] 本方法針對上、下臂的收縮、伸展和下臂的翻轉(zhuǎn)進行角度約束，通過空間三維變 換，得到以肩關(guān)節(jié)為中心節(jié)點的手臂運動姿態(tài)，所述的角度約束通過人體三維模型應用實 現(xiàn)手臂的姿態(tài)跟蹤與再現(xiàn)。
[0015] 其中，所述蟻群算法步驟如下：定義蟻群個數(shù)為100,根據(jù)自變量的范圍，隨機初 始群體位置，設目標函數(shù)值為信息素大小；每只螞蟻依據(jù)信息素的大小判斷其是否移動及 移動程度，轉(zhuǎn)移概率P為下一轉(zhuǎn)移點函數(shù)值與最小函數(shù)值之差與最小函數(shù)值的比值，若P大 于某個隨機數(shù)時，進行全局搜索，否則進行局部搜索；當原來值不是最優(yōu)值時，則螞蟻向最 優(yōu)值位置移動；同時更新當前點信息素：4 +/，其中&為函數(shù)值，<為第i只 螞蟻當前位置信息素的強度，p(〇<p< 1)為信息素蒸發(fā)系數(shù)對上述過程重復迭代，直 至得到最優(yōu)擬合參數(shù)或達到一定的迭代次數(shù)。
[0016] 本發(fā)明方法工作原理大致如下：
[0017] 首先通過分析MEMS加速度計的測量原理及其使用方法，確定其主要誤差來源，在 原有的誤差模型基礎(chǔ)上，提出了基于蟻群算法的加速度計靜態(tài)修正方法，得到最優(yōu)的加速 度值。然后采用卡爾曼濾波算法進行姿態(tài)解算，同時針對上下臂的收縮、伸展和下臂的翻轉(zhuǎn) 進行角度約束，通過手臂的骨骼約束計算肘節(jié)點、腕節(jié)點相對于肩部的位置，對手臂運動過 程中抖動和肌肉變形引起的隨機漂移誤差進行補償，得到手臂位置信息。
[0018] 與現(xiàn)有技術(shù)相比，本發(fā)明具有如下優(yōu)點：
[0019] 1、本方法有效地剔除采樣過程中的隨機誤差，提高MEMS加速度計的測量精度；
[0020] 2、本方法不需要三軸旋轉(zhuǎn)平臺，降低用戶使用成本，計算方法簡單、易于實現(xiàn)，使 MEMS加速度計更好的滿足工程實際要求，為戶外測試等環(huán)境提供新選擇；
[0021] 3、本方法通過設計手臂肘關(guān)節(jié)的幾何約束模型，對手臂運動過程中抖動和肌肉變 形引起的隨機漂移誤差進行了補償，能得到手臂關(guān)節(jié)的正確位置估計。

【專利附圖】

【附圖說明】
[0022] 圖1是本發(fā)明方法的工作流程圖。
[0023] 圖2是本方法中蟻群算法的尋優(yōu)流程圖。
[0024] 圖3是實施例一中加速度原始值與誤差修正后加速度幅值的對比圖。
[0025] 圖4是實施例一中腕關(guān)節(jié)和肘關(guān)節(jié)空間參考坐標圖。
[0026] 圖5是實施例一中關(guān)節(jié)伸展、收縮約束的簡易效果圖。
[0027] 圖6是實施例一中測量單元固定于腕關(guān)節(jié)手臂彎曲運動時腕關(guān)節(jié)運動軌跡圖。
[0028] 圖7是實施例一中測量單元固定于手臂中部手臂彎曲運動時腕關(guān)節(jié)運動軌跡圖。
[0029] 圖8是實施例一中肘關(guān)節(jié)的節(jié)點位置估計坐標圖。
[0030] 圖9是實施例一中腕關(guān)節(jié)的節(jié)點位置估計坐標圖。

【具體實施方式】
[0031] 本發(fā)明所述跟蹤方法包括如下步驟：
[0032] (1)將三軸MEMS加速度計、三軸陀螺儀、三軸磁力計和微控制器通過外圍電路焊 接于電路板上組成慣性傳感單元，并保證加速度計的正常操作和數(shù)據(jù)輸出；將慣性傳感單 元固定于可繞空間一點360度旋轉(zhuǎn)的轉(zhuǎn)動平臺上，保證電路板能夠在空間的三個軸向上自 由旋轉(zhuǎn)；
[0033] (2)通過外部裝置可360度旋轉(zhuǎn)的轉(zhuǎn)動平臺，以手動或者自動方式驅(qū)動慣性傳感 單元在空間旋轉(zhuǎn)，且旋轉(zhuǎn)速度小于10° /s，保證慣性傳感單元的旋轉(zhuǎn)軌跡覆蓋軌跡球面的 全部分布，同時記錄慣性傳感單元的實際輸出數(shù)據(jù)，建立數(shù)據(jù)庫；
[0034] (3)將采集的數(shù)據(jù)進行預處理后，去除錯誤數(shù)據(jù)，建立慣性傳感單元誤差模型，設 計加速度計的誤差估計算法；
[0035] (4)對誤差模型中慣性傳感單元的輸出數(shù)據(jù)進行分析，采用蟻群算法對誤差模型 進行參數(shù)擬合，對誤差模型進行靜態(tài)修正，設置加速度的誤差修正模型，設計加速度計的誤 差估計算法，得到最佳的誤差參數(shù)；
[0036] (5)采用卡爾曼濾波算法進行姿態(tài)解算，估計手臂關(guān)節(jié)位置；
[0037] (6)分析手臂姿態(tài)的運動規(guī)律，提出肘關(guān)節(jié)在手臂運動過程中的幾何約束條件，建 立腕、肘關(guān)節(jié)的空間坐標系，得到手臂的關(guān)節(jié)位置估計；建立關(guān)節(jié)伸展、收縮的平面約束， 補償漂移誤差；
[0038] (7)將慣性傳感單元固定在人體手臂上，對人體手臂運動姿態(tài)進行跟蹤。
[0039] 本方法針對上、下臂的收縮、伸展和下臂的翻轉(zhuǎn)進行角度約束，通過空間三維變 換，得到以肩關(guān)節(jié)為中心節(jié)點的手臂運動姿態(tài)，所述的角度約束通過人體三維模型應用實 現(xiàn)手臂的姿態(tài)跟蹤與再現(xiàn)。
[0040] 以下結(jié)合附圖與【具體實施方式】對本發(fā)明做進一步說明：
[0041] 實施例一：
[0042] 所述慣性傳感單元由微控制器、三軸加速度計、三軸陀螺儀、三軸磁力計組成。微 控制器發(fā)送姿態(tài)信息給個人計算機，進行約束的解算。為兼顧處理速度和功耗的平衡，選用 STM32作為姿態(tài)傳感單元姿態(tài)解算的核心。加速度計選用ADXL345,設定最大測量范圍為 ±2g;陀螺儀和磁力計分別選用意法半導體的L3G4200和Honeywell的HMC5883。
[0043] 圖1是本發(fā)明方法的工作流程圖。采用蟻群算法對加速度計靜態(tài)誤差進行校正， 補償加速度計輸出對系統(tǒng)精度的影響。采用卡爾曼濾波算法進行姿態(tài)解算，估計手臂關(guān)節(jié) 姿態(tài)信息。同時引入了運動幾何約束模型，對上下臂的收縮/伸展和下臂的翻轉(zhuǎn)進行了角 度約束，這些約束對手臂運動過程中由于抖動和肌肉收縮變形產(chǎn)生的漂移進行補償。
[0044]圖2是蟻群算法的尋優(yōu)流程圖。通過蟻群算法對加速度計誤差模型參數(shù)進行數(shù)據(jù) 擬合，蟻群算法根據(jù)群體中信息素大小指導螞蟻運動，從而得到待解決問題最優(yōu)解。
[0045] 蟻群算法步驟如下：定義蟻群個數(shù)為100,根據(jù)自變量的范圍，隨機初始群體位 置，設目標函數(shù)值就為信息素大??；每只螞蟻依據(jù)信息素的大小判斷其是否移動及移動程 度，轉(zhuǎn)移概率P為下一轉(zhuǎn)移點函數(shù)值（即信息素）與最小函數(shù)值之差與最小函數(shù)值的比值， 若P大于某個隨機數(shù)時，進行全局搜索，否則進行局部搜索；當原來值不是最優(yōu)值時，則螞 蟻向最優(yōu)值位置移動；同時更新當前點信息素：Tf-(I-p)rf+ ./；，其中fi為函數(shù)值，if為 第i只螞蟻當前位置信息素的強度，P(0 <P< 1)為信息素蒸發(fā)系數(shù)。對上述過程重復 迭代，直至得到最優(yōu)擬合參數(shù)或達到一定的迭代次數(shù)。
[0046] 擬合得到的最佳誤差模型參數(shù)值為：
[0047]

【權(quán)利要求】
1. 一種基于蟻群算法誤差修正的人體手臂運動跟蹤方法，其特征在于，所述跟蹤方法 包括如下步驟： (1) 將三軸MEMS加速度計、三軸陀螺儀、三軸磁力計和微控制器通過外圍電路焊接于 電路板上組成慣性傳感單元，并保證加速度計的正常操作和數(shù)據(jù)輸出；將慣性傳感單元固 定于可繞空間一點360度旋轉(zhuǎn)的轉(zhuǎn)動平臺上，保證電路板能夠在空間的三個軸向上自由旋 轉(zhuǎn)； (2) 通過可360度旋轉(zhuǎn)的轉(zhuǎn)動平臺，以手動或者自動方式驅(qū)動慣性傳感單元在空間旋 轉(zhuǎn)，且旋轉(zhuǎn)速度小于10° /s，保證慣性傳感單元的旋轉(zhuǎn)軌跡覆蓋加速度計旋轉(zhuǎn)球面的所有 分布，同時記錄慣性傳感單元的實際輸出數(shù)據(jù)； (3) 將采集的數(shù)據(jù)進行預處理后，去除錯誤數(shù)據(jù)，建立慣性傳感單元誤差模型，設計加 速度計的誤差估計算法； (4) 對誤差模型中慣性傳感單元的輸出數(shù)據(jù)進行分析，采用蟻群算法對誤差模型進行 參數(shù)擬合，對誤差模型進行靜態(tài)修正，設置加速度的誤差修正模型，設計加速度計的誤差估 計算法，得到最佳的誤差參數(shù)； (5) 采用卡爾曼濾波算法進行姿態(tài)解算，估計手臂關(guān)節(jié)位置； (6) 分析手臂姿態(tài)的運動規(guī)律，提出肘關(guān)節(jié)在手臂運動過程中的幾何約束條件，建立 腕、肘關(guān)節(jié)的空間坐標系，得出手臂的關(guān)節(jié)位置估計；建立關(guān)節(jié)伸展、收縮的平面約束，補償 漂移誤差； (7) 將慣性傳感單元固定在人體手臂上，對人體手臂運動姿態(tài)進行跟蹤。
2. 根據(jù)權(quán)利要求1所述的一種基于蟻群算法誤差修正的人體手臂運動跟蹤方法，其特 征在于：針對上、下臂的收縮、伸展和下臂的翻轉(zhuǎn)進行角度約束，通過空間三維變換，得到以 肩關(guān)節(jié)為中心節(jié)點的手臂運動姿態(tài)，所述的角度約束通過人體三維模型應用實現(xiàn)手臂的姿 態(tài)跟蹤與再現(xiàn)。
3. 根據(jù)權(quán)利要求1所述的一種基于蟻群算法誤差修正的人體手臂運動跟蹤方法，其特 征在于，所述蟻群算法步驟如下：定義蟻群個數(shù)為100,根據(jù)自變量的范圍，隨機初始群體 位置，設目標函數(shù)值為信息素大?。幻恐晃浵佉罁?jù)信息素的大小判斷其是否移動及移動程 度，轉(zhuǎn)移概率P為下一轉(zhuǎn)移點函數(shù)值與最小函數(shù)值之差與最小函數(shù)值的比值，若P大于某個 隨機數(shù)時，進行全局搜索，否則進行局部搜索；當原來值不是最優(yōu)值時，則螞蟻向最優(yōu)值位 置移動；同時更新當前點信息素：< ^0-ρ>?+/;，其中ft為函數(shù)值，<為第i只螞蟻 當前位置信息素的強度，P(〇 <P<1)為信息素蒸發(fā)系數(shù)，對上述過程重復迭代，直至得 到最優(yōu)擬合參數(shù)或達到一定的迭代次數(shù)。
【文檔編號】G01C21/00GK104457741SQ201410742272
【公開日】2015年3月25日 申請日期:2014年12月8日 優(yōu)先權(quán)日:2014年12月8日
【發(fā)明者】金梅, 張立國, 張志福, 朱文娟, 任立斌 申請人:燕山大學