CN113514062B - 一种多维地磁匹配定位方法及装置 - Google Patents
一种多维地磁匹配定位方法及装置 Download PDFInfo
- Publication number
- CN113514062B CN113514062B CN202110679942.7A CN202110679942A CN113514062B CN 113514062 B CN113514062 B CN 113514062B CN 202110679942 A CN202110679942 A CN 202110679942A CN 113514062 B CN113514062 B CN 113514062B
- Authority
- CN
- China
- Prior art keywords
- aircraft
- path
- transformation
- multidimensional
- magnetic field
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 230000009466 transformation Effects 0.000 claims abstract description 135
- 230000006855 networking Effects 0.000 claims abstract description 32
- 238000005259 measurement Methods 0.000 claims description 39
- 239000011159 matrix material Substances 0.000 claims description 14
- 238000010586 diagram Methods 0.000 claims description 12
- 238000005070 sampling Methods 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 230000004807 localization Effects 0.000 claims description 5
- 238000013519 translation Methods 0.000 claims description 4
- 238000012937 correction Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 2
- 238000000844 transformation Methods 0.000 claims description 2
- 230000008569 process Effects 0.000 description 5
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 230000005358 geomagnetic field Effects 0.000 description 3
- 230000006978 adaptation Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- Navigation (AREA)
Abstract
本发明提供一种多维地磁匹配定位方法及装置，所述方法包括获取主飞行器惯导指示路径到每一近似等值线的最近点序列；获取所述主飞行器的全部变换路径；获取所述飞行器组网中的各变换路径对应的全部从飞行器的位置；计算所述主飞行器的各变换路径对应的飞行器组网的多维特征相关度，将多维特征相关度最小的变换路径作为本次变换结果；记录所述本次变换结果对应的最小多维特征相关度；基于本次变换结果，使用相同的方法不断迭代变换；当迭代次数等于预设迭代次数时，获取最小多维特征相关度对应的各飞行器的位置，作为各飞行器的最终位置。根据本发明的方案，能够有效地提高定位准确度；具有抗区域环境干扰、匹配定位结果可靠性高等优点。
Description
技术领域
本发明涉及导航定位领域，尤其涉及一种多维地磁匹配定位方法及装置。
背景技术
巡航飞行器是用气动力支撑、靠吸气式发动机提供向前动力的飞行器，具有高速、低空、远程的特点，为实现精确、高效的打击提供了重要支撑。随着日益复杂的作战环境与作战需求，单一飞行器的作战样式已经很难完成作战任务，多飞行器组网协同攻击是适应未来作战环境的重要方式，而高精度、高可靠的导航定位信息则是顺利执行任务的关键。巡航飞行器作战协同作战可以实现各飞行器间的作战性能互补与信息共享，当某些飞行器受到外界环境影响而降低、丧失定位能力时，通过网内协同可以获得有界误差的定位能力。
惯性导航是目前在巡航飞行器上应用最广泛的导航定位方法，但其长时间工作时难以始终保持精确定位，因此需要通过其它辅助导航方式进行实时或定期修正。由于地磁场有隐蔽性、唯一性和全局可测性等优势，且地磁匹配算法不会产生累积误差，利用地磁场信息可以实现对飞行器的匹配定位，并且能够修正惯导累积的定位误差和航向误差。
发明内容
为解决上述技术问题，本发明提出了一种基于近似等值线最近点迭代(IterativeClosest Contour Point,ICCP)算法的多维地磁匹配定位方法及装置，所述方法及装置，用于解决现有技术中抗干扰能力差、定位准确度低的技术问题。
根据本发明的第一方面，提供一种多维地磁匹配定位方法，所述方法包括以下步骤：
步骤S1001：基于飞行器组网中的主飞行器的惯导指示路径{p'0i}、磁场序列{M'0i}及磁传感器测量精度，获取所述惯导指示路径{p'0i}到每一近似等值线的最近点序列，所述近似等值线包括等值线及测量误差范围内的所有可能磁场值的等值线；其中，1≤i≤N，N为主飞行器路径总点数，p'0i为路径上的每一点，{M'0i}为路径的磁场测量序列；
步骤S1002：对所述惯导指示路径{p'0i}，进行路径上点序列到每一近似等值线的最近点序列的刚性变换；初始化迭代次数Num＝0；
步骤S1003：判断迭代次数是否等于预设迭代次数，若是，进入步骤S1010；否则，进入步骤S1004；
步骤S1004：获取所述主飞行器的全部变换路径；获取所述飞行器组网中的全部从飞行器的位置；
步骤S1005：基于预先建立的磁场图，获取所述主飞行器的各变换路径对应的飞行器组网的各采样点的磁场值；
步骤S1006：计算所述主飞行器的各变换路径对应的飞行器组网的多维特征相关度，将多维特征相关度最小的变换路径作为本次变换结果；
步骤S1007：记录所述本次变换结果对应的飞行器组网中各飞行器的位置，以及所述本次变换结果对应的最小多维特征相关度minNum；
步骤S1008：基于所述本次变换结果对应的变换路径和磁场序列，获取所述本次变换结果对应的变换路径的每一近似等值线的最近点序列；
步骤S1009：对所述本次变换结果对应的变换路径，进行路径上点序列到每一近似等值线的最近点序列的刚性变换，迭代次数Num赋值为Num+1，进入步骤S1003；
步骤S1010：获取{min0，……，minNum}中的最小值minmin，获取最小值minmin对应的所述飞行器组网中各飞行器的位置，作为各飞行器的最终位置。
根据本发明第二方面，提供一种多维地磁匹配定位装置，所述装置包括：
第一最近点序列获取模块：配置为基于飞行器组网中的主飞行器的惯导指示路径{p'0i}、磁场序列{M'0i}及磁传感器测量精度，获取所述惯导指示路径{p'0i}到每一近似等值线的最近点序列，所述近似等值线包括等值线及测量误差范围内的所有可能磁场值的等值线；其中，1≤i≤N，N为主飞行器路径总点数，p'0i为路径上的每一点，{M'0i}为路径的磁场测量序列；
第一刚性变换模块：配置为对所述惯导指示路径{p'0i}，进行路径上点序列到每一近似等值线的最近点序列的刚性变换；初始化迭代次数Num＝0；
判断模块：配置为判断迭代次数是否等于预设迭代次数；
位置获取模块：配置为获取所述主飞行器的全部变换路径；获取所述飞行器组网中的全部从飞行器的位置；
磁场值获取模块：配置为基于预先建立的磁场图，获取所述主飞行器的各变换路径对应的飞行器组网的各采样点的磁场值；
第一计算模块：配置为计算所述主飞行器的各变换路径对应的飞行器组网的多维特征相关度，将多维特征相关度最小的变换路径作为本次变换结果；
最小维度获取模块：配置为记录所述本次变换结果对应的飞行器组网中各飞行器的位置，以及所述本次变换结果对应的最小多维特征相关度minNum；
第二最近点序列获取模块：配置为基于所述本次变换结果对应的变换路径和磁场序列，获取所述本次变换结果对应的变换路径的每一近似等值线的最近点序列；
第二刚性变换模块：配置为对所述本次变换结果对应的变换路径，进行路径上点序列到每一近似等值线的最近点序列的刚性变换，迭代次数Num赋值为Num+1；
最终位置获取模块：配置为获取{min0，……，minNum}中的最小值minmin，获取最小值minmin对应的所述飞行器组网中各飞行器的位置，作为各飞行器的最终位置。
根据本发明第三方面，提供一种多维地磁匹配定位系统，包括：
处理器，用于执行多条指令；
存储器，用于存储多条指令；
其中，所述多条指令，用于由所述存储器存储，并由所述处理器加载并执行如前所述的多维地磁匹配定位方法。
根据本发明第四方面，提供一种计算机可读存储介质，所述存储介质中存储有多条指令；所述多条指令，用于由处理器加载并执行如前所述的多维地磁匹配定位方法。
根据本发明的上述方案，本发明构建了具有主飞行器及若干从飞行器的飞机组网，利用基于近似等值线最近点迭代(ICCP)算法，用于实现组网应用场景下的巡航飞行器地磁匹配定位。基于近似ICCP算法的多维地磁匹配算法面向巡航飞行器组网飞行的条件，利用多维地磁匹配模型，在主飞行器近似ICCP迭代过程中，通过计算多个飞行器的相对位置信息和磁场测量信息，使用多维均方差算法(Mean Square Differences，MSD)来进行待匹配航迹的适应度计算，对待匹配航迹进行评估，使得待匹配航迹的适应度达到最小，从而得到最佳匹配的航迹。本发明相较于传统的ICCP算法，近似ICCP算法考虑了磁传感器受到的测量噪声影响，更加符合真实情况，能够在一定程度上容忍测量过程中的干扰，有效地提高定位准确度；与其他迭代的地磁匹配算法相比，如粒子群优化算法等，ICCP算法以地磁测量值为依据，无需人工设置学习参数，使结果不会因参数设置不准确导致结果陷于局部最优；在飞行器组网飞行的背景下，多维地磁匹配模型与传统一维地磁场匹配模型相比，具有抗区域环境干扰、匹配定位结果可靠性高等优点,同时多维地磁匹配在匹配概率、匹配定位精度等方面也具有较大的优势。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，并可依照说明书的内容予以实施，以下以本发明的较佳实施例并配合附图详细说明如后。
附图说明
构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明提供如下附图进行说明。在附图中：
图1为本发明一个实施方式的多维地磁匹配定位方法流程图；
图2为本发明一个实施方式的飞行器组网结构示意图；
图3为现有技术传统ICCP算法原理图；
图4为本发明一个实施方式的多维地磁匹配定位装置结构框图。
具体实施方式
首先结合图1说明本发明一个实施方式的为本发明一个实施方式的多维地磁匹配定位方法流程。如图1所示，所述方法包括以下步骤：
步骤S1001：基于飞行器组网中的主飞行器的惯导指示路径{p'0i}、磁场序列{M'0i}及磁传感器测量精度，获取所述惯导指示路径{p'0i}到每一近似等值线的最近点序列，所述近似等值线包括等值线及测量误差范围内的所有可能磁场值的等值线；其中，1≤i≤N，N为主飞行器路径总点数，p'0i为路径上的每一点，{M'0i}为路径的磁场测量序列；
步骤S1002：对所述惯导指示路径{p'0i}，进行路径上点序列到每一近似等值线的最近点序列的刚性变换；初始化迭代次数Num＝0；
步骤S1003：判断迭代次数是否等于预设迭代次数，若是，进入步骤S1010；否则，进入步骤S1004；
步骤S1004：获取所述主飞行器的全部变换路径；获取所述飞行器组网中的全部从飞行器的位置；
步骤S1005：基于预先建立的磁场图，获取所述主飞行器的各变换路径对应的飞行器组网的各采样点的磁场值；
步骤S1006：计算所述主飞行器的各变换路径对应的飞行器组网的多维特征相关度，将多维特征相关度最小的变换路径作为本次变换结果；
步骤S1007：记录所述本次变换结果对应的飞行器组网中各飞行器的位置，以及所述本次变换结果对应的最小多维特征相关度minNum；
步骤S1008：基于所述本次变换结果对应的变换路径和磁场序列，获取所述本次变换结果对应的变换路径的每一近似等值线的最近点序列；
步骤S1009：对所述本次变换结果对应的变换路径，进行路径上点序列到每一近似等值线的最近点序列的刚性变换，迭代次数Num赋值为Num+1，进入步骤S1003；
步骤S1010：获取{min0，……，minNum}中的最小值minmin，获取最小值minmin对应的所述飞行器组网中各飞行器的位置，作为各飞行器的最终位置。
如图2所示，本实施例中，所述飞行器组网的结构为：
所述飞行器组网具有一个主飞行器，若干从飞行器；从飞行器作为量测输入，对主飞行器进行轨迹校正；
设主飞行器真实位置为p0，在惯性系的惯导指示位置为p'0，第k个从飞行器的真实位置为pk，它相对于主飞行器的位置为Δpk，该位置的磁场读图值为M(pk)，实际的地磁测量值为M'k；表征主飞行器、从飞行器关系的多维地磁匹配模型表示为：
p0＝T(p'0)
pk＝p0+Δpk,k＝1,2,...,n
M'k＝M(pk)+Q,k＝0,1,...,n
其中，T(*)为惯性系统指示航迹到真实航迹的变换关系；Q为地磁测量误差和噪声，n为飞行器个数。根据多个从飞行器的磁场信息以及它们与主飞行器之间的位置关系，通过计算就可以确定主飞行器真实位置p0。
如图3所示，为现有技术中传统的ICCP算法原理图，本实施例中，对传统的ICCP算法进行了改进。本实施例中，考虑了磁传感器测量过程中的噪声，由原有的单纯寻找等值线最近点，改进为寻找等值线及测量误差范围内的所有可能磁场值的近似等值线最近点，通过迭代的方式来更新主飞行器的位置。
对于主飞行器来说，由于惯性导航系统不可避免地存在一定的误差，因此惯导指示路径通常与真实路径不完全重合。传统的ICCP算法假设地磁测量没有误差，因此通过测量到的磁场值，在其等值线上寻找与惯导指示路径中每一点距离最近的点，组成等值线路径，这些点与原惯导指示点存在一定的变换关系，对惯导指示路径进行整体的刚性变换，包括旋转和平移，使得变换后路径上的序列与等值线路径上的序列之间的距离平方和最小，重复这一过程，使得迭代直到收敛或收敛次数达到最大迭代次数上限为止。
由此可见，传统ICCP算法的关键是寻找最近等值线点和求取刚性变换，以逐步得到接近真实路径的位置序列。然而，传统ICCP算法对于地磁测量没有误差这一假设在实际测量过程中对磁传感器提出了较高的要求，几乎不可实现。因此，本实施例对寻找最近等值线点这一步骤进行改进。除了选取等值线最近点外，考虑测量时使用的磁传感器实际测量噪声Q，在选取测量值为M'等值线最近点的基础上，选取磁图中M'±Q范围内的多个近似等值线最近点，构成近似等值线最近点序列。通过以上改进，等值线路径的搜索范围扩大，更符合实际情况，提高了对传感器测量噪声的容忍程度。
所述步骤S1002及步骤S1009的路径上点序列到每一近似等值线的最近点序列的刚性变换，包括：
步骤S10021：全部点组成点集{p'0i}，求取点集{p'0i}与{t0i}的质心，各段子路径所占的比重用wi表示，即：
其中，i为路径中每点的编号,N为路径中所含采样点的数目，p'0i为路径中第i个点的位置，t0i为p'0i对应的近似等值线最近点，{t0i}为若干近似等值线最近点的集合；
步骤S10022：计算互协方差矩阵其中，/>为/>的转置；
记矩阵
其中，S11、S12、S21、S22分别为互协方差矩阵S中的各元素；
该矩阵W的四个特征值为：
在矩阵W的四个特征值中选取最大的特征值，记为λmax，求取旋转角θ：
步骤S10023：计算旋转矩阵R及平移变换矩阵b,
步骤S10024：路径上点序列到每一近似等值线的最近点序列进行刚性变换
Tp'0i＝Rp'0i+b
其中，T为包含旋转和平移变换的刚性变换。
本实施例中，在寻找到近似等值线最近点路径后，通过求取刚性变换，使结果逐步逼近真实结果。
进一步地，惯导指示路径经过刚性变换后，得到的结果与等值线路径不完全重合，因此需要进一步反复迭代，不断得到新的刚性变换，得到逼近等值线路径和真实路径的结果。
传统ICCP迭代终止的条件包括：相邻两次迭代的距离小于门限值或迭代次数达到最大迭代次数上限。
以上迭代终止的条件，都是将最后一次迭代结果作为最终结果。这种情况下，得到的结果可能是局部最优，因此，引入飞行器组网，在多维地磁匹配模型中，可以利用飞行器之间的相对位置信息和多飞行器测量值，提高定位结果精度。该多维地磁匹配模型，忽略网间坐标系的相对位置误差，认为飞行器之间的相对位置是准确的，则可以以单个轨迹为主，其他飞行器仅作为量测输入，来对主飞行器进行轨迹校正。
所述步骤S1004：获取所述主飞行器的全部变换路径，获取所述飞行器组网中的全部从飞行器的位置，包括：
当主飞行器经过ICCP变换后，得到多条变换路径，将第i条变换路径记为
{t'i1t'i2...t'iN}，其中t'iN为第i条变换路径中的第N个点；
根据主飞行器与其他从飞行器之间的关系，从飞行器的位置可以通过主飞行器和空间约束推算。
第k个从飞行器的第i条航迹上第j个点的位置为：
第k个从飞行器的第i条航迹记为
步骤S1005：基于预先建立的磁场图，获取所述主飞行器的各变换路径对应的飞行器组网的各个点的磁场值，其中：
通过地磁图，可以得到第k个飞行器第i条航迹的地磁读图值
所述步骤S1006：计算所述主飞行器的各变换路径对应的飞行器组网的多维特征相关度，将多维特征相关度最小的变换路径作为本次变换结果，其中：
飞行器组网的多维特征相关度表示为：
其中，Ek为第k个飞行器轨迹地磁测量值与地磁读图值的均方差，pkj为第k个飞行器航迹中的第j个匹配点，M'kj第k个飞行器航迹中的第j个匹配点实际的地磁测量值；M(pkj)为第k个飞行器航迹中的第j个匹配点的地磁读图值，n为总的飞行器个数。
本实施例中，将所述地磁读图值与实测的地磁测量值相比较，利用公式可以计算得到各变换路径对应的飞行器组网的多维特征相关度。本实施例中，在搜索空间内，对主飞行器不断进行旋转和平移变换，使整个飞行器网络对应的轨迹地磁总均方差E达到最小，此时主飞行器的位置为搜索区域的最优解，其对应的航迹为匹配航迹。
本实施例中，MSD为在一定的空间结构下，多点飞行器同步或异步测量空间磁场特征量，将其通过数据链共享，可以取得较好的空间大区域的磁场匹配效能。因此本实施例采用MSD计算待匹配航迹的适应度，来对匹配轨迹进行评估。
本实施例中，在每一次迭代过程中，除了计算主飞行器的刚性变换及变换后的位置结果外，还需计算各从飞行器的位置及整个飞行编队的多维特征相关度，为了减少后续的计算复杂度和存储空间，选取多条变换路径中特征相关度最小值作为本次迭代的最终结果，作为下一步迭代的基础路径，并记录本次迭代的结果和多维特征相关度，作为最终迭代结果的评价依据。设置迭代次数上限，当迭代达到上限时，取所有记录下的特征相关度最小值的飞行器位置为最优位置。
本发明实施例进一步给出一种多维地磁匹配定位装置，如图4所示，所述装置包括：
第一最近点序列获取模块：配置为基于飞行器组网中的主飞行器的惯导指示路径{p'0i}、磁场序列{M'0i}及磁传感器测量精度，获取所述惯导指示路径{p'0i}到每一近似等值线的最近点序列，所述近似等值线包括等值线及测量误差范围内的所有可能磁场值的等值线；其中，1≤i≤N，N为主飞行器路径总点数，p'0i为路径上的每一点，{M'0i}为路径的磁场测量序列；
第一刚性变换模块：配置为对所述惯导指示路径{p'0i}，进行路径上点序列到每一近似等值线的最近点序列的刚性变换；初始化迭代次数Num＝0；
判断模块：配置为判断迭代次数是否等于预设迭代次数；
位置获取模块：配置为获取所述主飞行器的全部变换路径；获取所述飞行器组网中的全部从飞行器的位置；
磁场值获取模块：配置为基于预先建立的磁场图，获取所述主飞行器的各变换路径对应的飞行器组网的各采样点的磁场值；
第一计算模块：配置为计算所述主飞行器的各变换路径对应的飞行器组网的多维特征相关度，将多维特征相关度最小的变换路径作为本次变换结果；
最小维度获取模块：配置为记录所述本次变换结果对应的飞行器组网中各飞行器的位置，以及所述本次变换结果对应的最小多维特征相关度minNum；
第二最近点序列获取模块：配置为基于所述本次变换结果对应的变换路径和磁场序列，获取所述本次变换结果对应的变换路径的每一近似等值线的最近点序列；
第二刚性变换模块：配置为对所述本次变换结果对应的变换路径，进行路径上点序列到每一近似等值线的最近点序列的刚性变换，迭代次数Num赋值为Num+1；
最终位置获取模块：配置为获取{min0，……，minNum}中的最小值minmin，获取最小值minmin对应的所述飞行器组网中各飞行器的位置，作为各飞行器的最终位置。
本发明实施例进一步给出一种多维地磁匹配定位系统，包括：
处理器，用于执行多条指令；
存储器，用于存储多条指令；
其中，所述多条指令，用于由所述存储器存储，并由所述处理器加载并执行如前所述的多维地磁匹配定位方法。
本发明实施例进一步给出一种计算机可读存储介质，所述存储介质中存储有多条指令；所述多条指令，用于由处理器加载并执行如前所述的多维地磁匹配定位方法。
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
在本发明所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。
上述以软件功能单元的形式实现的集成的单元，可以存储在一个计算机可读取存储介质中。上述软件功能单元存储在一个存储介质中，包括若干指令用以使得一台计算机装置(可以是个人计算机，实体机服务器，或者网络云服务器等，需安装Windows或者Windows Server操作系统)执行本发明各个实施例所述方法的部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(RandomAccess Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。
Claims (8)
1.一种多维地磁匹配定位方法，其特征在于，所述方法包括以下步骤：
步骤S1001：基于飞行器组网中的主飞行器的惯导指示路径{p'0i}、磁场序列{M'0i}及磁传感器测量精度，获取所述惯导指示路径{p'0i}到每一近似等值线的最近点序列，所述近似等值线包括等值线及测量误差范围内的所有可能磁场值的等值线；其中，1≤i≤N，N为主飞行器路径总点数，p'0i为路径上的每一点，{M'0i}为路径的磁场测量序列；
步骤S1002：对所述惯导指示路径{p'0i}，进行路径上点序列到每一近似等值线的最近点序列的刚性变换；初始化迭代次数Num＝0；
步骤S1003：判断迭代次数是否等于预设迭代次数，若是，进入步骤S1010；否则，进入步骤S1004；
步骤S1004：获取所述主飞行器的全部变换路径；获取所述飞行器组网中的全部从飞行器的位置；
步骤S1005：基于预先建立的磁场图，获取所述主飞行器的各变换路径对应的飞行器组网的各采样点的磁场值；
步骤S1006：计算所述主飞行器的各变换路径对应的飞行器组网的多维特征相关度，将多维特征相关度最小的变换路径作为本次变换结果；
步骤S1007：记录所述本次变换结果对应的飞行器组网中各飞行器的位置，以及所述本次变换结果对应的最小多维特征相关度minNum；
步骤S1008：基于所述本次变换结果对应的变换路径和磁场序列，获取所述本次变换结果对应的变换路径的每一近似等值线的最近点序列；
步骤S1009：对所述本次变换结果对应的变换路径，进行路径上点序列到每一近似等值线的最近点序列的刚性变换，迭代次数Num赋值为Num+1，进入步骤S1003；
步骤S1010：获取{min0，……，minNum}中的最小值minmin，获取最小值minmin对应的所述飞行器组网中各飞行器的位置，作为各飞行器的最终位置。
2.如权利要求1所述的多维地磁匹配定位方法，其特征在于，所述飞行器组网的结构为：
所述飞行器组网具有一个主飞行器，若干从飞行器；从飞行器作为量测输入，对主飞行器进行轨迹校正；
设主飞行器真实位置为p0，在惯性系的惯导指示位置为p'0，第k个从飞行器的真实位置为pk，它相对于主飞行器的位置为Δpk，该位置的磁场读图值为M(pk)，实际的地磁测量值为M'k；表征主飞行器、从飞行器关系的多维地磁匹配模型表示为：
p0＝T(p'0)
pk＝p0+Δpk,k＝1,2,...,n
M'k＝M(pk)+Q,k＝1,...,n
其中，T(*)为惯导指示航迹到真实航迹的变换关系；Q为地磁测量误差和噪声，n为飞行器个数。
3.如权利要求2所述的多维地磁匹配定位方法，其特征在于，所述进行路径上点序列到每一近似等值线的最近点序列进行刚性变换，包括：
步骤S10021：全部点组成点集{p'0i}，求取点集{p'0i}与{t0i}的质心，各段子路径所占的比重用wi表示，即：
其中，i为路径中每点的编号,p'0i为路径中第i个点的位置，t0i为p'0i对应的近似等值线最近点，{t0i}为若干近似等值线最近点的集合；
步骤S10022：计算互协方差矩阵其中，/>为/>的转置；
记矩阵
其中，S11、S12、S21、S22分别为互协方差矩阵S中的各元素；
该矩阵W的四个特征值为：
在矩阵W的四个特征值中选取最大的特征值，记为λmax，求取旋转角θ：
步骤S10023：计算旋转矩阵R及平移变换矩阵b,
步骤S10024：路径上点序列到每一近似等值线的最近点序列进行刚性变换
Tp'0i＝Rp'0i+b
其中，T为包含旋转和平移变换的刚性变换。
4.如权利要求3所述的多维地磁匹配定位方法，其特征在于，所述步骤S1004：获取所述主飞行器的全部变换路径，获取所述飞行器组网中的全部从飞行器的位置，包括：
当主飞行器经过等值线最近点迭代变换后，得到多条变换路径，将第ii条变换路径记为
{t'ii1t'ii2...t'iiN1}，其中t'iiN1为第ii条航迹中的第N1个点；
根据主飞行器与其他从飞行器之间的关系，通过主飞行器和空间约束推算从飞行器的位置；
第k个从飞行器的第ii条航迹上第j个点的位置为：
第k个从飞行器的第ii条航迹记为
5.如权利要求4所述的多维地磁匹配定位方法，其特征在于，所述步骤S1006：计算所述主飞行器的各变换路径对应的飞行器组网的多维特征相关度，将多维特征相关度最小的变换路径作为本次变换结果，其中：
飞行器组网的多维特征相关度表示为：
其中，Ek为第k个飞行器轨迹地磁测量值与地磁读图值的均方差，pkj为第k个飞行器航迹中的第j个匹配点，M′kj第k个飞行器航迹中的第j个匹配点实际的地磁测量值；M(pkj)为第k个飞行器航迹中的第j个匹配点的地磁读图值。
6.一种多维地磁匹配定位装置，其特征在于，所述装置包括：
第一最近点序列获取模块：配置为基于飞行器组网中的主飞行器的惯导指示路径{p'0i}、磁场序列{M'0i}及磁传感器测量精度，获取所述惯导指示路径{p'0i}到每一近似等值线的最近点序列，所述近似等值线包括等值线及测量误差范围内的所有可能磁场值的等值线；其中，1≤i≤N，N为主飞行器路径总点数，p'0i为路径上的每一点，{M'0i}为路径的磁场测量序列；
第一刚性变换模块：配置为对所述惯导指示路径{p'0i}，进行路径上点序列到每一近似等值线的最近点序列的刚性变换；初始化迭代次数Num＝0；
判断模块：配置为判断迭代次数是否等于预设迭代次数；
位置获取模块：配置为获取所述主飞行器的全部变换路径；获取所述飞行器组网中的全部从飞行器的位置；
磁场值获取模块：配置为基于预先建立的磁场图，获取所述主飞行器的各变换路径对应的飞行器组网的各采样点的磁场值；
第一计算模块：配置为计算所述主飞行器的各变换路径对应的飞行器组网的多维特征相关度，将多维特征相关度最小的变换路径作为本次变换结果；
最小维度获取模块：配置为记录所述本次变换结果对应的飞行器组网中各飞行器的位置，以及所述本次变换结果对应的最小多维特征相关度minNum；
第二最近点序列获取模块：配置为基于所述本次变换结果对应的变换路径和磁场序列，获取所述本次变换结果对应的变换路径的每一近似等值线的最近点序列；
第二刚性变换模块：配置为对所述本次变换结果对应的变换路径，进行路径上点序列到每一近似等值线的最近点序列的刚性变换，迭代次数Num赋值为Num+1；
最终位置获取模块：配置为获取{min0，……，minNum}中的最小值minmin，获取最小值minmin对应的所述飞行器组网中各飞行器的位置，作为各飞行器的最终位置。
7.一种多维地磁匹配定位系统，包括：
处理器，用于执行多条指令；
存储器，用于存储多条指令；
其中，所述多条指令，用于由所述存储器存储，并由所述处理器加载并执行如权利要求1-5中任一项所述的多维地磁匹配定位方法。
8.一种计算机可读存储介质，所述存储介质中存储有多条指令；所述多条指令，用于由处理器加载并执行如权利要求1-5中任一项所述的多维地磁匹配定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110679942.7A CN113514062B (zh) | 2021-06-18 | 2021-06-18 | 一种多维地磁匹配定位方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110679942.7A CN113514062B (zh) | 2021-06-18 | 2021-06-18 | 一种多维地磁匹配定位方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113514062A CN113514062A (zh) | 2021-10-19 |
| CN113514062B true CN113514062B (zh) | 2023-10-27 |
Family
ID=78065642
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110679942.7A Active CN113514062B (zh) | 2021-06-18 | 2021-06-18 | 一种多维地磁匹配定位方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113514062B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102809376A (zh) * | 2012-08-06 | 2012-12-05 | 哈尔滨工程大学 | 一种基于等值线的辅助导航定位方法 |
| CN103822634A (zh) * | 2014-02-24 | 2014-05-28 | 北京理工大学 | 一种基于改进的iccp算法的重力匹配辅助惯性导航方法 |
| CN104833373A (zh) * | 2015-04-21 | 2015-08-12 | 西安理工大学 | 一种基于Procrustes的地磁匹配导航方法 |
| CN107883948A (zh) * | 2017-10-23 | 2018-04-06 | 石家庄铁道大学 | 变步长iccp地磁匹配方法 |
| CN108871320A (zh) * | 2018-04-27 | 2018-11-23 | 石家庄铁道大学 | 基于概率数据关联的地磁匹配方法 |
| CN111895995A (zh) * | 2020-06-03 | 2020-11-06 | 东南大学 | 基于pso的飞行器编队多维地磁匹配导航方法及系统 |
-
2021
- 2021-06-18 CN CN202110679942.7A patent/CN113514062B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102809376A (zh) * | 2012-08-06 | 2012-12-05 | 哈尔滨工程大学 | 一种基于等值线的辅助导航定位方法 |
| CN103822634A (zh) * | 2014-02-24 | 2014-05-28 | 北京理工大学 | 一种基于改进的iccp算法的重力匹配辅助惯性导航方法 |
| CN104833373A (zh) * | 2015-04-21 | 2015-08-12 | 西安理工大学 | 一种基于Procrustes的地磁匹配导航方法 |
| CN107883948A (zh) * | 2017-10-23 | 2018-04-06 | 石家庄铁道大学 | 变步长iccp地磁匹配方法 |
| CN108871320A (zh) * | 2018-04-27 | 2018-11-23 | 石家庄铁道大学 | 基于概率数据关联的地磁匹配方法 |
| CN111895995A (zh) * | 2020-06-03 | 2020-11-06 | 东南大学 | 基于pso的飞行器编队多维地磁匹配导航方法及系统 |
Non-Patent Citations (3)
| Title |
|---|
| Simulation of Geomagnetic/ Inertial Integrated Navigation System;Zhou Yingying 等;《2011 Third International Conference on Measuring Technology and Mechatronics Automation》;全文 * |
| 基于ICCP 的地磁矢量匹配算法研究;吴凤贺 等;《中国测试》;第44卷(第2期);全文 * |
| 基于ICCP算法的地磁匹配定位方法;李豫泽 等;《现代电子技术》(第20期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113514062A (zh) | 2021-10-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107918925B (zh) | 磁跟踪系统与成像装置的配准 | |
| CN110930495A (zh) | 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质 | |
| CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| CN106597363A (zh) | 一种室内wlan环境下的行人定位方法 | |
| CN111707279B (zh) | 一种激光点云与地图的匹配评价方法、介质、终端和装置 | |
| CN111504318B (zh) | 一种基于多磁偶极子反演的海洋导航辅助方法 | |
| CN111473784B (zh) | 基于分布节点信息区块的无人机集群协同导航系统及方法 | |
| CN107633256A (zh) | 一种多源测距下联合目标定位与传感器配准方法 | |
| CN111895995B (zh) | 基于pso的飞行器编队多维地磁匹配导航方法及系统 | |
| Wang et al. | Matching error of the iterative closest contour point algorithm for terrain-aided navigation | |
| CN108332756B (zh) | 一种基于拓扑信息的水下航行器协同定位方法 | |
| CN109631883A (zh) | 一种基于节点信息共享的载机局部姿态精确估计方法 | |
| CN113342059B (zh) | 基于位置和速度误差的多无人机跟踪移动辐射源方法 | |
| CN110738275A (zh) | 基于ut-phd的多传感器序贯融合跟踪方法 | |
| CN113532438B (zh) | 一种大初始定位误差下的改进iccp地形匹配方法 | |
| CN113514062B (zh) | 一种多维地磁匹配定位方法及装置 | |
| CN113624227B (zh) | 一种基于马氏距离的等值线迭代重力匹配算法 | |
| Fernandes et al. | GNSS/MEMS-INS integration for drone navigation using EKF on lie groups | |
| CN108320302B (zh) | 基于随机超曲面的CBMeMBer多目标跟踪方法 | |
| CN114608568A (zh) | 一种基于多传感器信息即时融合定位方法 | |
| CN113639749A (zh) | 一种基于不确定性的多波束测深数据匹配检测方法 | |
| CN115979245B (zh) | 一种自校准估计的磁感应网络定位方法 | |
| CN116124161B (zh) | 一种基于先验地图的LiDAR/IMU融合定位方法 | |
| CN111896946A (zh) | 一种基于航迹拟合的连续时间目标跟踪方法 | |
| US20230101689A1 (en) | Method, apparatus, and program for matching point cloud data |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |