基于微动平台的磁跟踪系统的磁场源标定方法及系统

摘要

本发明属于电磁测量技术领域，具体为一种基于三轴微动平台的磁跟踪系统的磁场源原点及初始姿态标定方法及系统。本发明的标定系统，由三轴微动平台、三轴磁传感器、两个可旋转并实现空间任意指向的磁场源和控制处理显示装置组成，可实现磁场源之间相对位置及姿态的标定。具体标定方法是，由三轴微动平台控制磁传感器移动至选定的5个已知坐标点；再利用电磁跟踪系统对5个点的空间位置定位，获得磁场源与这些点之间的空间位置信息，然后计算出磁场源之间初始相对位置及姿态。根据标定结果对基于旋转磁场的磁跟踪算法进行校正，可显著提高定位精度。

说明书

技术领域

本发明属于电磁测量技术领域，具体涉及电磁跟踪系统中磁场源相对位置及初始姿态的标定方法与系统。

背景技术

电磁跟踪（Electromagnetic Tracking），或称电磁场定位，是一种利用磁场或电磁场对物体的空间位置和姿态进行检测和实时跟踪的方法。该方法可应用于微创手术的导航，亦可运用于虚拟现实、三维超声成像等领域。在含有2个或2个以上磁场源的电磁跟踪系统中，定位/跟踪算法通常要用到磁场源之间的空间位置关系，且位置关系是否准确直接影响跟踪精度，因此需要对电磁跟踪系统磁场源的空间位置进行标定。

目前常用的标定方法是光学标定。由于光学定位系统的精度高，可以得到较高的标定精度。但当在光源与标定目标之间有障碍物，或者标定目标隐藏在黑盒子中时，就不能使用光学定位系统进行标定。对于电磁跟踪系统而言，使用光学定位系统很难对三个正交线圈的原点进行标定。因此光学定位系统标定的实际应用有一定局限。

发明内容

本发明的目的在于提出一种基于三轴微动平台的磁场源空间相对位置及初始姿态的标定方法和系统，用于含有一个以上磁场源的电磁跟踪系统的标定。

本发明提出的标定系统由下述四个部分组成：磁场源、三轴磁传感器装置、控制处理显示装置以及三轴微动平台；其中：

所述磁场源为2个，均由正交三轴线圈和对应的三个控制驱动电路组成；正交三轴线圈的三个电磁铁线圈几何中心重合；磁场源2的原点在磁场源1的X轴上，两磁场源原点之间的距离为d，两磁场源的Y轴和Z轴相互平行；构成磁场源的电磁铁采用恒流激励方式，由控制处理显示装置控制每组激励电流强度；正交三轴线圈用以模拟空间任意指向的磁棒； 

所述磁传感器装置固定于三轴微动平台，由三轴微动平台控制其在空间精确移动，测量三个正交方向的磁感应强度；所述三轴磁场传感器装置包括：三轴分量传感器、一个信号调理和模拟-数字（AD）转换模块，三轴分量传感器分别用来检测三个正交方向X’、Y’和Z’的磁感应强度，其输出经后续的信号调理和模拟-数字（AD）转换模块送入控制处理显示装置，由控制处理显示装置中的采样处理模块采样处理； 

所述控制处理显示装置由控制单元、算法单元、显示输出单元和三轴微动平台控制单元组成，其中：

所述控制单元包括三个部分：采样处理模块、激励电流强度控制模块和微动平台控制模块；所述采样处理模块是微处理器自带的I/O口，用于采样处理来自信号调理和模拟-数字转换模块的信号；所述激励电流强度控制模块是微处理器根据计算出的模拟磁棒旋转角度所对应的磁场源三个正交线圈的激励电流，通过I/O口控制磁场源的控制驱动电路，提供对三轴正交线圈的激励控制，合成所需指向的模拟磁棒；所述微动平台控制模块是微处理器通过I/O口输出信号，根据要求控制微动平台三个轴的移动，并由此带动固定在微动平台上的磁传感器移动至选定的座标点（例如下文的n1、n2、n3（与m1重合）、m2、m3五个点）上；

所述算法单元在微处理器内部，可根据模拟磁棒旋转搜索并最终指向传感器过程中获得的旋转角度信息，用原点坐标标定算法，计算磁场源1、磁场源2的原点坐标；用初始姿态标定算法，计算磁场源1、磁场源2的各坐标轴的初始指向；

所述显示输出单元为与微处理器相连的液晶显示器和I/O口，用于将由算法单元计算得到的标定结果输出并在显示器上显示出来；

所述三轴微动平台控制单元在微处理器内部，根据要求通过I/O口输出的指令对微动平台三个轴的移动进行精确控制；

所述三轴微动平台由三条相互正交的轨道及可在轨道上精确移动的固定装置组成，用于控制三轴磁传感器在空间精确移动。

本发明中，所述传感器可以选用磁阻传感器、霍尔效应传感器或磁通门传感器等，用于测量空间三个正交方向的磁感应强度。

为了更好地描述本发明，根据右手螺旋法则定义了如表1所示的七个坐标系，其中CS1为全局坐标系，CS6为微动平台坐标系，也是定标时的参照坐标系。

表1 坐标系定义

坐标系名称缩写描述坐标系1 CS1磁场源1的原始坐标系坐标系2 CS2磁场源2的理想原始坐标系坐标系 3 CS3当磁场源1的x轴（磁棒轴线方向）指向传感器时，磁场源1的实际坐标系坐标系4 CS4当磁场源2的x轴（磁棒轴线方向）指向传感器时，磁场源2的实际坐标系坐标系5 CS5三轴磁传感器坐标系坐标系6 CS6微动平台坐标系坐标系7 CS7磁场源2的实际原始坐标系

本发明提出的使用上述基于三轴微动平台的磁场源空间相对位置及初始姿态的标定系统的标定方法，具体步骤如下：

步骤一、将标定系统初始化；

步骤二、由三轴微动平台控制三轴磁传感器装置移向空间已知坐标点；

步骤三、以磁场源1和磁场源2所对应的模拟磁棒，搜索并指向所述三轴磁传感器装置；

步骤四、记录磁场源1、磁场源2所模拟的磁棒从初始位置到指向三轴磁传感器装置过程中的水平和垂直旋转角； 

步骤五、判断三轴微动平台是否已将三轴磁传感器装置移至空间五个选定的已知坐标点，如果为否，则重复步骤三、四；如果为是，则转步骤六；

步骤六、在步骤五为肯定结果的条件下，运用磁场源原点坐标标定算法，计算磁场源1、磁场源2的原点坐标；

步骤七、运用磁场源初始姿态标定算法，计算磁场源1、磁场源2的各坐标轴的初始指向。

本发明方法的第一步，是在开机后，由控制处理显示装置控制微动平台、磁场源1和磁场源2移动/转动至各自的初始位置；具体为微动平台的三个轴均移动至坐标原点，磁场源1和磁场源2转动至两者的X轴相互平行，Y轴相互平行，Z轴相互平行（此为理想情况，实际可能有误差，这也是本发明姿态标定的目的）。

本发明方法的第二步，是由微动平台控制传感器移向空间已知坐标点。所述数控三轴微动平台包括相互正交的x,y,z三个轨道，定位精度达0.1mm。利用该微动平台，将磁传感器分别放置在n1、n2、n3（与m1重合）、m2、m3五个点上，并通过后续步骤，由磁场源1、磁场源2对这五个点进行定位。在CS6坐标系中，对这五个点的要求如下：

 n3和m1重合；

 n1、n2、n3在一条直线上，且n1、n2之间的距离与n2、n3之间的距离相等；

 m1、m2、m3在不与n1、n2、n3平行的另一条直线上，且m1、m2之间的距离与m2、m3之间的距离相等。

本发明方法的第三步，是当传感器到达n1、n2、n3（与m1重合）、m2、m3中的某一点时，由磁场源1、磁场源2对其进行定位。具体做法是由控制处理显示装置分别以恒流交替激励的方式激励组成磁场源的正交三轴线圈；所述的正交三轴线圈需满足三个线圈的几何中心重合，且三个线圈的轴线相互正交的条件；所述的恒流激励交替激励方式，是将每个激励周期分为至少两个时间段，在恒流激励周期的第一个时间段，在x轴发射线圈上给予幅度为Asin(φ)安培的激励电流，在y轴发射线圈上给予幅度Acos(φ)安培的激励电流，以合成大小为A安培，在x-y平面旋转φ度的磁棒，控制φ在0-2π变化，当模拟磁棒指向传感器在x-y平面投影方向时，传感器可检测到最大磁感应强度，此时模拟磁棒的旋转角度为φ_ij（其中i=1-2，对应磁场源1和磁场源2；j=1-5对应n1、n2、n3(或m1)、m2、m3五个坐标已知点）；之后，x轴发射线圈上给予幅度为Asin(φ_ij)* sin(θ)安培的激励电流, y轴发射线圈上给予幅度为Acos(φ_ij) * sin(θ)安培的激励电流，此时在z轴发射线圈上给予幅度为A cos (θ)安培的激励电流，即可合成在垂至于x-y平面旋转的模拟磁棒；控制θ 在0-π范围内变化，当模拟磁棒指向传感器时，传感器可检测到最大磁感应强度，此时模拟磁棒在垂直平面的旋转角度为θ_ij。记录此时的旋转角度θ_ij。

本发明方法的第四步，是记录磁场源1、2所模拟的磁棒从初始位置到指向传感器过程中的水平旋转角φ_ij（即模拟磁棒在x-y平面由初始位置旋转至指向传感器在该平面投影时的旋转角度）和垂直旋转角θ_ij（即模拟磁棒在垂直于x-y的平面上由初始位置旋转至指向传感器时的旋转角度）。

本发明方法的第五步，完成磁场源对位于n1、n2、n3（与m1重合）、m2、m3五个点的传感器分别定位。

本发明方法的第六步，是运用磁场源原点坐标标定算法，计算磁场源1、2的原点坐标（x_i,y_i,z_i）；为此，可将磁传感器放置在已知空间坐标的三个点上，即可建立反映模拟磁棒与磁传感器之间几何关系的三个方程组成的方程组，进而解出磁棒旋转中心坐标。但三方程方程组不一定有解，本发明设计了建立5个方程组的方案，在不过多增加方法复杂度的情况下，提高算法的稳定性。不同个数磁场源的标定方案可以由本发明类推得到。

参照附图4（仅画出了磁场源1，用相同方法可画出磁场源2）由方程组（1）即可计算出磁场源1、2的原点在CS6中的坐标（x1,y1,z1）、（x2,y2,z2）：

    （1）

其中（n1x, n1y, n1z）是n1点在CS6中的坐标，其余依此类推。r_i1、r_i2、r_i3、r_i5、r_i6可由式（2）～（6）计算。

    （2）

    （3）

    （4）

   (5)

   (6)

式（2）～（6）中的α_i1、α_i2、α_i3、α_i4可由式（7）计算得到：

（7）

式（7）中的P_ij、q_ij 和 k_ij可由方程组（8）计算得到：

    （8）

本发明方法的第七步，是实现磁场源初始姿态的标定。将CSr到CSs之间的旋转关系表示为R_rs，r=1-7，s=1-7（表示表1所定义的七个坐标系）。在表1定义的坐标系中，CS1是磁场源1的原始坐标系，也是系统坐标系。CS2是磁场源2理想的原始坐标系，其y、z轴与CS1的y、z轴平行，x轴与CS1的x轴重合，即CS2的坐标系原点在CS1的x轴上，但不与CS1的坐标系原点重合；但在实际安置磁场源时，两个磁场源之间的关系可能并不能达到上述理想情况，因此又定义了磁场源2的实际原始坐标系CS7，则对磁场源初始姿态的标定转化成寻找磁场源2理想原始坐标系CS2与其当前坐标系CS4之间的旋转关系，即获得R₂₄。

CS1到CS3的旋转矩阵可以利用测量得到的φ₁₁和 θ₁₁由式（9）计算得到：

    (9)

同样可以由式（10）计算CS7到CS4的旋转矩阵：

     (0)

假设把CS6的原点移动到CS1的原点，并使CS6的x轴指向传感器，可由式（11）求得CS6到CS3旋转矩阵：

     (11)

式(11)中，ω₀’，θ₀’和φ₀’ 为假设CS6的x轴指向传感器时所需旋转的欧拉角。为了求解ω₀’，移动传感器，获得另外一个标测点，以便联立成方程。保持CS6与CS1的坐标原点重合，并使CS6的x轴指向移动后的传感器，可由公式（12）求得CS6到CS3旋转矩阵：

    (12)

由于CS1与CS6的旋转矩阵是不变的， 即：

    (13)

将式（11)和(12)代入 (13)可以得到： (14)

求解方程组(14)，可以得到 和，代入式(13)中可以得到R₆₁。

同理，假设把CS6的原点移动到CS7的原点，并使CS6的x轴指向传感器，可由式（15）得到CS6到CS4的旋转矩阵，其中ω₆’，θ₆’和φ₆’ 为假设CS6的x轴指向传感器时所需旋转的欧拉角：

    (15)

    (16)

再根据式（17）和（18）可得式（19）：

    (17)

    (18)

    (19)

由式(10)和式(19) 可以推出式（20）：

    ()

此时已经得到CS2和CS4之间的旋转关系，通过分解式（20）中的R₂₄，即通过式（21）可以得到校正后的旋转角度, ，：

    ()。

根据标定结果对基于旋转磁场的磁跟踪算法进行校正，从而提高定位精度。   

本发明借助三轴微动平台，同时利用电磁跟踪系统本身的定位功能实现标定，可不受标定目标空间位置限制，显著提高电磁跟踪系统定位精度。

附图说明

图1为本发明实施例的系统组成。

图2为图1中的装置的细节框图。

图3为本发明的实施例的系统工作流程框图。

图4为本发明的标定原理。

具体实施方式

本实施例以包括两个磁场源（磁场源1和磁场源2），更多磁场源的情况可以类推得到），且磁场源采用直流脉冲方式激励的磁跟踪系统为例进行说明。图1所示为根据本发明设计的基于三轴微动平台的磁场源相对空间位置及初始姿态的标定系统，包括五个部分：磁场源1、磁场源2、三轴磁场传感器装置3、控制处理显示装置4以及三轴微动平台5。传感器装置3固定在三轴微动平台5上，由微动平台5控制其精确移动。磁场源1、磁场源2的空间位置固定，两者原点之间的距离为d，且理想情况下的初始姿态为：磁场源2的原点在磁场源1的x轴上，两磁场源的各坐标轴相互平行。控制处理显示装置4输出直流脉冲电流，激励磁场源1、磁场源2的三轴线圈，模拟空间任意指向的磁棒，通过同时改变输出到磁场源1、磁场源2三轴上的直流脉冲电流强度，实现磁棒的旋转。

系统各部分的分解框图如图2所示。传感器装置3选用三轴磁阻传感器，包括三轴分量传感器6、7和8，分别用来检测三个正交方向X’、Y’和Z’的磁感应强度。传感器输出经后续的信号调理和模拟-数字（AD）转换模块9送入控制处理显示装置4。

磁场源1、2均由正交三轴线圈装置组成，下面以磁场源1为例进行说明（磁场源2的情况类同，不再赘述）。磁场源1由三组电磁铁线圈10、11和12组成,线圈分别由电路13、14和15控制驱动。要求三组线圈几何中心重合，且要使三轴线圈合成的磁感应强度在相同距离情况下轴线处最大，且该处磁场方向沿着轴线。在本实施例中，三轴线圈绕在边长为2.5cm的立方体上，每个线圈的匝数为1000匝。另外，电磁铁由控制处理显示装置4输出的脉冲直流激励。 

控制处理显示装置4由控制单元18、算法单元19、显示输出单元20组成。其中：

控制单元18包括三个部分：采样处理模块16、激励电流强度控制模块17和微动平台控制模块29；其中：

激励电流强度控制模块17采用直流脉冲控制，是控制驱动电路13、14、15实现对构成磁场源1的三轴正交线圈10、11和12的激励，在本实施例中，磁棒激励采用脉冲直流方式，每周期分三个时间段，第一个时间段激励三轴正交线圈10、11和12合成模拟磁棒1；第二个时间段激励构成磁场源2的三个正交线圈（未画出）合成模拟磁棒2；第三个时间段不激励，将模拟磁棒激励时磁传感器测得的磁感应强度与第三时间段测得的磁感应强度相减作为模拟磁棒在传感器位置产生的磁感应强度。这种脉冲直流的激励方式有利于消除环境金属物质造成的涡流干扰，并抵消地磁场和环境铁磁性物质产生的背景磁场干扰。对于磁场源1而言（磁场源2的情况相同，不再赘述）在脉冲直流激励周期的第一个时间段，在x轴发射线圈10上给予幅度为Asin(φ)安培的激励电流，在y轴发射线圈11上给予幅度为Acos(φ)安培的激励电流，在z轴发射线圈12上给予0安培的激励电流，以合成大小为A安培，在x-y平面旋转φ度的磁棒，控制φ在0-2π变化，当模拟磁棒指向传感器在x-y平面投影方向时，传感器可检测到最大磁感应强度，记录此时的旋转角度φ_1j。之后，x轴发射线圈10上给予幅度为Asin(φ_1j)* sin(θ)安培的激励电流, y轴发射线圈11上给予幅度为Acos(φ_1j) * sin(θ)安培的激励电流（φ_1j即模拟磁棒在x-y平面指向传感器时的旋转角度），同时在z轴发射线圈12上给予幅度为A cos (θ)安培的激励电流，即可合成在垂至于x-y平面旋转的模拟磁棒；控制θ 在0-π（0-π/2）范围内变化，当模拟磁棒指向传感器时，传感器可检测到最大磁感应强度，此时的垂直旋转角记为θ_1j。在上述方法中，通过改变φ、θ即可实现模拟磁棒在空间的任意指向，并使其最终指向磁传感器；

微动平台控制模块29，是控制微动平台X轴23、Y轴24和Z轴15的移动，并由此带动固定在微动平台5上的磁传感器26移动至选定的n1、n2、n3（与m1重合）、m2、m3五个点上；

采样处理模块16是采集来自磁传感器26三个轴6、7、8所检测到的，经信号调理和AD转换9数字化了的磁感应强度信号，并通过矢量求和的方式将这些信号合成为一个具有大小和方向的磁感应强度矢量。

算法单元19根据上述步骤记录到的磁场源1、磁场源2从初始位置旋转搜索指向传感器的旋转角度φ_ij、θ_ij，由发明内容中的式（1）-（8），计算出磁场源1和磁场源2的原点在CS6坐标系中的坐标，实现对磁场源原点的标定；根据发明内容中的式（9）-(21)获得磁场源1和磁场源2初始位置坐标之间的旋转关系，实现对磁场源初始姿态的标定。

显示输出单元20则将算法单元19得到的标定结果进行输出和显示。

图3为系统工作流程框图，说明了实现标定的各个环节及顺序，通过步骤30-36，最终实现对磁场源原点及初始姿态的标定。

图4所示为实现磁场源原点位置标定算法32的原理图。本发明以对磁场源1的原点标定为例说明，用相同方法可对更多磁场源（包括磁场源2）的原点进行标定。磁场源1的原点位于构成磁场源1的三个正交线圈的共同中心，即图4所示的O1点。根据在测量过程37中记录到的φ_ij、θ_ij，根据式（1）－（8）即可求得磁场源1在CS6坐标系中的坐标（x1,y1,z1）；同理可求得磁场源2在CS6坐标系中的坐标（x2,y2,z2）。

磁场源初始姿态标定算法模块36的目的，是在系统包含多个磁场源的情况下，获得各磁场源初始姿态之间的旋转关系。在本实施例中，是获得磁场源1与磁场源2的初始姿态之间的旋转关系。为此，定义了表1所示的7个坐标系，将初始姿态的标定问题转化为求解坐标系之间的旋转关系。本发明根据式（9）－（21）的旋转过程，最终得到CS4到CS2之间的旋转关系R₂₄。

至此，即完成了对由多个磁场源组成的电磁跟踪系统磁场源原点坐标和初始姿态的标定。标定结果可用于修正电磁跟踪系统的定位算法，显著提高系统的定位精度。