專利名稱：一種基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤方法及系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明屬于電磁跟蹤技術(shù)領(lǐng)域，具體涉及用于電磁跟蹤的方法和系統(tǒng)。
背景技術(shù)：
電磁跟蹤(Electromagnetic Tracking)，或稱電磁場定位，是一種利用磁場或電 磁場對物體的位置和方向進行檢測和實時跟蹤的方法。該方法可應(yīng)用于微創(chuàng)手術(shù)的導(dǎo)航， 亦可運用于虛擬現(xiàn)實、三維超聲成像等領(lǐng)域。典型的電磁跟蹤系統(tǒng)一般由磁場源(如永磁 鐵、電磁鐵線圈)、磁場傳感器、控制處理單元三部分組成。傳感器通常附著待定位的物體 上，磁場源位于固定參考系的某幾個位置。假定磁場符合某種分布模型(如無限遠偶極子 模型)，利用多個或多軸的磁場源和傳感器的磁場耦合，根據(jù)模型估算的磁場和實測磁場的 關(guān)系列方程組，采用某種最優(yōu)化算法迭代地解出待測物體的空間位置和方向?，F(xiàn)有電磁跟蹤方法的缺點之一是大都依據(jù)某種磁場分布模型。目前采用最廣泛 的是無限遠偶極子模型，該模型僅在磁棒直徑遠小于磁棒與傳感器距離時有效。為了解決 這一問題，研究人員提出了一些改進的磁場模型，但是，磁場模型與真實的磁場分布仍然不 可避免地存在區(qū)別。由于磁場源體積做不到無窮小以及磁場源線圈繞制或形狀等因素不理 想，磁場源產(chǎn)生的磁場分布無法嚴(yán)格符合設(shè)定的磁場模型。這一差異性嚴(yán)重地制約了電磁 跟蹤的性能和精度?，F(xiàn)有的方法通常采用后期校準(zhǔn)來修正這一問題，校準(zhǔn)采用三維空間的 插值實現(xiàn)，測量一部分真實位置，再尋找測量位置和真實位置的映射關(guān)系。校準(zhǔn)能夠在一定 程度上減小模型與真實磁場差異造成的誤差，但校準(zhǔn)過程費時費力，往往在設(shè)備重新布置 時需都要要進行，更關(guān)鍵的是無法從根本上解決問題?，F(xiàn)有電磁跟蹤方法的另一個缺點是一般采用迭代的算法。迭代算法對磁場源和傳 感器之間的耦合關(guān)系要求比較簡單。但是迭代的算法具有計算復(fù)雜度高、易發(fā)散、存在局部 極值點等問題。這些問題極大地提高了電磁跟蹤算法的復(fù)雜性，而降低了其可靠性。采用 非迭代的算法，能夠較大地提高電磁跟蹤系統(tǒng)的性能。

發(fā)明內(nèi)容
本發(fā)明的目的在于提出一種可靠性好、計算復(fù)雜度低的電磁跟蹤方法和系統(tǒng)。本發(fā)明提出的電磁跟蹤方法，是一種新穎的基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤方 法。本方法根據(jù)磁棒指向傳感器時傳感器測得的磁場強度最大的特性，采用雙磁棒跟蹤傳 感器進行搜索定位，應(yīng)用幾何算法計算得到傳感器的空間位置；并進一步依據(jù)磁棒指向傳 感器時傳感器測出的磁場方向沿著磁棒軸線方向的原理，應(yīng)用幾何算法計算得到傳感器的 空間姿態(tài)。本方法的特點之一在于利用磁棒旋轉(zhuǎn)搜索傳感器，采用幾何算法確定傳感器的空 間位置和空間姿態(tài)，而不依賴于某種假定的磁場模型。本方法的特點之二在于采用非迭代 的算法，可以快速有效地實現(xiàn)對傳感器位置和方向的跟蹤。本方法的特點還包括采用脈沖 直流磁場，可實現(xiàn)六自由度的電磁跟蹤等。
本發(fā)明提出用于雙磁棒搜索電磁跟蹤的搜索策略。該搜索策略具有自適應(yīng)調(diào)整步 長的特點，能夠保證磁棒迅速有效地搜索到傳感器，從而實現(xiàn)優(yōu)化的跟蹤速度和跟蹤范圍。本發(fā)明提出用于雙磁棒搜索電磁跟蹤的待測物體位置和方向算法。該方法利用兩 磁棒指向傳感器時三者的幾何關(guān)系確定傳感器的位置，利用指向時傳感器位置的磁場方向 沿磁棒軸線方向確定傳感器的方向。方法具有純幾何和非迭代的特點。當(dāng)磁棒以固定中心在空間旋轉(zhuǎn)時，空間某處的傳感器檢測到的磁場強度在磁棒指 向傳感器時達到最大。本發(fā)明利用這一原理，采用兩個磁棒在空間旋轉(zhuǎn)搜索傳感器，以計算 傳感器的空間位置。進一步，當(dāng)磁棒指向傳感器時，該處磁場的空間指向是沿著磁棒軸線 的，本發(fā)明據(jù)此計算傳感器的空間姿態(tài)。這樣，可以通過兩根磁棒旋轉(zhuǎn)搜索實現(xiàn)對傳感器的 六自由度電磁跟蹤。本發(fā)明提出的電磁跟蹤系統(tǒng)，由傳感器、磁場源、旋轉(zhuǎn)控制裝置和控制處理顯示裝 置組成。傳感器附著于待跟蹤物體。旋轉(zhuǎn)控制裝置控制磁棒旋轉(zhuǎn)指向空間或半空間任意方 向。磁棒旋轉(zhuǎn)依據(jù)下文所述的旋轉(zhuǎn)策略，旋轉(zhuǎn)搜索傳感器，當(dāng)磁棒鎖定傳感器時，依據(jù)下文 所述的位置和方向算法，計算傳感器六自由度的位置和方向?？刂铺幚盹@示裝置一般采用 一臺PC機或服務(wù)器，對其他組件(如傳感器、磁場源、旋轉(zhuǎn)控制裝置等)進行控制，并進行 數(shù)據(jù)的采集和處理、跟蹤結(jié)果的顯示輸出等。磁棒采用脈沖直流的激勵方式，這有利于消除環(huán)境金屬物質(zhì)造成的渦流干擾，并 抵消地磁場和環(huán)境鐵磁性物質(zhì)產(chǎn)生的背景磁場干擾。對于本發(fā)明而言，其意義更在于可 以保證每個時刻只有一根磁棒被激勵，從而能夠順利地實現(xiàn)場強的峰值搜索并確定磁場方 向。傳感器可以采用磁阻傳感器、霍爾效應(yīng)傳感器或磁通門傳感器等。傳感器的選擇依據(jù) 測量范圍和精度的要求進行。系統(tǒng)的搜索過程分為兩個階段初始搜索階段和實時跟蹤階段。系統(tǒng)開始工作時 進行初始搜索，從旋轉(zhuǎn)的初始指向出發(fā)執(zhí)行搜索。初始搜索時，由于初始指向離傳感器實際 指向距離不確定，故搜索可能耗時較長。當(dāng)完成初始搜索后，由于短時間內(nèi)傳感器運動距離 有限，故磁棒當(dāng)前指向距離傳感器實際指向較近，可以在有限步數(shù)內(nèi)完成跟蹤，保證跟蹤的 實時性，故這一階段稱為實時跟蹤階段。由于搜索過程是三維空間中的搜索，而磁棒旋轉(zhuǎn)只 能在一個平面內(nèi)進行，故搜索過程分兩步進行，先在一個正交平面進行，即一個電機旋轉(zhuǎn)搜 索，尋找傳感器位置磁場強度的磁棒指向，當(dāng)搜索完成時，再在另一個正交平面進行，例如， 先垂直搜索，再水平搜索，這樣經(jīng)過兩次搜索，即可實現(xiàn)對傳感器位置的鎖定。在每個平面 上開始搜索時，先向一個方向試探，若場強增大，則表示試探方向正確，否則反向搜索。找到 正確方向后，沿該方向一步步搜索直至場強減小，則前一個搜索位置即為當(dāng)前搜索平面里 的最大場強方向。搜索步長是搜索時電機每次旋轉(zhuǎn)的角度，步長決定了磁棒指向的角度分辨率，進 而影響了系統(tǒng)誤差。初始搜索由于對實時性要求較低，可采用固定步長的方法，一般設(shè)定為 最短的步長。實時跟蹤時，由于隨著傳感器離磁棒距離的增加，相同角度差對應(yīng)的實際距離 相應(yīng)增加，若在整個區(qū)域采用一致的步長，會導(dǎo)致在較近處步數(shù)增多，跟蹤速度減慢，在較 遠處誤差增大。為了解決這個問題，可以根據(jù)傳感器當(dāng)前位置設(shè)定步長，步長大小隨傳感器 距離每個電機的距離的增大而減小。由于傳感器短時間內(nèi)運動距離有限，所以當(dāng)前位置可 以作為下一時刻位置的粗略估計，用來設(shè)定搜索步長。這種自適應(yīng)步長的跟蹤方法能夠有效的權(quán)衡系統(tǒng)的跟蹤速度和精度。傳感器的位置采用兩個磁棒中心和傳感器位置的幾何關(guān)系求解。已知兩磁棒指向 傳感器時的方向角以及兩磁棒間距離，由立體幾何方法可以解出傳感器位置坐標(biāo)。當(dāng)磁棒指向傳感器時，傳感器位置的磁感強度矢量是沿著磁棒軸線方向，因此，若 將傳感器測得的磁感強度矢量旋轉(zhuǎn)至兩磁棒軸線方向為z軸的坐標(biāo)系，則該矢量在相應(yīng)的 X軸和y軸方向的分量趨近于零。換言之，磁棒激勵時測得的場強，當(dāng)測得值在X、y方向的 分量為零時，傳感器坐標(biāo)z軸的指向即為磁棒軸線方向。將傳感器對磁棒1和磁棒2分別 做這樣的處理后，即可求得傳感器方向和磁棒當(dāng)前指向的關(guān)系。進而，由于兩磁棒當(dāng)前指向 已知，即可確定傳感器指向與參考坐標(biāo)系的關(guān)系，亦即求得傳感器的空間姿態(tài)。


圖1為按照本發(fā)明的實施例的裝置部分的視圖。圖2為圖1中的裝置的細(xì)節(jié)框圖。圖3為本發(fā)明的實施例的系統(tǒng)工作流程框圖。圖4為本發(fā)明的實施例的系統(tǒng)搜索策略框圖。圖5為本發(fā)明的實施例的待測物體位置算法示意圖。圖6為本發(fā)明的實施例的待測物體方向算法牽涉到的坐標(biāo)系的示意圖。圖7為本發(fā)明的實施例的待測物體方向算法的流程圖。圖中標(biāo)號1為電磁跟蹤系統(tǒng)，2為傳感器裝置，3為磁場源裝置，4為旋轉(zhuǎn)控制裝 置，5為控制處理顯示裝置。
具體實施例方式圖1所示為根據(jù)本發(fā)明設(shè)計的電磁跟蹤系統(tǒng)1。本電磁跟蹤系統(tǒng)1包括四個部分 傳感器裝置2、磁場源裝置3、旋轉(zhuǎn)控制裝置4、控制處理顯示裝置5。參考坐標(biāo)系以X、Y和 Z表示。傳感器裝置2附著于待跟蹤物體。旋轉(zhuǎn)控制裝置4固定于參考系，磁場源裝置3附 著于旋轉(zhuǎn)控制裝置4上，可由其控制在旋轉(zhuǎn)指向空間或半空間任意方向。各部分的分解框圖如圖2所示。傳感器裝置2包括三軸分量傳感器6、7和8，分別 用來檢測三個正交的局部方向X’、Y’和Z’的磁場。傳感器選用三軸磁阻傳感器。傳感器 輸出經(jīng)后續(xù)的信號調(diào)理和模擬-數(shù)字(AD)轉(zhuǎn)換模塊9送入控制處理顯示裝置5。磁場源裝置3由兩組電磁鐵線圈10和11組成，線圈分別由電路12和13控制驅(qū) 動。電磁鐵線圈10需要保證在相同距離情況下軸線上磁場強度最大，且該處磁場方向沿著 該軸線，一般簡單的棒狀電磁體均可滿足此要求。另外，電磁鐵采用脈沖直流的激勵方式， 由控制處理顯示裝置5控制脈沖直流的激勵。這里采用長10cm，直徑lcm的電磁鐵，線圈匝 數(shù)100匝，電流1A。旋轉(zhuǎn)控制裝置4包含兩個電機組15和16，電機組由步進電機驅(qū)動器17驅(qū)動，驅(qū) 動器由控制處理顯示裝置5控制。每個電機組包括兩個步進電機，第一電機帶動第二步進 電機轉(zhuǎn)動，第二電機帶動磁棒轉(zhuǎn)動，控制一磁棒沿兩正交方向旋轉(zhuǎn)，從而實現(xiàn)空間或半空間 任意指向。推薦的旋轉(zhuǎn)方式是第一電機平行于X-Z平面旋轉(zhuǎn)，第二電機垂直于該平面旋轉(zhuǎn)。 采用步進電機的原因是可以隨時獲得當(dāng)前磁棒的指向，不存在累積誤差。兩個旋轉(zhuǎn)中心的
7距離對于系統(tǒng)的跟蹤范圍等性能有一定的影響，這里選取的距離是30cm?？刂铺幚盹@示裝置5由控制單元20、算法單元21、顯示輸出單元22組成?？刂?單元20包括三個部分。脈沖直流控制14提供對磁棒激勵的控制，磁棒激勵采用脈沖直流 方式，每周期三個時間段，前兩個時間段順序激勵兩根磁棒10和11，第三個時間段兩磁棒 均不激勵。將每個磁棒激勵時測得的磁場與第三時間段的磁場相減作為該磁棒在傳感器位 置產(chǎn)生的磁場強度。這種脈沖直流的激勵方式有利于消除環(huán)境金屬物質(zhì)造成的渦流干擾， 并抵消地磁場和環(huán)境鐵磁性物質(zhì)產(chǎn)生的背景磁場干擾。對于本發(fā)明而言，采用脈沖直流方 式意義更在于可以保證每個時刻只有一根磁棒被激勵，從而能夠順利地實現(xiàn)場強的峰值搜 索。電機控制部分控制兩個電機組的電機旋轉(zhuǎn)，保證兩磁棒能分別在兩個正交方向上旋轉(zhuǎn)， 從而實現(xiàn)空間或半空間任意位置的指向。電機控制部分18依據(jù)下文的旋轉(zhuǎn)搜索策略，根據(jù) 當(dāng)前時刻測得的磁場強度與之前時刻測得的磁場強度的關(guān)系，決定電機旋轉(zhuǎn)方向，根據(jù)磁 棒當(dāng)前位置決定電機旋轉(zhuǎn)步進，控制磁棒以最佳的方式搜索到傳感器。采樣處理部分19在 脈沖直流控制部分14的同步下，采樣每個時間段的磁場，并求差以獲得每個磁棒在當(dāng)前周 期內(nèi)的傳感器位置的磁場強度。搜索完成后，控制部分20獲得完成時的兩個磁棒各自激勵 時的場強數(shù)據(jù)23和磁棒指向數(shù)據(jù)24，算法單元利用這些數(shù)據(jù)，依據(jù)后文的算法計算出傳感 器的空間位置和空間姿態(tài)，送顯示輸出單元22顯示。圖3所示為系統(tǒng)的工作流程。開機后系統(tǒng)首先進行初始化。初始化過程25包括 旋轉(zhuǎn)裝置的復(fù)位、系統(tǒng)一些基本參數(shù)的設(shè)定等。整個系統(tǒng)工作過程可分為搜索過程26和計 算過程27。搜索過程包括初始搜索過程28和實時跟蹤過程29。系統(tǒng)開始工作時進行初始 搜索，即從旋轉(zhuǎn)的初始指向開始搜索。初始搜索時，由于初始指向離傳感器實際指向距離不 確定，故搜索可能耗時較長。當(dāng)完成初始搜索后，系統(tǒng)進入實時跟蹤過程29，由于短時間內(nèi) 傳感器運動距離有限，故磁棒當(dāng)前指向距離傳感器實際指向較近，可以在有限步數(shù)內(nèi)完成 跟蹤，保證跟蹤的實時性。無論在初始搜索過程還是實時跟蹤過程，當(dāng)兩磁棒均鎖定傳感器 時，系統(tǒng)即進入計算過程27。計算過程27依次位置計算過程30和方向計算過程31，二者 依次進行，分別計算傳感器三自由度的位置和三自由度的方向，其中方向計算算法是依賴 于位置計算結(jié)果的。每次計算完成時，執(zhí)行顯示輸出過程32，實時更新跟蹤結(jié)果。圖4所示是初始搜索過程的框圖。如前所述，搜索可分為初始搜索和實施跟蹤兩 種情況，二者在流程上的區(qū)別主要在步長確定方面。這里的步長指的是單位時間里電機旋 轉(zhuǎn)的角度，實際使用時即為一個采樣周期中磁棒旋轉(zhuǎn)的角度，步長決定了磁棒指向的角度 分辨率，進而影響了系統(tǒng)誤差。初始搜索由于對實時性要求較低，可采用固定步長的方法， 一般設(shè)定為最短的步長。實時跟蹤時，由于隨著傳感器離磁棒距離的增加，相同角度差對 應(yīng)的實際距離相應(yīng)增加，若在整個區(qū)域采用一致的步長，會導(dǎo)致在較近處步數(shù)增多，跟蹤速 度減慢，在較遠處誤差增大。因此，我們提出在實時跟蹤過程中根據(jù)傳感器當(dāng)前位置，設(shè)定 步長，步長大小隨傳感器距離每個電機的距離的增大而減小。由于傳感器短時間內(nèi)運動距 離有限，所以當(dāng)前位置可以作為下一時刻位置的粗略估計，用來設(shè)定搜索步長。這種自適 應(yīng)步長的跟蹤方法能夠有效的權(quán)衡系統(tǒng)的跟蹤速度和精度。這里，初始跟蹤采用的步長是 0.45° ；實時跟蹤時，當(dāng)距離小于10cm時，設(shè)定步長1.8°，在距離大于25cm時，設(shè)定步長 0.45°，二者之間則設(shè)定為0.9°。每次搜索啟動時，系統(tǒng)先判定當(dāng)前搜索過程的種類，若為初始搜索階段，則設(shè)定步長為最小值，否則計算當(dāng)前距離，并據(jù)此設(shè)定步長。首先第一電機沿正方向搜索，若場強增 大，則證明搜索方向正確，否則沿反方向搜索。搜索過程每次前進一步，比較當(dāng)前位置與前 一個位置測得場強的大小，若發(fā)現(xiàn)場強減小，則說明前一個位置為場強最大位置，電機轉(zhuǎn)回 到這一位置，完成第一磁棒搜索。類似的，第二磁棒也經(jīng)過這樣一個搜索過程，找到另一正 交方向上的極值點。經(jīng)過兩個電機的旋轉(zhuǎn)搜索，即可實現(xiàn)磁棒指向傳感器方向。
圖5所示為位置算法的示意圖。坐標(biāo)系原點位于磁棒1中心。已知兩磁棒指向傳 感器時的方向角(Ql，3》和(a2，日2)，兩磁棒間距離d，這里的d為30cm，待求的是傳感 器坐標(biāo)(x，y，z)。由立體幾何知識可以解出傳感器位置坐標(biāo) 其中，a，b分別為傳感器坐標(biāo)在x，y平面的投影點(x，y，0)到兩磁棒的距離。圖6所示為方向算法所涉及的坐標(biāo)系。坐標(biāo)系&為系統(tǒng)默認(rèn)坐標(biāo)系，原點位于磁 棒1中心，Xl軸沿兩磁棒中心連線方向，yi軸沿磁棒初始方向，Zl軸垂直于Xl_yi平面；坐 標(biāo)系F2與坐標(biāo)系&方向均相同，僅原點位于磁棒2中心。坐標(biāo)系F3和F4表示兩磁棒當(dāng)前 姿態(tài)，z3、z4軸分別沿兩磁棒當(dāng)前指向；坐標(biāo)系F5表示傳感器當(dāng)前姿態(tài)。坐標(biāo)系&和F2僅 原點位置不同，空間方向是相同的。待求的傳感器空間姿態(tài)可表示為從坐標(biāo)系&到&的旋 轉(zhuǎn)關(guān)系。在確定了傳感器的空間位置后，兩磁棒的指向是已知的，故坐標(biāo)系& (或F2)到& 和坐標(biāo)系& (或F2)到&的旋轉(zhuǎn)關(guān)系是已知的。為了進一步確定傳感器的空間姿態(tài)，只需 確定傳感器坐標(biāo)與兩磁棒指向的相對空間關(guān)系，即坐標(biāo)系F3到F5或F4到F5的旋轉(zhuǎn)關(guān)系即 可。由于磁棒1和磁棒2、坐標(biāo)系F3和F4是對等的，不失一般性，下文算法采取計算坐標(biāo)系 F3和F5之間的旋轉(zhuǎn)關(guān)系的方式。當(dāng)磁棒指向傳感器時，傳感器位置的磁感強度矢量是沿著磁棒軸線方向，即z3(或 Z4)方向的，因此，若將傳感器測得的磁感強度矢量旋轉(zhuǎn)至z3(或z4)方向，則該矢量在X3、 y3(或X4、y4)方向的分量趨近于零。換言之，旋轉(zhuǎn)傳感器坐標(biāo)系F5，當(dāng)測得的磁感強度矢量 在x、y方向的分量為零時，傳感器坐標(biāo)z軸的指向即為磁棒軸線方向(z3或z4)，亦即當(dāng)前 坐標(biāo)系已變換為磁棒坐標(biāo)系(F3或F4)。將傳感器對磁棒1和磁棒2分別做這樣的處理后， 即可求解坐標(biāo)系F5相對于F3或F4的相對空間關(guān)系，進而確定傳感器的空間姿態(tài)。圖7所示為方向算法的流程圖。首先須計算的是坐標(biāo)系F5到F3的旋轉(zhuǎn)關(guān)系，已知 的是坐標(biāo)系Fi到F3和坐標(biāo)系Fi到F4的旋轉(zhuǎn)關(guān)系。用^，e和屯表示坐標(biāo)系從F5旋轉(zhuǎn)到 F3的三個‘ZXZ’歐拉角，當(dāng)兩磁棒指向傳感器時，將傳感器磁場分別旋轉(zhuǎn)到當(dāng)前磁棒的坐標(biāo) 系坑和⑴，令其在x，y方向的場強值為零，共可獲得四個方程。這里，磁棒1場強的旋轉(zhuǎn) 可以直接由坐標(biāo)系F5旋轉(zhuǎn)到F3，對于磁棒2場強，為了不涉及坐標(biāo)系F5與F4之間的旋轉(zhuǎn)關(guān) 系，將其旋轉(zhuǎn)過程拆分為F5到F3的旋轉(zhuǎn)，F(xiàn)3到&的旋轉(zhuǎn)和&到F4的旋轉(zhuǎn)三個階段。于是
9所有方程僅有p e和▽三個未知數(shù)，選取其中任意三個即可求解。場強&是傳感器在磁棒1激勵是測得的場強，將其旋轉(zhuǎn)到坐標(biāo)系F3后得到場強矢 量B' i，其在x3、y3S向上的分量為零，即B' ！ = A^Bi =
T(4)其中A53表示從坐標(biāo)系F5到F3的旋轉(zhuǎn)矩陣由于W角是繞z3軸的轉(zhuǎn)動，所以，若磁棒1產(chǎn)生的場強矢量在x3、y3方向上為零， 則該旋轉(zhuǎn)角不會影響依據(jù)磁棒1場強列出的方程。因此，我們可以利用這兩個方程解出兩 個未知量p和e
^xs^i-cos^ + ^ijsin^-0(5)
B\y = Bu sin 0 + BXy cos ^ cos q)-Bucos6sin(p = 0(6)這里，參量場強矢量B'工的下標(biāo)x，y, z分別表示該參量在x，y, z方向上的分量， 下同。類似的，當(dāng)磁棒2激勵時，用B2表示坐標(biāo)系F5下的實測場強，B' 2表示旋轉(zhuǎn)到坐 標(biāo)系F3下的場強，A14表示從坐標(biāo)系Fi到F3的旋轉(zhuǎn)矩陣，A31表示從坐標(biāo)系F4到Fi的旋轉(zhuǎn) 矩陣B' 2 = A14A31A53B2 =
T(7)將已知的兩個未知量帶入磁棒2的方程中的任意一個，即可求解夾角叫。這里選 取x方向的方程，代入化簡得B' 2x = Acos^+Bsin^+C = 0(8)式中其中A、B、C均為常數(shù)。如上所述，由方程(5)、(6)、(8)可求解三個歐拉角。以上方程的求解只涉及三個 夾角的簡單三角函數(shù)，可以直接利用非迭代的方法計算，算法簡單可靠。進一步，由三個歐拉角表示的坐標(biāo)系F5到F3的旋轉(zhuǎn)關(guān)系，以及坐標(biāo)系&到F3的旋 轉(zhuǎn)關(guān)系，可以計算出坐標(biāo)系&到F5的旋轉(zhuǎn)關(guān)系，并將其表示成歐拉角或者其他期望的形式。A15 = (A53A31)(9)采用仿真方法對以上實施方式的系統(tǒng)進行了評估。仿真采用計算磁棒周圍的磁 場，分析靜態(tài)時的跟蹤誤差0和動態(tài)是的跟蹤速度。設(shè)定傳感器采樣率為900Hz。跟蹤范圍 為坐標(biāo)系？工下^ G [-15cm,45cm], G [5cm，20cm]、G [-30cm, 30cm]的空間范圍內(nèi)， 由于磁棒長度為10cm，故同時限定跟蹤位置距離兩磁棒中心至少6cm。跟蹤的位置誤差可 以通過估計位置和實際位置之間的歐氏距離AD評估，而方向誤差則采用螺旋角A小。AD = ^(x-x')2+(y-y')2+(z-z'f(27)A 小=cos-1[(An+A22+A33-l)/2](28)經(jīng)過5000組仿真，得到平均位置誤差為0. 1484cm，平均方向誤差為0. 5474°，最 大位置誤差0.7261cm，最大方向誤差3.8852°，若減小步長，還可進一步提高精度。跟蹤性 能方面，初始跟蹤平均步數(shù)230步；實時跟蹤時，在瞬時位移不超過2cm時最大跟蹤步數(shù)為 26步。相當(dāng)于，初始跟蹤平均0. 767s,實時跟蹤最大跟蹤時間0. 087s?？梢詫崿F(xiàn)每秒10次 以上的更新。若提高采樣率，還可進一步提高刷新速度。
權(quán)利要求
一種基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤系統(tǒng)，其特征在于由傳感器、磁場源、旋轉(zhuǎn)控制裝置和控制處理顯示裝置組成；所述傳感器附著于待跟蹤物體，旋轉(zhuǎn)控制裝置控制磁棒旋轉(zhuǎn)指向空間或半空間任意方向；磁棒旋轉(zhuǎn)依據(jù)旋轉(zhuǎn)策略，旋轉(zhuǎn)搜索傳感器，當(dāng)磁棒鎖定傳感器時，依據(jù)位置和方向算法，計算傳感器六自由度的位置和方向；控制處理顯示裝置采用一臺pc機或服務(wù)器，對其他組件進行控制，并進行數(shù)據(jù)的采集和處理、跟蹤結(jié)果的顯示輸出。
2.根據(jù)權(quán)利要求1所述的基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤系統(tǒng)，其特征在于所述的傳 感器為三軸分量傳感器，分別用來檢測三個正交的局部方向r、r和v的磁場；傳感器輸 出經(jīng)后續(xù)的信號調(diào)理和模擬-數(shù)字轉(zhuǎn)換模塊送入控制處理顯示裝置。
3.根據(jù)權(quán)利要求2所述的基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤系統(tǒng)，其特征在于所述的磁 場源由兩組電磁鐵線圈組成，兩組電磁鐵線圈組分別由兩個驅(qū)動電路控制驅(qū)動；其中一組 電磁鐵線圈保證在相同距離情況下軸線上磁場強度最大，且該處磁場方向沿著該軸線；另 外，電磁鐵采用脈沖直流的激勵方式，由控制處理顯示裝置控制脈沖直流的激勵。
4.根據(jù)權(quán)利要求3所述的基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤系統(tǒng)，其特征在于所述的旋 轉(zhuǎn)控制裝置包含兩個電機組，兩個電機組由步進電機驅(qū)動器驅(qū)動，驅(qū)動器由控制處理顯示 裝置控制；每個電機組包括兩個步進電機，第一步進電機帶動第二步進電機轉(zhuǎn)動，第二步進 電機帶動磁棒轉(zhuǎn)動，控制一磁棒沿兩正交方向旋轉(zhuǎn)，實現(xiàn)空間或半空間任意指向。
5.根據(jù)權(quán)利要求4所述的基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤系統(tǒng)，其特征在于所述的控 制處理顯示裝置由控制單元、算法單元、顯示輸出單元組成；其中，控制單元包括三個部分 (1)脈沖直流控制部分，提供對磁棒激勵的控制，磁棒激勵采用脈沖直流方式，每周期三個 時間段，前兩個時間段順序激勵兩根磁棒，第三個時間段兩磁棒均不激勵；將每個磁棒激勵 時測得的磁場與第三時間段的磁場相減作為該磁棒在傳感器位置產(chǎn)生的磁場強度；(2)電 機控制部分，控制兩個電機組的電機旋轉(zhuǎn)，保證兩磁棒能分別在兩個正交方向上旋轉(zhuǎn)，實現(xiàn) 空間或半空間任意位置的指向；電機控制部分依據(jù)旋轉(zhuǎn)搜索策略，根據(jù)當(dāng)前時刻測得的磁 場強度與之前時刻測得的磁場強度的關(guān)系，決定電機旋轉(zhuǎn)方向，根據(jù)磁棒當(dāng)前位置決定電 機旋轉(zhuǎn)步進，控制磁棒以最佳的方式搜索到傳感器；(3)采樣處理部分，在脈沖直流控制部 分的同步下，采樣每個時間段的磁場，并求差以獲得每個磁棒在當(dāng)前周期內(nèi)的傳感器位置 的磁場強度；搜索完成后，控制單元獲得完成時的兩個磁棒各自激勵時的場強數(shù)據(jù)和磁棒 指向數(shù)據(jù)，算法單元根據(jù)這些數(shù)據(jù)，依據(jù)算法計算出傳感器的空間位置和空間姿態(tài)，送顯示 輸出單元顯示。
6.一種利用如權(quán)利要求5所述的基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤系統(tǒng)進行電磁跟蹤 的方法，其特征在于步驟如下根據(jù)磁棒指向傳感器時傳感器測得的磁場強度最大的特性， 采用雙磁棒跟蹤傳感器進行搜索定位，應(yīng)用幾何算法計算得到傳感器的空間位置；并進一 步依據(jù)磁棒指向傳感器時傳感器測出的磁場方向沿著磁棒軸線方向的原理，應(yīng)用幾何算法 計算得到傳感器的空間姿態(tài)。
7.根據(jù)權(quán)利要求6所述的基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤方法，其特征在于所述的搜 索分為兩個階段初始搜索階段和實時跟蹤階段；系統(tǒng)開始工作時進行初始搜索，從旋轉(zhuǎn) 的初始指向出發(fā)執(zhí)行搜索；當(dāng)完成初始搜索后，由于短時間內(nèi)傳感器運動距離有限，故磁棒 當(dāng)前指向距離傳感器實際指向較近，即在有限步數(shù)內(nèi)完成跟蹤，保證跟蹤的實時性，故這一 階段稱為實時跟蹤階段；搜索過程分兩步進行，先在一個正交平面進行，即一個電機旋轉(zhuǎn)搜索，尋找傳感器位置磁場強度的磁棒指向，當(dāng)搜索完成時，再在另一個正交平面進行，在每 個平面上開始搜索時，先向一個方向試探，若場強增大，則表示試探方向正確，否則反向搜 索；找到正確方向后，沿該方向一步步搜索直至場強減小，則前一個搜索位置即為當(dāng)前搜索 平面里的最大場強方向。
8.根據(jù)權(quán)利要求7所述的基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤方法，其特征在于在所述的 初始搜索階段采用固定步長的方法，；實時跟蹤階段時，根據(jù)傳感器當(dāng)前位置設(shè)定步長，步 長大小隨傳感器距離每個電機的距離的增大而減小。
9.根據(jù)權(quán)利要求7所述的基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤方法，其特征在于所述應(yīng)用 幾何算法計算得到傳感器的空間位置，具體步驟如下已知兩磁棒指向傳感器時的方向角(h，和(a2，3 2)，兩磁棒間距離d，待求的傳 感器坐標(biāo)為(X，1，z)，傳感器位置坐標(biāo)的算式為 其中，a，b分別為傳感器坐標(biāo)在x，y平面的投影點(x，y,0)到兩磁棒的距離。
10.根據(jù)權(quán)利要求7所述的基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤方法，其特征在于所述應(yīng) 用幾何算法計算得到傳感器的空間姿態(tài)，具體步驟如下設(shè)坐標(biāo)系Fi為系統(tǒng)默認(rèn)坐標(biāo)系，原點位于磁棒1中心，Xl軸沿兩磁棒中心連線方向，yi 軸沿磁棒初始方向，Zl軸垂直于Xl-yi平面；坐標(biāo)系F2與坐標(biāo)系&方向均相同，僅原點位于 磁棒2中心；坐標(biāo)系F3和F4表示兩磁棒當(dāng)前姿態(tài)，z3、z4軸分別沿兩磁棒當(dāng)前指向；坐標(biāo)系 F5表示傳感器當(dāng)前姿態(tài)；坐標(biāo)系&和F2僅原點位置不同，空間方向相同；待求的傳感器空 間姿態(tài)表示為從坐標(biāo)系&到F5的旋轉(zhuǎn)關(guān)系；在確定了傳感器的空間位置后，兩磁棒的指向 是已知的，故坐標(biāo)系&或&到F3，和坐標(biāo)系&或&到F4的旋轉(zhuǎn)關(guān)系是已知的；為了進一步 確定傳感器的空間姿態(tài)，下面計算坐標(biāo)系F3和F5之間的旋轉(zhuǎn)關(guān)系首先須計算坐標(biāo)系F5到F3的旋轉(zhuǎn)關(guān)系，已知坐標(biāo)系&到F3和坐標(biāo)系&到F4的旋轉(zhuǎn) 關(guān)系，用< ，e和v分別表示坐標(biāo)系從F5旋轉(zhuǎn)到F3的三個‘ZXZ’歐拉角，當(dāng)兩磁棒指向傳 感器時，將傳感器磁場分別旋轉(zhuǎn)到當(dāng)前磁棒的坐標(biāo)系F3和F4，令其在X，y方向的場強值為 零，共獲得四個方程；這里，磁棒1場強的旋轉(zhuǎn)可以直接由坐標(biāo)系F5旋轉(zhuǎn)到F3，對于磁棒2 場強，為了不涉及坐標(biāo)系F5與F4之間的旋轉(zhuǎn)關(guān)系，將其旋轉(zhuǎn)過程拆分為F5到F3的旋轉(zhuǎn)，F(xiàn)3 到&的旋轉(zhuǎn)和&到F4的旋轉(zhuǎn)三個階段；于是所有方程僅有P 9和V三個未知數(shù)，選取其 中任意三個即可求解；設(shè)場強Bi是傳感器在磁棒1激勵是測得的場強，將其旋轉(zhuǎn)到坐標(biāo)系F3后得到場強矢量 B'工，其在x3、y3S向上的分量為零，即B' ！ = A53B, =
T(4)其中A53表示從坐標(biāo)系f5到F3的旋轉(zhuǎn)矩陣；由于v角是繞Z3軸的轉(zhuǎn)動，所以，若磁棒1產(chǎn)生的場強矢量在X3、y3方向上為零，則該 旋轉(zhuǎn)角不會影響依據(jù)磁棒1場強列出的方程；因此，可以利用這兩個方程解出兩個未知量^ 和e B[x = Blx costp + BXy sin^ = 0(5)BXy = 5lz sin ^ + BXy cos 0cos q> - Blx cos ^ sin ^ = 0(6)類似的，當(dāng)磁棒2激勵時，用B2表示坐標(biāo)系F5下的實測場強，B' 2表示旋轉(zhuǎn)到坐標(biāo)系 F3下的場強，A14表示從坐標(biāo)系&到F3的旋轉(zhuǎn)矩陣，A31表示從坐標(biāo)系F4到&的旋轉(zhuǎn)矩陣 B' 2 = A14A31A53B2 =
T(7)將已知的兩個未知量帶入磁棒2的方程中的任意一個，即可求解夾角V，這里選取x方 向的方程，代入化簡得B ‘ 2x = Acos ￥ +Bsin ￥+C = 0(8)式中其中A、B、C均為常數(shù)；由方程(5)、(6)、(8)可求解三個歐拉角；進一步，由三個歐拉角表示的坐標(biāo)系F5到F3的旋轉(zhuǎn)關(guān)系，以及坐標(biāo)系&到F3的旋轉(zhuǎn)關(guān) 系，可以計算出坐標(biāo)系&到F5的旋轉(zhuǎn)關(guān)系，并將其表示成歐拉角或者其他期望的形式 A15= (A^r1(9)。
全文摘要
本發(fā)明屬于電磁跟蹤技術(shù)領(lǐng)域，具體為一種基于雙磁棒旋轉(zhuǎn)搜索的電磁跟蹤方法及系統(tǒng)。本發(fā)明的電磁跟蹤系統(tǒng)，由傳感器、磁場源、旋轉(zhuǎn)控制裝置和控制處理顯示裝置組成。本發(fā)明根據(jù)磁棒指向傳感器時傳感器測得的磁場強度最大的特性，采用雙磁棒跟蹤傳感器進行搜索定位，應(yīng)用幾何算法計算得到傳感器的空間位置；并進一步依據(jù)磁棒指向傳感器時傳感器測出的磁場方向沿著磁棒軸線方向的原理，應(yīng)用幾何算法計算得到傳感器的空間姿態(tài)。本發(fā)明利用磁棒旋轉(zhuǎn)搜索傳感器，采用非迭代算法，可以快速有效地實現(xiàn)對傳感器位置和方向的跟蹤，可靠性好、計算復(fù)雜度低。
文檔編號G01C21/00GK101852868SQ20101017933
公開日2010年10月6日 申請日期2010年5月20日 優(yōu)先權(quán)日2010年5月20日
發(fā)明者方祖祥, 葛鑫, 賴大坤, 鄔小玫 申請人:復(fù)旦大學(xué)