[发明专利]无线胶囊定位装置、磁场传感器定位方法和装置

权利要求书

1.一种磁场传感器定位方法，其特征在于，包括如下步骤：

获取多个不同频率磁源环境下的交变磁场信号；

根据所述交变磁场信号，计算每个磁源的在空间中若干方向的测量磁场幅值；

根据局域坐标系和世界坐标系的对应关系，将磁源的理论幅值算法修正为所述局域坐标系中的理论磁场幅值算法信息，其中，所述局域坐标系为所述磁场传感器所在坐标系，所述世界坐标系为所述磁源所在坐标系，所述理论磁场幅值算法信息包括用于标定所述磁场传感器的位置和/或姿态的变量参数；

根据所述测量磁场幅值和所述理论磁场幅值算法信息，计算所述磁场传感器的位置和/或姿态的变量参数的值。

2.根据权利要求1所述的磁场传感器定位方法，其特征在于，所述获取多个不同频率磁源环境下的交变磁场信号包括：

获取至少两个不同频率磁源环境下的交变磁场信号。

3.根据权利要求2所述的磁场传感器定位方法，其特征在于，所述获取多个不同频率磁源环境下的交变磁场信号包括：

获取至少两个磁源的频率呈倍数关系的环境下的交变磁场信号。

4.根据权利要求3所述的磁场传感器定位方法，其特征在于，所述磁源为永磁体，所述永磁体做旋转运动，且至少两个所述永磁体的转速呈倍数关系。

5.根据权利要求1所述的磁场传感器定位方法，其特征在于，所述交变磁场信号包括每个磁源在所述世界坐标系中的空间坐标信息、以及若干个坐标轴方向的磁场向量信息。

6.根据权利要求1所述的磁场传感器定位方法，其特征在于，所述计算每个磁源的在空间中若干方向的测量磁场幅值包括：

计算每个磁源在所述局域坐标系中的X、Y、Z轴方向的磁场的幅值。

7.根据权利要求1所述的磁场传感器定位方法，其特征在于，所述将磁源的理论幅值算法修正为所述局域坐标系中的理论磁场幅值算法信息包括：

在所述理论幅值算法中引入旋转矩阵R，并将所述理论幅值算法中的场点坐标替换为所述磁场传感器与第n个磁源的相对位置信息，其中所述旋转矩阵R是所述世界坐标系到所述局域坐标系的旋转矩阵；

所述理论磁场幅值算法信息包括每个磁源在所述局域坐标系中的X、Y、Z轴方向的理论幅值的计算公式，所述计算公式中包括用于标定所述磁场传感器的位置和/或姿态的变量参数。

8.根据权利要求1所述的磁场传感器定位方法，其特征在于，其中，计算所述磁场传感器的位置和/或姿态的变量参数的值的步骤包括：

根据所述测量磁场幅值和所述理论磁场幅值算法信息，建立计算模型，所述计算模型为：

，

其中，为磁源的理论磁场幅值，为磁源的测量磁场幅值，n表示对应的第n个磁源，i为所述局域坐标系的X、Y、Z轴，为所述磁场传感器的坐标向量，R为包括所述磁场传感器姿态参数的旋转矩阵。

9.根据权利要求8所述的磁场传感器定位方法，其特征在于，当所述坐标向量和/或所述姿态参数出现多个解时，还包括步骤：

通过惯性导航算法，根据上一时刻的解计算下一时刻的预测解；

将所述预测解与下一时刻的实测值比较计算差值；

将所述差值小于预设差值的预测解设定为正确解。