[发明专利]基于微动平台的磁跟踪系统的磁场源标定方法及系统

权利要求书

1.一种基于微动平台和旋转磁场的磁跟踪系统的磁场源标定系统，其特征在于由下述四个部分组成：磁场源、三轴磁传感器装置、控制处理显示装置以及三轴微动平台；其中：

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

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

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

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

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

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

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

2.一种基于权利要求1所述的电磁跟踪系统磁场源原点及初始姿态标定方法，其特征在于，首先根据右手螺旋法则定义如表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的实际原始坐标系

具体步骤如下：

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

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

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

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

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

步骤六、运用磁场源原点坐标标定算法，计算第一磁场源、第二磁场源的原点坐标；

步骤七、运用磁场源初始姿态标定算法，计算第一磁场源、第二磁场源的各坐标轴的初始指向；

步骤一所述标定系统初始化，是指开机后，由控制处理显示装置控制三轴微动平台、第一磁场源和第二磁场源移动/转动至各自初始位置；

步骤二所述由三轴微动平台控制三轴磁传感器装置移向空间已知坐标点，是通过三轴微动平台控制单元分别控制微动平台三个轴移动，将三轴磁传感器装置置于选定的已知坐标点；设选定的已知坐标点为5个： n1、n2、n3、m2、m3，设m1 与n3重合，这些坐标点符合如下条件：  

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

步骤三所述以第一磁场源和第二磁场源所对应的模拟磁棒，搜索并指向所述三轴磁传感器装置，是由激励电流强度控制模块交替激励由正交三轴线圈组成的第一磁场源和第二磁场源，模拟两个可旋转的磁棒；具体激励方式为：设组成磁场源的三个正交线圈的过圆心轴分别为x轴，y轴，z轴，在激励周期的第一个时间段，在x轴发射线圈上给予幅度为Asin(φ)安培的激励电流，在y轴发射线圈上给予幅度为Acos(φ)安培的激励电流，以合成大小为A安培，在x-y平面旋转φ度的磁棒，控制φ在0-2π变化，当模拟磁棒指向传感器在x-y平面投影方向时，传感器可检测到x-y平面上的最大磁感应强度；之后，x轴发射线圈上给予幅度为Asin(φ_ij)* sin(θ)安培的激励电流, y轴发射线圈上给予幅度为Acos(φ_ij) * sin(θ)安培的激励电流，φ_ij即模拟磁棒在x-y平面指向传感器投影时的旋转角度；此时在z轴发射线圈上给予幅度为A cos (θ)安培的激励电流，即合成在垂至于x-y平面旋转的模拟磁棒；控制θ 在0-π范围内变化，当模拟磁棒指向传感器时，传感器检测到最大磁感应强度，记此时模拟磁棒在垂直平面的旋转角度为θ_ij ；

步骤四所述记录第一磁场源、第二磁场源所模拟的磁棒从初始位置到指向三轴磁传感器装置过程中的水平和垂直旋转角，即水平旋转角度φ_ij和垂直旋转角度θ_ij，其中i=1-2对应于两个磁场源，j=1-5对应于空间五个坐标已知的点，下同；

步骤六所述磁场源原点坐标标定算法，是根据步骤四记录到的φ_ij和θ_ij，计算第一磁场源、第二磁场源在微动平台坐标系中的原点坐标（x1,y1,z1）、（x2,y2,z2），其算式为：

其中（n1x, n1y, n1z）是n1点在CS6中的坐标，其余依此类推，（x_i,y_i,z_i）为磁场源原点坐标；r_i1、r_i2、r_i3、r_i5、r_i6则由下面一组公式计算：

其中的α_ij则由下式得到：

 ，

i=1、2, j=1、2、3、4;

步骤七所述磁场源初始姿态标定算法，根据表1，将CSr到CSs之间的旋转关系表示为R_rs，r=1-7，s=1-7，对磁场源初始姿态的标定转化成寻找第二磁场源理想原始坐标系CS2与其当前坐标系CS4之间的旋转关系，即获得R₂₄：

；

, ，为得到校正后的旋转角度。