CN103412337A - 基于双独立旋转磁棒电磁跟踪的位置跟踪方法及系统 - Google Patents

基于双独立旋转磁棒电磁跟踪的位置跟踪方法及系统 Download PDF

Info

Publication number
CN103412337A
CN103412337A CN2013102986909A CN201310298690A CN103412337A CN 103412337 A CN103412337 A CN 103412337A CN 2013102986909 A CN2013102986909 A CN 2013102986909A CN 201310298690 A CN201310298690 A CN 201310298690A CN 103412337 A CN103412337 A CN 103412337A
Authority
CN
China
Prior art keywords
plane
rotation
bar magnet
angle
bar
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN2013102986909A
Other languages
English (en)
Other versions
CN103412337B (zh
Inventor
邬小玫
王一枫
丁宁
沙敏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Fudan University
Original Assignee
Fudan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Fudan University filed Critical Fudan University
Priority to CN201310298690.9A priority Critical patent/CN103412337B/zh
Publication of CN103412337A publication Critical patent/CN103412337A/zh
Application granted granted Critical
Publication of CN103412337B publication Critical patent/CN103412337B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Measurement Of Length, Angles, Or The Like Using Electric Or Magnetic Means (AREA)
  • Control Of Position Or Direction (AREA)

Abstract

本发明属于电磁跟踪技术领域，具体为基于双独立旋转磁棒电磁跟踪的位置跟踪方法及系统。本发明方法包括：首先，使用基于双磁棒的可变旋转平面的电磁跟踪搜索方法，得到双磁棒旋转角度；然后，对双磁棒旋转角度进行角度转换预处理和角度校准；最后，运用平均位置跟踪算法，得到跟踪物体的位置；本发明电磁跟踪系统包括实现所述双独立磁棒可变旋转平面的电磁跟踪搜索方法的控制模块，以及对双磁棒旋转角度进行角度转换预处理和角度校准的模块，平均位置跟踪算法模块。本发明针对现有的映射法电磁跟踪位置算法存在低精度区的问题进行了改进：计算前采用双重估计求平均值，可以提高角度测量的精确度，从而提升电磁跟踪系统的位置定位精度。

Description

基于双独立旋转磁棒电磁跟踪的位置跟踪方法及系统
技术领域
本发明属于电磁跟踪技术领域，具体涉及一种电磁跟踪系统的优化位置方法及其系统。
背景技术
电磁跟踪（Electromagnetic Tracking），或称电磁场定位，是一种利用磁场或电磁场对跟踪目标的空间位置和空间姿态进行检测和实时跟踪的方法。该方法可应用于微创手术的导航，亦可运用于虚拟现实、三维超声成像等领域。电磁跟踪系统一般由磁场源（如永磁铁、电磁铁线圈）、磁场传感器、控制处理单元三部分组成。通过磁场源在固定位置产生磁场，然后利用附着在跟踪目标上的传感器测得的磁感应强度数据，求解出跟踪目标的空间位置和姿态。
基于旋转磁棒的电磁跟踪算法根据磁场轴向最大原理，通过旋转磁棒、捕捉磁感应强度最大值，可以确定磁棒指向跟踪目标的方位。因此，如果采用两个相距固定距离同向放置在水平面上的磁棒，进行交替激励地旋转搜索，可以获取跟踪目标与双磁棒之间的几何关系，非迭代地直接计算出跟踪目标六自由度的位置和姿态。相比迭代的位置和姿态算法，非迭代算法计算速度快、运算简单、对硬件配置要求低，可以克服迭代算法需要依赖无限远偶极子模型、计算复杂度高、易发散、存在局部极值点等的问题。
然而，对于上述基于双独立旋转磁棒的电磁跟踪系统，由于旋转角度的非连续性和磁场畸变的影响，无法实现双磁棒精确指向传感器。角度的非精确性将会导致基于角度进行计算的位置精度下降，并进一步降低姿态精度。尤其对于传统的基于双独立旋转磁棒的映射法电磁跟踪位置算法而言，其采用将双磁棒、传感器构成的空间三角形投影到双磁棒初始放置的水平面的方式来进行位置计算，当传感器不断靠近双磁棒初始所在的垂直平面时，空间三角形的投影将由锐角三角形变成钝角三角形，乃至蜕变成一条直线。相同程度的角度偏差对于存在较小角的钝角投影三角形的形变影响更大，计算的位置偏差也更大。因此，靠近双磁棒初始所在的垂直平面的区域是低精度区。如何解决全空间中角度指向非精确性的问题将直接影响系统精度。
发明内容
本发明的目的在于提供一种定位精度高的基于双独立旋转磁棒电磁跟踪的位置跟踪方法及系统。
本发明提出的基于双独立旋转磁棒电磁跟踪的位置跟踪方法，首先，使用基于双磁棒的可变旋转平面的电磁跟踪搜索方法，得到双磁棒旋转角度；然后，对双磁棒旋转角度进行角度转换预处理和角度校准，最后，运用平均位置跟踪算法得到跟踪物体的位置。该方法可用于跟踪目标位置和姿态六自由度的非迭代电磁跟踪系统。
本发明中，所述的基于双磁棒的可变旋转平面的电磁跟踪搜索方法，是对基础的可变旋转平面电磁跟踪搜索方法的扩展，是利用两个独立磁棒的可变旋转平面进行旋转搜索的过程，具体步骤为：设两个相距固定距离d同向放置在水平面上的磁棒，分别根据前一时刻跟踪物体的位置（x 0, y 0, z 0），各自独立地选择最优的第一旋转平面，每个磁棒分别进行两个正交平面上的旋转搜索。两个磁棒分别记为磁棒A和磁棒B。
本发明中，所述的跟踪物体的位置（x 0, y 0, z 0）由参考坐标系确定，参考坐标系的原点为磁棒A的固定不动点，X轴为磁棒A和磁棒B连线方向，Z轴为水平面向上的法线方向，Y轴符合右手坐标系法则。
本发明中，所述的第一旋转平面有三种可选平面：
（1）T平面：与平面XOY平行的平面；
（2）C平面：与平面XOZ平行的平面；
（3）S平面：与平面YOZ平行的平面；
本发明中，所述的最优第一旋转平面的选择依据是：
对于磁棒A而言，当                                                时，选择跟踪目标所在的S平面作为磁棒A的第一旋转平面，垂直于S平面、与S平面相交于第一次旋转结束时磁棒指向的平面作为磁棒A的第二旋转平面；当时，选择跟踪目标所在的C平面作为磁棒A的第一旋转平面，垂直于C平面、与C平面相交于第一次旋转结束时磁棒指向的平面作为磁棒A的第二旋转平面；当
Figure 2013102986909100002DEST_PATH_IMAGE003
时，选择跟踪目标所在的T平面作为磁棒A的第一旋转平面，垂直于T平面、与T平面相交于第一次旋转结束时磁棒指向的平面作为磁棒A的第二旋转平面；当
Figure 762663DEST_PATH_IMAGE004
中的某两个或三个相等时，优先选择与前一时刻磁棒A的第一旋转平面一致的搜索效率较高的第一旋转平面。
对于磁棒B而言，当
Figure 2013102986909100002DEST_PATH_IMAGE005
时，选择跟踪目标所在的S平面作为磁棒B的第一旋转平面，垂直于S平面、与S平面相交于第一次旋转结束时磁棒指向的平面作为磁棒B的第二旋转平面；当
Figure 645037DEST_PATH_IMAGE006
时，选择跟踪目标所在的C平面作为磁棒B的第一旋转平面，垂直于C平面、与C平面相交于第一次旋转结束时磁棒指向的平面作为磁棒B的第二旋转平面；当
Figure 2013102986909100002DEST_PATH_IMAGE007
最小时，选择跟踪目标所在的T平面作为磁棒B的第一旋转平面，垂直于T平面、与T平面相交于第一次旋转结束时磁棒指向的平面作为磁棒B的第二旋转平面；当中的某两个或三个相等时，优先选择与磁棒B的前一时刻第一旋转平面一致的搜索效率较高的第一旋转平面。
对于上述基于双磁棒的可变旋转平面的电磁跟踪搜索系统，由于旋转角度的非连续性和磁场畸变的影响，无法实现双磁棒精确指向传感器。角度的非精确性将会导致基于角度进行计算的位置精度下降，并进一步降低姿态精度。尤其对于基于双独立旋转磁棒的映射法电磁跟踪位置算法而言，其采用将双磁棒、传感器构成的空间三角形投影到双磁棒初始放置的XOY平面的方式来进行位置计算，当传感器不断靠近双磁棒初始所在的XOZ平面时，空间三角形的投影将由锐角三角形变成钝角三角形，乃至蜕变成一条直线。相同程度的角度偏差对于存在较小角的钝角投影三角形的形变影响更大，计算的位置偏差也更大。因此，靠近双磁棒初始所在的XOZ平面的区域是低精度区。
由于低精度区的存在，本发明进一步提出了基于双独立旋转磁棒的位置跟踪方法。不同于传统电磁跟踪位置方法将空间三角形投影到XOY平面进行计算，本发明提出的平均法位置跟踪方法，将XOY平面进行旋转，去拟合空间三角形所在的平面。一方面，可以降低空间三角形在投影过程中产生的形变，另一方面，可以使用磁棒A和磁棒B在S平面的第一旋转角度α 1s 和α 2s 双重估计XOY平面拟合空间三角形所在的平面所要旋转的角度α，从而提高角度测量的精确度。因此，对可变旋转平面的电磁跟踪搜索方法得到的双磁棒旋转角度，需要进行角度转换预处理和角度校准，再运用平均位置跟踪算法得到跟踪物体的位置。
本发明中，所述的角度转换预处理，即如果任意磁棒的第一旋转平面不是S平面，需要对该磁棒的第一旋转角α、第二旋转角β进行变换，以便运用在后续的平均位置跟踪算法中。角度转换存在如下两种情况：
（1）T平面转换为S平面：
此时旋转角度的切换如式（1）所示：
Figure 2013102986909100002DEST_PATH_IMAGE009
    当
Figure 692945DEST_PATH_IMAGE010
时              
或当
Figure 477361DEST_PATH_IMAGE012
时     （1）
（2）C平面转换为S平面：
此时旋转角度的切换如式（2）所示：
Figure 2013102986909100002DEST_PATH_IMAGE013
                         （2）
式（1）-（2）中，第一旋转角
Figure 766522DEST_PATH_IMAGE014
，第二旋转角，*=1或2；α 1t 为磁棒A在T平面上的第一旋转角度，β 1t 为磁棒A在与T平面正交的平面上的第二旋转角；α 1c 为磁棒A在C平面上的第一旋转角度，β 1c 为磁棒A在与C平面正交的平面上的第二旋转角度；α 1s 为磁棒A在S平面上的第一旋转角度，β 1s 为磁棒A在与S平面正交的平面上的第二旋转角度；α 2t 为磁棒B在T平面上的第一旋转角度，β 2t 为磁棒B在与T平面正交的平面上的第二旋转角；α 2c 为磁棒B在C平面上的第一旋转角度，β 2c 为磁棒B在与C平面正交的平面上的第二旋转角度；α 2s 为磁棒B在S平面上的第一旋转角度，β 2s 为磁棒B在与S平面正交的平面上的第二旋转角度；
本发明中，所述的角度校准，采用平均法，即由于α 1s 和α 2s 都表示双磁棒、传感器构成的空间平面与平面XOY之间的二面角α，因此可以通过求平均值校准二面角α，如式（3）所示：
Figure 495444DEST_PATH_IMAGE016
                            （3）
其中，k是调整参数，其取值如式（4）所示：
Figure 2013102986909100002DEST_PATH_IMAGE017
 （4）
本发明中，所述的平均位置跟踪算法，即根据磁棒A的第二旋转角度β 1s ，磁棒B的第二旋转角度β 2s 和经过平均法角度校准的二面角α，计算跟踪物体的位置（x, y, z）：
Figure 105417DEST_PATH_IMAGE018
                            （5）
Figure 2013102986909100002DEST_PATH_IMAGE019
                            （6）
Figure 744339DEST_PATH_IMAGE020
                            （7）
本发明提出的基于双独立旋转磁棒的平均法位置跟踪方法,针对基于双独立旋转磁棒的映射法电磁跟踪位置算法存在低精度区的问题进行了改进：不是将空间三角形投影到XOY平面进行计算，而是旋转XOY平面去拟合空间三角形所在平面进行计算。具体地，旋转平面拟合计算可以降低空间三角形在投影过程中产生的形变，而且可以使用磁棒A和磁棒B在S平面的第一旋转角度α 1s 和α 2s 双重估计XOY平面拟合空间三角形所在的平面所要旋转的角度α，从而提高角度测量的精确度，提升电磁跟踪系统的位置定位精度。
本发明还提供应用上述位置跟踪方法的电磁跟踪系统。该电磁跟踪系统，包括实现上述双独立磁棒可变旋转平面的电磁跟踪搜索方法的控制模块，以及实现上述双独立旋转磁棒的位置跟踪方法的运算模块（对双磁棒旋转角度进行角度转换预处理和角度校准的模块，平均位置跟踪算法模块）。该电磁跟踪系统在原有非迭代算法计算速度快的基础上，使得定位精度大幅提高，可以实现跟踪目标的精准定位。
附图说明
图1为基于双独立旋转磁棒的电磁跟踪系统的流程图。
图2为T平面作为第一旋转平面的示意图。
图3为C平面作为第一旋转平面的示意图。
图4为S平面作为第一旋转平面的示意图。
图5为投影法位置跟踪算法示意图。
图6为投影法位置算法存在低精度区的原因分析图。
图7为平均法位置跟踪算法示意图。
图8为T平面为第一旋转平面的双磁棒旋转角度示意图。
图9为C平面为第一旋转平面的双磁棒旋转角度示意图。
图10为S平面为第一旋转平面的双磁棒旋转角度示意图。
图中标号：1为第一旋转轴是Z轴的T旋转平面，2为第一旋转轴是Y轴的C旋转平面，3为第一旋转轴是X轴的S旋转平面，4为投影法位置跟踪算法的低精度平面，5为投影法位置算法的计算平面，6为双磁棒与传感器构成的空间三角形所在的位置平面。
具体实施方式
下面结合附图和仿真结果对本发明作进一步说明。
图1是基于双独立旋转磁棒的电磁跟踪系统的流程图。全过程主要分为旋转搜索阶段和位置计算阶段两部分。在旋转搜索阶段中，对于初次跟踪，初始化T平面为磁棒A和磁棒B的第一旋转平面，磁棒A和磁棒B分别在两个正交平面内依次旋转，磁棒A和磁棒B都实现指向传感器后进入位置计算阶段。再次跟踪时，根据计算阶段得到的前一时刻跟踪物体的位置，各自选择磁棒A和磁棒B的最优第一旋转平面，更高效地完成两个正交平面内的旋转搜索。在位置计算阶段，即图1中虚线框中的部分，系统先对旋转搜索阶段得到的双磁棒旋转角度进行角度转换预处理和平均法角度校准，再运用平均位置跟踪算法更精准地计算跟踪物体的位置。
图2、图3、图4显示了旋转搜索阶段中三种可选的第一旋转平面。图2中标号1为第一旋转轴是Z轴的T旋转平面，即与平面XOY平行的平面；图3中标号2为第一旋转轴是Y轴的C旋转平面，即与平面XOZ平行的平面；图4中标号3为第一旋转轴是X轴的S旋转平面，即与平面YOZ平行的平面。双磁棒选择最优第一旋转平面的依据是：
对于一端固定在位置（0,0,0）的磁棒A而言，当
Figure 250407DEST_PATH_IMAGE001
时，选择跟踪目标所在的S平面作为磁棒A的第一旋转平面，垂直于S平面、与S平面相交于第一次旋转结束时磁棒指向的平面作为磁棒A的第二旋转平面；当
Figure 466625DEST_PATH_IMAGE002
时，选择跟踪目标所在的C平面作为磁棒A的第一旋转平面，垂直于C平面、与C平面相交于第一次旋转结束时磁棒指向的平面作为磁棒A的第二旋转平面；当
Figure 880289DEST_PATH_IMAGE003
时，选择跟踪目标所在的T平面作为磁棒A的第一旋转平面，垂直于T平面、与T平面相交于第一次旋转结束时磁棒指向的平面作为磁棒A的第二旋转平面；
对于一端固定在位置（d,0,0）的磁棒B而言，当
Figure 232773DEST_PATH_IMAGE005
时，选择跟踪目标所在的S平面作为磁棒B的第一旋转平面，垂直于S平面、与S平面相交于第一次旋转结束时磁棒指向的平面作为磁棒B的第二旋转平面；当
Figure 627851DEST_PATH_IMAGE006
时，选择跟踪目标所在的C平面作为磁棒B的第一旋转平面，垂直于C平面、与C平面相交于第一次旋转结束时磁棒指向的平面作为磁棒B的第二旋转平面；当
Figure 65785DEST_PATH_IMAGE007
最小时，选择跟踪目标所在的T平面作为磁棒B的第一旋转平面，垂直于T平面、与T平面相交于第一次旋转结束时磁棒指向的平面作为磁棒B的第二旋转平面；
对于磁棒A，当
Figure 486402DEST_PATH_IMAGE004
中的某两个或三个相等时，优先选择与前一时刻磁棒A的第一旋转平面一致的搜索效率较高的第一旋转平面。对于磁棒B，当
Figure 365497DEST_PATH_IMAGE008
中的某两个或三个相等时，优先选择与前一时刻磁棒B的第一旋转平面一致的搜索效率较高的第一旋转平面。如：当
Figure 2013102986909100002DEST_PATH_IMAGE021
时，对于磁棒A而言，S平面和C平面的搜索效率一致，都较高；若前一时刻磁棒A的第一旋转平面为S平面，则仍然以S平面为磁棒A的第一旋转平面；若前一时刻磁棒A的第一旋转平面为C平面，则仍然以C平面为磁棒A的第一旋转平面；若前一时刻磁棒A的第一旋转平面为T平面，则可任意选择S平面或C平面为磁棒A的第一旋转平面。
旋转搜索阶段完成后，进入位置计算阶段。此时，根据双磁棒旋转角度计算传感器的空间位置。基于双独立旋转磁棒的映射法电磁跟踪位置算法，如图5所示，采用将双磁棒、传感器构成的空间三角形投影到双磁棒初始放置的XOY平面的方式来计算传感器位置（x, y, z）：
Figure 213367DEST_PATH_IMAGE022
                          （8）
                          （9）
Figure 404177DEST_PATH_IMAGE024
                            （10）
Figure DEST_PATH_IMAGE025
                             （11）
Figure 573690DEST_PATH_IMAGE026
                             （12）
其中，at，bt分别是磁棒A，磁棒B与传感器的连线在XOY平面上的投影，α 1t 为磁棒A在T平面上的第一旋转角度，β 1t 为磁棒A在与T平面正交的平面上的第二旋转角，α 2t 为磁棒B在T平面上的第一旋转角度，β 2t 为磁棒B在与T平面正交的平面上的第二旋转角。
跟踪精度降低的原因，是由于当传感器不断靠近双磁棒初始所在的XOZ平面时，如图6所示，传感器从位置L1，移动到位置L2，再移动到位置L3，那么双磁棒和传感器构成的空间三角形ABL1，ABL2，ABL3的投影将由锐角三角形ABP1变成钝角三角形ABP2和ABP3，乃至蜕变成一条直线。相同程度的角度偏差对于存在较小角的钝角投影三角形的形变影响较大，计算的位置偏差也更大。因此，靠近双磁棒初始所在的XOZ平面（即图6中标号4所示的平面）的区域是低精度区。
由于低精度区的存在，本发明提出了基于双独立旋转磁棒的平均法位置跟踪算法（如图7所示）。不同于传统电磁跟踪位置算法将空间三角形投影到XOY平面进行计算，本发明提出的平均法位置跟踪算法将XOY平面(图7中标号5所示平面)进行旋转，去拟合空间三角形所在的平面(图7中标号6所示平面)。这样不仅可以降低空间三角形在投影过程中产生的形变，还可以使用磁棒A和磁棒B在S平面的第一旋转角度α 1s 和α 2s 双重估计XOY平面拟合空间三角形所在的平面所要旋转的角度α，从而提高角度测量的精确度。
如果任意磁棒的第一旋转平面不是S平面，则需要对该磁棒的第一旋转角α、第二旋转角β进行变换，以便运用在平均位置跟踪算法中。
图8、图9、图10定义了不同旋转平面下的旋转角度计量方式。
当第一旋转平面为T平面时，如图8所示，α 1t 为磁棒A在T平面上的第一旋转角度，β 1t 为磁棒A在与T平面正交的平面上的第二旋转角；α 2t 为磁棒B在T平面上的第一旋转角度，β 2t 为磁棒B在与T平面正交的平面上的第二旋转角。磁棒A和磁棒B经过第一旋转角
Figure DEST_PATH_IMAGE027
，第二旋转角
Figure 900767DEST_PATH_IMAGE028
，可以指向空间任意卦限中的任意位置。规定X轴正方向为第一旋转角等于零的方向，从X轴正方向至Y轴正方向的变化过程为第一旋转角递增过程，从X轴正方向至Y轴负方向的变化过程为第一旋转角递减过程。规定在XOY平面内的第二旋转角为零，从XOY平面出发向Z轴正方向变化是第二旋转角递增，从XOY平面出发向Z轴负方向变化的过程是第二旋转角递增过程。
当第一旋转平面为C平面时，如图9所示，α 1c 为磁棒A在C平面上的第一旋转角度，β 1c 为磁棒A在与C平面正交的平面上的第二旋转角度；α 2c 为磁棒B在C平面上的第一旋转角度，β 2c 为磁棒B在与C平面正交的平面上的第二旋转角度。磁棒A和磁棒B经过第一旋转角
Figure DEST_PATH_IMAGE029
，第二旋转角
Figure 591642DEST_PATH_IMAGE030
，可以指向空间任意卦限中的任意位置。规定Z轴正方向为第一旋转角等于零的方向，从Z轴正方向至X轴正方向的变化过程为第一旋转角递增过程，从Z轴正方向至X轴负方向的变化过程为第一旋转角递减过程。规定在XOZ平面内的第二旋转角为零，从XOZ平面出发向Y轴正方向变化是第二旋转角递增，从XOZ平面出发向Y轴负方向变化的过程是第二旋转角递增过程。
当第一旋转平面为S平面时，如图10所示，α 1s 为磁棒A在S平面上的第一旋转角度，β 1s 为磁棒A在与S平面正交的平面上的第二旋转角度；α 2s 为磁棒B在S平面上的第一旋转角度，β 2s 为磁棒B在与S平面正交的平面上的第二旋转角度。磁棒A和磁棒B经过第一旋转角
Figure DEST_PATH_IMAGE031
，第二旋转角
Figure 925540DEST_PATH_IMAGE032
，可以指向空间任意卦限中的任意位置。规定Y轴正方向为第一旋转角等于零的方向，从Y轴正方向至Z轴正方向的变化过程为第一旋转角递增过程，从Y轴正方向至Z轴负方向的变化过程为第一旋转角递减过程。规定在YOZ平面内的第二旋转角为零，从YOZ平面出发向X轴正方向变化是第二旋转角递增，从YOZ平面出发向X轴负方向变化的过程是第二旋转角递增过程。
假设跟踪目标与磁棒A的距离为r 1，则跟踪目标的位置（x,y,z）可以写成如式（13）所示的形式；假设跟踪目标与磁棒B的距离为r 2，则跟踪目标的位置（x,y,z）可以写成如式（14）所示的形式：
Figure 687960DEST_PATH_IMAGE034
                      （13）
Figure 72805DEST_PATH_IMAGE036
            （14）
经化简可得，磁棒A和磁棒B的角度计量方式是相似的，都可以写成如式（15）、（16）的形式： 
Figure DEST_PATH_IMAGE037
                     （15）
Figure 528057DEST_PATH_IMAGE038
        （16）
由于
Figure 693459DEST_PATH_IMAGE040
与反正弦函数的值域相同，式（15）可直接求解得式（17）：
Figure DEST_PATH_IMAGE041
            （17）
由于与反正弦、反余弦函数的值域不同，式（16）的求解需要分类讨论。
（1）T平面转换为S平面时：
Figure DEST_PATH_IMAGE043
（18）
（2）C平面转换为S平面时：
Figure 452916DEST_PATH_IMAGE044
（19）
因此，通过式（17）-（19）可以将任何搜索状态下磁棒A和磁棒B的旋转角度，变换到S平面下的第一旋转角α 1s ，α 2s 和第二旋转角β 1s ，β 2s 。
最终，由于α 1s 和α 2s 都表示双磁棒、传感器构成的空间平面与平面XOY之间的二面角α，因此可以通过求α 1s 和α 2s 的平均值对二面角α进行校准，如式（20）：
Figure DEST_PATH_IMAGE045
                         （20）
值得注意的是，由于
Figure 282332DEST_PATH_IMAGE046
，在边缘取值时会产生校准误差。如：当
Figure DEST_PATH_IMAGE047
，
Figure 200609DEST_PATH_IMAGE048
时，磁棒A和磁棒B均沿着近似Y负半轴的方向，但是根据式（20）校准的结果是，即表示近似Y正半轴的方向。这是由于区间
Figure 695044DEST_PATH_IMAGE050
在描述-π到π的空间范围的不连续性导致的，因此需要引入校正参数k，对式（20）进行修正，得到平均法角度校准方法（式（3）-（4））。
进一步，根据图7，用磁棒A的第二旋转角度β 1s ，磁棒B的第二旋转角度β 2s 和经过平均法角度校准的二面角α，来表示跟踪物体的位置（x, y, z），如式（21）-（23）所示：
Figure DEST_PATH_IMAGE051
            （21）
Figure 474388DEST_PATH_IMAGE052
                           （22）
Figure DEST_PATH_IMAGE053
                              （23）
其中，as，bs分别是磁棒A，磁棒B与传感器的连线在S平面上两段长度相等的投影。
由式（21）不难推导出as，bs的长度：
             （24）
将式（24）代入式（21）-（23）中，可得平均法位置跟踪算法（式（5）-（7））。
运用Matlab 2009进行仿真：双磁棒之间的固定距离设为30cm。在x=-5~35cm，y=-35~35cm，z=-35~35cm的空间范围内进行了320个不同位置的静态跟踪，为了模拟实际系统中指向角度的不精确性，在精准指向角度的基础上叠加指向角度偏差随机噪声，共进行了3组该空间范围内的静态跟踪实验。每一组实验的角度偏差随机噪声幅度不同， 分别为±0.5°，±1°，±2°。每组实验均采用投影法电磁跟踪位置算法和平均法电磁跟踪位置算法，这两种算法进行定位跟踪。跟踪的位置误差通过估计位置和实际位置之间的欧氏距离进行评估。仿真结果显示：不论是投影法电磁跟踪位置算法，还是平均法电磁跟踪位置算法，随机噪声幅度越大，该组实验的平均位置误差越大。±0.5°角度噪声下，投影法电磁跟踪位置算法的位置误差为11.722cm，平均法电磁跟踪位置算法的位置误差为0.336cm，平均法电磁跟踪位置算法的位置误差相比投影法电磁跟踪位置算法的位置误差下降了97.1%。±1°角度噪声下，投影法电磁跟踪位置算法的位置误差为12.027cm，平均法电磁跟踪位置算法的位置误差为0.628cm，平均法电磁跟踪位置算法的位置误差相比投影法电磁跟踪位置算法的位置误差下降了94.8%。±2°角度噪声下，投影法电磁跟踪位置算法的位置误差为12.036cm，平均法电磁跟踪位置算法的位置误差为1.322cm，平均法电磁跟踪位置算法的位置误差相比投影法电磁跟踪位置算法的位置误差下降了89.0%。因此，平均法电磁跟踪位置算法的定位精度更高。

Claims (2)

1.一种基于双独立旋转磁棒电磁跟踪的位置跟踪方法，其特征在于具体步骤为：
（一）首先，使用基于双磁棒的可变旋转平面的电磁跟踪搜索方法，得到双磁棒旋转角度；
（二）然后，对双磁棒旋转角度进行角度转换预处理和角度校准；
（三）最后，运用平均位置跟踪算法，得到跟踪物体的位置；
步骤（一）所述的基于双磁棒的可变旋转平面的电磁跟踪搜索方法，是利用两个独立磁棒的可变旋转平面进行旋转搜索的过程，具体步骤为：设两个相距固定距离d同向放置在水平面上的磁棒，分别根据前一时刻跟踪物体的位置（x 0, y 0, z 0），各自独立地选择最优的第一旋转平面，每个磁棒分别进行两个正交平面上的旋转搜索；两个磁棒分别记为磁棒A和磁棒B；其中：
所述的跟踪物体的位置（x 0, y 0, z 0）由参考坐标系确定，参考坐标系的原点为磁棒A的固定不动点，X轴为磁棒A和磁棒B连线方向，Z轴为水平面向上的法线方向，Y轴符合右手坐标系法则；
所述的第一旋转平面有三种可选平面：
（1）T平面：与平面XOY平行的平面；
（2）C平面：与平面XOZ平行的平面；
（3）S平面：与平面YOZ平行的平面；
所述的最优第一旋转平面的选择依据是：
对于磁棒A而言，当                                                
Figure 2013102986909100001DEST_PATH_IMAGE001
时，选择跟踪目标所在的S平面作为磁棒A的第一旋转平面，垂直于S平面、与S平面相交于第一次旋转结束时磁棒指向的平面作为磁棒A的第二旋转平面；当
Figure 284756DEST_PATH_IMAGE002
时，选择跟踪目标所在的C平面作为磁棒A的第一旋转平面，垂直于C平面、与C平面相交于第一次旋转结束时磁棒指向的平面作为磁棒A的第二旋转平面；当
Figure 2013102986909100001DEST_PATH_IMAGE003
时，选择跟踪目标所在的T平面作为磁棒A的第一旋转平面，垂直于T平面、与T平面相交于第一次旋转结束时磁棒指向的平面作为磁棒A的第二旋转平面；当中的某两个或三个相等时，选择与前一时刻磁棒A的第一旋转平面一致的搜索效率较高的第一旋转平面；
对于磁棒B而言，当
Figure 2013102986909100001DEST_PATH_IMAGE005
时，选择跟踪目标所在的S平面作为磁棒B的第一旋转平面，垂直于S平面、与S平面相交于第一次旋转结束时磁棒指向的平面作为磁棒B的第二旋转平面；当时，选择跟踪目标所在的C平面作为磁棒B的第一旋转平面，垂直于C平面、与C平面相交于第一次旋转结束时磁棒指向的平面作为磁棒B的第二旋转平面；当
Figure DEST_PATH_IMAGE007
最小时，选择跟踪目标所在的T平面作为磁棒B的第一旋转平面，垂直于T平面、与T平面相交于第一次旋转结束时磁棒指向的平面作为磁棒B的第二旋转平面；当
Figure 32635DEST_PATH_IMAGE008
中的某两个或三个相等时，选择与磁棒B的前一时刻第一旋转平面一致的搜索效率较高的第一旋转平面；
步骤（二）中所述的角度转换预处理，是当任意磁棒的第一旋转平面不是S平面，需要对该磁棒的第一旋转角α、第二旋转角β进行变换：
（1）T平面转换为S平面：
Figure DEST_PATH_IMAGE009
      当
Figure 112586DEST_PATH_IMAGE010
时
或
Figure DEST_PATH_IMAGE011
当时
Figure 967672DEST_PATH_IMAGE012
（2）C平面转换为S平面：
Figure DEST_PATH_IMAGE013
其中，第一旋转角
Figure 562602DEST_PATH_IMAGE014
，第二旋转角
Figure DEST_PATH_IMAGE015
；*=1或2，α 1t 为磁棒A在T平面上的第一旋转角度，β 1t 为磁棒A在与T平面正交的平面上的第二旋转角；α 1c 为磁棒A在C平面上的第一旋转角度，β 1c 为磁棒A在与C平面正交的平面上的第二旋转角度；α 1s 为磁棒A在S平面上的第一旋转角度，β 1s 为磁棒A在与S平面正交的平面上的第二旋转角度；α 2t 为磁棒B在T平面上的第一旋转角度，β 2t 为磁棒B在与T平面正交的平面上的第二旋转角；α 2c 为磁棒B在C平面上的第一旋转角度，β 2c 为磁棒B在与C平面正交的平面上的第二旋转角度；α 2s 为磁棒B在S平面上的第一旋转角度，β 2s 为磁棒B在与S平面正交的平面上的第二旋转角度；
所述的角度校准采用平均法，由于α 1s 和α 2s 都表示双磁棒、传感器构成的空间平面与平面XOY之间的二面角α，因此，通过求平均值校准二面角α，其中：
Figure DEST_PATH_IMAGE017
；
步骤（三）所述的平均位置跟踪算法，是根据磁棒A的第二旋转角度β 1s ，磁棒B的第二旋转角度β 2s 和经过平均法角度校准的二面角α，计算跟踪物体的位置（x, y, z）：
Figure DEST_PATH_IMAGE019
Figure DEST_PATH_IMAGE021
Figure DEST_PATH_IMAGE023
。
2.一种使用如权利要求1所述的基于双独立旋转磁棒电磁跟踪的位置跟踪方法的电磁跟踪系统，其特征在于包括实现所述双独立磁棒可变旋转平面的电磁跟踪搜索方法的控制模块，以及对双磁棒旋转角度进行角度转换预处理和角度校准的模块，平均位置跟踪算法模块。
CN201310298690.9A 2013-07-16 2013-07-16 基于双独立旋转磁棒电磁跟踪的位置跟踪方法及系统 Expired - Fee Related CN103412337B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201310298690.9A CN103412337B (zh) 2013-07-16 2013-07-16 基于双独立旋转磁棒电磁跟踪的位置跟踪方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201310298690.9A CN103412337B (zh) 2013-07-16 2013-07-16 基于双独立旋转磁棒电磁跟踪的位置跟踪方法及系统

Publications (2)

Publication Number Publication Date
CN103412337A true CN103412337A (zh) 2013-11-27
CN103412337B CN103412337B (zh) 2015-12-09

Family

ID=49605364

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201310298690.9A Expired - Fee Related CN103412337B (zh) 2013-07-16 2013-07-16 基于双独立旋转磁棒电磁跟踪的位置跟踪方法及系统

Country Status (1)

Country Link
CN (1) CN103412337B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104776865A (zh) * 2015-03-21 2015-07-15 复旦大学 基于最大磁感应强度矢量旋转角快速测定的电磁跟踪系统及方法
US10151606B1 (en) 2016-02-24 2018-12-11 Ommo Technologies, Inc. Tracking position and movement using a magnetic field
US10276289B1 (en) 2018-06-01 2019-04-30 Ommo Technologies, Inc. Rotating a permanent magnet in a position detection system
WO2022198615A1 (zh) * 2021-03-26 2022-09-29 中国科学院深圳先进技术研究院 一种双臂机器人穿刺系统标定方法及系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2007069170A2 (en) * 2005-12-15 2007-06-21 Koninklijke Philips Electronics, N.V. Method and apparatus for position optimization of a field generator
NO329285B1 (no) * 2008-05-30 2010-09-27 Forsvarets Forsknings Meldingsutvekslingssystem og -anordning
CN101852868A (zh) * 2010-05-20 2010-10-06 复旦大学 一种基于双磁棒旋转搜索的电磁跟踪方法及系统
CN102736112A (zh) * 2012-07-23 2012-10-17 复旦大学 可变旋转平面的电磁跟踪搜索方法及其系统

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2007069170A2 (en) * 2005-12-15 2007-06-21 Koninklijke Philips Electronics, N.V. Method and apparatus for position optimization of a field generator
NO329285B1 (no) * 2008-05-30 2010-09-27 Forsvarets Forsknings Meldingsutvekslingssystem og -anordning
CN101852868A (zh) * 2010-05-20 2010-10-06 复旦大学 一种基于双磁棒旋转搜索的电磁跟踪方法及系统
CN102736112A (zh) * 2012-07-23 2012-10-17 复旦大学 可变旋转平面的电磁跟踪搜索方法及其系统

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104776865A (zh) * 2015-03-21 2015-07-15 复旦大学 基于最大磁感应强度矢量旋转角快速测定的电磁跟踪系统及方法
CN104776865B (zh) * 2015-03-21 2017-07-07 复旦大学 基于最大磁感应强度矢量旋转角快速测定的电磁跟踪系统及方法
US10151606B1 (en) 2016-02-24 2018-12-11 Ommo Technologies, Inc. Tracking position and movement using a magnetic field
US10704929B1 (en) 2016-02-24 2020-07-07 Ommo Technologies, Inc. Tracking position and movement using a magnetic field
US10276289B1 (en) 2018-06-01 2019-04-30 Ommo Technologies, Inc. Rotating a permanent magnet in a position detection system
WO2022198615A1 (zh) * 2021-03-26 2022-09-29 中国科学院深圳先进技术研究院 一种双臂机器人穿刺系统标定方法及系统

Also Published As

Publication number Publication date
CN103412337B (zh) 2015-12-09

Similar Documents

Publication Publication Date Title
CN102426392B (zh) 一种基于正交磁棒旋转搜索的电磁跟踪方法及系统
CN108230247B (zh) 基于云端的三维地图的生成方法、装置、设备及计算机可读的存储介质
US10234306B2 (en) Method and apparatus for synchronizing a transmitter and receiver in a magnetic tracking system
CN103575271A (zh) 基于电控旋转磁场的电磁跟踪系统及方法
CN111815686B (zh) 基于几何特征由粗到细点云配准方法
WO2019161517A1 (zh) 基于云端的轨迹地图生成方法、装置、设备及应用程序
CN103412337B (zh) 基于双独立旋转磁棒电磁跟踪的位置跟踪方法及系统
CN111986219B (zh) 一种三维点云与自由曲面模型的匹配方法
CN106091924B (zh) 一种使用激光校正贴码误差的系统和方法
CN111982099B (zh) 机器人混合定位方法、装置、设备及计算机可读介质
CN101852868B (zh) 一种基于双磁棒旋转搜索的电磁跟踪方法及系统
CN108662973A (zh) 基于鉴相技术的电磁跟踪系统及方法
He et al. Simple 3-D point reconstruction methods with accuracy prediction for multiocular system
Li et al. Kfs-lio: Key-feature selection for lightweight lidar inertial odometry
Ma et al. Efficient rotation estimation for 3D registration and global localization in structured point clouds
CN105203096B (zh) 基于四点测量的旋转磁场快速跟踪方法及系统
CN108592860A (zh) 用于机载光电观瞄系统基轴对准操作后的瞄准线输出角度计算方法
You et al. Global localization for a small mobile robot using magnetic patterns
CN201576308U (zh) 一种复杂曲面的配准系统
CN104977559A (zh) 一种在干扰环境下的目标定位方法
CN102736112B (zh) 可变旋转平面的电磁跟踪搜索方法及其系统
CN104391272B (zh) 利用测向数据进行目标定位的方法与系统
Wang et al. High precision and fast estimation of position and attitude measurement for space targets
Kupervasser et al. Robust positioning of drones for land use monitoring in strong terrain relief using vision-based navigation
Thomas et al. A monocular SLAM method for satellite proximity operations

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20151209

Termination date: 20200716