一種基于慣性和地磁傳感器的目標姿態(tài)跟蹤方法
【技術領域】
[0001] 本發(fā)明涉及智能傳感與人機交互技術領域，尤其涉及一種基于慣性和地磁傳感器 的目標姿態(tài)跟蹤方法。
【背景技術】
[0002] 基于姿態(tài)跟蹤的增強現(xiàn)實技術將虛擬的三維物體、視頻、文字、圖片等計算機生成 的信息實時疊加顯示到真實場景中，通過姿態(tài)跟蹤實現(xiàn)自然的人機交互，在醫(yī)療衛(wèi)生、軍事 仿真、工業(yè)維修、教育娛樂等行業(yè)具有廣泛的應用前景。增強現(xiàn)實中的姿態(tài)跟蹤的目的在于 識別運動目標并獲得其姿態(tài)變化信息，所獲取的姿態(tài)信息通常被應用在如下五方面：
[0003] 1)視野控制，利用姿態(tài)信息控制虛擬攝像機的位置和方向；
[0004] 2)導航，協(xié)助用戶在虛擬環(huán)境中進行移動；
[0005] 3)虛實交互，通過跟蹤用戶的肢體，實現(xiàn)抓取、移動等交互功能；
[0006] 4)儀器跟蹤，跟蹤系統(tǒng)中的儀器設備，如手術工具、維修工具等；
[0007] 5)姿態(tài)傳遞，將用戶的姿態(tài)信息傳遞給隨動設備，如手術機器人或電影中的虛擬 角色。
[0008] 常用于姿態(tài)跟蹤的慣性傳感器件包括慣性陀螺儀和加速計等。在測量中采用捷聯(lián) 式慣性導航原理目標姿態(tài)，其中慣性陀螺主要用于測量三軸旋轉(zhuǎn)角速度或角加速度，并利 用增量積分法獲得跟蹤器的方向信息，但會產(chǎn)生隨時間增加的漂移誤差；加速計則主要用 于測量三個敏感軸上的重力加速度分量，再利用余弦算法獲得跟蹤器的俯仰角和橫滾角并 克服陀螺儀帶來的這兩個姿態(tài)角上的漂移誤差。慣性跟蹤器具有體積小、重量輕、無遮擋問 題、工作范圍不受限制以及刷新率高等優(yōu)點，同時利用磁場傳感器可以得到航向角的特點 可以實現(xiàn)對目標的三維姿態(tài)跟蹤。
[0009] 利用慣性傳感器進行人體姿態(tài)跟蹤的方法提出于1999年（參見Jihong Lee, Insoo Ha. Sensor fusion and calibration for motion captures using accelerometers. Robotics and Automation, Volume 3, Page 1954 - 1959. 1999) 〇 該方法 利用地球重力在加速度計三個敏感軸上的分量計算目標的俯仰角和橫滾角，可以得到目標 在兩個自由度上的姿態(tài)信息，缺點是僅通過重力分量無法得到完整的三自由度姿態(tài)信息并 且加速度傳感器數(shù)據(jù)不適用于可提高系統(tǒng)精度的卡爾曼濾波器。
[0010] 利用陀螺儀、加速度計和磁傳感器的姿態(tài)跟蹤方法如2007年，Rehbinder, H等 (參見 Rehbinder, H.，Xiaoming Hu. Drift-free attitude estimation for accelerated rigid bodies. Robotics and Automation, Volume 4, Page 4244 - 4249.2001)提出使用多 傳感器融合方法，通過結(jié)合角速度、重力加速度和磁場分量并利用卡爾曼濾波器進行誤差 校正，實現(xiàn)了對目標三維姿態(tài)的實時跟蹤，但是該方法沒能解決線性加速度對目標姿態(tài)跟 蹤的影響。

【發(fā)明內(nèi)容】
toon] 有鑒于此，本發(fā)明提供了一種基于慣性和地磁傳感器的目標姿態(tài)跟蹤方法，無需 復雜的跟蹤設備和特定的跟蹤環(huán)境，使用基于慣性和地磁傳感器即可完成對目標的實時三 維姿態(tài)跟蹤，且跟蹤結(jié)果具有精度和刷新率高的特點。
[0012] 本發(fā)明的一種基于慣性和地磁傳感器的目標姿態(tài)跟蹤方法，采用陀螺儀、加速度 計以及地磁傳感器分別采集目標當前姿態(tài)所對應的角速度、加速度和磁強度數(shù)據(jù)在三個敏 感軸上的分量，并采用卡爾曼濾波器對上述三個傳感器數(shù)據(jù)進行融合并計算目標的姿態(tài)信 息，具體方法如下：
[0013] 步驟1、通過卡爾曼濾波器對目標姿態(tài)信息進行時間更新，具體為：
[0014] S10、在時間更新方程中對目標的狀態(tài)矢量尤進行估計：
【主權項】
1. 一種基于慣性和地磁傳感器的目標姿態(tài)跟蹤方法，其特征在于，采用陀螺儀、加速度 計以及地磁傳感器分別采集目標當前姿態(tài)所對應的角速度、加速度和磁強度數(shù)據(jù)在三個敏 感軸上的分量，并采用卡爾曼濾波器對上述三個傳感器數(shù)據(jù)進行融合并計算目標的姿態(tài)信 息，具體方法如下： 步驟1、通過卡爾曼濾波器對目標姿態(tài)信息進行時間更新，具體為： 510、 在時間更新方程中對目標的狀態(tài)矢量足進行估計： 其中，A為所述陀螺儀測得的目標在三個敏感軸上角速度測量值構(gòu)成的濾波器增益矩 陣；尤為當前時刻目標狀態(tài)矢量估計值，尤_,為卡爾曼濾波器上一時刻輸出的目標狀態(tài)矢 量； 511、 對卡爾曼濾波器的傳遞函數(shù)的估計值4進行更新： P； = APk_xAT + Q 其中，Q值為系統(tǒng)噪聲協(xié)方差矩陣;At為增益矩陣A的轉(zhuǎn)置矩陣；PlrtS卡爾曼濾波器 上一時刻輸出的傳遞函數(shù)； 步驟2、通過卡爾曼濾波器對目標姿態(tài)信息進行測量更新，具體為： 520、 基于步驟1中更新得到的傳遞函數(shù)的估計值4 ,獲得卡爾曼濾波器的測量置信參 數(shù)Kk: Kk =Pk Ht (HPk Ht +R)-1 其中，H為測量模型與估計模型間轉(zhuǎn)換矩陣：
Iiq1 q2 q3 q4]表示組成目標的狀態(tài)矢量估計值足的旋轉(zhuǎn)四元數(shù)，即尤七f: ft f4f ;R 為測量誤差模型：A =g2;其中ACCX、ACC# ACC z分別為加速度傳感器 測得的目標在三個敏感軸上的加速度分量，g為重力加速度； 521、 對目標的狀態(tài)矢量的測量結(jié)果足進行更新： Xt-Xl+Kk-iZk-HXp
表示目標的姿態(tài)，其中，Θ =arcsin(ACCx)表示目標的俯仰角，
表示目標的橫滾角，
表示目標的航向角，A = MAGxCos ( Θ ) +MAGySin ( Θ ) +MAGzCos (Θ ) sin ( γ )，B = MGyCos ( γ ) +MGzSin ( γ )，MAGX、MAGjP MG 2分別表示磁場傳感器獲得 的目標所在位置的地球磁場在體坐標系的三個敏感軸上的分量； 522、 對傳遞函數(shù)的測量結(jié)果Pk進行更新： Pk=(I-KkIl)Pk 步驟3、目標狀態(tài)矢量的測量結(jié)果足即為當前目標姿態(tài)的跟蹤結(jié)果，將當前時刻測量 結(jié)果尤和傳遞函數(shù)的測量結(jié)果Pk輸出并作為下一時刻濾波器的輸入結(jié)果，返回步驟1，進 行下一時刻的目標姿態(tài)跟蹤。
2.如權利要求1所述的一種基于慣性和地磁傳感器的目標姿態(tài)跟蹤方法，其特征在 于，所述步驟1中系統(tǒng)噪聲協(xié)方差矩陣Q取值為〇. 01。
【專利摘要】本發(fā)明公開了一種基于慣性和地磁傳感器的目標姿態(tài)跟蹤方法，無需任何參考標志物和特定跟蹤環(huán)境即可實現(xiàn)姿態(tài)跟蹤，方法簡單易行；采用陀螺儀、加速度計以及地磁傳感器分別采集目標當前姿態(tài)所對應的角速度、加速度和磁強度數(shù)據(jù)在三個敏感軸上的分量，利用加速度計和磁傳感器校正陀螺儀的姿態(tài)跟蹤結(jié)果，消除了漂移誤差，從而提高跟蹤結(jié)果的精度；本發(fā)明使用高效離線運行的卡爾曼濾波算法，融合了多傳感器跟蹤結(jié)果，實現(xiàn)了實時在線姿態(tài)跟蹤。
【IPC分類】G01C21-16, G01C21-08
【公開號】CN104764451
【申請?zhí)枴緾N201510196626
【發(fā)明人】劉越, 賀長宇, 閆達遠, 常軍, 翁冬冬
【申請人】北京理工大學
【公開日】2015年7月8日
【申請日】2015年4月23日