用于跟蹤運(yùn)動(dòng)物體體內(nèi)具有磁信號(hào)的微型裝置的系統(tǒng)和方法
【專利摘要】本發(fā)明公開(kāi)了一種用于跟蹤運(yùn)動(dòng)物體體內(nèi)具有磁信號(hào)的微型裝置的系統(tǒng)和方法，優(yōu)點(diǎn)在于通過(guò)具有定位標(biāo)志的參考目標(biāo)和立體相機(jī)建立了外在的參考坐標(biāo)系，由參考坐標(biāo)系對(duì)于微型裝置基于傳感器坐標(biāo)系的定位數(shù)據(jù)進(jìn)行修正，能夠消除由于運(yùn)動(dòng)體移動(dòng)、轉(zhuǎn)動(dòng)、和相對(duì)伸縮引起的微型裝置相對(duì)檢測(cè)磁傳感器的移動(dòng)而帶來(lái)的測(cè)量誤差，提高定位精確度。
【專利說(shuō)明】
用于跟蹤運(yùn)動(dòng)物體體內(nèi)具有磁信號(hào)的微型裝置的系統(tǒng)和方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及一種體內(nèi)微型裝置的跟蹤技術(shù)領(lǐng)域，尤其是設(shè)及一種用于跟蹤運(yùn)動(dòng)物 體體內(nèi)具有磁信號(hào)的微型裝置的系統(tǒng)和方法。
【背景技術(shù)】
[0002] 近年來(lái)，運(yùn)動(dòng)物體體內(nèi)的目標(biāo)跟蹤越來(lái)越受重視，如運(yùn)動(dòng)平臺(tái)上物體內(nèi)目標(biāo)的跟 蹤，人體體內(nèi)裝置(如膠囊內(nèi)窺鏡)的跟蹤等。對(duì)于運(yùn)類運(yùn)動(dòng)物體體內(nèi)微型裝置的位置確定， 由于光線被遮擋，難W采用采用光學(xué)視覺(jué)的方法。而會(huì)考慮采用X射線檢查、CT檢查、核磁共 振檢查、Ξ維超聲檢查和磁定位等等。其中，X射線檢查、CT檢查、核磁共振檢查、Ξ維超聲檢 查運(yùn)幾種方法可W提供清晰度比較高的二維或Ξ維影像，但是運(yùn)些方法提供的是圖像信 息，需要進(jìn)一步的處理計(jì)算才能給出運(yùn)動(dòng)體內(nèi)微型裝置的Ξ維位置，且不能準(zhǔn)確給出運(yùn)動(dòng) 物體體內(nèi)微型裝置的對(duì)準(zhǔn)方向。同時(shí)，由于X射線、CT、核磁共振、Ξ維超聲運(yùn)些設(shè)備價(jià)格昂 貴，而且操作復(fù)雜，不可能長(zhǎng)時(shí)間進(jìn)行操作，而且對(duì)人會(huì)有副作用，所W希望其它更可行的 方法。
[0003] 磁定位是一種合理的方法，在運(yùn)動(dòng)物體對(duì)磁場(chǎng)沒(méi)有反射和透射阻擋的情況下，磁 性的微型裝置的定位可W獲得很高的精度。例如美國(guó)專利2005/0143648A1將小的永磁體放 入無(wú)線膠囊內(nèi)鏡中，并將兩根圓環(huán)分別固定在人體的胸部和腰部，兩根圓環(huán)上分別固定了4 個(gè)磁傳感器，使磁傳感器與體內(nèi)無(wú)線膠囊內(nèi)鏡的相對(duì)位置固定，來(lái)檢測(cè)膠囊內(nèi)鏡的位置和 方向。但是運(yùn)種跟蹤方法需要設(shè)置固定的傳感器機(jī)構(gòu)，對(duì)運(yùn)動(dòng)物體體內(nèi)微型裝置定位時(shí)，會(huì) 因運(yùn)動(dòng)物體的移動(dòng)而造成干擾，難W保證跟蹤精度。

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

[0004] 本發(fā)明所要解決的技術(shù)問(wèn)題是提供一種用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁信號(hào)的微型 裝置的系統(tǒng)，借助于外在的紅外標(biāo)志立體視覺(jué)定位的校正，消除由于運(yùn)動(dòng)物體移動(dòng)、轉(zhuǎn)動(dòng)、 和相對(duì)伸縮引起的微型裝置相對(duì)檢測(cè)磁傳感器的移動(dòng)而帶來(lái)的測(cè)量誤差。
[0005] 本發(fā)明解決上述技術(shù)問(wèn)題所采用的技術(shù)方案為:一種用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁 信號(hào)的微型裝置的系統(tǒng)，包括相對(duì)固定在運(yùn)動(dòng)體上的穿戴物，所述穿戴物上設(shè)有用于感應(yīng) 運(yùn)動(dòng)體體內(nèi)被測(cè)目標(biāo)微型裝置發(fā)出的磁信號(hào)的磁傳感器陣列；
[0006] 數(shù)據(jù)采集儀器，用于采集和預(yù)處理磁傳感器陣列發(fā)送的信息；
[0007] 和與數(shù)據(jù)采集儀器連接的數(shù)據(jù)處理與顯示中屯、，根據(jù)數(shù)據(jù)采集儀器傳送的信息計(jì) 算微型裝置在不同時(shí)間的定位數(shù)據(jù)，然后根據(jù)定位數(shù)據(jù)建立并顯示微型裝置的運(yùn)動(dòng)軌跡， 所述定位數(shù)據(jù)包括空間坐標(biāo)和方向參數(shù)；
[000引還包括安裝在運(yùn)動(dòng)體體表的至少兩個(gè)具有定位標(biāo)志的參考目標(biāo)，用于校正微型裝 置定位數(shù)據(jù)由于運(yùn)動(dòng)體移動(dòng)帶來(lái)的誤差；
[0009] 紅外雙目或多目立體相機(jī)，用于拍攝參考目標(biāo)的空間坐標(biāo)和方向參數(shù)；
[0010] 所述立體相機(jī)與數(shù)據(jù)處理與顯示中屯、連接；
[0011] 所述數(shù)據(jù)處理與顯示中屯、根據(jù)立體相機(jī)拍攝獲得的參考目標(biāo)的空間坐標(biāo)和方向 參數(shù)對(duì)計(jì)算獲得的微型裝置的定位數(shù)據(jù)進(jìn)行校正。
[0012] 其中，所述參考目標(biāo)的定位標(biāo)志為4個(gè)具有特定空間位置關(guān)系并形成方向指向的 紅外標(biāo)志球。
[0013] 作為優(yōu)選，所述微型裝置有多個(gè)，其中相對(duì)運(yùn)動(dòng)體運(yùn)動(dòng)的微型裝置為被測(cè)目標(biāo)，相 對(duì)運(yùn)動(dòng)體靜止的微型裝置為輔助參考目標(biāo);所述數(shù)據(jù)處理與顯示中屯、包括立體視覺(jué)的多目 標(biāo)定位計(jì)算單元;所述多目標(biāo)定位計(jì)算單元根據(jù)被測(cè)目標(biāo)相對(duì)輔助參考目標(biāo)的運(yùn)動(dòng)軌跡， 對(duì)被測(cè)目標(biāo)進(jìn)行相對(duì)定位，獲得被測(cè)目標(biāo)的校正定位數(shù)據(jù)。
[0014] 與現(xiàn)有技術(shù)相比，本發(fā)明的優(yōu)點(diǎn)在于通過(guò)具有定位標(biāo)志的參考目標(biāo)和立體相機(jī)建 立了外在的參考坐標(biāo)系，由參考坐標(biāo)系對(duì)于微型裝置基于傳感器坐標(biāo)系的定位數(shù)據(jù)進(jìn)行修 正，能夠消除由于運(yùn)動(dòng)體移動(dòng)、轉(zhuǎn)動(dòng)、和相對(duì)伸縮引起的微型裝置相對(duì)檢測(cè)磁傳感器的移動(dòng) 而帶來(lái)的測(cè)量誤差。
[0015] 本發(fā)明所要解決的另一技術(shù)問(wèn)題是提供一種用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁信號(hào)的 微型裝置的方法，借助于外在的基于立體視覺(jué)的參考坐標(biāo)系的修正，消除由于運(yùn)動(dòng)體移動(dòng)、 轉(zhuǎn)動(dòng)、和相對(duì)伸縮引起的微型裝置相對(duì)檢測(cè)磁傳感器的移動(dòng)而帶來(lái)的測(cè)量誤差。
[0016] 本方法的具體技術(shù)方案為:一種用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁信號(hào)的微型裝置的方 法，包括W下具體步驟：
[0017] S1W磁傳感器陣列某一點(diǎn)為標(biāo)準(zhǔn)構(gòu)建傳感器坐標(biāo)系，獲取磁傳感器陣列上各磁傳 感器所處的空間位置，并采集被測(cè)目標(biāo)微型裝置作用在所述各磁傳感器處的磁感應(yīng)強(qiáng)度數(shù) 據(jù)；
[0018] S2利用步驟S1采集到的各磁傳感器處的磁感應(yīng)強(qiáng)度數(shù)據(jù)，W及描述磁信號(hào)微型裝 置的磁偶極子模型，定義傳感器坐標(biāo)系下微型裝置形成的磁場(chǎng)與模型的誤差目標(biāo)函數(shù)；
[0019] S3利用非線性優(yōu)化算法尋找所述微型裝置的空間坐標(biāo)，使所述誤差目標(biāo)函數(shù)為最 小，此時(shí)所述微型裝置的空間坐標(biāo)和方向參數(shù)即為微型裝置的定位數(shù)據(jù)；
[0020] 還包括W下步驟:S4基于紅外立體相機(jī)拍攝固定在運(yùn)動(dòng)體體表的參考目標(biāo)的紅外 標(biāo)志球，獲得的參考目標(biāo)的空間坐標(biāo)和方向參數(shù)，然后根據(jù)參考目標(biāo)的空間坐標(biāo)和方向參 數(shù)校正微型裝置的定位數(shù)據(jù)由于運(yùn)動(dòng)體移動(dòng)帶來(lái)的誤差，獲得校正后的微型裝置的定位數(shù) 據(jù)；
[0021] S5基于校正后的微型裝置的定位數(shù)據(jù)，建立和顯示微型裝置的運(yùn)動(dòng)軌跡。
[0022] 其中，步驟S2所述的誤差目標(biāo)函數(shù)為Ξ個(gè)誤差函數(shù)分量之和，Ξ個(gè)誤差函數(shù)分量 分別為志X、玄r和石/，
[0025]
[00%] Bix、Biy和Biz為第1個(gè)磁傳感器所在位置磁場(chǎng)的Ξ個(gè)正交磁感應(yīng)強(qiáng)度分量測(cè)量值； a、b、c為微型裝置的空間坐標(biāo)參數(shù);m、n、p為微型裝置的方向參數(shù);xi、yi、zi為第1個(gè)磁傳感 器的空間坐標(biāo)分量;N表示磁傳感器陣列中的磁傳感器數(shù)量;化為微型裝置的磁體常數(shù);扣為 第1個(gè)磁傳感器到微型裝置內(nèi)磁體的距離：
[0027] 步驟S4的具體措施如下：
[00%] S4.1通過(guò)標(biāo)定，建立紅外標(biāo)志球組合的參考坐標(biāo)系與傳感器坐標(biāo)系一致;計(jì)算運(yùn) 動(dòng)體體表的兩個(gè)參考目標(biāo)的空間坐標(biāo)和方向參數(shù);獲得新的參考坐標(biāo)系，并選擇新參考坐 標(biāo)系的Ξ個(gè)參考坐標(biāo)軸為：
[0032] 其中，ex'為參考坐標(biāo)系下X'軸單位向量;Hi = (mi, ηι,ρι)為第一個(gè)參考目標(biāo)在傳感 器坐標(biāo)系下的方向向量;mx'，ηχ'，ρχ'為參考坐標(biāo)系下X'軸單位向量在傳感器坐標(biāo)系下的分 量;ez'為參考坐標(biāo)系下Ζ'軸單位向量;mz'，ηζ'，ρζ'為參考坐標(biāo)系下Ζ'軸單位向量在傳感器 坐標(biāo)系下的分量;出=(1112瓜瓜)為第二個(gè)參考目標(biāo)在傳感器坐標(biāo)系下的方向向量牌為傳 感器坐標(biāo)系下X軸單位向量;ΘΥ為傳感器坐標(biāo)系下Υ軸單位向量;ez為傳感器坐標(biāo)系下Ζ軸單 位向量;ΘΥ'為參考坐標(biāo)系下Y'軸單位向量;my'，ηγ'，ργ'為參考坐標(biāo)系下Y'軸單位向量在傳 感器坐標(biāo)系下的分量；
[0033] S4.2建立傳感器坐標(biāo)系與參考坐標(biāo)系的轉(zhuǎn)換矩陣R，
[0034] S4.3對(duì)微型裝置進(jìn)行坐標(biāo)轉(zhuǎn)換：
[0035] p，=R-i(p-AP)
[0036] 其中，P'是坐標(biāo)轉(zhuǎn)換后體內(nèi)被測(cè)目標(biāo)的空間坐標(biāo)，擴(kuò)1是R的逆矩陣，P是傳感器坐標(biāo) 系下體內(nèi)被測(cè)目標(biāo)的空間坐標(biāo)，A P是傳感器坐標(biāo)系原點(diǎn)指向參考坐標(biāo)系原點(diǎn)(可選第一個(gè) 參考目標(biāo)中屯、位置)的向量。
[0037] 作為改進(jìn)，所述微型裝置有多個(gè)，其中相對(duì)運(yùn)動(dòng)體運(yùn)動(dòng)的微型裝置設(shè)為被測(cè)目標(biāo)， 相對(duì)運(yùn)動(dòng)體靜止的微型裝置設(shè)為輔助參考目標(biāo);所述數(shù)據(jù)處理與顯示中屯、的多目標(biāo)定位計(jì) 算單元計(jì)算被測(cè)目標(biāo)相對(duì)輔助參考目標(biāo)的運(yùn)動(dòng)軌跡，對(duì)被測(cè)目標(biāo)進(jìn)行相對(duì)定位，修正被測(cè) 目標(biāo)步驟S3獲得的定位數(shù)據(jù)；W獲得的被測(cè)目標(biāo)的修正定位數(shù)據(jù)為基礎(chǔ)進(jìn)行步驟S4的定位 數(shù)據(jù)校正，從而減小運(yùn)動(dòng)體干擾對(duì)定位的影響。
[0038] 在被測(cè)目標(biāo)的定位數(shù)據(jù)修正后，還包括:分析各微型裝置定位數(shù)據(jù)的合理性，剔除 誤差大的不合理數(shù)據(jù)，然后用綜合優(yōu)化擬合算法對(duì)各微型裝置定位數(shù)據(jù)進(jìn)行處理，獲取各 微型裝置的定位定向結(jié)果。能夠通過(guò)輔助參考目標(biāo)的移動(dòng)確定微型裝置的非正常定位信 息，在消除微型裝置的非正常定位信息后，通過(guò)綜合優(yōu)化擬合算法能夠使被測(cè)目標(biāo)的微型 裝置的定位更為準(zhǔn)確，為后續(xù)的坐標(biāo)轉(zhuǎn)換提供更準(zhǔn)確的定位數(shù)據(jù)。
[0039] 本方法的優(yōu)點(diǎn)在于通過(guò)具有定位標(biāo)志的參考目標(biāo)和立體相機(jī)建立了外在的參考 坐標(biāo)系，由參考坐標(biāo)系對(duì)于微型裝置基于傳感器坐標(biāo)系的定位數(shù)據(jù)進(jìn)行修正，能夠消除由 于運(yùn)動(dòng)體移動(dòng)、轉(zhuǎn)動(dòng)、和相對(duì)伸縮引起的微型裝置相對(duì)檢測(cè)磁傳感器的移動(dòng)而帶來(lái)的測(cè)量 誤差，提局定位精確度。
【附圖說(shuō)明】
[0040] 圖1為本發(fā)明的系統(tǒng)原理框圖。
[0041] 圖2為本發(fā)明參考目標(biāo)紅外標(biāo)志球的其中一種結(jié)構(gòu)圖。
[0042] 圖3為本發(fā)明參考目標(biāo)紅外標(biāo)志球的另一種結(jié)構(gòu)圖。
[0043] 圖4為本發(fā)明的磁傳感器陳列示意圖。
[0044] 圖5為本發(fā)明的磁偶極子定位示意圖。
[0045] 圖6為本發(fā)明的紅外立體相機(jī)示意圖。
[0046] 圖7為本發(fā)明的參考坐標(biāo)系建立圖。
【具體實(shí)施方式】
[0047] W下結(jié)合附圖實(shí)施例對(duì)本發(fā)明作進(jìn)一步詳細(xì)描述。
[0048] 本優(yōu)選實(shí)施例如圖1所示為一種用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁信號(hào)的微型裝置的系 統(tǒng)，包括相對(duì)固定在運(yùn)動(dòng)體上的穿戴物，所述穿戴物上設(shè)有用于感應(yīng)運(yùn)動(dòng)體體內(nèi)被測(cè)目標(biāo) 微型裝置發(fā)出的磁信號(hào)的磁傳感器陣列，如圖4所示；
[0049] 數(shù)據(jù)采集儀器，用于采集和預(yù)處理磁傳感器陣列發(fā)送的信息；
[0050] 和與數(shù)據(jù)采集儀器連接的數(shù)據(jù)處理與顯示中屯、，根據(jù)數(shù)據(jù)采集儀器傳送的信息計(jì) 算微型裝置在不同時(shí)間的定位數(shù)據(jù)，然后根據(jù)定位數(shù)據(jù)建立并顯示微型裝置的運(yùn)動(dòng)軌跡， 所述定位數(shù)據(jù)包括空間坐標(biāo)和方向參數(shù)；
[0051] 還包括安裝在運(yùn)動(dòng)體體表的至少兩個(gè)具有定位標(biāo)志的參考目標(biāo)，用于校正微型裝 置定位數(shù)據(jù)由于運(yùn)動(dòng)體移動(dòng)帶來(lái)的誤差；
[0052] 紅外雙目或多目立體相機(jī)，用于拍攝參考目標(biāo)的空間坐標(biāo)和方向參數(shù)，如圖6所 示；
[0053] 所述立體相機(jī)與數(shù)據(jù)處理與顯示中屯、連接；
[0054] 所述數(shù)據(jù)處理與顯示中屯、根據(jù)立體相機(jī)拍攝獲得的參考目標(biāo)的空間坐標(biāo)和方向 參數(shù)對(duì)計(jì)算獲得的被測(cè)目標(biāo)微型裝置的定位數(shù)據(jù)進(jìn)行校正。
[0055] 其中，所述參考目標(biāo)的定位標(biāo)志為4個(gè)具有特定空間位置關(guān)系并形成方向指向的 紅外標(biāo)志球，如圖2和3所示，四個(gè)紅外標(biāo)志球的相對(duì)位置始終保持不變，它們組成的四面體 的質(zhì)屯、位置即為參考目標(biāo)的位置。
[0056] 作為優(yōu)選，所述微型裝置有多個(gè)，其中相對(duì)運(yùn)動(dòng)體運(yùn)動(dòng)的微型裝置為被測(cè)目標(biāo)，相 對(duì)運(yùn)動(dòng)體靜止的微型裝置為輔助參考目標(biāo);所述數(shù)據(jù)處理與顯示中屯、包括立體視覺(jué)的多目 標(biāo)定位計(jì)算單元;所述多目標(biāo)定位計(jì)算單元根據(jù)被測(cè)目標(biāo)相對(duì)輔助參考目標(biāo)的運(yùn)動(dòng)軌跡， 對(duì)被測(cè)目標(biāo)進(jìn)行相對(duì)定位，獲得被測(cè)目標(biāo)的校正定位數(shù)據(jù)。
[0057] 基于上述系統(tǒng)定位微型裝置的跟蹤方法，具體如下：
[0058] S1W磁傳感器陣列某一點(diǎn)為標(biāo)準(zhǔn)構(gòu)建傳感器坐標(biāo)系，獲取磁傳感器陣列上各磁傳 感器所處的空間位置，并采集被測(cè)目標(biāo)微型裝置作用在所述各磁傳感器處的磁感應(yīng)強(qiáng)度數(shù) 據(jù)；
[0059] S2利用步驟S1采集到的各磁傳感器處的磁感應(yīng)強(qiáng)度數(shù)據(jù)，W及描述磁信號(hào)微型裝 置的磁偶極子模型，定義傳感器坐標(biāo)系下微型裝置形成的磁場(chǎng)與模型的誤差目標(biāo)函數(shù)；
[0060] S3利用非線性優(yōu)化算法尋找所述微型裝置的空間坐標(biāo)，使所述誤差目標(biāo)函數(shù)為最 小，此時(shí)所述微型裝置的空間坐標(biāo)和方向參數(shù)即為微型裝置的定位數(shù)據(jù)；
[0061 ] S4基于紅外立體相機(jī)拍攝固定在運(yùn)動(dòng)體體表的參考目標(biāo)的紅外標(biāo)志球，獲得的參 考目標(biāo)的空間坐標(biāo)和方向參數(shù)，然后根據(jù)參考目標(biāo)的空間坐標(biāo)和方向參數(shù)校正微型裝置的 定位數(shù)據(jù)由于運(yùn)動(dòng)體移動(dòng)帶來(lái)的誤差，獲得校正后的微型裝置的定位數(shù)據(jù)；
[0062] S5基于校正后的微型裝置的定位數(shù)據(jù)，建立和顯示微型裝置的運(yùn)動(dòng)軌跡。
[0063] S1~S3采用現(xiàn)有技術(shù)，具體參見(jiàn)授權(quán)公告號(hào)為CN100594840C的中國(guó)發(fā)明專利一種 跟蹤體內(nèi)微型裝置的方法。在本實(shí)施例中，磁傳感器陳列如圖4所示，在步驟S1中傳感器坐 標(biāo)系構(gòu)建步驟如下：
[0064] A01、在定位區(qū)域內(nèi)規(guī)定至少一個(gè)空間點(diǎn)作為校正點(diǎn)；
[0065] A02、將一個(gè)標(biāo)定磁體按確定方向置于一所述校正點(diǎn)上，測(cè)試所述磁傳感器陣列中 各磁傳感器的磁感應(yīng)強(qiáng)度數(shù)據(jù)，W及用畢奧一薩伐爾定律計(jì)算所述各傳感器處的磁場(chǎng)強(qiáng) 度；
[0066] A03、比較各磁傳感器計(jì)算獲取的磁場(chǎng)強(qiáng)度和測(cè)量獲取的磁感應(yīng)強(qiáng)度數(shù)據(jù)，得到一 誤差分值；
[0067] A04、重復(fù)步驟A02和A03,得到所述標(biāo)定磁體在各校正點(diǎn)時(shí)的誤差分值，并獲得一 誤差值，所述誤差值為所述各誤差分值之和；
[0068] A05、調(diào)整所述磁傳感器的靈敏度、位置和方向參數(shù)定標(biāo)參數(shù)，使所述誤差值為最 小，此時(shí)，所述磁傳感器的靈敏度、位置和方向即為磁傳感器的標(biāo)定參數(shù)。
[0069] 步驟S2中，磁偶極子定位如圖5所示，其模型為：
其中，^表示第1個(gè)磁傳感器的磁感應(yīng)強(qiáng)度，如為微型裝置的磁體常數(shù)代表傳感器所 在磁場(chǎng)方向，^表示第1個(gè)磁傳感器的位置，Ri表示第1個(gè)磁傳感器到微型裝置內(nèi)磁體的距 離；
[0070] 誤差目標(biāo)函數(shù)為Ξ個(gè)誤差函數(shù)分量石.、、石> 及玄Z之和，分別為：
[0071]

[0074]其中，Bix、Biy和Biz為第1個(gè)磁傳感器所在位置磁場(chǎng)的立個(gè)正交磁感應(yīng)強(qiáng)度分量測(cè)量 值，a、b、c為微型裝置的空間坐標(biāo)參數(shù);m、n、p為微型裝置的方向參數(shù);xi、yi、z功氣個(gè)磁傳感器 的空間坐標(biāo)分量;N表示磁傳感器陣列中的磁傳感器數(shù)量
[00巧]步驟S3中的非線性優(yōu)化算法采用Levenberg-Marquar化算法。
[0076] 步驟S4具體如下：
[0077] S4.1通過(guò)標(biāo)定，建立紅外標(biāo)志球組合的參考坐標(biāo)系與傳感器坐標(biāo)系一致;計(jì)算運(yùn) 動(dòng)體體表的兩個(gè)參考目標(biāo)的空間坐標(biāo)和方向參數(shù)。獲得新的參考坐標(biāo)系，并選擇新參考坐 標(biāo)系的Ξ個(gè)參考坐標(biāo)軸為：
[0081]其中，ex'為參考坐標(biāo)系下X'軸單位向量;Hi= (mi, ηι,ρι)為第一個(gè)參考目標(biāo)在傳感 器坐標(biāo)系下的方向向量;mx'，ηχ'，ρχ'為參考坐標(biāo)系下X'軸單位向量在傳感器坐標(biāo)系下的分 量;ez'為參考坐標(biāo)系下Ζ'軸單位向量;mz'，ηζ'，ρζ'為參考坐標(biāo)系下Ζ'軸單位向量在傳感器 坐標(biāo)系下的分量;出=(m2，Π2，Ρ2)為第二個(gè)參考目標(biāo)在傳感器坐標(biāo)系下的方向向量;ex為傳 感器坐標(biāo)系下X軸單位向量;eY為傳感器坐標(biāo)系下Y軸單位向量;ez為傳感器坐標(biāo)系下Z軸單 位向量;6Y'為參考坐標(biāo)系下Y'軸單位向量;??γ'，ηγ'，ργ'為參考坐標(biāo)系下Y'軸單位向量在傳 感器坐標(biāo)系下的分量；
[0082] S4.2建立傳感器坐標(biāo)系與參考坐標(biāo)系的轉(zhuǎn)換矩陣R
[0083] S4.3對(duì)微型裝置進(jìn)行坐標(biāo)轉(zhuǎn)換：
[0084] P'=rHP-AP)
[0085] 其中，P'是坐標(biāo)轉(zhuǎn)換后體內(nèi)被測(cè)目標(biāo)的空間坐標(biāo)，R-1是R的逆矩陣，P是傳感器坐標(biāo) 系下體內(nèi)被測(cè)目標(biāo)的空間坐標(biāo)，A P是傳感器坐標(biāo)系原點(diǎn)指向參考坐標(biāo)系原點(diǎn)的向量。參考 坐標(biāo)系原點(diǎn)可選第一個(gè)參考目標(biāo)中屯、位置。
[0086] 作為改進(jìn)，所述微型裝置有多個(gè)，其中相對(duì)運(yùn)動(dòng)體運(yùn)動(dòng)的微型裝置設(shè)為被測(cè)目標(biāo)， 相對(duì)運(yùn)動(dòng)體靜止的微型裝置可設(shè)為輔助參考目標(biāo)(也可取代紅外參考目標(biāo)）；所述數(shù)據(jù)處理 與顯示中屯、的多目標(biāo)定位計(jì)算單元計(jì)算被測(cè)目標(biāo)相對(duì)輔助參考目標(biāo)的運(yùn)動(dòng)軌跡，對(duì)被測(cè)目 標(biāo)進(jìn)行相對(duì)定位，修正被測(cè)目標(biāo)步驟S3獲得的定位數(shù)據(jù)；W獲得的被測(cè)目標(biāo)的修正定位數(shù) 據(jù)為基礎(chǔ)進(jìn)行步驟S4的定位數(shù)據(jù)校正，從而減小運(yùn)動(dòng)體干擾對(duì)定位的影響。
[0087] 在被測(cè)目標(biāo)的定位數(shù)據(jù)修正后，還包括:分析各微型裝置定位數(shù)據(jù)的合理性，剔除 誤差大的不合理數(shù)據(jù)，然后用綜合優(yōu)化擬合算法對(duì)各微型裝置定位數(shù)據(jù)進(jìn)行處理，獲取各 微型裝置的定位定向結(jié)果;將定位定向結(jié)果作為下一次計(jì)算的歷史數(shù)據(jù)，依次循環(huán)，實(shí)現(xiàn)實(shí) 時(shí)跟蹤。能夠通過(guò)輔助參考目標(biāo)的移動(dòng)確定被測(cè)目標(biāo)的非正常定位信息，在消除被測(cè)目標(biāo) 的非正常定位信息后，通過(guò)綜合優(yōu)化擬合算法能夠使被測(cè)目標(biāo)微型裝置的定位數(shù)據(jù)更為準(zhǔn) 確。
[0088] 在本實(shí)施例中，微型裝置采用永磁體作為磁信號(hào)源，永磁體為圓柱體或圓環(huán);所述 磁傳感器為單軸或雙軸或Ξ軸的磁場(chǎng)傳感器。
【主權(quán)項(xiàng)】
1. 一種用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁信號(hào)的微型裝置的系統(tǒng)，包括相對(duì)固定在運(yùn)動(dòng)體上 的穿戴物，所述穿戴物上設(shè)有用于感應(yīng)運(yùn)動(dòng)體體內(nèi)被測(cè)目標(biāo)微型裝置發(fā)出的磁信號(hào)的磁傳 感器陣列； 數(shù)據(jù)采集儀器，用于采集和預(yù)處理磁傳感器陣列發(fā)送的信息； 和與數(shù)據(jù)采集儀器連接的數(shù)據(jù)處理與顯示中心，根據(jù)數(shù)據(jù)采集儀器傳送的信息計(jì)算微 型裝置在不同時(shí)間的定位數(shù)據(jù)，然后根據(jù)定位數(shù)據(jù)建立并顯示微型裝置的運(yùn)動(dòng)軌跡，所述 定位數(shù)據(jù)包括空間坐標(biāo)和方向參數(shù)； 其特征在于:還包括安裝在運(yùn)動(dòng)體體表的至少兩個(gè)具有定位標(biāo)志的參考目標(biāo)，用于校 正微型裝置定位數(shù)據(jù)由于運(yùn)動(dòng)體移動(dòng)帶來(lái)的誤差； 紅外雙目或多目立體相機(jī)，用于拍攝參考目標(biāo)的空間坐標(biāo)和方向參數(shù)； 所述立體相機(jī)與數(shù)據(jù)處理與顯示中心相連接； 所述數(shù)據(jù)處理與顯示中心根據(jù)立體相機(jī)拍攝獲得的參考目標(biāo)的空間坐標(biāo)和方向參數(shù) 對(duì)計(jì)算獲得的微型裝置的定位數(shù)據(jù)進(jìn)行校正。2. 根據(jù)權(quán)利要求1所述的用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁信號(hào)的微型裝置的系統(tǒng)，其特征 在于:所述參考目標(biāo)的定位標(biāo)志為4個(gè)具有特定空間位置關(guān)系并形成方向指向的紅外標(biāo)志 球。3. 根據(jù)權(quán)利要求1所述的用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁信號(hào)的微型裝置的系統(tǒng)，其特征 在于:所述微型裝置有多個(gè)，其中相對(duì)運(yùn)動(dòng)體運(yùn)動(dòng)的微型裝置為被測(cè)目標(biāo)，相對(duì)運(yùn)動(dòng)體靜止 的微型裝置為輔助參考目標(biāo);所述數(shù)據(jù)處理與顯示中心包括立體視覺(jué)的多目標(biāo)定位計(jì)算單 元;所述多目標(biāo)定位計(jì)算單元根據(jù)被測(cè)目標(biāo)相對(duì)輔助參考目標(biāo)的運(yùn)動(dòng)軌跡，對(duì)被測(cè)目標(biāo)進(jìn) 行相對(duì)定位，獲得被測(cè)目標(biāo)的校正定位數(shù)據(jù)。4. 一種用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁信號(hào)的微型裝置的方法，包括以下具體步驟： S1以磁傳感器陣列某一點(diǎn)為標(biāo)準(zhǔn)構(gòu)建傳感器坐標(biāo)系，獲取磁傳感器陣列上各磁傳感器 所處的空間位置，并采集被測(cè)目標(biāo)微型裝置作用在所述各磁傳感器處的磁感應(yīng)強(qiáng)度數(shù)據(jù)； S2利用步驟S1采集到的各磁傳感器處的磁感應(yīng)強(qiáng)度數(shù)據(jù)，以及描述磁信號(hào)微型裝置的 磁偶極子模型，定義傳感器坐標(biāo)系下微型裝置形成的磁場(chǎng)與模型的誤差目標(biāo)函數(shù)； S3利用非線性優(yōu)化算法尋找所述微型裝置的空間坐標(biāo)，使所述誤差目標(biāo)函數(shù)為最小， 此時(shí)所述微型裝置的空間坐標(biāo)和方向參數(shù)即為微型裝置的定位數(shù)據(jù)； 其特征在于，還包括以下步驟： S4基于紅外立體相機(jī)拍攝固定在運(yùn)動(dòng)體體表的參考目標(biāo)的紅外標(biāo)志球，獲得的參考目 標(biāo)的空間坐標(biāo)和方向參數(shù)，然后根據(jù)參考目標(biāo)的空間坐標(biāo)和方向參數(shù)校正微型裝置的定位 數(shù)據(jù)由于運(yùn)動(dòng)體移動(dòng)帶來(lái)的誤差，獲得校正后的微型裝置的定位數(shù)據(jù)； S5基于校正后的微型裝置的定位數(shù)據(jù)，建立和顯示微型裝置的運(yùn)動(dòng)軌跡。5. 根據(jù)權(quán)利要求4所述的用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁信號(hào)的微型裝置的方法，其特征 在于：步驟S2所述的誤差目標(biāo)函數(shù)為三個(gè)誤差函數(shù)分量之和，三個(gè)誤差函數(shù)分量分別為 和瓦其中，Bix、Biy和Biz為第1個(gè)磁傳感器所在位置磁場(chǎng)的三個(gè)正交磁感應(yīng)強(qiáng)度分量測(cè)量值;a、b、c 為微型裝置的空間坐標(biāo)參數(shù);m、n、p為微型裝置的方向參數(shù);xi、yi、zi為第1個(gè)磁傳感器的空 間坐標(biāo)分量;N表不磁傳感器陣列中的磁傳感器數(shù)量;Βτ為微型裝置的磁體常數(shù);Ri為第1個(gè) 磁傳感器到微型裝置內(nèi)磁體的距離6. 根據(jù)權(quán)利要求5所述的用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁信號(hào)的微型裝置的方法，其特征 在于:步驟S4具體如下： S4.1通過(guò)標(biāo)定，建立紅外標(biāo)志球組合的參考坐標(biāo)系與傳感器坐標(biāo)系一致;計(jì)算運(yùn)動(dòng)體 體表的兩個(gè)參考目標(biāo)的空間坐標(biāo)和方向參數(shù);獲得參考坐標(biāo)系，并選擇新的參考坐標(biāo)軸為： ex，= Hi = (mx '，nx '，px '）= (mi，m，pi);θυ，= (ι?υ '，ηγ '，ργ '）= ex，X ez，；其中，ex'為參考坐標(biāo)系下X'軸單位向量;Hi= (mi，m,pi)為第一個(gè)參考目標(biāo)在傳感器坐 標(biāo)系下的方向向量;mx'，ηχ'，ρχ'為參考坐標(biāo)系下X'軸單位向量在傳感器坐標(biāo)系下的分量； ez為參考坐標(biāo)系下Ζ'軸單位向量;mz'，ηζ'，ρζ'為參考坐標(biāo)系下Ζ'軸單位向量在傳感器坐標(biāo) 系下的分量;Η2 = (m2 ,Π 2 ,Ρ2)為第二個(gè)參考目標(biāo)在傳感器坐標(biāo)系下的方向向量;ex為傳感器 坐標(biāo)系下X軸單位向量;eY為傳感器坐標(biāo)系下Y軸單位向量;ez為傳感器坐標(biāo)系下Z軸單位向 量;er為參考坐標(biāo)系下Y'軸單位向量;Π 1Υ'，ηγ'，ργ'為參考坐標(biāo)系下Y'軸單位向量在傳感器 坐標(biāo)系下的分量； S4.2建立傳感器坐標(biāo)系與參考坐標(biāo)系的轉(zhuǎn)換矩陣R S4.3對(duì)微型裝置進(jìn)行坐標(biāo)轉(zhuǎn)換： P，=R-Ηρ-ΔΡ) 其中，Ρ'是坐標(biāo)轉(zhuǎn)換后體內(nèi)被測(cè)目標(biāo)的空間坐標(biāo)，IT1是R的逆矩陣，Ρ是傳感器坐標(biāo)系下 體內(nèi)被測(cè)目標(biāo)的空間坐標(biāo)，△ Ρ是傳感器坐標(biāo)系原點(diǎn)指向參考坐標(biāo)系原點(diǎn)的向量。7. 根據(jù)權(quán)利要求5所述的用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁信號(hào)的微型裝置的方法，其特征 在于:所述微型裝置有多個(gè)，其中相對(duì)運(yùn)動(dòng)體運(yùn)動(dòng)的微型裝置為被測(cè)目標(biāo)，相對(duì)運(yùn)動(dòng)體靜止 的微型裝置為輔助參考目標(biāo);所述數(shù)據(jù)處理與顯示中心的多目標(biāo)定位計(jì)算單元計(jì)算被測(cè)目 標(biāo)相對(duì)輔助參考目標(biāo)的運(yùn)動(dòng)軌跡，對(duì)被測(cè)目標(biāo)進(jìn)行相對(duì)定位，修正被測(cè)目標(biāo)步驟S3獲得的 定位數(shù)據(jù)；以獲得的被測(cè)目標(biāo)的修正定位數(shù)據(jù)為基礎(chǔ)進(jìn)行步驟S4的定位數(shù)據(jù)校正。8.根據(jù)權(quán)利要求7所述的用于跟蹤運(yùn)動(dòng)體體內(nèi)具有磁信號(hào)的微型裝置的方法，其特征 在于:在被測(cè)目標(biāo)的定位數(shù)據(jù)修正后，還包括:分析各微型裝置定位數(shù)據(jù)的合理性，剔除誤 差大的不合理數(shù)據(jù)，然后用綜合優(yōu)化擬合算法對(duì)各微型裝置定位數(shù)據(jù)進(jìn)行處理，獲取各微 型裝置的定位定向結(jié)果。
【文檔編號(hào)】A61B5/06GK105919595SQ201610326767
【公開(kāi)日】2016年9月7日
【申請(qǐng)日】2016年5月17日
【發(fā)明人】胡超, 尤曉赫, 任宇鵬
【申請(qǐng)人】浙江大學(xué)寧波理工學(xué)院