[发明专利]用于动态校准磁场传感器的方法和系统

权利要求书

1.一种用于动态校准磁场传感器的方法，其特征在于，包括以下步骤：

步骤S1，将磁场传感器的原始三维磁场数据顺序保存于长度为L的队列中；

步骤S2，在所述的长度为L的队列中的数据中寻找出满足预设条件的N个数据作为椭球拟合运算的拟合数组；

步骤S3，根据所述的N个数据的拟合数组，进行椭球拟合运算，拟合出椭球的6个校准拟合参数；

步骤S4，用所述的6个校准拟合参数对所述的原始三维磁场数据进行校准，获得校准后的磁场数据，返回步骤S1。

2.根据权利要求1所述的方法，其特征在于，其中所述的步骤S1中的队列是先进先出(First In First Out，FIFO)队列，或环形队列。

3.根据权利要求1所述的方法，其特征在于，其中所述的步骤S2中的寻找出满足预设条件的N个数据包括：步骤S21，寻找X列的最大值和最小值，将所对应的三维数据保存于所述的拟合数组中；步骤S22，寻找Y列的最大值和最小值，将所对应的三维数据保存于所述的拟合数组中；步骤S23，寻找Z列的最大值和最小值，将所对应的三维数据保存于所述的拟合数组中。

4.根据权利要求3所述的方法，其特征在于，其中所述的步骤S2中的寻找出满足预设条件的N个数据还包括：步骤S24，根据X列最大值和最小值对应的索引值，分别取最大索引值减最小索引值的1/4索引和3/4索引，对应的三维数据保存于所述的拟合数组中；步骤S25，根据Y列最大值和最小值对应的索引值，分别取最大索引值减最小索引值的1/4索引和3/4索引，对应的三维数据保存于所述的拟合数组中；步骤S26，根据Z列最大值和最小值对应的索引值，分别取最大索引值减最小索引值的1/4索引和3/4索引，对应的三维数据保存于所述的拟合数组中；其中，所述的索引值为最大值和最小值在长度为L的队列中的序号。

5.根据权利要求3或4所述的方法，其特征在于，其中所述的步骤S2中的寻找出满足预设条件的N个数据还包括：随机选取预定数量的三维数据保存于所述的拟合数组中。

6.根据权利要求1所述的方法，其特征在于，所述的步骤2中的N≥6。

7.根据权利要求1所述的方法，其特征在于，所述的步骤S3中的根据所述的N个数据的拟合数组，拟合出椭球的6个校准拟合参数，采用的是最小二乘拟合方法，包括：

步骤S300，利用(x-x₀)²+A²(y-y₀)²+B²(z-z₀)²＝R²来定义椭球，[x₀，y₀，z₀]代表椭球的偏离坐标原点的坐标，也是椭球拟合得到的椭球中心，A代表椭球Y轴的半轴长/椭球X轴的半轴长的比例，B代表椭球Z轴的半轴长/椭球X轴的半轴长的比例，R代表椭球X轴的半轴长，xyz为所述N个数据的三维磁场数据；

步骤S301，将椭球公式进一步整理为：

步骤S302，将上式用矩阵表示：

DW＝C

其中，

该矩阵表达式有最小二乘解：

W＝(D^TD)^-1D^TC，

得出6个拟合参数X₀、Y₀、Z₀、A、B、R。

8.根据权利要求7所述的方法，其特征在于，所述的步骤S3之后还包括：步骤S31，对所述的校准拟合参数进行检查，判断校准拟合参数是否正确，如果正确则进行后续步骤，如果不正确则返回步骤S1。