专利名称：一种基于双磁棒旋转搜索的电磁跟踪方法及系统的制作方法
技术领域：
本发明属于电磁跟踪技术领域，具体涉及用于电磁跟踪的方法和系统。
背景技术：
电磁跟踪(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，直径lcm的电磁铁，线圈匝 数100匝，电流1A。旋转控制装置4包含两个电机组15和16，电机组由步进电机驱动器17驱动，驱 动器由控制处理显示装置5控制。每个电机组包括两个步进电机，第一电机带动第二步进 电机转动，第二电机带动磁棒转动，控制一磁棒沿两正交方向旋转，从而实现空间或半空间 任意指向。推荐的旋转方式是第一电机平行于X-Z平面旋转，第二电机垂直于该平面旋转。 采用步进电机的原因是可以随时获得当前磁棒的指向，不存在累积误差。两个旋转中心的
7距离对于系统的跟踪范围等性能有一定的影响，这里选取的距离是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中心。已知两磁棒指向传 感器时的方向角(Ql，3》和(a2，日2)，两磁棒间距离d，这里的d为30cm，待求的是传感 器坐标(x，y，z)。由立体几何知识可以解出传感器位置坐标 其中，a，b分别为传感器坐标在x，y平面的投影点(x，y，0)到两磁棒的距离。图6所示为方向算法所涉及的坐标系。坐标系&为系统默认坐标系，原点位于磁 棒1中心，Xl轴沿两磁棒中心连线方向，yi轴沿磁棒初始方向，Zl轴垂直于Xl_yi平面；坐 标系F2与坐标系&方向均相同，仅原点位于磁棒2中心。坐标系F3和F4表示两磁棒当前 姿态，z3、z4轴分别沿两磁棒当前指向；坐标系F5表示传感器当前姿态。坐标系&和F2仅 原点位置不同，空间方向是相同的。待求的传感器空间姿态可表示为从坐标系&到&的旋 转关系。在确定了传感器的空间位置后，两磁棒的指向是已知的，故坐标系& (或F2)到& 和坐标系& (或F2)到&的旋转关系是已知的。为了进一步确定传感器的空间姿态，只需 确定传感器坐标与两磁棒指向的相对空间关系，即坐标系F3到F5或F4到F5的旋转关系即 可。由于磁棒1和磁棒2、坐标系F3和F4是对等的，不失一般性，下文算法采取计算坐标系 F3和F5之间的旋转关系的方式。当磁棒指向传感器时，传感器位置的磁感强度矢量是沿着磁棒轴线方向，即z3(或 Z4)方向的，因此，若将传感器测得的磁感强度矢量旋转至z3(或z4)方向，则该矢量在X3、 y3(或X4、y4)方向的分量趋近于零。换言之，旋转传感器坐标系F5，当测得的磁感强度矢量 在x、y方向的分量为零时，传感器坐标z轴的指向即为磁棒轴线方向(z3或z4)，亦即当前 坐标系已变换为磁棒坐标系(F3或F4)。将传感器对磁棒1和磁棒2分别做这样的处理后， 即可求解坐标系F5相对于F3或F4的相对空间关系，进而确定传感器的空间姿态。图7所示为方向算法的流程图。首先须计算的是坐标系F5到F3的旋转关系，已知 的是坐标系Fi到F3和坐标系Fi到F4的旋转关系。用^，e和屯表示坐标系从F5旋转到 F3的三个‘ZXZ’欧拉角，当两磁棒指向传感器时，将传感器磁场分别旋转到当前磁棒的坐标 系坑和⑴，令其在x，y方向的场强值为零，共可获得四个方程。这里，磁棒1场强的旋转 可以直接由坐标系F5旋转到F3，对于磁棒2场强，为了不涉及坐标系F5与F4之间的旋转关 系，将其旋转过程拆分为F5到F3的旋转，F3到&的旋转和&到F4的旋转三个阶段。于是
9所有方程仅有p e和▽三个未知数，选取其中任意三个即可求解。场强&是传感器在磁棒1激励是测得的场强，将其旋转到坐标系F3后得到场强矢 量B' i，其在x3、y3S向上的分量为零，即B' ！ = A^Bi =
T(4)其中A53表示从坐标系F5到F3的旋转矩阵由于W角是绕z3轴的转动，所以，若磁棒1产生的场强矢量在x3、y3方向上为零， 则该旋转角不会影响依据磁棒1场强列出的方程。因此，我们可以利用这两个方程解出两 个未知量p和e
^xs^i-cos^ + ^ijsin^-0(5)
B\y = Bu sin 0 + BXy cos ^ cos q)-Bucos6sin(p = 0(6)这里，参量场强矢量B'工的下标x，y, z分别表示该参量在x，y, z方向上的分量， 下同。类似的，当磁棒2激励时，用B2表示坐标系F5下的实测场强，B' 2表示旋转到坐 标系F3下的场强，A14表示从坐标系Fi到F3的旋转矩阵，A31表示从坐标系F4到Fi的旋转 矩阵B' 2 = A14A31A53B2 =
T(7)将已知的两个未知量带入磁棒2的方程中的任意一个，即可求解夹角叫。这里选 取x方向的方程，代入化简得B' 2x = Acos^+Bsin^+C = 0(8)式中其中A、B、C均为常数。如上所述，由方程(5)、(6)、(8)可求解三个欧拉角。以上方程的求解只涉及三个 夹角的简单三角函数，可以直接利用非迭代的方法计算，算法简单可靠。进一步，由三个欧拉角表示的坐标系F5到F3的旋转关系，以及坐标系&到F3的旋 转关系，可以计算出坐标系&到F5的旋转关系，并将其表示成欧拉角或者其他期望的形式。A15 = (A53A31)(9)采用仿真方法对以上实施方式的系统进行了评估。仿真采用计算磁棒周围的磁 场，分析静态时的跟踪误差0和动态是的跟踪速度。设定传感器采样率为900Hz。跟踪范围 为坐标系？工下^ G [-15cm,45cm], G [5cm，20cm]、G [-30cm, 30cm]的空间范围内， 由于磁棒长度为10cm，故同时限定跟踪位置距离两磁棒中心至少6cm。跟踪的位置误差可 以通过估计位置和实际位置之间的欧氏距离AD评估，而方向误差则采用螺旋角A小。AD = ^(x-x')2+(y-y')2+(z-z'f(27)A 小=cos-1[(An+A22+A33-l)/2](28)经过5000组仿真，得到平均位置误差为0. 1484cm，平均方向误差为0. 5474°，最 大位置误差0.7261cm，最大方向误差3.8852°，若减小步长，还可进一步提高精度。跟踪性 能方面，初始跟踪平均步数230步；实时跟踪时，在瞬时位移不超过2cm时最大跟踪步数为 26步。相当于，初始跟踪平均0. 767s,实时跟踪最大跟踪时间0. 087s。可以实现每秒10次 以上的更新。若提高采样率，还可进一步提高刷新速度。
权利要求
一种基于双磁棒旋转搜索的电磁跟踪系统，其特征在于由传感器、磁场源、旋转控制装置和控制处理显示装置组成；所述传感器附着于待跟踪物体，旋转控制装置控制磁棒旋转指向空间或半空间任意方向；磁棒旋转依据旋转策略，旋转搜索传感器，当磁棒锁定传感器时，依据位置和方向算法，计算传感器六自由度的位置和方向；控制处理显示装置采用一台pc机或服务器，对其他组件进行控制，并进行数据的采集和处理、跟踪结果的显示输出。
2.根据权利要求1所述的基于双磁棒旋转搜索的电磁跟踪系统，其特征在于所述的传 感器为三轴分量传感器，分别用来检测三个正交的局部方向r、r和v的磁场；传感器输 出经后续的信号调理和模拟-数字转换模块送入控制处理显示装置。
3.根据权利要求2所述的基于双磁棒旋转搜索的电磁跟踪系统，其特征在于所述的磁 场源由两组电磁铁线圈组成，两组电磁铁线圈组分别由两个驱动电路控制驱动；其中一组 电磁铁线圈保证在相同距离情况下轴线上磁场强度最大，且该处磁场方向沿着该轴线；另 外，电磁铁采用脉冲直流的激励方式，由控制处理显示装置控制脉冲直流的激励。
4.根据权利要求3所述的基于双磁棒旋转搜索的电磁跟踪系统，其特征在于所述的旋 转控制装置包含两个电机组，两个电机组由步进电机驱动器驱动，驱动器由控制处理显示 装置控制；每个电机组包括两个步进电机，第一步进电机带动第二步进电机转动，第二步进 电机带动磁棒转动，控制一磁棒沿两正交方向旋转，实现空间或半空间任意指向。
5.根据权利要求4所述的基于双磁棒旋转搜索的电磁跟踪系统，其特征在于所述的控 制处理显示装置由控制单元、算法单元、显示输出单元组成；其中，控制单元包括三个部分 (1)脉冲直流控制部分，提供对磁棒激励的控制，磁棒激励采用脉冲直流方式，每周期三个 时间段，前两个时间段顺序激励两根磁棒，第三个时间段两磁棒均不激励；将每个磁棒激励 时测得的磁场与第三时间段的磁场相减作为该磁棒在传感器位置产生的磁场强度；(2)电 机控制部分，控制两个电机组的电机旋转，保证两磁棒能分别在两个正交方向上旋转，实现 空间或半空间任意位置的指向；电机控制部分依据旋转搜索策略，根据当前时刻测得的磁 场强度与之前时刻测得的磁场强度的关系，决定电机旋转方向，根据磁棒当前位置决定电 机旋转步进，控制磁棒以最佳的方式搜索到传感器；(3)采样处理部分，在脉冲直流控制部 分的同步下，采样每个时间段的磁场，并求差以获得每个磁棒在当前周期内的传感器位置 的磁场强度；搜索完成后，控制单元获得完成时的两个磁棒各自激励时的场强数据和磁棒 指向数据，算法单元根据这些数据，依据算法计算出传感器的空间位置和空间姿态，送显示 输出单元显示。
6.一种利用如权利要求5所述的基于双磁棒旋转搜索的电磁跟踪系统进行电磁跟踪 的方法，其特征在于步骤如下根据磁棒指向传感器时传感器测得的磁场强度最大的特性， 采用双磁棒跟踪传感器进行搜索定位，应用几何算法计算得到传感器的空间位置；并进一 步依据磁棒指向传感器时传感器测出的磁场方向沿着磁棒轴线方向的原理，应用几何算法 计算得到传感器的空间姿态。
7.根据权利要求6所述的基于双磁棒旋转搜索的电磁跟踪方法，其特征在于所述的搜 索分为两个阶段初始搜索阶段和实时跟踪阶段；系统开始工作时进行初始搜索，从旋转 的初始指向出发执行搜索；当完成初始搜索后，由于短时间内传感器运动距离有限，故磁棒 当前指向距离传感器实际指向较近，即在有限步数内完成跟踪，保证跟踪的实时性，故这一 阶段称为实时跟踪阶段；搜索过程分两步进行，先在一个正交平面进行，即一个电机旋转搜索，寻找传感器位置磁场强度的磁棒指向，当搜索完成时，再在另一个正交平面进行，在每 个平面上开始搜索时，先向一个方向试探，若场强增大，则表示试探方向正确，否则反向搜 索；找到正确方向后，沿该方向一步步搜索直至场强减小，则前一个搜索位置即为当前搜索 平面里的最大场强方向。
8.根据权利要求7所述的基于双磁棒旋转搜索的电磁跟踪方法，其特征在于在所述的 初始搜索阶段采用固定步长的方法，；实时跟踪阶段时，根据传感器当前位置设定步长，步 长大小随传感器距离每个电机的距离的增大而减小。
9.根据权利要求7所述的基于双磁棒旋转搜索的电磁跟踪方法，其特征在于所述应用 几何算法计算得到传感器的空间位置，具体步骤如下已知两磁棒指向传感器时的方向角(h，和(a2，3 2)，两磁棒间距离d，待求的传 感器坐标为(X，1，z)，传感器位置坐标的算式为 其中，a，b分别为传感器坐标在x，y平面的投影点(x，y,0)到两磁棒的距离。
10.根据权利要求7所述的基于双磁棒旋转搜索的电磁跟踪方法，其特征在于所述应 用几何算法计算得到传感器的空间姿态，具体步骤如下设坐标系Fi为系统默认坐标系，原点位于磁棒1中心，Xl轴沿两磁棒中心连线方向，yi 轴沿磁棒初始方向，Zl轴垂直于Xl-yi平面；坐标系F2与坐标系&方向均相同，仅原点位于 磁棒2中心；坐标系F3和F4表示两磁棒当前姿态，z3、z4轴分别沿两磁棒当前指向；坐标系 F5表示传感器当前姿态；坐标系&和F2仅原点位置不同，空间方向相同；待求的传感器空 间姿态表示为从坐标系&到F5的旋转关系；在确定了传感器的空间位置后，两磁棒的指向 是已知的，故坐标系&或&到F3，和坐标系&或&到F4的旋转关系是已知的；为了进一步 确定传感器的空间姿态，下面计算坐标系F3和F5之间的旋转关系首先须计算坐标系F5到F3的旋转关系，已知坐标系&到F3和坐标系&到F4的旋转 关系，用< ，e和v分别表示坐标系从F5旋转到F3的三个‘ZXZ’欧拉角，当两磁棒指向传 感器时，将传感器磁场分别旋转到当前磁棒的坐标系F3和F4，令其在X，y方向的场强值为 零，共获得四个方程；这里，磁棒1场强的旋转可以直接由坐标系F5旋转到F3，对于磁棒2 场强，为了不涉及坐标系F5与F4之间的旋转关系，将其旋转过程拆分为F5到F3的旋转，F3 到&的旋转和&到F4的旋转三个阶段；于是所有方程仅有P 9和V三个未知数，选取其 中任意三个即可求解；设场强Bi是传感器在磁棒1激励是测得的场强，将其旋转到坐标系F3后得到场强矢量 B'工，其在x3、y3S向上的分量为零，即B' ！ = A53B, =
T(4)其中A53表示从坐标系f5到F3的旋转矩阵；由于v角是绕Z3轴的转动，所以，若磁棒1产生的场强矢量在X3、y3方向上为零，则该 旋转角不会影响依据磁棒1场强列出的方程；因此，可以利用这两个方程解出两个未知量^ 和e B[x = Blx costp + BXy sin^ = 0(5)BXy = 5lz sin ^ + BXy cos 0cos q> - Blx cos ^ sin ^ = 0(6)类似的，当磁棒2激励时，用B2表示坐标系F5下的实测场强，B' 2表示旋转到坐标系 F3下的场强，A14表示从坐标系&到F3的旋转矩阵，A31表示从坐标系F4到&的旋转矩阵 B' 2 = A14A31A53B2 =
T(7)将已知的两个未知量带入磁棒2的方程中的任意一个，即可求解夹角V，这里选取x方 向的方程，代入化简得B ‘ 2x = Acos ￥ +Bsin ￥+C = 0(8)式中其中A、B、C均为常数；由方程(5)、(6)、(8)可求解三个欧拉角；进一步，由三个欧拉角表示的坐标系F5到F3的旋转关系，以及坐标系&到F3的旋转关 系，可以计算出坐标系&到F5的旋转关系，并将其表示成欧拉角或者其他期望的形式 A15= (A^r1(9)。
全文摘要
本发明属于电磁跟踪技术领域，具体为一种基于双磁棒旋转搜索的电磁跟踪方法及系统。本发明的电磁跟踪系统，由传感器、磁场源、旋转控制装置和控制处理显示装置组成。本发明根据磁棒指向传感器时传感器测得的磁场强度最大的特性，采用双磁棒跟踪传感器进行搜索定位，应用几何算法计算得到传感器的空间位置；并进一步依据磁棒指向传感器时传感器测出的磁场方向沿着磁棒轴线方向的原理，应用几何算法计算得到传感器的空间姿态。本发明利用磁棒旋转搜索传感器，采用非迭代算法，可以快速有效地实现对传感器位置和方向的跟踪，可靠性好、计算复杂度低。
文档编号G01C21/00GK101852868SQ20101017933
公开日2010年10月6日 申请日期2010年5月20日 优先权日2010年5月20日
发明者方祖祥, 葛鑫, 赖大坤, 邬小玫 申请人:复旦大学