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

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