CN101852868B - 一种基于双磁棒旋转搜索的电磁跟踪方法及系统 - Google Patents

Abstract

本发明属于电磁跟踪技术领域，具体为一种基于双磁棒旋转搜索的电磁跟踪方法及系统。本发明的电磁跟踪系统，由传感器、磁场源、旋转控制装置和控制处理显示装置组成。本发明根据磁棒指向传感器时传感器测得的磁场强度最大的特性，采用双磁棒跟踪传感器进行搜索定位，应用几何算法计算得到传感器的空间位置；并进一步依据磁棒指向传感器时传感器测出的磁场方向沿着磁棒轴线方向的原理，应用几何算法计算得到传感器的空间姿态。本发明利用磁棒旋转搜索传感器，采用非迭代算法，可以快速有效地实现对传感器位置和方向的跟踪，可靠性好、计算复杂度低。

Description

一种基于双磁棒旋转搜索的电磁跟踪方法及系统

技术领域

本发明属于电磁跟踪技术领域，具体涉及用于电磁跟踪的方法和系统。

背景技术

电磁跟踪(Electromagnetic Tracking)，或称电磁场定位，是一种利用磁场或电磁场对物体的位置和方向进行检测和实时跟踪的方法。该方法可应用于微创手术的导航，亦可运用于虚拟现实、三维超声成像等领域。典型的电磁跟踪系统一般由磁场源(如永磁铁、电磁铁线圈)、磁场传感器、控制处理单元三部分组成。传感器通常附着待定位的物体上，磁场源位于固定参考系的某几个位置。假定磁场符合某种分布模型(如无限远偶极子模型)，利用多个或多轴的磁场源和传感器的磁场耦合，根据模型估算的磁场和实测磁场的关系列方程组，采用某种最优化算法迭代地解出待测物体的空间位置和方向。

现有电磁跟踪方法的缺点之一是大都依据某种磁场分布模型。目前采用最广泛的是无限远偶极子模型，该模型仅在磁棒直径远小于磁棒与传感器距离时有效。为了解决这一问题，研究人员提出了一些改进的磁场模型，但是，磁场模型与真实的磁场分布仍然不可避免地存在区别。由于磁场源体积做不到无穷小以及磁场源线圈绕制或形状等因素不理想，磁场源产生的磁场分布无法严格符合设定的磁场模型。这一差异性严重地制约了电磁跟踪的性能和精度。现有的方法通常采用后期校准来修正这一问题，校准采用三维空间的插值实现，测量一部分真实位置，再寻找测量位置和真实位置的映射关系。校准能够在一定程度上减小模型与真实磁场差异造成的误差，但校准过程费时费力，往往在设备重新布置时需都要要进行，更关键的是无法从根本上解决问题。

现有电磁跟踪方法的另一个缺点是一般采用迭代的算法。迭代算法对磁场源和传感器之间的耦合关系要求比较简单。但是迭代的算法具有计算复杂度高、易发散、存在局部极值点等问题。这些问题极大地提高了电磁跟踪算法的复杂性，而降低了其可靠性。采用非迭代的算法，能够较大地提高电磁跟踪系统的性能。

发明内容

本发明的目的在于提出一种可靠性好、计算复杂度低的电磁跟踪方法和系统。

本发明提出的电磁跟踪方法，是一种新颖的基于双磁棒旋转搜索的电磁跟踪方法。本方法根据磁棒指向传感器时传感器测得的磁场强度最大的特性，采用双磁棒跟踪传感器进行搜索定位，应用几何算法计算得到传感器的空间位置；并进一步依据磁棒指向传感器时传感器测出的磁场方向沿着磁棒轴线方向的原理，应用几何算法计算得到传感器的空间姿态。

本方法的特点之一在于利用磁棒旋转搜索传感器，采用几何算法确定传感器的空间位置和空间姿态，而不依赖于某种假定的磁场模型。本方法的特点之二在于采用非迭代的算法，可以快速有效地实现对传感器位置和方向的跟踪。本方法的特点还包括采用脉冲直流磁场，可实现六自由度的电磁跟踪等。

本发明提出用于双磁棒搜索电磁跟踪的搜索策略。该搜索策略具有自适应调整步长的特点，能够保证磁棒迅速有效地搜索到传感器，从而实现优化的跟踪速度和跟踪范围。

本发明提出用于双磁棒搜索电磁跟踪的待测物体位置和方向算法。该方法利用两磁棒指向传感器时三者的几何关系确定传感器的位置，利用指向时传感器位置的磁场方向沿磁棒轴线方向确定传感器的方向。方法具有纯几何和非迭代的特点。

当磁棒以固定中心在空间旋转时，空间某处的传感器检测到的磁场强度在磁棒指向传感器时达到最大。本发明利用这一原理，采用两个磁棒在空间旋转搜索传感器，以计算传感器的空间位置。进一步，当磁棒指向传感器时，该处磁场的空间指向是沿着磁棒轴线的，本发明据此计算传感器的空间姿态。这样，可以通过两根磁棒旋转搜索实现对传感器的六自由度电磁跟踪。

本发明提出的电磁跟踪系统，由传感器、磁场源、旋转控制装置和控制处理显示装置组成。传感器附着于待跟踪物体。旋转控制装置控制磁棒旋转指向空间或半空间任意方向。磁棒旋转依据下文所述的旋转策略，旋转搜索传感器，当磁棒锁定传感器时，依据下文所述的位置和方向算法，计算传感器六自由度的位置和方向。控制处理显示装置一般采用一台pc机或服务器，对其他组件(如传感器、磁场源、旋转控制装置等)进行控制，并进行数据的采集和处理、跟踪结果的显示输出等。

磁棒采用脉冲直流的激励方式，这有利于消除环境金属物质造成的涡流干扰，并抵消地磁场和环境铁磁性物质产生的背景磁场干扰。对于本发明而言，其意义更在于可以保证每个时刻只有一根磁棒被激励，从而能够顺利地实现场强的峰值搜索并确定磁场方向。传感器可以采用磁阻传感器、霍尔效应传感器或磁通门传感器等。传感器的选择依据测量范围和精度的要求进行。

系统的搜索过程分为两个阶段：初始搜索阶段和实时跟踪阶段。系统开始工作时进行初始搜索，从旋转的初始指向出发执行搜索。初始搜索时，由于初始指向离传感器实际指向距离不确定，故搜索可能耗时较长。当完成初始搜索后，由于短时间内传感器运动距离有限，故磁棒当前指向距离传感器实际指向较近，可以在有限步数内完成跟踪，保证跟踪的实时性，故这一阶段称为实时跟踪阶段。由于搜索过程是三维空间中的搜索，而磁棒旋转只能在一个平面内进行，故搜索过程分两步进行，先在一个正交平面进行，即一个电机旋转搜索，寻找传感器位置磁场强度的磁棒指向，当搜索完成时，再在另一个正交平面进行，例如，先垂直搜索，再水平搜索，这样经过两次搜索，即可实现对传感器位置的锁定。在每个平面上开始搜索时，先向一个方向试探，若场强增大，则表示试探方向正确，否则反向搜索。找到正确方向后，沿该方向一步步搜索直至场强减小，则前一个搜索位置即为当前搜索平面里的最大场强方向。

搜索步长是搜索时电机每次旋转的角度，步长决定了磁棒指向的角度分辨率，进而影响了系统误差。初始搜索由于对实时性要求较低，可采用固定步长的方法，一般设定为最短的步长。实时跟踪时，由于随着传感器离磁棒距离的增加，相同角度差对应的实际距离相应增加，若在整个区域采用一致的步长，会导致在较近处步数增多，跟踪速度减慢，在较远处误差增大。为了解决这个问题，可以根据传感器当前位置设定步长，步长大小随传感器距离每个电机的距离的增大而减小。由于传感器短时间内运动距离有限，所以当前位置可以作为下一时刻位置的粗略估计，用来设定搜索步长。这种自适应步长的跟踪方法能够有效的权衡系统的跟踪速度和精度。

传感器的位置采用两个磁棒中心和传感器位置的几何关系求解。已知两磁棒指向传感器时的方向角以及两磁棒间距离，由立体几何方法可以解出传感器位置坐标。

当磁棒指向传感器时，传感器位置的磁感强度矢量是沿着磁棒轴线方向，因此，若将传感器测得的磁感强度矢量旋转至两磁棒轴线方向为z轴的坐标系，则该矢量在相应的x轴和y轴方向的分量趋近于零。换言之，磁棒激励时测得的场强，当测得值在x、y方向的分量为零时，传感器坐标z轴的指向即为磁棒轴线方向。将传感器对磁棒1和磁棒2分别做这样的处理后，即可求得传感器方向和磁棒当前指向的关系。进而，由于两磁棒当前指向已知，即可确定传感器指向与参考坐标系的关系，亦即求得传感器的空间姿态。

附图说明

图1为按照本发明的实施例的装置部分的视图。

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

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

图4为本发明的实施例的系统搜索策略框图。

图5为本发明的实施例的待测物体位置算法示意图。

图6为本发明的实施例的待测物体方向算法牵涉到的坐标系的示意图。

图7为本发明的实施例的待测物体方向算法的流程图。

图中标号：1为电磁跟踪系统，2为传感器装置，3为磁场源装置，4为旋转控制装置，5为控制处理显示装置。

具体实施方式

图1所示为根据本发明设计的电磁跟踪系统1。本电磁跟踪系统1包括四个部分：传感器装置2、磁场源装置3、旋转控制装置4、控制处理显示装置5。参考坐标系以X、Y和Z表示。传感器装置2附着于待跟踪物体。旋转控制装置4固定于参考系，磁场源装置3附着于旋转控制装置4上，可由其控制在旋转指向空间或半空间任意方向。

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

磁场源装置3由两组电磁铁线圈10和11组成，线圈分别由电路12和13控制驱动。电磁铁线圈10需要保证在相同距离情况下轴线上磁场强度最大，且该处磁场方向沿着该轴线，一般简单的棒状电磁体均可满足此要求。另外，电磁铁采用脉冲直流的激励方式，由控制处理显示装置5控制脉冲直流的激励。这里采用长10cm，直径1cm的电磁铁，线圈匝数100匝，电流1A。

旋转控制装置4包含两个电机组15和16，电机组由步进电机驱动器17驱动，驱动器由控制处理显示装置5控制。每个电机组包括两个步进电机，第一电机带动第二步进电机转动，第二电机带动磁棒转动，控制一磁棒沿两正交方向旋转，从而实现空间或半空间任意指向。推荐的旋转方式是第一电机平行于X-Z平面旋转，第二电机垂直于该平面旋转。采用步进电机的原因是可以随时获得当前磁棒的指向，不存在累积误差。两个旋转中心的距离对于系统的跟踪范围等性能有一定的影响，这里选取的距离是30cm。

控制处理显示装置5由控制单元20、算法单元21、显示输出单元22组成。控制单元20包括三个部分。脉冲直流控制14提供对磁棒激励的控制，磁棒激励采用脉冲直流方式，每周期三个时间段，前两个时间段顺序激励两根磁棒10和11，第三个时间段两磁棒均不激励。将每个磁棒激励时测得的磁场与第三时间段的磁场相减作为该磁棒在传感器位置产生的磁场强度。这种脉冲直流的激励方式有利于消除环境金属物质造成的涡流干扰，并抵消地磁场和环境铁磁性物质产生的背景磁场干扰。对于本发明而言，采用脉冲直流方式意义更在于可以保证每个时刻只有一根磁棒被激励，从而能够顺利地实现场强的峰值搜索。电机控制部分控制两个电机组的电机旋转，保证两磁棒能分别在两个正交方向上旋转，从而实现空间或半空间任意位置的指向。电机控制部分18依据下文的旋转搜索策略，根据当前时刻测得的磁场强度与之前时刻测得的磁场强度的关系，决定电机旋转方向，根据磁棒当前位置决定电机旋转步进，控制磁棒以最佳的方式搜索到传感器。采样处理部分19在脉冲直流控制部分14的同步下，采样每个时间段的磁场，并求差以获得每个磁棒在当前周期内的传感器位置的磁场强度。搜索完成后，控制部分20获得完成时的两个磁棒各自激励时的场强数据23和磁棒指向数据24，算法单元利用这些数据，依据后文的算法计算出传感器的空间位置和空间姿态，送显示输出单元22显示。

图3所示为系统的工作流程。开机后系统首先进行初始化。初始化过程25包括旋转装置的复位、系统一些基本参数的设定等。整个系统工作过程可分为搜索过程26和计算过程27。搜索过程包括初始搜索过程28和实时跟踪过程29。系统开始工作时进行初始搜索，即从旋转的初始指向开始搜索。初始搜索时，由于初始指向离传感器实际指向距离不确定，故搜索可能耗时较长。当完成初始搜索后，系统进入实时跟踪过程29，由于短时间内传感器运动距离有限，故磁棒当前指向距离传感器实际指向较近，可以在有限步数内完成跟踪，保证跟踪的实时性。无论在初始搜索过程还是实时跟踪过程，当两磁棒均锁定传感器时，系统即进入计算过程27。计算过程27依次位置计算过程30和方向计算过程31，二者依次进行，分别计算传感器三自由度的位置和三自由度的方向，其中方向计算算法是依赖于位置计算结果的。每次计算完成时，执行显示输出过程32，实时更新跟踪结果。

图4所示是初始搜索过程的框图。如前所述，搜索可分为初始搜索和实施跟踪两种情况，二者在流程上的区别主要在步长确定方面。这里的步长指的是单位时间里电机旋转的角度，实际使用时即为一个采样周期中磁棒旋转的角度，步长决定了磁棒指向的角度分辨率，进而影响了系统误差。初始搜索由于对实时性要求较低，可采用固定步长的方法，一般设定为最短的步长。实时跟踪时，由于随着传感器离磁棒距离的增加，相同角度差对应的实际距离相应增加，若在整个区域采用一致的步长，会导致在较近处步数增多，跟踪速度减慢，在较远处误差增大。因此，我们提出在实时跟踪过程中根据传感器当前位置，设定步长，步长大小随传感器距离每个电机的距离的增大而减小。由于传感器短时间内运动距离有限，所以当前位置可以作为下一时刻位置的粗略估计，用来设定搜索步长。这种自适应步长的跟踪方法能够有效的权衡系统的跟踪速度和精度。这里，初始跟踪采用的步长是0.45°；实时跟踪时，当距离小于10cm时，设定步长1.8°，在距离大于25cm时，设定步长0.45°，二者之间则设定为0.9°。

每次搜索启动时，系统先判定当前搜索过程的种类，若为初始搜索阶段，则设定步长为最小值，否则计算当前距离，并据此设定步长。首先第一电机沿正方向搜索，若场强增大，则证明搜索方向正确，否则沿反方向搜索。搜索过程每次前进一步，比较当前位置与前一个位置测得场强的大小，若发现场强减小，则说明前一个位置为场强最大位置，电机转回到这一位置，完成第一磁棒搜索。类似的，第二磁棒也经过这样一个搜索过程，找到另一正交方向上的极值点。经过两个电机的旋转搜索，即可实现磁棒指向传感器方向。

图5所示为位置算法的示意图。坐标系原点位于磁棒1中心。已知两磁棒指向传感器时的方向角(α₁，β₁)和(α₂，β₂)，两磁棒间距离d，这里的d为30cm，待求的是传感器坐标(x，y，z)。由立体几何知识可以解出传感器位置坐标：

$x=\frac{{d\tan \mathrm{\α}}_2}{{\tan \mathrm{\α}}_2-{\tan \mathrm{\α}}_1}---\left(1\right)$

$y=\frac{{d\tan \mathrm{\α}}_1\tan {\mathrm{\α}}_2}{{\tan \mathrm{\α}}_2-{\tan \mathrm{\α}}_1}---\left(2\right)$

$z=\left\{\begin{array}{c}\frac{d\tan {\mathrm{\&alpha;}}_2\tan {\mathrm{\&beta;}}_1}{\cos {\mathrm{\&alpha;}}_1{\tan \mathrm{\&alpha;}}_2-{\sin \mathrm{\&alpha;}}_1},a<b\\ \frac{d{\tan \mathrm{\&alpha;}}_1{\tan \mathrm{\&beta;}}_2}{{\sin \mathrm{\&alpha;}}_2-\cos {\mathrm{\&alpha;}}_2{\tan \mathrm{\&alpha;}}_1},a\&GreaterEqual;b\end{array}\right.---\left(3\right)$

其中，a，b分别为传感器坐标在x，y平面的投影点(x，y，0)到两磁棒的距离。

图6所示为方向算法所涉及的坐标系。坐标系F₁为系统默认坐标系，原点位于第一磁棒中心，x₁轴沿两磁棒中心连线方向，y₁轴沿磁棒初始方向，z₁轴垂直于x₁-y₁平面；坐标系F₂与坐标系F₁方向均相同，仅原点位于第二磁棒中心。坐标系F₃和F₄表示两磁棒当前姿态，z₃、z₄轴分别沿两磁棒当前指向；坐标系F₅表示传感器当前姿态。坐标系F₁和F₂仅原点位置不同，空间方向是相同的。待求的传感器空间姿态可表示为从坐标系F₁到F₅的旋转关系。在确定了传感器的空间位置后，两磁棒的指向是已知的，故坐标系F₁(或F₂)到F₃和坐标系F₁(或F₂)到F₄的旋转关系是已知的。为了进一步确定传感器的空间姿态，只需确定传感器坐标与两磁棒指向的相对空间关系，即坐标系F₃到F₅或F₄到F₅的旋转关系即可。由于第一磁棒1和第二磁棒2、坐标系F₃和F₄是对等的，不失一般性，下文算法采取计算坐标系F₃和F₅之间的旋转关系的方式。

当磁棒指向传感器时，传感器位置的磁感强度矢量是沿着磁棒轴线方向，即z₃(或z₄)方向的，因此，若将传感器测得的磁感强度矢量旋转至z₃(或z₄)方向，则该矢量在x₃、y₃(或x₄、y₄)方向的分量趋近于零。换言之，旋转传感器坐标系F₅，当测得的磁感强度矢量在x、y方向的分量为零时，传感器坐标z轴的指向即为磁棒轴线方向(z₃或z₄)，亦即当前坐标系已变换为磁棒坐标系(F₃或F₄)。将传感器对第一磁棒和第二磁棒分别做这样的处理后，即可求解坐标系F₅相对于F₃或F₄的相对空间关系，进而确定传感器的空间姿态。

图7所示为方向算法的流程图。首先须计算的是坐标系F₅到F₃的旋转关系，已知的是坐标系F₁到F₃和坐标系F₁到F₄的旋转关系。用

θ和ψ表示坐标系从F₅旋转到F₃的三个‘ZXZ’欧拉角，当两磁棒指向传感器时，将传感器磁场分别旋转到当前磁棒的坐标系(F₃和F₄)，令其在x，y方向的场强值为零，共可获得四个方程。这里，第一磁棒场强的旋转可以直接由坐标系F₅旋转到F₃，对于第二磁棒场强，为了不涉及坐标系F₅与F₄之间的旋转关系，将其旋转过程拆分为F₅到F₃的旋转，F₃到F₁的旋转和F₁到F₄的旋转三个阶段。于是所有方程仅有

θ和ψ三个未知数，选取其中任意三个即可求解。

场强B₁是传感器在第一磁棒激励时测得的场强，将其旋转到坐标系F₃后得到场强矢量B′₁，其在x₃、y₃方向上的分量为零，即

B′₁＝A₅₃B₁＝[0，0，||B₁||]^T    (4)

其中A₅₃表示从坐标系F₅到F₃的旋转矩阵

由于ψ角是绕z₃轴的转动，所以，若第一磁棒产生的场强矢量在x₃、y₃方向上为零，则该旋转角不会影响依据第一磁棒场强列出的方程。因此，我们可以利用这两个方程解出两个未知量

和θ：

这里，参量场强矢量B′₁的下标x，y，z分别表示该参量在x，y，z方向上的分量，下同。

类似的，当第二磁棒激励时，用B₂表示坐标系F₅下的实测场强，B′₂表示旋转到坐标系F₃下的场强，A₁₄表示从坐标系F₁到F₃的旋转矩阵，A₃₁表示从坐标系F₄到F₁的旋转矩阵

B′₂＝A₁₄A₃₁A₅₃B₂＝[0，0，||B₂||]^T    (7)

将已知的两个未知量带入第二磁棒的方程中的任意一个，即可求解夹角ψ。这里选取x方向的方程，代入化简得

B′_2x＝Acosψ+Bsinψ+C＝0    (8)

式中其中A、B、C均为常数。

如上所述，由方程(5)、(6)、(8)可求解三个欧拉角。以上方程的求解只涉及三个夹角的简单三角函数，可以直接利用非迭代的方法计算，算法简单可靠。

进一步，由三个欧拉角表示的坐标系F₅到F₃的旋转关系，以及坐标系F₁到F₃的旋转关系，可以计算出坐标系F₁到F₅的旋转关系，并将其表示成欧拉角或者其他期望的形式。

A₁₅＝(A₅₃A₃₁)^-1    (9)

采用仿真方法对以上实施方式的系统进行了评估。仿真采用计算磁棒周围的磁场，分析静态时的跟踪误差，和动态是的跟踪速度。设定传感器采样率为900Hz。跟踪范围为坐标系F₁下x₁∈[-15cm，45cm]、y₁∈[5cm，20cm]、z₁∈[-30cm，30cm]的空间范围内，由于磁棒长度为10cm，故同时限定跟踪位置距离两磁棒中心至少6cm。跟踪的位置误差可以通过估计位置和实际位置之间的欧氏距离ΔD评估，而方向误差则采用螺旋角Δφ。

$\mathrm{\&Delta;D}=\sqrt{{(x-x^{\&prime;})}^2+{(y-y^{\&prime;})}^2+{(z-z^{\&prime;})}^2}---\left(27\right)$

Δφ＝cos^-1[(A₁₁+A₂₂+A₃₃-1)/2]    (28)

经过5000组仿真，得到平均位置误差为0.1484cm，平均方向误差为0.5474°，最大位置误差0.7261cm，最大方向误差3.8852°，若减小步长，还可进一步提高精度。跟踪性能方面，初始跟踪平均步数230步；实时跟踪时，在瞬时位移不超过2cm时最大跟踪步数为26步。相当于，初始跟踪平均0.767s，实时跟踪最大跟踪时间0.087s。可以实现每秒10次以上的更新。若提高采样率，还可进一步提高刷新速度。

Claims (10)

1.一种基于双磁棒旋转搜索的电磁跟踪系统，其特征在于由传感器、磁场源、旋转控制装置和控制处理显示装置组成；所述传感器附着于待跟踪物体，旋转控制装置控制磁棒旋转指向空间或半空间任意方向；磁棒旋转依据旋转策略，旋转搜索传感器，当磁棒锁定传感器时，依据位置和方向算法，计算传感器六自由度的位置和方向；控制处理显示装置采用一台pc机或服务器，对其他组件进行控制，并进行数据的采集和处理、跟踪结果的显示输出。

2.根据权利要求1所述的基于双磁棒旋转搜索的电磁跟踪系统，其特征在于所述的传感器为三轴分量传感器，分别用来检测三个正交的局部方向X’、Y’和Z’的磁场；传感器输出经后续的信号调理和模拟-数字转换模块送入控制处理显示装置。

3.根据权利要求2所述的基于双磁棒旋转搜索的电磁跟踪系统，其特征在于所述的磁场源由两组电磁铁线圈组成，两组电磁铁线圈分别由两个驱动电路控制驱动；其中一组电磁铁线圈保证在相同距离情况下轴线上磁场强度最大，且该处磁场方向沿着该轴线；另外，电磁铁采用脉冲直流的激励方式，由控制处理显示装置控制脉冲直流的激励。

4.根据权利要求3所述的基于双磁棒旋转搜索的电磁跟踪系统，其特征在于所述的旋转控制装置包含两个电机组，两个电机组由步进电机驱动器驱动，驱动器由控制处理显示装置控制；每个电机组包括两个步进电机，第一步进电机带动第二步进电机转动，第二步进电机带动磁棒转动，控制一磁棒沿两正交方向旋转，实现空间或半空间任意指向。

5.根据权利要求4所述的基于双磁棒旋转搜索的电磁跟踪系统，其特征在于所述的控制处理显示装置由控制单元、算法单元、显示输出单元组成；其中，控制单元包括三个部分：(1)脉冲直流控制部分，提供对磁棒激励的控制，磁棒激励采用脉冲直流方式，每周期三个时间段，前两个时间段顺序激励两根磁棒，第三个时间段两磁棒均不激励；将每个磁棒激励时测得的磁场与第三时间段的磁场相减作为该磁棒在传感器位置产生的磁场强度；(2)电机控制部分，控制两个电机组的电机旋转，保证两磁棒能分别在两个正交方向上旋转，实现空间或半空间任意位置的指向；电机控制部分依据旋转搜索策略，根据当前时刻测得的磁场强度与之前时刻测得的磁场强度的关系，决定电机旋转方向，根据磁棒当前位置决定电机旋转步进，控制磁棒以最佳的方式搜索到传感器；(3)采样处理部分，在脉冲直流控制部分的同步下，采样每个时间段的磁场，并求差以获得每个磁棒在当前周期内的传感器位置的磁场强度；搜索完成后，控制单元获得完成时的两个磁棒各自激励时的场强数据和磁棒指向数据，算法单元根据这些数据，依据算法计算出传感器的空间位置和空间姿态，送显示输出单元显示。

6.一种利用如权利要求5所述的基于双磁棒旋转搜索的电磁跟踪系统进行电磁跟踪的方法，其特征在于步骤如下：根据磁棒指向传感器时传感器测得的磁场强度最大的特性，采用双磁棒跟踪传感器进行搜索定位，应用几何算法计算得到传感器的空间位置；并进一步依据磁棒指向传感器时传感器测出的磁场方向沿着磁棒轴线方向的原理，应用几何算法计算得到传感器的空间姿态。

7.根据权利要求6所述的基于双磁棒旋转搜索的电磁跟踪方法，其特征在于所述的搜索分为两个阶段：初始搜索阶段和实时跟踪阶段；系统开始工作时进行初始搜索，从旋转的初始指向出发执行搜索；当完成初始搜索后，由于短时间内传感器运动距离有限，故磁棒当前指向距离传感器实际指向较近，即在有限步数内完成跟踪，保证跟踪的实时性，故这一阶段称为实时跟踪阶段；搜索过程分两步进行，先在一个正交平面进行，即一个电机旋转搜索，寻找传感器位置磁场强度的磁棒指向，当搜索完成时，再在另一个正交平面进行，在每个平面上开始搜索时，先向一个方向试探，若场强增大，则表示试探方向正确，否则反向搜索；找到正确方向后，沿该方向一步步搜索直至场强减小，则前一个搜索位置即为当前搜索平面里的最大场强方向。

8.根据权利要求7所述的基于双磁棒旋转搜索的电磁跟踪方法，其特征在于在所述的初始搜索阶段采用固定步长的方法，；实时跟踪阶段时，根据传感器当前位置设定步长，步长大小随传感器距离每个电机的距离的增大而减小。

9.根据权利要求7所述的基于双磁棒旋转搜索的电磁跟踪方法，其特征在于所述应用几何算法计算得到传感器的空间位置，具体步骤如下：

已知两磁棒指向传感器时的方向角(α₁，β₁)和(α₂，β₂)，两磁棒间距离d，待求的传感器坐标为(x，y，z)，传感器位置坐标的算式为：

$x=\frac{{d\tan \mathrm{\&alpha;}}_2}{{\tan \mathrm{\&alpha;}}_2-{\tan \mathrm{\&alpha;}}_1}---\left(1\right)$

$y=\frac{{d\tan \mathrm{\&alpha;}}_1\tan {\mathrm{\&alpha;}}_2}{{\tan \mathrm{\&alpha;}}_2-{\tan \mathrm{\&alpha;}}_1}---\left(2\right)$

$z=\left\{\begin{array}{c}\frac{d\tan {\mathrm{\&alpha;}}_2\tan {\mathrm{\&beta;}}_1}{\cos {\mathrm{\&alpha;}}_1{\tan \mathrm{\&alpha;}}_2-{\sin \mathrm{\&alpha;}}_1},a<b\\ \frac{d{\tan \mathrm{\&alpha;}}_1{\tan \mathrm{\&beta;}}_2}{{\sin \mathrm{\&alpha;}}_2-\cos {\mathrm{\&alpha;}}_2{\tan \mathrm{\&alpha;}}_1},a\&GreaterEqual;b\end{array}\right.---\left(3\right)$

其中，a，b分别为传感器坐标在x，y平面的投影点(x，y，0)到两磁棒的距离。

10.根据权利要求7所述的基于双磁棒旋转搜索的电磁跟踪方法，其特征在于所述应用几何算法计算得到传感器的空间姿态，具体步骤如下：

设坐标系F₁为系统默认坐标系，原点位于第一磁棒中心，x₁轴沿两磁棒中心连线方向，y₁轴沿磁棒初始方向，z₁轴垂直于x₁-y₁平面；坐标系F₂与坐标系F₁方向均相同，仅原点位于第二磁棒中心；坐标系F₃和F₄表示两磁棒当前姿态，z₃、z₄轴分别沿两磁棒当前指向；坐标系F₅表示传感器当前姿态；坐标系F₁和F₂仅原点位置不同，空间方向相同；待求的传感器空间姿态表示为从坐标系F₁到F₅的旋转关系；在确定了传感器的空间位置后，两磁棒的指向是已知的，故坐标系F₁或F₂到F₃，和坐标系F₁或F₂到F₄的旋转关系是已知的；为了进一步确定传感器的空间姿态，下面计算坐标系F₃和F₅之间的旋转关系：

首先须计算坐标系F₅到F₃的旋转关系，已知坐标系F₁到F₃和坐标系F₁到F₄的旋转关系，用

θ和ψ分别表示坐标系从F₅旋转到F₃的三个‘ZXZ’欧拉角，当两磁棒指向传感器时，将传感器磁场分别旋转到当前磁棒的坐标系F₃和F₄，令其在x，y方向的场强值为零，共获得四个方程；这里，第一磁棒场强的旋转可以直接由坐标系F₅旋转到F₃，对于第二磁棒场强，为了不涉及坐标系F₅与F₄之间的旋转关系，将其旋转过程拆分为F₅到F₃的旋转，F₃到F₁的旋转和F₁到F₄的旋转三个阶段；于是所有方程仅有

θ和ψ三个未知数，选取其中任意三个即可求解；

设场强B₁是传感器在第一磁棒激励时测得的场强，将其旋转到坐标系F₃后得到场强矢量B′₁，其在x₃、y₃方向上的分量为零，即

B′₁＝A₅₃B₁＝[0，0，||B₁||]^T    (4)

其中A₅₃表示从坐标系F₅到F₃的旋转矩阵；

由于ψ角是绕z₃轴的转动，所以，若第一磁棒产生的场强矢量在x₃、y₃方向上为零，则该旋转角不会影响依据第二磁棒场强列出的方程；因此，可以利用这两个方程解出两个未知量

和θ：

类似的，当第二磁棒激励时，用B₂表示坐标系F₅下的实测场强，B′₂表示旋转到坐标系F₃下的场强，A₁₄表示从坐标系F₁到F₃的旋转矩阵，A₃₁表示从坐标系F₄到F₁的旋转矩阵

B′₂＝A₁₄A₃₁A₅₃B₂＝[0，0，||B₂||]^T    (7)

将已知的两个未知量带入第二磁棒的方程中的任意一个，即可求解夹角ψ，这里选取x方向的方程，代入化简得：

B′_2x＝Acosψ+Bsinψ+C＝0    (8)

式中其中A、B、C均为常数；

由方程(5)、(6)、(8)可求解三个欧拉角；

进一步，由三个欧拉角表示的坐标系F₅到F₃的旋转关系，以及坐标系F₁到F₃的旋转关系，可以计算出坐标系F₁到F₅的旋转关系，并将其表示成欧拉角的形式：

A₁₅＝(A₅₃A₃₁)^-1    (9)。

Images