[发明专利]基于仿射变换的惯性/地磁匹配迭代定位方法

摘要

本发明涉及一种基于仿射变换的惯性/地磁匹配迭代定位方法，步骤包括：从惯导系统读取待匹配位置测量值，从磁强计获得地磁场强度信息；根据惯导系统指示的位置，从地磁数据库中读取地磁场强度和地磁场强度的梯度；引入并初始化匹配位置偏移量、角度偏移量和伸缩系数；计算迭代参数；计算匹配位置偏移量的增量、角度偏移量的增量和伸缩系数的增量；更新匹配位置偏移量、角度偏移量和伸缩系数；根据终止迭代条件判断是否更新迭代变量，进行重复迭代；计算输出匹配定位结果。本发明能同时修正惯导系统的初始位置误差、初始航向误差和初始速度误差，提高载体的定位精度；用数值迭代方法取代遍历搜索的求解手段，减小了匹配定位时间，提高了实时性。

主权项

1.一种基于仿射变换的惯性/地磁匹配迭代定位方法，其特征在于包括如下步骤：步骤1，从惯导系统读取当前时刻以及前n‑1个时刻的待匹配点的位置测量值xi和yi，其中xi表示经度，yi表示纬度，下标i表示不同时刻，i＝1…n，n为整数且n>2，i为1表示当前时刻；通过磁强计获得当前时刻以及前n‑1个时刻的地磁场强度的测量值Tr,i；步骤2，根据惯导系统指示的n个待匹配点的位置，从预先存储的地磁数据库中分别读取相应的地磁场强度的参考值Tm,i、地磁场强度的梯度的参考值Tx,i和Ty,i，其中Tx,i表示地磁场强度在经度方向的梯度在第i点位置上的取值，Ty,i表示地磁场强度在纬度方向的梯度在第i点位置上的取值；步骤3，引入并初始化匹配位置偏移量、角度偏移量和伸缩系数M：M＝[Δx Δy α k1 k2]T＝[0 0 0 1 1]T     (1)步骤4，根据公式(2)、公式(3)和公式(4)计算迭代参数G、F和H，G＝g(M)     (2)F＝f(M)    (3)H＝F‑1    (4)其中，g(M)＝[g1(M) g2(M) g3(M) g4(M) g5(M)]T；x′i＝xi‑xn，y′i＝yi‑yn，Tt,i＝Tm,i‑Tr,i；步骤5，计算匹配位置偏移量的增量、角度偏移量的增量和伸缩系数的增量δM：δM＝‑H×G     (5)步骤6，更新匹配位置偏移量、角度偏移量和伸缩系数M：M＝M+δM    (6)步骤7，判断是否满足终止迭代条件，若满足则停止迭代并跳到步骤10，否则跳到步骤8；终止迭代条件为①、②中任意一个或两个：①迭代次数达到预设次数；②匹配位置偏移量的增量、角度偏移量的增量和伸缩系数的增量δM的2范数小于设定值，即||δM||2<ε     (7)其中ε为预先设定的迭代最小误差；步骤8，根据更新后的M计算参数N和δN：N＝g(M)      (8)δN＝N‑G      (9)步骤9，根据公式(10)和公式(11)更新迭代变量H和G，并跳到步骤5，H＝H+[(δM)‑H(δN)](δM)TH/[(δM)TH(δN)]    (10)G＝N    (11)步骤10，根据迭代计算得到的匹配位置偏移量、角度偏移量和伸缩系数M，按公式(12)计算输出匹配结果，其中xr,i为第i时刻匹配结果的位置经度，yr,i为第i时刻匹配结果的位置纬度。