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

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

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

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