CN102499616A - 基于加速度传感器的内窥镜探头三维磁场定位系统及定位方法 - Google Patents

Abstract

基于加速度传感器的内窥镜探头三维磁场定位系统及定位方法。本发明基于加速度传感器的内窥镜三维磁场定位系统包括，励磁线圈、励磁线圈驱动电路、三轴磁场传感器和三轴加速的传感器电路、数字处理电路、计算机数据处理和图像显示软件。本发明区别于单纯利用磁场实现内窥镜探头定位的方式，引入加速度传感器，实现了内窥镜探头位置定位和角度定位过程的分离，加速度传感器的引入增多了定位的信息，使得定位算法得到很大程度的简化，同时为定位精度的提高提供了条件。

Description

基于加速度传感器的内窥镜探头三维磁场定位系统及定位方法

技术领域

本发明属于磁场三维定位技术领域，尤其适合内窥镜探头体内位置和角度定位。

背景技术

医学内窥镜诊察中，位于人体内部的内窥镜探头处于不可见状态，医生只能依据内窥镜图像对病灶的位置作经验判断，所以内窥镜探头在人体内的定位和追踪对于医学诊断和治疗十分重要。目前通常采用的X射线透视成像定位，需要用二维图像信息重构三维图像信息，实时性差且对人体有伤害。磁场定位技术以其精度较高、成本低、辐射低、实时性好等优点，吸引了许多研究者参与研究，同时该项技术还可以应用于运动跟踪、三维坐标测定、虚拟现实等方面。

磁场定位技术利用磁场传感器检测空间中磁场源产生的三维磁场，依据磁场分布规律对内窥镜探头进行定位(包括空间位置和角度姿态)，在实验研究中，有些系统固定磁场传感器的位置，将磁场源(励磁线圈或者永磁体)作为定位目标，另一些则固定磁场源的位置，将磁场传感器作为定位目标，本发明专利系统即属于后者。目前国内外磁场定位的研究基本上是单纯依靠磁场进行定位，为了提高系统定位精度，往往需要数十个励磁线圈或十几磁场传感器，不仅增加了系统的制作难度和成本，而且使得定位算法十分复杂，定位的实时性受到一定限制。由于系统应用于内窥镜探头定位，对于定位装置的体积有严格的限制，定位装置体积较大也是一些研究中存在的问题。

发明内容

本发明的目的是解决现有磁场定位系统需要的励磁线圈和磁场传感器的数量较多，制作难度大、成本高，以及定位计算复杂，实时性差等问题，提供一种基于加速度传感器的内窥镜探头三维磁场定位系统。

该系统引入了三轴加速度传感器用于角度定位。加速度传感器的引入实现了系统位置定位和角度定位过程的分离：通过加速度传感器采集重力场、磁场传感器采集地磁场，以实现角度定位；通过磁场传感器采集人工磁场源产生的磁场，以实现位置定位。加速度传感器的引入增加了定位信息，使得定位算法得到很大程度的简化，提高了定位精度，同时加速度传感器和磁场传感器体积很小(立方毫米级)，二者可以方便的安置在内窥镜探头上，适用于内窥镜探头定位以及其他对定位设备的体积有严格限制的场合。

本发明提供的基于加速度传感器的内窥镜探头三维磁场定位系统包括：

励磁线圈：利用漆包铜丝平绕而成的多层空心电感线圈，通电后能够在其周围产生磁场，用于系统位置定位。

所述的励磁线圈的绕线骨架采用有机玻璃制作，该材料具有逆磁性，对于励磁线圈通电后产生的磁场影响很小。

励磁线圈驱动电路：亦即励磁线圈的电源开关电路，控制励磁线圈的通电时序，包括励磁线圈充电回路和放电回路(图2所示)，充电回路由励磁线圈、电阻、MOS管组成，电阻的作用为降低充电回路的充电时间常数，MOS管在微处理器的I/O口控制下开启和关闭充电回路。放电回路由励磁线圈、电阻、二极管组成，电阻的作用为降低放电回路的放电时间常数，二极管的作用为在励磁线圈放电过程中自动开启放电回路，防止产生电弧。每个励磁线圈的充放电回路与电源之间接入一个二极管，防止各个励磁线圈之间的相互干扰。

传感器电路：传感器电路主要由三轴加速度传感器和三轴磁场传感器构成，分别用于探测重力场和磁场(地磁场、励磁线圈磁场)，其输出的信号为数字信号。磁场传感器固定在内窥镜探头上，对内窥镜探头的定位即通过对磁场传感器的定位实现：系统在磁场传感器运动的空间范围内建立一个全局坐标系O-XYZ做为基准(图3)，全局坐标系的Z轴垂直水平面向上。在坐标系O-XYZ中，磁场传感器的位置O₁由坐标(x，y，z)来表示；以磁场传感器的三个正交传感轴X₁、Y₁、Z₁为坐标轴建立局部坐标系O₁-X₁Y₁Z₁，当局部坐标系与全局坐标系的三个轴线相互平行时，认为是磁场传感器的初始角度姿态，其任意角度姿态都可以通过初始角度姿态的磁场传感器绕着全局坐标系O-XYZ三个轴的方向旋转得到，即先绕着X轴方向旋转角度a，之后绕Y轴方向旋转角度b，最后绕Z轴方向旋转角度c，规定旋转时逆着全局坐标系的轴看去，逆时针旋转为正方向，磁场传感器的角度姿态即利用有序实数组(a，b，c)来确定。

数字处理电路：包含传感器通讯接口、微处理器、PC机通讯接口等部分。传感器通讯接口(I2C接口)完成两传感器与微处理器的数据通讯，PC机通讯接口(串口)完成PC机同微处理器的数据通讯。微处理器从两传感器获得测量数据，进行初步的数据处理，之后将数据传送至上位PC机，同时，微处理器直接控制励磁线圈驱动电路，决定三个励磁线圈的通电时序。

数据处理和图像显示软件：在PC机上利用Visual C++编写窗口程序软件，软件通过串口发送指令，控制微处理器，进而实现对系统的控制，同时通过串口从微处理器获得测量数据，并计算得到传感器的位置和角度姿态数据，并显示在软件窗口界面上，同时将传感器的角度姿态以立体图像的形式直观的显示在屏幕上。同时软件可以接收用户输入的指令，包括系统的启动、运行、停止等。

本发明提供的基于加速度传感器的内窥镜探头三维磁场定位方法包括如下步骤：

固定在内窥镜探头上的加速度和磁场传感器随内窥镜探头进入人体内，传感器通过内窥镜管道内的导线与体外的微处理器电路板相连，励磁线圈放置在人体躯干下方，在驱动电路的驱动下，三个励磁线圈分时地产生磁场，微处理器控制传感器采集空间磁场和重力场数据，并将数据通过串口送至PC机，由PC机内部的软件算法得到磁场传感器的位置和角度信息，实现内窥镜探头的定位。

系统上电后，系统进入定位循环，每一个定位周期分为两个部分：角度定位和位置定位，各个部分的定位过程如下：

1.角度定位过程

微处理器控制励磁线圈驱动电路，使得励磁线圈中的电流为0，即不产生磁场。空间中只有地磁场和重力场。地磁场和重力场可以看做稳恒场。

在全局坐标系O-XYZ中，地磁场和重力场可以分别用两个常矢量M₀(M₀＝[X₀ Y₀ Z₀])和G₀(G₀＝[0 0 -1])表示，微处理器控制两传感器采集数据，并对采集到的重力场和地磁场的场矢量的模做归一化处理，假如此时的角度矢量为(a，b，c)，磁场传感器的输出为M₁(M₁＝[X₁ Y₁ Z₁])，重力传感器的输出为G₁(G₁＝[j l m])，那么M₀与M₁，G₀与G₁就可以通过旋转矩阵联系起来。

旋转矩阵表示的是任意矢量绕坐标系O-XYZ轴线旋转时的坐标变换，矢量绕坐标系O-XYZ的X，Y，Z三个轴线旋转的变换矩阵为X(a)、Y(b)、Z(c)。

表1.旋转矩阵

磁场传感器和加速度传感器由初始姿态先后绕全局坐标系O-XYZ的X，Y，Z三个轴旋转，其输出分别为M₁、G₁，那么M₀与M₁，G₀与G₁的关系为：

G₀＝G₁·X(a)Y(b)Z(c)                  (1)

M₀＝M₁·X(a)Y(b)Z(c)                  (2)

由公式(1)、(2)分别可以得到公式(3)、(4)。

$\left\{\begin{array}{c}\sin b=j\\ -\cos b\·\sin a=m\\ -\cos b\·\cos a=n\end{array}\right.---\left(3\right)$

$\left\{\begin{array}{c}X\cos b\·\cos c+Y\cos b\·\sin c=X_1-Z\sin b\\ (X\sin a\·\sin b+Y\cos a)\cos c+(Y\sin a\·\sin b-X\cos a)\sin c=Y_1-Z\sin a\cos b\\ (X\cos a\·\sin b-Y\sin a)\cos c+(Y\cos a\·\sin b+X\sin a)\sin c=Z_1-Z\cos a\cos b\end{array}\right.---\left(4\right)$

由于角度矢量(a，b，c)和磁场传感器的空间角度姿态并非一一对应，例如角度矢量(30°，60°，100°)和(-150°，120°，-80°)表示的是同一个空间角度姿态，为了消除这种重复的表示，需要限制各个角度的取值范围：-180°≤a＜180°，-90°≤b＜90°，-180°≤c＜180°。

利用公式(3)和(4)即可求解得到角度信息(a，b，c)。

2.位置定位过程

角度定位过程结束后，微控制器控制三个励磁线圈分时的通以电流(每次只有一个线圈通电)，以产生磁场，在线圈中电流稳定时，其产生的磁场为静磁场。由于线圈是由铜丝密绕而成的空芯圆环线圈，其周围的磁场可以等效成多匝圆环线圈磁场的迭加，以毕萨定理为基础可以得到励磁线圈轴线上磁感应强度的表达式：

$B=\frac{{\mathrm{\μ}}_0\mathrm{IN}}{2}\frac{R^2}{{(R^2+L^2)}^{1.5}}---\left(5\right)$

其中μ₀是真空介电常数，I是线圈电流，N是线圈的圈数，R是线圈的等效半径(与线圈模型有关)，L是磁场传感器到励磁线圈中心的距离。

设计线圈模型的内径、外径以及厚度等参数，使得线圈周围的等磁感应强度曲面接近于球面，这样对于励磁线圈轴线外的磁感应强度，也可以使用公式(5)作为近似公式，公式(5)即确定了空间中某点的磁感应强度B同该点到励磁线圈中心点距离L的近似函数关系。

三个线圈的中心分别位于点O(0，0，0)、P(x_P，0，0)、Q(x_Q，y_Q，0)；第一、第二、第三三个线圈分别对应图3中标注4、5、6。第一个线圈通电时，利用磁场传感器测得其所在位置的磁感应强度B₁，可以得到L₁，即传感器所在位置O₁到第一个线圈中心O距离。用同样的方式，第二个线圈、第三个线圈分别通电，得到L₂，L₃；L₁，L₂，L₃分别表示传感器所在位置点O₁(x，y，z)到三个线圈所在位置点O(0，0，0)、P(x_P，0，0)、Q(x_Q，y_Q，0)的距离，依据空间两点间的距离公式即可求解磁场传感器的位置坐标(x，y，z)；

3.定位结果显示

PC机中的数据处理和图像显示软件将定位结果显示在屏幕上，并在屏幕上显示一虚拟的内窥镜探头图像，该图像可以依据系统角度定位的结果，实时的反映当前内窥镜探头的角度姿态，便于操作者观察。

本发明的优点和积极效果：

(1)结构简单。与单纯使用磁场进行定位的装置相比，使用的励磁线圈(3个)和磁场磁场传感器(1个)数量少。磁场传感器和加速度传感器的输出为数字量，可以直接进行数据处理。

(2)体积微小。磁场传感器和加速度传感器的体积微小，能够安置在内窥镜探头上进入人体。

(3)实时性好。引入加速度传感器，实现了内窥镜探头位置定位和角度定位过程的分离，加速度传感器的引入增多了定位的信息，使得定位算法得到很大程度的简化，提高了系统定位的实时性。

【附图说明】：

图1是本发明基于加速度传感器的内窥镜探头三维磁场定位系统原理图。

图2是本发明励磁线圈驱动电路。

图3是本发明的目标角度和位置定位的定位参数示意图。

图4是本发明的定位系统结构框图。

图5是本发明数据处理程序流程图。

图中，1为磁场传感器，2加速度传感器，3励磁线圈I，4励磁线圈II，5励磁线圈III，6定位平台，7三维定位空间。

【具体实施方式】：

实施例1、基于加速度传感器的内窥镜探头三维磁场定位系统

如图1、图4所示，本发明提供的基于加速度传感器的内窥镜探头三维磁场定位系统包括五个部分：励磁线圈(由图1中部件3～5组成)、励磁线圈驱动电路、传感器电路(由1～2组成)、数字处理电路、含有数据处理和图像显示软件的PC机。

系统拓扑结构如下：系统以数字处理电路为中心，数字处理电路控制励磁线圈驱动电路，驱动励磁线圈以一定的时序产生电磁场，同时数字处理电路控制传感器电路采集磁场和重力场信息，将采集到的信息传输到PC机，PC机完成图像处理和显示。

系统各部分详述如下：

1、励磁线圈

利用漆包铜丝平绕而成的多层空心电感线圈，通电后可以在其周围产生磁场，励磁线圈的绕线骨架采用有机玻璃制作。励磁线圈工作时通以直流电。

2、励磁线圈驱动电路

利用场效应管设计了励磁线圈驱动电路，如图2所示，励磁线圈驱动电路包括充电回路和放电回路。充电回路由励磁线圈、电阻、MOS管串联组成，电阻的作用为降低充电回路的充电时间常数，MOS管在微处理器的I/O口控制下开启和关闭充电回路；放电回路是将二极管和电阻串联后，并联在励磁线圈与电阻串联电路的两端，放电回路中电阻的作用为降低放电回路的放电时间常数，二极管的作用为在励磁线圈放电过程中正向导通，自动开启放电回路，防止产生电弧；每个励磁线圈的充电、放电回路与电源之间串联接入一个二极管，防止各个励磁线圈之间的相互干扰

3、传感器电路

传感器电路包含三轴磁场传感器和三轴加速度传感器。两传感器芯片体积微小，可以安置于内窥镜探头上。固定在内窥镜探头上的传感器随内窥镜探头进入人体内，并通过内窥镜管道内的导线与体外的微处理器电路板相连。定位系统中，两传感器的三个传感轴分别相互平行，从而两者具有相同的角度姿态。两传感器将各个轴线上的物理场分量以12bit数据输出。

系统在磁场传感器运动的空间范围内建立一个全局坐标系O-XYZ做为基准(图3)，全局坐标系的Z轴垂直水平面向上。在坐标系O-XYZ中，磁场传感器的位置O1由坐标(x，y，z)来表示；以磁场传感器当传感器的位置O₁为原点，以其三个正交传感轴X₁、Y₁、Z₁为坐标轴，建立局部坐标系O₁-X₁Y₁Z₁，当局部坐标系与全局坐标系的三个轴线相互平行时，认为是磁场传感器的初始角度姿态，其任意角度姿态都可以通过初始姿态的磁场传感器绕着全局坐标系O-XYZ三个轴的方向旋转得到，即先绕着X轴方向旋转角度a，之后绕Y轴方向旋转角度b，最后绕Z轴方向旋转角度c，规定旋转时逆着全局坐标系的轴看去，逆时针旋转为正方向，磁场传感器的角度姿态即利用矢量(a，b，c)来确定。

4、数字处理电路

包含传感器通讯接口、微处理器、PC机通讯接口等部分。传感器通讯接口(I2C接口)完成两传感器与微处理器的数据通讯，PC机通讯接口(串口)完成PC机同微处理器的数据通讯。微处理器从两传感器获得测量数据，进行初步的数据处理，之后将数据传送至上位PC机，同时，微处理器直接控制励磁线圈驱动电路，决定三个励磁线圈的通电时序。

5、计算机数据处理和图像显示软件

在PC机上利用Visual C++编写窗口程序软件，软件通过串口发送指令，控制微处理器，进而实现对系统的控制，同时通过串口从微处理器获得测量数据，并利用相应算法计算出传感器的位置和角度姿态数据，并显示在软件窗口界面上，同时将传感器的角度姿态以立体图像的形式直观的显示在屏幕上。同时软件可以接收用户输入的指令，包括系统的启动、运行、停止等。

实施例2、基于加速度传感器的内窥镜探头三维磁场定位方法(角度定位和位置定位分离算法)

加速度传感器的引入实现了系统位置定位和角度定位过程的分离：通过加速度传感器采集重力场、磁场传感器采集地磁场，以实现角度定位；通过磁场传感器采集人工磁场源产生的磁场，以实现位置定位。加速度传感器的引入增加了定位信息，使得定位算法得到很大程度的简化，提高了定位精度。

角度定位和位置定位算法叙述如下：

1.角度定位算法

微处理器控制励磁线圈驱动电路，使得励磁线圈中的电流为0，即不产生磁场。空间中只有地磁场和重力场。地磁场和重力场可以看做稳恒场。

在全局坐标系O-XYZ中，地磁场和重力场可以分别用两个常矢量M₀(M₀＝[X₀ Y₀ Z₀])和G₀(G₀＝[0 0 -1])表示，微处理器控制两传感器采集数据，并对采集到的重力场和地磁场的场矢量的模做归一化处理，假如此时的角度矢量为(a，b，c)，磁场传感器的输出为M₁(M₁＝[X₁ Y₁ Z₁])，重力传感器的输出为G₁(G₁＝[j l m])，那么M₀与M₁，G₀与G₁就可以通过旋转矩阵联系起来。

实际测量中的一组数据为：M₀＝[X₀ Y₀ Z₀]＝[-0.3560 -0.5926 -0.7221]、G₀＝[0 0 -1]、M₁＝[X₁ Y₁ Z₁]＝[0.0373 0.0075 -0.9993]、G₁＝[j l m]＝[-0.4423 0.5000 -0.7445]。

求解角度信息需要通过以下公式：

G₀＝G₁·X(a)Y(b)Z(c)           (1)

M₀＝M₁·X(a)Y(b)Z(c)           (2)

由公式(1)、(2)分别可以得到公式(3)、(4)。

$\left\{\begin{array}{c}\sin b=j\\ -\cos b\·\sin a=m\\ -\cos b\·\cos a=n\end{array}\right.---\left(3\right)$

$\left\{\begin{array}{c}X\cos b\·\cos c+Y\cos b\·\sin c=X_1-Z\sin b\\ (X\sin a\·\sin b+Y\cos a)\cos c+(Y\sin a\·\sin b-X\cos a)\sin c=Y_1-Z\sin a\cos b\\ (X\cos a\·\sin b-Y\sin a)\cos c+(Y\cos a\·\sin b+X\sin a)\sin c=Z_1-Z\cos a\cos b\end{array}\right.---\left(4\right)$

由于角度矢量(a，b，c)和磁场传感器的空间角度姿态并非一一对应，为了消除这种重复的表示，需要限制各个角度的取值范围：-180°≤a＜180°，-90°≤b＜90°，-180°≤c＜180°。

将测量数据代入公式(3)和(4)，可以得到角度信息(a，b，c)为(-30.7，30，11.93)，其中(a、b、c的单位是度)，实际测量得到的(a，b，c)为(-30.0，30.0，12)，两者之间的空间角度的夹角(误差)小于1°。

2.位置定位算法

角度定位过程结束后，微控制器控制三个励磁线圈分时的通以电流(每次只有一个线圈通电)，以产生磁场，在线圈中电流稳定时，其产生的磁场为静磁场。由于线圈是由铜丝密绕而成的空芯圆环线圈，其周围磁场的可以等效成多匝圆环线圈磁场的迭加，以毕萨定理为基础可以得到励磁线圈轴线上磁感应强度的表达式：

$B=\frac{{\mathrm{\μ}}_0\mathrm{IN}}{2}\frac{R^2}{{(R^2+L^2)}^{1.5}}---\left(5\right)$

线圈参数设计如下：μ₀是真空介电常数，线圈电流I为0.17A，线圈的圈数N为1829，线圈的等效半径R为0.039m，L是磁场传感器到励磁线圈中心的距离(待求)。

通过合理设计线圈模型，可以使得线圈周围的等磁感应强度曲面接近于球面，这样对于励磁线圈轴线外的磁感应强度，也可以使用公式(5)作为近似公式，公式(5)即确定了空间中某点的磁感应强度B同该点到励磁线圈中心点距离L的近似函数关系。

实际测量中的一组数据为：

线圈1、2、3先后通电时，磁场传感器测得其所在位置O₁的磁感应强度先后为B₁＝0.0080T、B₂＝0.0064T、B₃＝0.0097T。

由公式(5)和以上测量数据分别得到L₁＝31.0cm、L₂＝25.0cm、L₃＝38.0cm，(L₁、L₂、L₃分别代表O₁到三个线圈中心O(0，0，0)、P(x_P，0，0)、Q(x_Q，y_Q，0)的距离)。

依据以上三个距离值和空间中两点间的距离公式，可以求解磁场传感器的位置坐标(x，y，z)为(9.8，9.0，28.0)，坐标单位为厘米。实际测量的坐标值为(10.5，9.7，28.0)，空间距离误差小于1cm。

Claims (4)

1.一种基于加速度传感器的内窥镜探头三维磁场定位系统，其特征在于该定位系统包括：

三个励磁线圈：即第一励磁线圈、第二励磁线圈和第三励磁线圈，三个励磁线圈由驱动电路驱动，用于确定安装在内窥镜探头上的磁场传感器的空间位置；

励磁线圈驱动电路：与数字处理电路连接，用于循环控制三个励磁线圈的通电时序；

传感器电路：包括安装在内窥镜探头上的三轴磁场传感器和三轴加速度传感器，三轴磁场传感器和三轴加速度传感器分别通过信号放大及AD转换电路连接数字处理电路，用于将三轴磁场传感器和三轴加速度传感器采集的模拟信号转换成数字信号传送给数字处理电路；

数字处理电路：用于控制传感器采集空间磁场和重力场数据，并将数据通过串口送至PC机；

PC机：通过PC机内部的软件算法计算出传感器的位置和角度姿态数据，实现内窥镜探头的定位，并进行显示。

2.根据权利要求1所述的定位系统，其特征在于所述的励磁线圈由铜丝绕制，绕线骨架采用具有很弱的逆磁性的有机材料制作，以减小对励磁线圈通电后产生的磁场的影响。

3.根据权利要求1所述的定位系统，其特征在于所述的励磁线圈驱动电路包括励磁线圈充电回路和放电回路，充电回路由励磁线圈、电阻、MOS管串联组成，电阻的作用为降低充电回路的充电时间常数，MOS管在微处理器的I/O口控制下开启和关闭充电回路；放电回路是将二极管和电阻串联后，并联在励磁线圈与电阻串联电路的两端，放电回路中电阻的作用为降低放电回路的放电时间常数，二极管的作用为在励磁线圈放电过程中正向导通，自动开启放电回路，防止产生电弧；每个励磁线圈的充电、放电回路与电源之间串联接入一个二极管，防止各个励磁线圈之间的相互干扰。

4.一种基于加速度传感器的内窥镜探头三维磁场定位方法，其特征在于该方法的步骤是：

第1、将三轴磁场传感器和三轴加速度传感器固定在内窥镜探头上，并随内窥镜探头进入人体内，三轴磁场传感器和三轴加速度传感器通过内窥镜管道内的导线与体外的数字处理电路相连，三个励磁线圈分别放置在人体躯干下方，在驱动电路的驱动下，三个励磁线圈分时地产生磁场；

第2、在磁场传感器运动的空间范围内建立一个全局坐标系O-XYZ做为基准，全局坐标系Z轴的正方向垂直水平面向上；

第3、以磁场传感器在全局坐标系O-XYZ中的初始位置O₁ (x, y, z) 为原点，以磁场传感器的三个正交传感轴X₁、Y₁、Z₁为坐标轴，建立局部坐标系O₁ -X₁Y₁Z₁，并规定局部坐标系与全局坐标系的三个轴线相互平行时为磁场传感器的初始角度姿态，磁场传感器任意角度姿态都能够通过初始角度姿态的磁场传感器绕着全局坐标系O-XYZ三个轴的方向旋转得到，即先绕着X轴方向旋转角度a，之后绕Y轴方向旋转角度b，最后绕Z轴方向旋转角度c，规定旋转时逆着全局坐标系的坐标轴看去，逆时针旋转为正方向，则磁场传感器的角度姿态用有序实数组(a, b, c)来表示；

第4、角度定位

系统上电后进入定位循环，每一个定位周期分为两个部分：角度定位和位置定位；

角度定位过程如下：

微处理器控制励磁线圈驱动电路，使得励磁线圈中的电流为0，即不产生磁场，空间中只有地磁场和重力场，地磁场和重力场可以看做稳恒场；

在全局坐标系O-XYZ中，地磁场和重力场可以分别用两个常矢量                                                

和

表示，其中X₀、Y₀、Z₀分别为地磁场在全局坐标系X、Y、Z三个轴线上的分量，由于全局坐标系Z轴竖直向上，所以重力场在全局坐标系的X、Y、Z三个轴线上分量分别为0、0、-1；微处理器控制两传感器采集数据，并对采集到的重力场和地磁场的场矢量的模做归一化处理，假如此时的角度矢量为(a, b, c)，磁场传感器的输出为，重力传感器的输出为

，利用

、

、

、

 建立方程组，求解得到角度信息(a, b, c)；

第5、位置定位

微控制器控制三个励磁线圈分时的通以电流，每次只有一个线圈通电，以产生磁场，在线圈中电流稳定时，其产生的磁场为静磁场；由于线圈是由铜丝密绕而成的空芯圆环线圈，其周围的磁场可以等效成多匝圆环线圈磁场的迭加；

线圈周围的某点的磁感应强度与该点的空间位置具有固定的关系，三个线圈的中心分别位于点O(0, 0, 0)、P(x_P, 0, 0)、Q(x_Q, y_Q, 0)；第一个线圈通电时，利用磁场传感器测得其所在位置的磁感应强度B₁，可以得到L₁，即传感器所在位置O₁到第一个线圈中心O距离；用同样的方式，第二个线圈、第三个线圈分别通电，得到L₂，L₃；L₁，L₂，L₃分别表示传感器所在位置点O₁(x, y, z)到三个线圈所在位置点O(0, 0, 0)、P(x_P, 0, 0)、Q(x_Q, y_Q, 0)的距离，依据空间两点间的距离公式即可求解磁场传感器的位置坐标(x, y, z)；

第6、定位结果显示

由PC机中的数据处理和图像显示软件将定位结果显示在屏幕上，并在屏幕上显示一个虚拟的内窥镜探头图像，该图像能够依据系统角度定位的结果，实时的反映当前内窥镜探头的角度姿态，便于操作者观察。

Images