專利名稱：基于加速度傳感器的內(nèi)窺鏡探頭三維磁場定位系統(tǒng)及定位方法
技術(shù)領(lǐng)域：
本發(fā)明屬于磁場三維定位技術(shù)領(lǐng)域，尤其適合內(nèi)窺鏡探頭體內(nèi)位置和角度定位。
背景技術(shù)：
醫(yī)學(xué)內(nèi)窺鏡診察中，位于人體內(nèi)部的內(nèi)窺鏡探頭處于不可見狀態(tài)，醫(yī)生只能依據(jù)內(nèi)窺鏡圖像對病灶的位置作經(jīng)驗判斷，所以內(nèi)窺鏡探頭在人體內(nèi)的定位和追蹤對于醫(yī)學(xué)診斷和治療十分重要。目前通常采用的X射線透視成像定位，需要用二維圖像信息重構(gòu)三維圖像信息，實時性差且對人體有傷害。磁場定位技術(shù)以其精度較高、成本低、輻射低、實時性好等優(yōu)點，吸引了許多研究者參與研究，同時該項技術(shù)還可以應(yīng)用于運動跟蹤、三維坐標測定、虛擬現(xiàn)實等方面。磁場定位技術(shù)利用磁場傳感器檢測空間中磁場源產(chǎn)生的三維磁場，依據(jù)磁場分布規(guī)律對內(nèi)窺鏡探頭進行定位(包括空間位置和角度姿態(tài))，在實驗研究中，有些系統(tǒng)固定磁場傳感器的位置，將磁場源(勵磁線圈或者永磁體)作為定位目標，另一些則固定磁場源的位置，將磁場傳感器作為定位目標，本發(fā)明專利系統(tǒng)即屬于后者。目前國內(nèi)外磁場定位的研究基本上是單純依靠磁場進行定位，為了提高系統(tǒng)定位精度，往往需要數(shù)十個勵磁線圈或十幾磁場傳感器，不僅增加了系統(tǒng)的制作難度和成本，而且使得定位算法十分復(fù)雜，定位的實時性受到一定限制。由于系統(tǒng)應(yīng)用于內(nèi)窺鏡探頭定位，對于定位裝置的體積有嚴格的限制，定位裝置體積較大也是一些研究中存在的問題。

發(fā)明內(nèi)容
本發(fā)明的目的是解決現(xiàn)有磁場定位系統(tǒng)需要的勵磁線圈和磁場傳感器的數(shù)量較多，制作難度大、成本高，以及定位計算復(fù)雜，實時性差等問題，提供一種基于加速度傳感器的內(nèi)窺鏡探頭三維磁場定位系統(tǒng)。該系統(tǒng)引入了三軸加速度傳感器用于角度定位。加速度傳感器的引入實現(xiàn)了系統(tǒng)位置定位和角度定位過程的分離通過加速度傳感器采集重力場、磁場傳感器采集地磁場， 以實現(xiàn)角度定位；通過磁場傳感器采集人工磁場源產(chǎn)生的磁場，以實現(xiàn)位置定位。加速度傳感器的引入增加了定位信息，使得定位算法得到很大程度的簡化，提高了定位精度，同時加速度傳感器和磁場傳感器體積很小(立方毫米級)，二者可以方便的安置在內(nèi)窺鏡探頭上， 適用于內(nèi)窺鏡探頭定位以及其他對定位設(shè)備的體積有嚴格限制的場合。本發(fā)明提供的基于加速度傳感器的內(nèi)窺鏡探頭三維磁場定位系統(tǒng)包括勵磁線圈利用漆包銅絲平繞而成的多層空心電感線圈，通電后能夠在其周圍產(chǎn)生磁場，用于系統(tǒng)位置定位。所述的勵磁線圈的繞線骨架采用有機玻璃制作，該材料具有逆磁性，對于勵磁線圈通電后產(chǎn)生的磁場影響很小。勵磁線圈驅(qū)動電路亦即勵磁線圈的電源開關(guān)電路，控制勵磁線圈的通電時序，包括勵磁線圈充電回路和放電回路(圖2所示)，充電回路由勵磁線圈、電阻、MOS管組成，電阻的作用為降低充電回路的充電時間常數(shù)，MOS管在微處理器的I/O 口控制下開啟和關(guān)閉充電回路。放電回路由勵磁線圈、電阻、二極管組成，電阻的作用為降低放電回路的放電時間常數(shù)，二極管的作用為在勵磁線圈放電過程中自動開啟放電回路，防止產(chǎn)生電弧。每個勵磁線圈的充放電回路與電源之間接入一個二極管，防止各個勵磁線圈之間的相互干擾。傳感器電路傳感器電路主要由三軸加速度傳感器和三軸磁場傳感器構(gòu)成，分別用于探測重力場和磁場(地磁場、勵磁線圈磁場)，其輸出的信號為數(shù)字信號。磁場傳感器固定在內(nèi)窺鏡探頭上，對內(nèi)窺鏡探頭的定位即通過對磁場傳感器的定位實現(xiàn)系統(tǒng)在磁場傳感器運動的空間范圍內(nèi)建立一個全局坐標系O-XYZ做為基準(圖3)，全局坐標系的Z軸垂直水平面向上。在坐標系O-XYZ中，磁場傳感器的位置O1由坐標(X，y，ζ)來表示；以磁場傳感器的三個正交傳感軸Xp Y1^ Z1為坐標軸建立局部坐標系OfiC1Yyi,當局部坐標系與全局坐標系的三個軸線相互平行時，認為是磁場傳感器的初始角度姿態(tài)，其任意角度姿態(tài)都可以通過初始角度姿態(tài)的磁場傳感器繞著全局坐標系O-XYZ三個軸的方向旋轉(zhuǎn)得到，即先繞著X軸方向旋轉(zhuǎn)角度a，之后繞Y軸方向旋轉(zhuǎn)角度b，最后繞Z軸方向旋轉(zhuǎn)角度c，規(guī)定旋轉(zhuǎn)時逆著全局坐標系的軸看去，逆時針旋轉(zhuǎn)為正方向，磁場傳感器的角度姿態(tài)即利用有序?qū)崝?shù)組(a,b,c)來確定。數(shù)字處理電路包含傳感器通訊接口、微處理器、PC機通訊接口等部分。傳感器通訊接口(I2C接口)完成兩傳感器與微處理器的數(shù)據(jù)通訊，PC機通訊接口(串口)完成PC 機同微處理器的數(shù)據(jù)通訊。微處理器從兩傳感器獲得測量數(shù)據(jù)，進行初步的數(shù)據(jù)處理，之后將數(shù)據(jù)傳送至上位PC機，同時，微處理器直接控制勵磁線圈驅(qū)動電路，決定三個勵磁線圈的通電時序。數(shù)據(jù)處理和圖像顯示軟件在PC機上利用Visual C++編寫窗口程序軟件，軟件通過串口發(fā)送指令，控制微處理器，進而實現(xiàn)對系統(tǒng)的控制，同時通過串口從微處理器獲得測量數(shù)據(jù)，并計算得到傳感器的位置和角度姿態(tài)數(shù)據(jù)，并顯示在軟件窗口界面上，同時將傳感器的角度姿態(tài)以立體圖像的形式直觀的顯示在屏幕上。同時軟件可以接收用戶輸入的指令，包括系統(tǒng)的啟動、運行、停止等。本發(fā)明提供的基于加速度傳感器的內(nèi)窺鏡探頭三維磁場定位方法包括如下步驟固定在內(nèi)窺鏡探頭上的加速度和磁場傳感器隨內(nèi)窺鏡探頭進入人體內(nèi)，傳感器通過內(nèi)窺鏡管道內(nèi)的導(dǎo)線與體外的微處理器電路板相連，勵磁線圈放置在人體軀干下方，在驅(qū)動電路的驅(qū)動下，三個勵磁線圈分時地產(chǎn)生磁場，微處理器控制傳感器采集空間磁場和重力場數(shù)據(jù)，并將數(shù)據(jù)通過串口送至PC機，由PC機內(nèi)部的軟件算法得到磁場傳感器的位置和角度信息，實現(xiàn)內(nèi)窺鏡探頭的定位。系統(tǒng)上電后，系統(tǒng)進入定位循環(huán)，每一個定位周期分為兩個部分角度定位和位置定位，各個部分的定位過程如下1.角度定位過程微處理器控制勵磁線圈驅(qū)動電路，使得勵磁線圈中的電流為0，即不產(chǎn)生磁場?？臻g中只有地磁場和重力場。地磁場和重力場可以看做穩(wěn)恒場。在全局坐標系O-XYZ中，地磁場和重力場可以分別用兩個常矢量Mtl (Mtl = [X0 Y0Z0])和(^( =
)表示，微處理器控制兩傳感器采集數(shù)據(jù)，并對采集到的重力場和地磁場的場矢量的模做歸一化處理，假如此時的角度矢量為(a，b，C)，磁場傳感器的輸出為 M1 (M1 = [X1 Y1 Z1])，重力傳感器的輸出為GJG1 = [j 1 !^，那么禮與禮，^與^就可以通過旋轉(zhuǎn)矩陣聯(lián)系起來。旋轉(zhuǎn)矩陣表示的是任意矢量繞坐標系O-XYZ軸線旋轉(zhuǎn)時的坐標變換，矢量繞坐標系O-XYZ的X，Y，Z三個軸線旋轉(zhuǎn)的變換矩陣為X (a)、Y (b)、Z (c)。表1.旋轉(zhuǎn)矩陣
繞X軸旋轉(zhuǎn)角度a繞Y軸旋轉(zhuǎn)角度b繞Z軸旋轉(zhuǎn)角度c
10 0cosb 0 - sin bcosc sin c 0
旋轉(zhuǎn)矩陣0 cosa sin α Y φ)= 0 1 0 Z(c) = -sine cosc 0
0 - sin a cos αsin b 0 cos α0 0 1磁場傳感器和加速度傳感器由初始姿態(tài)先后繞全局坐標系O-XYZ的X，Y，Z三個軸旋轉(zhuǎn)，其輸出分別為MpG1，那么Mtl與M1, (^與G1的關(guān)系為G0 = G1 · X (a) Y (b) Z (c)(1)M0 = M1 · X (a) Y (b) Z (c)(2)由公式⑴、⑵分別可以得到公式(3)、⑷。
sin b = j\ - cos b-sina = m(3)
-cosb-cosa = η
Xcosb · cosc + Y cosb -sine = X1 -Z sin b\ (X sin α · sin + 7 cos a) cos c + (7 sin a · sin - X cos a) sin c = T1 - Z sin a cos (4)
(Xcosa· sin - 7 sin a)cosc + (7 cos a· sin + X sin a) sin C = Z1-Z cos a cos 由于角度矢量(a，b，c)和磁場傳感器的空間角度姿態(tài)并非一一對應(yīng)，例如角度矢量(30°，60°，100° )和(-150°，120°，-80° )表示的是同一個空間角度姿態(tài)，為了消除這種重復(fù)的表示，需要限制各個角度的取值范圍-180°180°，-90° ^b < 90°，-180° ^ c < 180°。利用公式(3)和(4)即可求解得到角度信息(a，b，c)。2.位置定位過程角度定位過程結(jié)束后，微控制器控制三個勵磁線圈分時的通以電流(每次只有一個線圈通電)，以產(chǎn)生磁場，在線圈中電流穩(wěn)定時，其產(chǎn)生的磁場為靜磁場。由于線圈是由銅絲密繞而成的空芯圓環(huán)線圈，其周圍的磁場可以等效成多匝圓環(huán)線圈磁場的迭加，以畢薩定理為基礎(chǔ)可以得到勵磁線圈軸線上磁感應(yīng)強度的表達式Β = μ°ΙΝ R\ ,,(5)
2 {R2+L2)15其中l(wèi)·^是真空介電常數(shù)，I是線圈電流，N是線圈的圈數(shù)，R是線圈的等效半徑 (與線圈模型有關(guān))，L是磁場傳感器到勵磁線圈中心的距離。設(shè)計線圈模型的內(nèi)徑、外徑以及厚度等參數(shù)，使得線圈周圍的等磁感應(yīng)強度曲面接近于球面，這樣對于勵磁線圈軸線外的磁感應(yīng)強度，也可以使用公式(5)作為近似公式， 公式(5)即確定了空間中某點的磁感應(yīng)強度B同該點到勵磁線圈中心點距離L的近似函數(shù)關(guān)系。三個線圈的中心分別位于點0(0，0，0)、P(Xp，0，0)、Q(XQ，yQ，0)；第一、第二、第三三個線圈分別對應(yīng)圖3中標注4、5、6。第一個線圈通電時，利用磁場傳感器測得其所在位置的磁感應(yīng)強度B1，可以得到L1,即傳感器所在位置O1到第一個線圈中心0距離。用同樣的方式，第二個線圈、第三個線圈分別通電，得到L2, L3 ;L1 L2, L3分別表示傳感器所在位置點 Ο^χ,γ,ζ)到三個線圈所在位置點0(0，0，0)、？0^，0，0)、00^，70，0)的距離，依據(jù)空間兩點間的距離公式即可求解磁場傳感器的位置坐標(x，y，z)；3.定位結(jié)果顯示PC機中的數(shù)據(jù)處理和圖像顯示軟件將定位結(jié)果顯示在屏幕上，并在屏幕上顯示一虛擬的內(nèi)窺鏡探頭圖像，該圖像可以依據(jù)系統(tǒng)角度定位的結(jié)果，實時的反映當前內(nèi)窺鏡探頭的角度姿態(tài)，便于操作者觀察。本發(fā)明的優(yōu)點和積極效果(1)結(jié)構(gòu)簡單。與單純使用磁場進行定位的裝置相比，使用的勵磁線圈(3個)和磁場磁場傳感器(1個)數(shù)量少。磁場傳感器和加速度傳感器的輸出為數(shù)字量，可以直接進行數(shù)據(jù)處理。(2)體積微小。磁場傳感器和加速度傳感器的體積微小，能夠安置在內(nèi)窺鏡探頭上進入人體。(3)實時性好。引入加速度傳感器，實現(xiàn)了內(nèi)窺鏡探頭位置定位和角度定位過程的分離，加速度傳感器的引入增多了定位的信息，使得定位算法得到很大程度的簡化，提高了系統(tǒng)定位的實時性。

圖1是本發(fā)明基于加速度傳感器的內(nèi)窺鏡探頭三維磁場定位系統(tǒng)原理圖。圖2是本發(fā)明勵磁線圈驅(qū)動電路。圖3是本發(fā)明的目標角度和位置定位的定位參數(shù)示意圖。圖4是本發(fā)明的定位系統(tǒng)結(jié)構(gòu)框圖。圖5是本發(fā)明數(shù)據(jù)處理程序流程圖。圖中，1為磁場傳感器，2加速度傳感器，3勵磁線圈1，4勵磁線圈11，5勵磁線圈 III，6定位平臺，7三維定位空間。
具體實施方式實施例1、基于加速度傳感器的內(nèi)窺鏡探頭三維磁場定位系統(tǒng)如圖1、圖4所示，本發(fā)明提供的基于加速度傳感器的內(nèi)窺鏡探頭三維磁場定位系統(tǒng)包括五個部分勵磁線圈(由圖1中部件3 5組成)、勵磁線圈驅(qū)動電路、傳感器電路 (由1 2組成)、數(shù)字處理電路、含有數(shù)據(jù)處理和圖像顯示軟件的PC機。系統(tǒng)拓撲結(jié)構(gòu)如下系統(tǒng)以數(shù)字處理電路為中心，數(shù)字處理電路控制勵磁線圈驅(qū)動電路，驅(qū)動勵磁線圈以一定的時序產(chǎn)生電磁場，同時數(shù)字處理電路控制傳感器電路采集磁場和重力場信息，將采集到的信息傳輸?shù)絇C機，PC機完成圖像處理和顯示。系統(tǒng)各部分詳述如下
1、勵磁線圈利用漆包銅絲平繞而成的多層空心電感線圈，通電后可以在其周圍產(chǎn)生磁場，勵磁線圈的繞線骨架采用有機玻璃制作。勵磁線圈工作時通以直流電。2、勵磁線圈驅(qū)動電路利用場效應(yīng)管設(shè)計了勵磁線圈驅(qū)動電路，如圖2所示，勵磁線圈驅(qū)動電路包括充電回路和放電回路。充電回路由勵磁線圈、電阻、MOS管串聯(lián)組成，電阻的作用為降低充電回路的充電時間常數(shù)，MOS管在微處理器的I/O 口控制下開啟和關(guān)閉充電回路；放電回路是將二極管和電阻串聯(lián)后，并聯(lián)在勵磁線圈與電阻串聯(lián)電路的兩端，放電回路中電阻的作用為降低放電回路的放電時間常數(shù)，二極管的作用為在勵磁線圈放電過程中正向?qū)?，自動開啟放電回路，防止產(chǎn)生電?。幻總€勵磁線圈的充電、放電回路與電源之間串聯(lián)接入一個二極管，防止各個勵磁線圈之間的相互干擾3、傳感器電路傳感器電路包含三軸磁場傳感器和三軸加速度傳感器。兩傳感器芯片體積微小， 可以安置于內(nèi)窺鏡探頭上。固定在內(nèi)窺鏡探頭上的傳感器隨內(nèi)窺鏡探頭進入人體內(nèi)，并通過內(nèi)窺鏡管道內(nèi)的導(dǎo)線與體外的微處理器電路板相連。定位系統(tǒng)中，兩傳感器的三個傳感軸分別相互平行，從而兩者具有相同的角度姿態(tài)。兩傳感器將各個軸線上的物理場分量以 12bit數(shù)據(jù)輸出。系統(tǒng)在磁場傳感器運動的空間范圍內(nèi)建立一個全局坐標系O-XYZ做為基準(圖 3)，全局坐標系的Z軸垂直水平面向上。在坐標系O-XYZ中，磁場傳感器的位置01由坐標 (X，y，ζ)來表示；以磁場傳感器當傳感器的位置O1為原點，以其三個正交傳感軸Xp Y1^ Z1 為坐標軸，建立局部坐標系O1-X1YA,當局部坐標系與全局坐標系的三個軸線相互平行時， 認為是磁場傳感器的初始角度姿態(tài)，其任意角度姿態(tài)都可以通過初始姿態(tài)的磁場傳感器繞著全局坐標系O-XYZ三個軸的方向旋轉(zhuǎn)得到，即先繞著X軸方向旋轉(zhuǎn)角度a，之后繞Y軸方向旋轉(zhuǎn)角度b，最后繞Z軸方向旋轉(zhuǎn)角度c，規(guī)定旋轉(zhuǎn)時逆著全局坐標系的軸看去，逆時針旋轉(zhuǎn)為正方向，磁場傳感器的角度姿態(tài)即利用矢量(a，b，c)來確定。4、數(shù)字處理電路包含傳感器通訊接口、微處理器、PC機通訊接口等部分。傳感器通訊接口(I2C接口)完成兩傳感器與微處理器的數(shù)據(jù)通訊，PC機通訊接口(串口)完成PC機同微處理器的數(shù)據(jù)通訊。微處理器從兩傳感器獲得測量數(shù)據(jù)，進行初步的數(shù)據(jù)處理，之后將數(shù)據(jù)傳送至上位PC機，同時，微處理器直接控制勵磁線圈驅(qū)動電路，決定三個勵磁線圈的通電時序。
5、計算機數(shù)據(jù)處理和圖像顯示軟件在PC機上利用Visual C++編寫窗口程序軟件，軟件通過串口發(fā)送指令，控制微處理器，進而實現(xiàn)對系統(tǒng)的控制，同時通過串口從微處理器獲得測量數(shù)據(jù)，并利用相應(yīng)算法計算出傳感器的位置和角度姿態(tài)數(shù)據(jù)，并顯示在軟件窗口界面上，同時將傳感器的角度姿態(tài)以立體圖像的形式直觀的顯示在屏幕上。同時軟件可以接收用戶輸入的指令，包括系統(tǒng)的啟動、運行、停止等。實施例2、基于加速度傳感器的內(nèi)窺鏡探頭三維磁場定位方法(角度定位和位置定位分離算法)加速度傳感器的引入實現(xiàn)了系統(tǒng)位置定位和角度定位過程的分離通過加速度傳感器采集重力場、磁場傳感器采集地磁場，以實現(xiàn)角度定位；通過磁場傳感器采集人工磁場源產(chǎn)生的磁場，以實現(xiàn)位置定位。加速度傳感器的引入增加了定位信息，使得定位算法得到很大程度的簡化，提高了定位精度。角度定位和位置定位算法敘述如下1.角度定位算法微處理器控制勵磁線圈驅(qū)動電路，使得勵磁線圈中的電流為0，即不產(chǎn)生磁場?？臻g中只有地磁場和重力場。地磁場和重力場可以看做穩(wěn)恒場。在全局坐標系O-XYZ中，地磁場和重力場可以分別用兩個常矢量Mtl (Mtl = [X0 Y0 Z0])和(UGtl=
)表示，微處理器控制兩傳感器采集數(shù)據(jù)，并對采集到的重力場和地磁場的場矢量的模做歸一化處理，假如此時的角度矢量為(a，b，c)，磁場傳感器的輸出為 M1 (M1 = [X1 Y1 Z1])，重力傳感器的輸出為GJG1 = [j 1 !^，那么禮與禮，^與^就可以通過旋轉(zhuǎn)矩陣聯(lián)系起來。實際測量中的一組數(shù)據(jù)為=M0= [X0 Y0 Z0] = [-0. 3560 -0. 5926 -0. 7221]、G0 =
,M1 = [X1 Y1 Z1] =
,G1 = [j 1 m] = [-0. 4423 0. 5000 -0·7445]。求解角度信息需要通過以下公式
G0 = G1 · X (a) Y (b) Z (c)(1)M0 = M1 · X (a) Y (b) Z (c)(2)由公式⑴、⑵分別可以得到公式(3)、⑷。 sin b = j
權(quán)利要求
1.一種基于加速度傳感器的內(nèi)窺鏡探頭三維磁場定位系統(tǒng)，其特征在于該定位系統(tǒng)包括三個勵磁線圈即第一勵磁線圈、第二勵磁線圈和第三勵磁線圈，三個勵磁線圈由驅(qū)動電路驅(qū)動，用于確定安裝在內(nèi)窺鏡探頭上的磁場傳感器的空間位置；勵磁線圈驅(qū)動電路與數(shù)字處理電路連接，用于循環(huán)控制三個勵磁線圈的通電時序；傳感器電路包括安裝在內(nèi)窺鏡探頭上的三軸磁場傳感器和三軸加速度傳感器，三軸磁場傳感器和三軸加速度傳感器分別通過信號放大及AD轉(zhuǎn)換電路連接數(shù)字處理電路，用于將三軸磁場傳感器和三軸加速度傳感器采集的模擬信號轉(zhuǎn)換成數(shù)字信號傳送給數(shù)字處理電路；數(shù)字處理電路用于控制傳感器采集空間磁場和重力場數(shù)據(jù)，并將數(shù)據(jù)通過串口送至 PC機；PC機通過PC機內(nèi)部的軟件算法計算出傳感器的位置和角度姿態(tài)數(shù)據(jù)，實現(xiàn)內(nèi)窺鏡探頭的定位，并進行顯示。
2.根據(jù)權(quán)利要求1所述的定位系統(tǒng)，其特征在于所述的勵磁線圈由銅絲繞制，繞線骨架采用具有很弱的逆磁性的有機材料制作，以減小對勵磁線圈通電后產(chǎn)生的磁場的影響。
3.根據(jù)權(quán)利要求1所述的定位系統(tǒng)，其特征在于所述的勵磁線圈驅(qū)動電路包括勵磁線圈充電回路和放電回路，充電回路由勵磁線圈、電阻、MOS管串聯(lián)組成，電阻的作用為降低充電回路的充電時間常數(shù)，MOS管在微處理器的I/O 口控制下開啟和關(guān)閉充電回路；放電回路是將二極管和電阻串聯(lián)后，并聯(lián)在勵磁線圈與電阻串聯(lián)電路的兩端，放電回路中電阻的作用為降低放電回路的放電時間常數(shù)，二極管的作用為在勵磁線圈放電過程中正向?qū)ǎ詣娱_啟放電回路，防止產(chǎn)生電?。幻總€勵磁線圈的充電、放電回路與電源之間串聯(lián)接入一個二極管，防止各個勵磁線圈之間的相互干擾。
4.一種基于加速度傳感器的內(nèi)窺鏡探頭三維磁場定位方法，其特征在于該方法的步驟是第1、將三軸磁場傳感器和三軸加速度傳感器固定在內(nèi)窺鏡探頭上，并隨內(nèi)窺鏡探頭進入人體內(nèi)，三軸磁場傳感器和三軸加速度傳感器通過內(nèi)窺鏡管道內(nèi)的導(dǎo)線與體外的數(shù)字處理電路相連，三個勵磁線圈分別放置在人體軀干下方，在驅(qū)動電路的驅(qū)動下，三個勵磁線圈分時地產(chǎn)生磁場；第2、在磁場傳感器運動的空間范圍內(nèi)建立一個全局坐標系O-XYZ做為基準，全局坐標系Z軸的正方向垂直水平面向上；第3、以磁場傳感器在全局坐標系O-XYZ中的初始位置O1 (x, y, ζ)為原點，以磁場傳感器的三個正交傳感軸Xp Y1^ Z1為坐標軸，建立局部坐標系O1 -X1Yy1，并規(guī)定局部坐標系與全局坐標系的三個軸線相互平行時為磁場傳感器的初始角度姿態(tài)，磁場傳感器任意角度姿態(tài)都能夠通過初始角度姿態(tài)的磁場傳感器繞著全局坐標系O-XYZ三個軸的方向旋轉(zhuǎn)得到，即先繞著X軸方向旋轉(zhuǎn)角度a，之后繞Y軸方向旋轉(zhuǎn)角度b，最后繞Z軸方向旋轉(zhuǎn)角度 c，規(guī)定旋轉(zhuǎn)時逆著全局坐標系的坐標軸看去，逆時針旋轉(zhuǎn)為正方向，則磁場傳感器的角度姿態(tài)用有序?qū)崝?shù)組(a，b, c)來表示；第4、角度定位系統(tǒng)上電后進入定位循環(huán)，每一個定位周期分為兩個部分角度定位和位置定位；角度定位過程如下微處理器控制勵磁線圈驅(qū)動電路，使得勵磁線圈中的電流為0，即不產(chǎn)生磁場，空間中只有地磁場和重力場，地磁場和重力場可以看做穩(wěn)恒場；在全局坐標系O-XYZ中，地磁場和重力場可以分別用兩個常矢量 M, = [X0 η 4]和0。=
表示，其中；V A分別為地磁場在全局坐標系X、Y、Z三個軸線上的分量，由于全局坐標系Z軸豎直向上，所以重力場在全局坐標系的X、Y、Z三個軸線上分量分別為0、0、-1 ；微處理器控制兩傳感器采集數(shù)據(jù)，并對采集到的重力場和地磁場的場矢量的模做歸一化處理，假如此時的角度矢量為(a，b, C)，磁場傳感器的輸出為 M1^X1 Y1 & ],重力傳感器的輸出為G1=L/ I m],利用M13、AfpG^CJ1建立方程組，求解得到角度信息(a, b, c)； 第5、位置定位微控制器控制三個勵磁線圈分時的通以電流，每次只有一個線圈通電，以產(chǎn)生磁場，在線圈中電流穩(wěn)定時，其產(chǎn)生的磁場為靜磁場；由于線圈是由銅絲密繞而成的空芯圓環(huán)線圈， 其周圍的磁場可以等效成多匝圓環(huán)線圈磁場的迭加；線圈周圍的某點的磁感應(yīng)強度與該點的空間位置具有固定的關(guān)系，三個線圈的中心分別位于點0(0，0，0)、Ρ(χΡ，0，0)、Q(Xq，yQ, 0)；第一個線圈通電時，利用磁場傳感器測得其所在位置的磁感應(yīng)強度B1，可以得到L1,即傳感器所在位置O1到第一個線圈中心0距離；用同樣的方式，第二個線圈、第三個線圈分別通電，得到L2, L3 ;L1 L2, L3分別表示傳感器所在位置點O1U, y, ζ)到三個線圈所在位置點0(0，0，0)、P(xP，0，0)、Q(Xq，yQ, 0)的距離，依據(jù)空間兩點間的距離公式即可求解磁場傳感器的位置坐標(x，y, ζ)； 第6、定位結(jié)果顯示由PC機中的數(shù)據(jù)處理和圖像顯示軟件將定位結(jié)果顯示在屏幕上，并在屏幕上顯示一個虛擬的內(nèi)窺鏡探頭圖像，該圖像能夠依據(jù)系統(tǒng)角度定位的結(jié)果，實時的反映當前內(nèi)窺鏡探頭的角度姿態(tài)，便于操作者觀察。
全文摘要
基于加速度傳感器的內(nèi)窺鏡探頭三維磁場定位系統(tǒng)及定位方法。本發(fā)明基于加速度傳感器的內(nèi)窺鏡三維磁場定位系統(tǒng)包括，勵磁線圈、勵磁線圈驅(qū)動電路、三軸磁場傳感器和三軸加速的傳感器電路、數(shù)字處理電路、計算機數(shù)據(jù)處理和圖像顯示軟件。本發(fā)明區(qū)別于單純利用磁場實現(xiàn)內(nèi)窺鏡探頭定位的方式，引入加速度傳感器，實現(xiàn)了內(nèi)窺鏡探頭位置定位和角度定位過程的分離，加速度傳感器的引入增多了定位的信息，使得定位算法得到很大程度的簡化，同時為定位精度的提高提供了條件。
文檔編號A61B1/00GK102499616SQ20111029966
公開日2012年6月20日 申請日期2011年9月28日 優(yōu)先權(quán)日2011年9月28日
發(fā)明者柳立坤, 汪毅, 王森, 郁道銀, 陳曉冬 申請人:天津大學(xué)