[发明专利]一种基于已知姿态角的磁力计标定方法

权利要求书

1.一种基于已知姿态角的磁力计标定方法，其特征在于，包括如下步骤：

步骤1，在室外空旷区域选择任意选择一个位置，并根据位置的经纬度坐标查询地球磁场参考模型，得到该位置的磁场总强度参考值M_{n_IGRF}；

步骤2，以传感器测量中心为旋转中心，使传感器绕着载体坐标系的X、Y、Z三轴各旋转多圈，并获取标定过程中每个历元的姿态角及磁场信息；

步骤3，建立磁力计测量模型，确定待估参数；对磁力计的测量模型进行建模时，考虑磁力计受到软磁误差、比例因子误差、交轴耦合误差、零位偏置误差、硬铁误差及传感器噪声，载体坐标系下磁力计的测量模型如下：

对磁力计原始观测数据m_{b_init}进行均值滤波，降低磁力计噪声n_M，得到m_b；将的乘积作为一个待估矩阵，记为将b_M和b_HI之和作为一个待估矩阵，记为B_M；模型简化后如下式，待估参数为和B_M；

其中，b表示载体坐标系b系，b系是以惯性传感器IMU相位中心为原点，x轴指向载体前进方向，y轴垂直于x轴指向载体右侧，z轴与x轴和y轴垂直并构成右手系；表示载体坐标系下磁场强度的真实值；表示磁力计坐标系到惯性传感器IMU坐标系的姿态旋转矩阵，为3×3方阵；C_SI表示软磁效应变化矩阵；C_NO是磁力计三轴由非正交转化为正交的变化矩阵，为3×3方阵，主对角线元素为0；S_M为比例因子，为3×3方阵，只有主对角线元素有数据，其余元素为0；m_{b_init}表示磁力计原始观测数据；b_M表示磁力计零位偏置；b_HI表示硬磁效应误差；n_M为磁力计噪声矢量；

将磁力计的测量模型进行泰勒展开，使用最小二乘法迭代求得待估参数。

2.根据权利要求1所述一种基于已知姿态角的磁力计标定方法，其特征在于：对磁力计测量模型进行迭代计算时，首先以m_b作为初次迭代计算时载体坐标系下磁场强度参考真值，即之后迭代计算时通过求得，然后求得当地坐标系下的磁场强度真值计算关系如下式：

式中，k表示第k个历元，j表示历元的总个数；表示第k个历元载体坐标系到当地坐标系的姿态旋转矩阵，表示第k个历元载体坐标系下磁场强度参考真值；n表示当地坐标n系，n系是以惯性传感器IMU相位中心为原点，x轴平行于当地水平面指向正北，y轴平行于当地水平面指向正东，z轴垂直于当地水平面向下，三者构成右手系；

在基于磁强总强度不变的假设下，根据M_{n_IGRF}求得其磁场总强度||M_{n_IGRF}||，通过计算出与||M_{n_IGRF}||的比例关系，将各轴的数据等比例变化，得到M_{n_ture}，将其作为当地坐标系下的磁场真值，进而得到载体坐标系下磁场强度参考真值M_{n_ture}及计算过程如下：

其中，表示当地坐标系到载体坐标系的姿态旋转矩阵。