(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211207979.0 (22)申请日 2022.09.30 (71)申请人 南京理工大 学 地址 210094 江苏省南京市玄武区孝陵卫 街道孝陵卫 街200号 (72)发明人 钱晨　高阳　陈庆伟　吴益飞　 郭健　 (74)专利代理 机构 南京天华专利代理有限责任 公司 32218 专利代理师 刘畅　徐冬涛 (51)Int.Cl. G01C 25/00(2006.01) G06F 30/20(2020.01) G06F 17/16(2006.01) G06F 17/18(2006.01) (54)发明名称 基于采样点权重自适应分配的三轴磁传感 器误差补偿方法 (57)摘要 本发明公开了一种基于采样点权重自适应 分配的三轴磁传感器误差补偿方法， 建立考虑载 体磁场误差和自身误差的三轴地磁传感器误差 模型； 在测量过程中地磁强度恒定的假设下， 建 立量测的椭球面模型； 利用最小二乘法处理量测 数据对椭球面进行拟合获取椭球面参数初始值； 计算各采样点的残差并带入构造的权重代价函 数中， 为各采样点分配权重系数； 通过权重系数 组成的权重矩阵对椭球面参数进行更新， 直至满 足收敛条件； 最后计算校正矩阵和偏移矢量完成 误差补偿 。 本发明方法针对量测中的异常值引入 基于残差的权重自适应分配算法， 降低异常值在 椭球拟合中的影响， 提高误差补偿算法的鲁棒 性。 权利要求书3页 说明书8页 附图4页 CN 115420305 A 2022.12.02 CN 115420305 A 1.一种基于采样点权重自适应分配的三轴磁传感器误差补偿方法， 其特征在于， 包括 以下步骤： 步骤1， 考虑载体磁场误差和自身误差对地磁传感器量测带来的影响， 建立磁测误差模 型； 步骤2： 根据地磁场强度为恒定常值的特性， 结合磁测误差模型， 建立三轴磁传感器量 测的椭球面模型； 步骤3： 根据磁传感器采样数据对椭球面进行拟合， 利用最小二乘法计算椭球面参数的 估计值作为初始值； 步骤4： 针对采样点异常值引入稳健算法， 构造基于残差的权重代价函数， 为各采样点 量测信息分配权 重系数并构成权 重矩阵， 降低异常采样点在 椭球拟合过程中带来的影响； 步骤5： 结合权重矩阵对椭球参数进行更新， 并判断是否满足收敛条件， 若不满足返回 步骤4再次计算权 重矩阵， 若满足输出椭球面 参数； 步骤6： 根据椭球面 参数， 计算校正矩阵和偏移矢量， 完成误差补偿。 2.根据权利要求1所述的方法， 其特征在于， 步骤1中， 磁测误差包括磁传感器的误差和 载体磁场的干扰误差， 其中： 磁传感器的误差模型 具体为： 其中Hm为考虑了自身误差的地磁量测值， 为真实磁场强度； CN和Csi分别表示非正交 误差的非正交矩阵和灵敏度误差矩阵， 表示 为： 其中， 和φ为轴偏移误差角， Sx、 Sy、 Sz分别为x、 y、 z轴上灵敏度 误差系数； b0＝[b0x b0y  b0z]T为零位漂移误差矩阵， b0x、 b0y和b0z分别为三轴方向对应的零偏值； 载体磁场模型： 其中， 表示地磁场矢量值， 表示感应磁场干扰， Cs为磁化系数； 为 固定磁场干扰， 是一个常值向量； ε∈R3是高斯白噪声； 磁测误差模型为： Hm＝CsiCN((I+Cs)He+bh)+b0+ ε＝CsiCN(I+Cs)He+CsiCNbh+b0+ ε   式(4) 式中， 为三轴磁传感器量测值， 为当地地磁场强 度， CN为非正交误差的非正交矩阵， Csi为灵敏度误差矩阵， Cs为磁化系数， bh∈R3为固定磁场 干扰， b0∈R3为零位偏移误差 矢量。 3.根据权利要求2所述的方法， 其特征在于， 步骤2中， 建立三轴磁传感器量测的椭球面 模型的具体方法为： 首先， 保证误差补偿测量过程中磁场满足以下要求： (1)区域内磁场保持恒定无变化； (2)测量期间无外 部磁场干扰； 然后， 建立 三轴磁传感器量测的椭球面模型：权　利　要　求　书 1/3 页 2 CN 115420305 A 2其中， bm表示偏移矢量， 记为bm＝CsiCNbh+b0， 旋转矩阵Dm＝[CsiCN(I+Cs)]‑1。 4.根据权利要求1所述的方法， 其特征在于， 步骤3 中， 实现以最小二乘法计算椭球面参 数初始值的具体方法为： 椭球面的归一 化方程写为： ax2+by2+cz2+2dxy+2exz+2fyz+2px+2qy+2 rz＝1      式(8) 将公式(8)简化 为向量形式则有： 式中， 代表了由第i个 采样点量测值构成的向量， ξ＝[a  b c d e f p q r]表示椭球面的参数； 由于采样点数量 远大于椭球面参数个数， 因此椭球面参数无精确解， 通过最小二乘法进 行椭球参数拟合， 计 算公式为： ξ(0)＝(HTH)‑1HTη       式(10) 其中， η为元素全为1的n维列向量， n表示采样点个数， 量测值矩阵H由各采样点量测组 合而成： 5.根据权利要求1所述的方法， 其特征在于， 步骤4中， 为各采样点量测信息分配权重系 数并构成权重矩阵： 针对采样过程中的异常值会影响椭球参数的问题， 引入稳健算法自适 应为各采样点分配权重， 为偏离较大 的采样点设置低权重， 为偏离值较小的采样点设置高 权重， 从而降低异常点对拟合结果的影响。 6.根据权利要求5所述的方法， 其特 征在于， 权 重矩阵获取的具体步骤为： 步骤4‑1， 计算各采样点的残差ri， 计算公式为： 其中， 代表了由第i个 采样点量测值构成的向量， ξ(t)为更新t次后的椭球面 参数， 步骤4‑2， 为了获得估计的尺度同变性， 对残差进行 标准化处理， 公式为： μi＝ri/MAD         式(12) 式中， MAD表示 绝对离差中位数， 定义 为： 步骤4‑3， 设计基于标注化残差的权 重代价函数： 式中， k1和k2为权重代价函数的参数； 步骤4‑4， 最终构成采样点的权 重矩阵， 记为： Ψt＝diag[ωi(ui)]          式(14) 式中， t∈N表示迭代次数。权　利　要　求　书 2/3 页 3 CN 115420305 A 3