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

Abstract

本发明属于电磁跟踪技术领域，具体为一种基于正交磁棒旋转搜索的电磁跟踪系统及方法。本发明的系统，由三轴磁场传感器装置、可以模拟空间任意指向的正交三轴线圈装置和控制处理显示装置组成。本发明采用直流脉冲交替激励两个正交磁棒，依据磁棒轴线上磁场强度最大且方向沿着轴线的原理，旋转正交磁棒组进行搜索，使三轴磁场传感器测得的主磁棒的磁场强度最大，此时主磁棒指向传感器，进一步通过一维磁场标定和非迭代算法，计算传感器的位置和姿态。本发明的搜索策略采用自适应步长先后在主磁棒所在的两个正交平面内旋转搜索，可快速跟踪传感器位置和方向。本发明可靠性好、计算复杂度低，可应用于微创手术的导航，亦可运用于虚拟现实、三维超声成像等领域。

Description

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

技术领域

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

背景技术

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

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

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

发明内容

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

本发明提出的电磁跟踪系统，由三部分构成：一个三轴磁场传感器装置、一个可以模拟空间任意指向的正交三轴线圈装置、一个控制处理显示装置。传感器装置附着于待跟踪物体，用于测量三个正交方向的磁场，可以采用磁阻传感器、霍尔效应传感器或磁通门传感器等。传感器的选择依据测量范围和精度的要求进行。正交三轴线圈由控制处理显示装置提供直流脉冲激励，用以模拟轴线所在方向为空间任意方向的两个正交的磁棒；通过同时改变输出到正交线圈三轴上的直流脉冲电流大小，实现磁棒轴线所在方向的改变，即磁棒的旋转。磁棒旋转依据下文所述的搜索策略，旋转搜索传感器，当磁棒锁定传感器时，依据下文所述的位置和姿态算法，计算传感器六自由度的位置和姿态。磁棒采用脉冲直流的激励方式，这有利于消除环境金属物质造成的涡流干扰，并抵消地磁场和环境铁磁性物质产生的背景磁场干扰。对于本发明而言，其意义更在于可以保证每个时刻只有一根磁棒被激励，从而能够顺利地实现场强的峰值搜索。控制处理显示装置一般由嵌入式系统（或其它微处理器）和显示器组成，其功能是实现其他组件的控制、数据的采集和处理、跟踪结果的显示输出等工作。其结构如图2所示。 

本发明中，所述三轴磁场传感器装置2包括三轴分量传感器5、6、7，和一个信号调理和模拟-数字（AD）转换模块8，三轴分量传感器5、6、7分别用来检测三个正交方向X’、Y’和Z’的磁场，其输出经后续信号调理和模拟-数字（AD）转换模块8送入控制处理显示装置4，由控制处理显示装置4中的采样处理模块16采样处理。

本发明中，所述正交三轴线圈装置3包括：三组电磁铁线圈9、10、11,和3个控制驱动电路12、13和14，三组电磁铁线圈9、10和11分别由控制驱动电路12、13和14控制驱动。三组电磁铁线圈几何中心重合，且为保证三轴线圈合成的电磁铁线圈在相同距离情况下轴线上磁场强度最大，且该处磁场方向沿着轴线，这里三轴线圈均为简单的棒状电磁体。另外，电磁铁采用脉冲直流的激励方式，由控制处理显示装置4控制脉冲直流的激励。

本发明中，所述控制处理显示装置4由控制单元17、算法单元18、显示输出单元19组成。控制单元17包括两个部分：采样处理模块16和脉冲直流控制模块15。采样处理模块16用于采样处理来自信号调理和模拟-数字（AD）转换模块8的信号；脉冲直流控制模块15提供对三轴正交线圈9、10和11的激励控制；由于电磁铁采用脉冲直流的激励方式，可将脉冲直流的激励的每周期分三个时间段，前两个时间段分别激励三轴正交线圈9、10和11合成主磁棒和副磁棒，第三个时间段三轴正交线圈均不激励。将主、副磁棒合成激励时测得的磁场与第三时间段的磁场相减作为对应磁棒在传感器位置产生的磁场强度。这种脉冲直流的激励方式有利于消除环境金属物质造成的涡流干扰，并抵消地磁场和环境铁磁性物质产生的背景磁场干扰。

本发明提出的基于上述的电磁跟踪系统的电磁跟踪方法，其步骤为，通过直流脉冲激励方式交替激励由正交三轴线圈装置3模拟的两个可旋转的正交磁棒，分别产生磁场分布情况已知的电磁场；用三轴磁场传感器装置2测量其所在位置的磁场大小和方向，用控制处理显示装置4根据搜索策略进行搜索，并计算求得其六自由度的空间位置和空间姿态；其中：

所述的直流脉冲激励方式交替激励，是将每个激励周期分三个时间段，在脉冲直流激励周期的第一个时间段，在x轴发射线圈上给予sinαcosβ安培的激励电流，在y轴发射线圈上给予cosαcosβ安培的激励电流，在z轴发射线圈上给予sinβ安培的激励电流，以合成大小为1安培电流激励的（若需改变电流强度，可按比例调整各轴的激励电流强度）、从x轴旋转水平角度α、垂直角度β的主磁棒；在脉冲直流激励周期的第二个时间段，在x轴发射线圈上给予sin(α+90°)安培的激励电流，在y轴发射线圈上给予cos(α+90°)安培的激励电流，在z轴发射线圈上给予0安培的激励电流，用于合成与前一时间段合成主磁棒相正交的副磁棒；在脉冲直流激励周期的第三时间段，各轴线圈均不给予激励电流；将各磁棒激励时测得的磁场与第三时间段的磁场（环境磁场强度）相减作为对应磁棒在传感器位置产生的磁场强度；

所述的搜索策略为正交磁棒组的主磁棒在两个正交平面内依次旋转，副磁棒始终与即时的主磁棒保持正交；根据磁棒指向传感器装置时传感器测得的磁场强度最大的特性，锁定主磁棒指向传感器的位置；其中磁棒旋转角度采用自适应步长，即每次旋转的角度根据传感器距离正交磁棒组的距离的增大而减小。

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

本发明的搜索策略具有自适应调整搜索步长的特点。搜索步长是主副磁棒每次旋转的角度，步长决定了磁棒指向的角度分辨率，进而影响了系统误差。初始搜索由于对实时性要求较低，可采用固定步长的方法，一般设定为系统最短的步长（该最短的步长可根据系统对定位精度和磁场变化的分辨率确定）。实时跟踪时，由于随着传感器离正交磁棒组距离的增加，相同角度差对应的实际距离相应增加，若在整个区域采用一致的步长，会导致在较近处步数增多，跟踪速度减慢，在较远处误差增大。为了解决这个问题，可以根据传感器当前位置设定步长，步长大小随传感器与正交磁棒组之间距离的增大而减小。由于传感器短时间内运动距离有限，所以当前位置可以作为下一时刻位置的粗略估计，用来设定搜索步长。这种自适应步长的跟踪方法能够有效的平衡系统的跟踪速度和精度，保证主磁棒迅速有效地搜索到传感器，实现跟踪速度和跟踪范围的优化。

本发明的电磁跟踪方法中，所述的求空间位置的算法如下：根据主磁棒在两正交平面内旋转的角度α、β和采用一维磁场标定求得的传感器与磁棒之间的距离r，传感器三自由度的空间位置为：x =r cosαcosβ，y =r sinαcosβ，z =r sinβ。

本发明的电磁跟踪方法中，所述的求空间姿态的算法如下：采用非迭代算法，将在传感器坐标系F₃测得的主、副磁棒磁场强度，旋转变换到当前正交磁棒坐标系F₂，主磁棒y轴、z轴上的磁场强度分量为0，                                                

，副磁棒x轴、z轴上的磁场强度分量为0，

，从而求得旋转矩阵A₃₂，又已知原始坐标系F₁经过水平旋转α、垂直旋转β后得到当前正交磁棒坐标系F₂，得旋转矩阵A₁₂，从而根据

的关系，计算传感器三自由度的空间姿态A₁₃，所有旋转矩阵转换成欧拉角的形式求解；其中，

是传感器在主磁棒激励时测得的坐标系

下的场强，

是将

旋转到坐标系

后得到场强，

是传感器在副磁棒激励时测得的坐标系

下的场强，是

旋转到坐标系

下的场强。

本发明提出的用于正交磁棒搜索电磁跟踪的待测物体空间位置和姿态算法，是利用主磁棒指向传感器时传感器测得的主磁棒磁场强度最大的特点进行搜索定位。当主磁棒锁定传感器后，对此时的主磁棒磁场强度进行一维磁场标定，计算传感器的空间位置。然后，依据当主磁棒指向传感器时传感器处的主磁棒磁场方向沿着主磁棒轴线方向、同时副磁棒的磁场方向与副磁棒轴线方向相反的原理，将传感器测得的主、副磁棒磁感强度矢量分别旋转至主磁棒为x轴、副磁棒为y轴的坐标系中，则旋转变换后的主磁棒磁场强度矢量在相应的y轴和z轴方向的分量趋近于零，旋转变换后的副磁棒磁场强度矢量在相应的x轴和z轴方向的分量也趋近于零。列方程计算传感器的空间姿态。由此实现传感器六自由度的电磁跟踪。该方法具有不依赖磁场模型和非迭代的特点。

本发明可靠性好、计算复杂度低，可应用于微创手术的导航，亦可运用于虚拟现实、三维超声成像等领域。

附图说明

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

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

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

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

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

图6为本发明的实施例的待测物体姿态算法涉及的坐标系的示意图。

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

图中标号：1为电磁跟踪系统，2为三轴磁场传感器装置，3为正交三轴线圈装置，4为控制处理显示装置。5、6、7分别为传感器装置的三轴分量传感器，8为数字转换模块，9、10、11分别为正交三轴线圈装置3的三组电磁铁线圈, 12、13、14分别为三组电磁铁线圈线圈9、10、11的控制驱动电路，15为脉冲直流控制模块，16为采样处理模块，17为控制处理显示装置4的控制单元，18为算法单元，19为显示输出单元；20为场强数据流，21为主磁棒指向数据流，22为初始化过程模块，23为搜索过程模块，24为计算过程模块，25为初始搜索过程模块，26为实时跟踪过程模块，27为位置计算过程模块，28为姿态计算过程模块，29为执行显示输出过程模块。

具体实施方式

图1所示为根据本发明设计的电磁跟踪系统1。电磁跟踪系统1包括三个部分：三轴磁场传感器装置2、正交三轴线圈装置3、控制处理显示装置4。传感器装置2附着于待跟踪物体。正交三轴线圈装置3固定于参考坐标系，且三个正交线圈轴线方向分别与参考坐标系X轴、Y轴、Z轴一致，正交三轴线圈中心与参考坐标系原点重合。控制处理显示装置4输出直流脉冲电流，激励正交三轴线圈，模拟空间任意方向的主、副磁棒，通过同时改变输出到正交线圈三轴上的直流脉冲电流大小，实现磁棒的旋转。

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

正交三轴线圈装置3一般由三组电磁铁线圈9、10和11组成,线圈分别由电路12、13和14控制驱动。要求三组线圈几何中心重合，且为保证三轴线圈合成的电磁铁线圈在相同距离情况下轴线上磁场强度最大，且该处磁场方向沿着轴线，这里三轴线圈均为简单的棒状电磁体，长10cm，直径1cm的电磁铁，线圈匝数100匝。另外，电磁铁采用脉冲直流的激励方式，由控制处理显示装置4控制脉冲直流的激励。

控制处理显示装置4由控制单元17、算法单元18、显示输出单元19组成。控制单元17包括两个部分。脉冲直流控制15提供对三轴正交线圈9、10和11的激励控制，磁棒激励采用脉冲直流方式，每周期分三个时间段，前两个时间段分别激励三轴正交线圈9、10和11合成主磁棒和副磁棒，第三个时间段三轴正交线圈均不激励。将主、副磁棒合成激励时测得的磁场与第三时间段测得的磁场相减作为对应磁棒在传感器位置产生的磁场强度。这种脉冲直流的激励方式有利于消除环境金属物质造成的涡流干扰，并抵消地磁场和环境铁磁性物质产生的背景磁场干扰。

对于本发明而言，采用脉冲直流方式意义更在于可以保证每个时刻只有一根磁棒同时激励，从而能够顺利地实现场强的峰值搜索。假设沿着参考坐标系x轴正方向为合成的主磁棒的初始方向，y轴正方向为合成的副磁棒的初始方向。在脉冲直流激励周期的第一个时间段，为合成大小为1安培电流激励的、在初始方向上旋转水平角度α、垂直角度β后的主磁棒，须在x轴发射线圈9上给予sinαcosβ安培的激励电流，在y轴发射线圈10上给予cosαcosβ安培大小的激励电流，在z轴发射线圈11上给予sinβ安培的激励电流。在脉冲直流激励周期的第二个时间段，须在x轴发射线圈9上给予sin(α+90°)安培的激励电流，在y轴发射线圈10上给予cos(α+90°)安培大小的激励电流，在z轴发射线圈11上给予0安培的激励电流，用于合成与在初始方向上旋转水平角度α、垂直角度β后的主磁棒相正交的副磁棒。改变水平角度α的大小、保持垂直角度β不变，即可实现正交磁棒组在水平面的旋转；改变垂直角度β的大小、保持水平角度α不变，即可实现正交磁棒组的主磁棒在垂直面的旋转，从而实现空间或半空间任意位置的指向。

本发明的旋转搜索策略根据当前时刻测得的磁场强度与之前时刻测得的磁场强度的关系，决定增加或减小一个搜索步长的水平角度α或垂直角度β，搜索步长采用自适应步长，控制主磁棒以最佳的方式搜索到传感器。采样处理部分16在脉冲直流控制部分15的同步下，采样每个时间段的磁场，并作差获得每个磁棒在当前周期内的传感器位置的磁场强度。搜索完成后，控制部分17获得完成时的主副磁棒各自激励时的场强数据20和主磁棒指向数据21（即水平角度α、垂直角度β），算法单元18利用这些数据，依据后文的算法计算出传感器的空间位置和空间姿态，送显示输出单元19显示。

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

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

每次搜索启动时，系统先判定当前搜索过程的种类，若为初始搜索阶段，则设定步长为最小值（如0.5°），否则计算当前距离，并据此设定步长。首先在主磁棒所在的水平面内进行正方向搜索（增大水平角度α），若场强增大，则证明搜索方向正确，否则沿反方向搜索（减小水平角度α）。搜索过程每次前进一步，比较当前位置与前一个位置测得场强的大小，若发现场强减小，则说明前一个位置为场强最大位置，转回到并记录下前一位置的水平角度α，完成主磁棒水平面内的搜索。类似的，主磁棒垂直面内的搜索也经过这样一个搜索过程，最终实现磁棒指向传感器方向。

图5所示为位置算法的示意图。坐标系原点位于正交磁棒组中心。已知主磁棒在传感器位置的磁场强度，由于完成搜索时主磁棒总是指向传感器，只需对磁场轴线上的磁场进行简单的一维标定，即可利用测得的磁场强度计算距离r。式（1）表示标量形式的磁棒轴线上磁场大小与距离的关系，n匝线圈绕成的磁棒相当于n个偶极子环的叠加。其中μ₀表示真空磁导率，μ表示偶极子的磁矩。r_i是每个偶极子环到传感器的距离，与磁棒中心到传感器的距离r的关系如式（2）所示。其中l为磁棒的长度。实际计算时可以利用有限元分析方法对磁棒轴线上的磁场分布进行差值标定测得距离r：

                                     (1)

                                     (2)

然后，根据已知主磁棒指向传感器时的水平角度α、垂直角度β，通过极坐标到直角坐标的变换，可求得传感器坐标（x,y,z）：

                                    (3)

                                    (4)

                                       (5)

图6所示为姿态算法牵涉的坐标系。所有坐标系均为右手坐标系。坐标系

为系统默认坐标系，原点位于正交三轴线圈中心，也是合成的正交磁棒组的中心，正交三轴线圈固定于坐标系

， 

轴、轴、

轴分别与正交的三轴线圈方向一致，并且

轴为合成的主磁棒的初始方向，

轴为合成的副磁棒的初始方向。坐标系

表示正交磁棒组当前姿态，

轴为合成的主磁棒旋转后的方向，

轴为合成的副磁棒旋转后的方向。坐标系

表示传感器当前姿态。待求的传感器空间姿态可表示为从坐标系

到

的旋转关系。在确定了传感器的空间位置后，主磁棒的指向是已知的，故坐标系

到的旋转关系是已知的。为了进一步确定传感器的空间姿态，只需确定传感器坐标与正交磁棒组坐标的相对空间关系，即坐标系

到

的旋转关系即可。

当主磁棒指向传感器时，传感器位置的主磁棒磁感强度矢量是沿着主磁棒轴线方向,即

方向的，因此，若将传感器测得的主磁棒磁感强度矢量旋转至

方向，则该矢量在

、

方向的分量趋近于零。换言之，旋转传感器坐标系

，当测得的磁感强度矢量在y、z方向的分量为零时，当前坐标系已变换为磁棒坐标系。类似地，当主磁棒指向传感器时，传感器位置的副磁棒磁感强度矢量与副磁棒轴线方向相反,即反方向，因此，若将传感器测得的副磁棒磁感强度矢量旋转至方向，则该矢量在

、

方向的分量趋近于零。换言之，旋转传感器坐标系，当测得的磁感强度矢量在x、z方向的分量为零时，当前坐标系已变换为磁棒坐标系。由此可求解坐标系

相对于

的相对空间关系，进而确定传感器的空间姿态。

图7所示为姿态算法的流程图。首先须计算的是坐标系到的旋转关系，已知的是坐标系

到

的旋转关系。用

，

和

表示坐标系从

旋转到

的三个‘ZYX’欧拉角，当主磁棒指向传感器时，将传感器测得的主磁棒磁场和副磁棒磁场分别旋转到当前正交磁棒组的坐标系,令旋转后的主磁棒磁场在y，z方向的场强值为零，令旋转后的副磁棒磁场在x，z方向的场强值为零，共可获得四个方程。于是所有方程仅有，

和

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

场强是传感器在主磁棒激励时测得的坐标系

下的场强，将其旋转到坐标系

后得到场强矢量

，其在

、

方向上的分量为零，即

                                   （6）

其中

表示从坐标系到

的旋转矩阵。                         

由于

角是绕

轴的转动，所以，若主磁棒产生的场强矢量在在

、

方向上为零，则该旋转角不会影响依据主磁棒场强列出的方程。因此，我们可以利用这两个方程解出两个未知量

和

。

                        （7）

                           （8）

类似的，场强

是传感器在副磁棒激励时测得的坐标系

下的场强，

表示旋转到坐标系

下的场强，其在

、

方向上的分量为零，即

                                （9）

将已知的两个未知量带入副磁棒的方程中的任意一个，即可求解夹角

。这里选取z方向的方程，代入化简得

                                 （10）

式中其中

，

。

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

进一步，由三个欧拉角表示的坐标系

到的旋转关系，以及坐标系

到

的旋转关系，可以计算出坐标系

到

的旋转关系，并将其表示成欧拉角或者其他期望的形式。

                                    （11）

采用仿真方法对以上实施方式的系统进行了评估。仿真采用计算磁棒周围的磁场，分析静态时的跟踪误差，和动态时的跟踪速度。设定传感器采样率为900Hz。跟踪范围为坐标系

下 

、

、

的空间范围内，由于磁棒长度为10cm，故同时限定跟踪位置距离两磁棒中心至少6cm。跟踪的位置误差可以通过估计位置和实际位置之间的欧氏距离评估，而姿态误差则采用螺旋角

。

                              (12)

 

                               (13)

经过400组仿真，得到平均位置误差为0.1063cm，平均姿态误差为0.4719°，最大位置误差0.3834cm，最大姿态误差2.4096°，若减小步长，还可进一步提高精度。跟踪性能方面，实时跟踪时，在瞬时位移不超过2cm时最大跟踪步数为13步。相当于，实时跟踪最大跟踪时间0.043s。可以实现每秒20次以上的更新。若提高采样率，还可进一步提高刷新速度。

Claims (5)

1. 一种基于正交磁棒旋转搜索的电磁跟踪系统，其特征在于：由一个传感器装置、一个正交三轴线圈装置、一个控制处理显示装置组成；所述传感器装置附着于待跟踪物体，用于测量三个正交方向的磁场；所述正交三轴线圈装置由控制处理显示装置提供直流脉冲激励，用以模拟轴线所在方向为空间任意方向的两个正交的磁棒；该磁棒通过同时改变输出到正交线圈三轴上的直流脉冲电流大小，实现磁棒轴线所在方向的改变即旋转，来搜索传感器装置；所述控制处理显示装置由嵌入式系统和显示器组成，实现对其他组件的控制、数据的采集和处理、跟踪结果的显示、输出；其中：

所述三轴磁场传感器装置（2）包括三轴分量传感器（5）、（6）、（7），和一个信号调理和模拟-数字转换模块（8），三轴分量传感器（5）、（6）、（7）分别用来检测三个正交方向X’、Y’和Z’的磁场，其输出经后续的信号调理和模拟-数字转换模块（8）送入控制处理显示装置（4），由控制处理显示装置（4）中的采样处理模块（16）采样处理；

所述正交三轴线圈装置（3）包括：三组电磁铁线圈（9）、（10）、（11）,和三个控制驱动电路（12）、（13）、（14），三组电磁铁线圈（9）、（10）和（11）分别由控制驱动电路（12）、（13）和（14）控制驱动，三组电磁铁线圈几何中心重合，电磁铁采用脉冲直流的激励方式，由控制处理显示装置（4）控制脉冲直流的激励；

所述控制处理显示装置（4）由控制单元（17）、算法单元（18）、显示输出单元（19）组成，所述控制单元（17）包括两个部分：采样处理模块（16）和脉冲直流控制模块（15），采样处理模块（16）用于采样处理来自信号调理和模拟-数字转换模块（8）的信号；脉冲直流控制模块（15）提供对三轴正交线圈（9）、（10）和（11）的激励控制；脉冲直流的激励的每个周期分三个时间段，前两个时间段分别激励三轴正交线圈（9）、（10）和（11）合成主磁棒和副磁棒，第三个时间段三轴正交线圈均不激励；将主、副磁棒合成激励时测得的磁场与第三时间段的磁场相减作为对应磁棒在传感器位置产生的磁场强度。

2. 一种基于权利要求1所述的电磁跟踪系统的电磁跟踪方法，其特征在于通过直流脉冲激励方式交替激励由正交三轴线圈装置（3）模拟的两个可旋转的正交磁棒，分别产生磁场分布情况已知的电磁场；用三轴磁场传感器装置（2）测量其所在位置的磁场大小和方向，用控制处理显示装置（4）根据搜索策略进行搜索，当主磁棒指向传感器时搜索完毕；此时根据磁棒旋转的角度信息和沿主磁棒轴向的磁场强度标定信息计算求得其六自由度的空间位置和空间姿态；

所述的直流脉冲激励方式为交替激励，是将每个激励周期分三个时间段，在脉冲直流激励周期的第一个时间段，在x轴发射线圈上给予sinαcosβ安培的激励电流，在y轴发射线圈上给予cosαcosβ安培的激励电流，在z轴发射线圈上给予sinβ安培的激励电流，以合成大小为1安培电流激励的、从x轴旋转水平角度α、垂直角度β的主磁棒；在脉冲直流激励周期的第二个时间段，在x轴发射线圈上给予sin(α+90°)安培的激励电流，在y轴发射线圈上给予cos(α+90°)安培的激励电流，在z轴发射线圈上给予0安培的激励电流，用于合成与前一时间段合成主磁棒相正交的副磁棒；在脉冲直流激励周期的第三时间段，各轴线圈均不给予激励电流；将各磁棒激励时测得的磁场与第三时间段的磁场相减作为对应磁棒在传感器位置产生的磁场强度；

所述的搜索策略为正交磁棒组的主磁棒在两个正交平面内依次旋转，副磁棒始终与即时的主磁棒保持正交；根据磁棒指向传感器装置时传感器测得的磁场强度最大的特性，锁定主磁棒指向传感器的位置；其中磁棒旋转角度采用自适应步长，即每次旋转的角度根据传感器距离正交磁棒组的距离的增大而减小；

所述电磁跟踪的搜索策略，具体过程分为两个阶段：初始搜索阶段和实时跟踪阶段；系统开始工作时进行初始搜索，从旋转的初始指向出发执行搜索；完成初始搜索后，进入实时跟踪阶段，实时跟踪搜索过程分两步进行，即正交磁棒组的主磁棒在两个正交平面内依次旋转进行搜索，在主磁棒搜索过程中，副磁棒始终与即时的主磁棒保持正交；这样经过两次搜索，当主磁棒指向传感器时，停止搜索，实现对传感器位置的锁定；其中，主磁棒在每个平面上开始搜索时，先向一个方向试探，若场强增大，则表示试探方向正确，否则反向搜索；找到正确方向后，沿该方向一步步搜索，直至场强减小，则主磁棒的前一个搜索位置即为当前搜索平面里的最大场强方向。

3. 根据权利要求2所述的电磁跟踪方法，其特征在于在所述初始搜索时采用固定步长；实时跟踪时，根据传感器当前位置设定步长，步长大小随传感器与正交磁棒组之间距离的增大而减小，并且以当前位置作为下一时刻位置的粗略估计，来设定搜索步长。

4. 根据权利要求2所述的电磁跟踪方法，其特征在于所述的求空间位置的算法如下：根据主磁棒在两正交平面内旋转的角度α、β和采用一维磁场标定求得的传感器与磁棒之间的距离r，传感器三自由度的空间位置为：x =r cosαcosβ，y =r sinαcosβ，z =r sinβ。

5. 根据权利要求4所述的电磁跟踪方法，其特征在于所述的求空间姿态的算法如下：采用非迭代算法，将在传感器坐标系F₃测得的主、副磁棒磁场强度，旋转变换到当前正交磁棒坐标系F₂，主磁棒y轴、z轴上的磁场强度分量为0，                                               

，副磁棒x轴、z轴上的磁场强度分量为0，

，从而求得旋转矩阵A₃₂，又已知原始坐标系F₁经过水平旋转α、垂直旋转β后得到当前正交磁棒坐标系F₂，得旋转矩阵A₁₂，从而根据

的关系，计算传感器三自由度的空间姿态A₁₃，所有旋转矩阵转换成欧拉角的形式求解；其中，

是传感器在主磁棒激励时测得的坐标系

下的场强，

是将

旋转到坐标系

后得到场强，

是传感器在副磁棒激励时测得的坐标系

下的场强，

是

旋转到坐标系

下的场强。

Images