[发明专利]基于抗差估计的惯性地磁匹配定位方法

摘要

本发明提供了一种基于抗差估计的惯性地磁匹配定位方法，属于惯性地磁匹配定位方法技术领域。本发明以惯导系统输出的参考轨迹、计算机中的地磁图、地磁传感器实测值作为输入，用基于抗差估计的惯性地磁匹配定位方法解算出参考轨迹初始经纬度误差、航向误差，将解算得到的经纬度误差、航向误差代入匹配轨迹与参考轨迹的关系方程即得到匹配结果。本发明有效的消除地磁传感器测量数据中存在野值对匹配定位精度的影响，提高了对于测量误差不太大也不太小的介于野值和有效数据之间的地磁测量数据的处理能力，在磁场实时测量信息中存在野值的情况下仍然可以有效的实现高精度匹配定位，具有较高的定位精度，具备抗野值能力。

主权项

一种基于抗差估计的惯性地磁匹配定位方法，其特征在于，步骤1，从惯导系统读取当前时刻以及前N‑1个时刻的待匹配点的位置测量值ai和bi，其中ai表示经度，bi表示纬度，下标i表示不同时刻，i＝1…N，N为整数且N＞2，i为1表示当前时刻，通过地磁传感器获得当前时刻以及前N‑1个时刻的地磁场强度的测量值Ii；步骤2，根据惯导系统指示的N个待匹配点的位置，从预先存储的地磁数据库中分别读取相应的地磁场强度的参考值I(ai,bi)、地磁场强度的梯度的参考值Ix,i和Iy,i，其中Ix,i表示地磁场强度在经度方向的梯度在第i点位置上的取值，Iy,i表示地磁场强度在纬度方向的梯度在第i点位置上的取值；步骤3，引入并初始化经纬度误差、航向误差M：M＝[Δx Δy α]T＝[0 0 0]T步骤4，根据公式(1)和公式(2)计算参数h和l：h=1R---(1)]]>l＝‑hg(M)  (2)其中R为预设循环次数，g(M)＝[g1(M) g2(M) g3(M)]T；g1(M)=Σi=1NPiIx,i;]]>g2(M)=Σi=1NPiIy,i;]]>g3(M)=Σi=1NPi[Ix,i(-ai′sinα+bi′cosα)+Iy,i(-ai′cosα-bi′sinα)];]]>Pi=QiQi2+ϵ;]]>Qi＝Ix,i(a′icosα+b′isinα‑a′i+Δx)+Iy,i(‑a′isinα+b′icosα‑b′i+Δy)+It,i；a′i＝ai‑a1，b′i＝bi‑b1，It,i＝I(ai,bi)‑Ii；ε为调节参数，ε＞0且使ε与处于同一数量级；步骤5，根据公式(3)～(6)计算参数k1、k2、k3、k4：k1＝[f(Y)]‑1|Y＝M×l  (3)k2=[f(Y)]-1|Y=M+12k1×l---(4)]]>k3=[f(Y)]-1|Y=M+12k2×l---(5)]]>k4=[f(Y)]-1|Y=M+k3×l---(6)]]>其中f(Y)=f11(Y)f12(Y)f13(Y)f12(Y)f22(Y)f23(Y)f13(Y)f23(Y)f33(Y);]]>f11(Y)=Σi=1NIx,i2Hi;]]>f12(Y)=Σi=1NIx,iIy,iHi;]]>f13(Y)=Σi=1NIx,i[Ix,i(-ai′sinα+bi′cosα)+Iy,i(-ai′cosα-bi′sinα)]Hi;]]>f22(Y)=Σi=1NIy,i2Hi;]]>f23(Y)=Σi=1NIy,i[Ix,i(-ai′sinα+bi′cosα)+Iy,i(-ai′cosα-bi′sinα)]Hi;]]>f33(Y)=Σi=1N[Ix,i(-ai′sinα+bi′cosα)+Iy,i(-ai′cosα-bi′sinα)]2Hi+[Ix,i(-ai′cosα-bi′sinα)+Iy,i(ai′sinα-bi′cosα)]Pi;]]>Hi=ϵ(Qi2+ϵ)3;]]>步骤6，计算经纬度误差、航向误差的增量δM：δM=16(k1+2k2+2k3+k4)---(7)]]>步骤7，更新经纬度误差、航向误差M：M＝M+δM  (8)步骤8，判断迭代次数是否达到预设循环次数R，若达到则停止迭代并跳到步骤9，否则跳到步骤5；步骤9，根据迭代计算得到经纬度误差和航向误差Δx、Δy和α，将所得结果代入匹配轨迹与参考轨迹的关系方程(9)即得匹配轨迹；uivi=cosαsinα-sinαcosαai-a1bi-b1+a1b1+ΔxΔy---(9)]]>其中ui为第i时刻匹配结果的位置经度，vi为第i时刻匹配结果的位置纬度。