基于相关滤波的重力匹配方法

1.本发明涉及导航、制导与控制技术领域，具体涉及一种基于相关滤波的重力匹配方法。


背景技术：

2.惯性导航系统由于能够实时地为载体提供位置、速度、姿态等导航定位信息，在陆、海、空、天各类载体中广泛应用。对水下航行器等长航时载体，惯性导航系统的误差随时间发散，不能满足导航需求。而海洋重力测量具有无源性且重力信息稳定，重力辅助惯性导航成为水下自主导航的研究热点。重力匹配算法是重力辅助惯性导航关键技术之一，很大程度上影响最终导航结果。现有匹配算法主要有迭代最近等值线点算法(iccp)和桑迪亚惯性辅助导航(sitan)算法等。sitan算法是基于递推滤波的算法，由于重力异常分布不规则及不准确性，在非线性观测模型的线性化过程容易引起较大的观测模型误差。iccp算法是基于相关分析的序列匹配算法，不存在线性化问题，但有测量误差时存在搜索范围不准的问题，匹配结果中常存在误差较大的点，这些点可能对后续匹配过程产生影响，而目前算法中较少有对误匹配点的处理方法。


技术实现要素：

3.有鉴于此，本发明提供了一种基于相关滤波的重力匹配方法，能够有效减小重力测量误差的影响，提高重力辅助惯性导航系统的研究效率。
4.本发明的基于相关滤波的重力匹配方法，包括如下步骤：
5.步骤一，沿水下航行器的行驶轨迹进行重力测量，并对重力测量数据进行预处理，去除测量噪声；
6.步骤二，确定匹配范围为：根据重力测量数据绘制的等值线带范围与根据惯性导航系统的定位位置绘制的圆概率半径范围的交集；匹配范围内的重力背景图中的网格交点即为候选匹配点；
7.步骤三，对候选匹配点进行重力值相关计算；同时，利用惯性导航系统定位位置的轨迹对候选匹配点进行轨迹形状相关计算；根据重力值相关计算结果和轨迹形状相关计算结果确定最终的重力值匹配点；其中，重力值相关计算和轨迹形状相关计算时，对相关计算进行快速傅里叶变换。
8.较优的，所述步骤三中，首先根据重力测量数据设计重力值相关计算的滤波模板h
g
：
9.根据g为输出响应；f为输入，即重力测量值；最大输出响应所对应的h
g
即为所求。
10.较优的，所述步骤三中，对于t
n
时刻，提取t
n
时刻以前的m个重力测量数据，将其按由小到大的顺序排列，然后选取中间的l个重力测量值设计重力值相关性计算的滤波模板，即l个重力测量值的误差最小平方和所对应的重力值相关计算的滤波模板即为所求。
11.较优的，所述步骤三中，轨迹形状相关计算时，以惯性导航系统定位位置的轨迹的斜率作为轨迹形状模板进行轨迹形状相关计算。
12.较优的，还包括步骤四，误匹配检测：对惯性导航系统系统3个连续的定位点组成的三角形和对应的3个连续的重力值匹配点组成的三角形进行三角形相似性判断；若判断两个三角形相似，则认为当前重力值匹配点可行；否则，认为当前重力值匹配点为误匹配点，剔除该误匹配点，由三角形相似性构建出一个虚拟的匹配点进行下一次误匹配检测。
13.较优的，根据边角边判据进行三角形相似性判断。
14.有益效果：
15.(1)首先，本发明综合考虑重力误差与惯性导航系统误差特点，通过两者进行匹配范围的确定，使搜索范围更精确；其次，重力匹配时，在利用重力值相关计算的同时，将惯性导航系统解算轨迹与候选匹配点位置组成的轨迹的相关性也作为评价准则之一进行匹配，提高匹配准确度；并且，在重力值相关计算以及轨迹形状相关计算的过程使用快速傅里叶变换，加快运算速度，减少匹配算法耗时。
16.(2)本发明根据匹配过程中重力测量误差特性，采用一种类似中值滤波的非线性滤波方法作为重力测量数据预处理滤波方法，以减小脉冲噪声的影响，进而减小测量误差对后续匹配的影响。
17.(3)误匹配算法不仅可以检测出异常匹配点，还可根据几何关系对异常匹配点进行估计，减小误匹配点的影响。
附图说明
18.图1为本发明基于相关滤波的重力匹配方法的流程图；
19.图2为ins三角形与匹配三角形关系图；
20.图3为误匹配点及虚拟匹配点的生成图。
具体实施方式
21.下面结合附图并举实施例，对本发明进行详细描述。
22.本发明提供了一种基于相关滤波的重力匹配方法，方法流程图如图1所示，具体包括如下步骤：
23.步骤1，重力测量数据预处理，去除重力异常测量的噪声。
24.沿水下航行器的行驶轨迹，进行重力值测量。在重力仪采样频率为1hz的情况下，短时间内水下航行器行驶距离较短且远小于重力背景图的分辨率，则可认为重力仪测量值为一固定点的重力异常值。但由于重力测量误差的存在，测量的重力异常值并非测量点真实的重力异常值，而是存在一定的误差。为减小测量误差对后续匹配的影响，需要对重力测量数据进行预处理，去除测量噪声。一般可采用中值滤波、均值滤波等方式进行预处理。
25.重力测量误差可认为为高斯白噪声，方差为σ。在1到t
n
时刻，重力测量数据为α1,α2,
…
α
n
，其中n为1到t
n
时间内重力测量采样个数。本实施例采用一种类似中值滤波的非线性滤波方法作为重力测量数据预处理滤波方法，以减小脉冲噪声的影响。首先取t
n
时刻以前的m个重力测量值，m可根据水下航行器速度和重力背景图分辨率确定，并按由小到大的顺序排列α
′1,α
′2,
…
α
′
m
，然后在序列中选取中间l个采样重力测量值，这l个采样重力测量
值将作为样本采样重力测量值，并应用于后续的模板选择中。其中，l可根据水下航行器速度确定。
26.步骤2，根据重力测量噪声特点及惯性导航系统误差特性确定匹配范围，保证真实位置在此范围之中。
27.根据步骤1获得的重力测量数据，绘制等值线。若等值线带宽度为2σ，则真实位置在等值线带的概率为0.9544，若等值线带宽度为3σ，则真实位置在等值线带的概率为0.9974。若直接在等值线带进行搜索，则会出现计算量过大的情况，因此还需根据惯性导航系统位置与真实位置关系进一步确定最终范围。惯性导航系统误差一般在圆概率误差半径内，因此可利用当前时刻惯性导航系统位置误差的圆概率误差半径和重力测量噪声特点进行配合衡量。最终匹配范围为等值线带与圆概率误差半径范围的交集。
28.匹配范围内的重力背景图(网格图)中的网格交点即为候选匹配点。
29.步骤3，根据处理后的重力测量数据，设计重力值相关滤波模板；
30.在重力匹配中，需要在匹配范围内找到和真实轨迹最相似的一条作为匹配轨迹。按照这个思路，需要找到一个滤波器，使其在目标上的响应最大，则
[0031][0032]
其中g为相应输出，f为输入，即为重力测量值，h
g
为重力值相关计算的滤波模板。
[0033]
重力值相关计算的滤波模板h
g
需要进行卷积计算，在计算机中的计算耗时很大，因此本发明对式(1)进行快速傅里叶变换，这样卷积变成点乘，极大减小运算量。为方便描述，经傅里叶变换后式(1)可写为
[0034][0035]
则
[0036][0037]
对于本实施例，由于在重力数据预处理时每个位置获取了l个样本采样重力测量值f
i
序列，i＝1,2,
…
,l，实际匹配的过程中不仅考虑了测量误差等因素的影响，同时考虑目标的l个轨迹作为参考，提高滤波器模版的鲁棒性。其中，本实施例选取l个样本采样重力测量值f
i
的误差最小平方和所对应的重力值相关计算的滤波模板进行相关滤波，其中，误差最小平方和(mosse)模型公式为：
[0038][0039]
求解得到滤波器的模型公式如式(5)所示：
[0040][0041]
式(5)中，g
i
为g
i
的傅里叶变换结果；g
i
通过高斯函数产生，并且其峰值位置是在f
i
的中心位置。f
i
为f
i
的傅里叶变换结果。上标“*”表示复共轭。通过式(5)可计算得到h
g
，然后再进行逆傅里叶变换，即可得到滤波器h
g
的值。
[0042]
步骤4，将重力值相关计算的滤波模板和候选匹配点进行重力值相关计算，并结合惯性导航系统轨迹相关性确定最终匹配位置。
[0043]
将重力值相关滤波模板和候选匹配点进行相关滤波，根据相关性即可确定重力匹
配点。但是若直接使用候选匹配点位置重力值与重力值相关滤波模板进行匹配得到的相关值作为判断准则，则容易出现重力值相同但位置错误的现象，这是因为重力值相关滤波模板只有重力值的原因，并未考虑各点之间位置的相对关系。因此，本发明将惯性导航系统解算的轨迹与候选匹配点位置组成的轨迹的相关性也作为评价准则之一进行匹配。这里考虑惯性导航系统轨迹时只需考虑形状是否相似即可，因此本实施例将计算惯性导航系统轨迹各个点(x
i
,y
i
),i＝1,2,
…
,n之间的斜率k
i
，并将其作为一个序列，称其为轨迹形状模板h
ins
。候选匹配点位置p
ins
与轨迹形状模板进行匹配，得到轨迹形状相关性。
[0044]
因此，最终的位置匹配相关性应分为两部分，一部分是重力值的相关性，另一部分为轨迹形状相关性。则总响应j为
[0045][0046]
其中，p
g
为重力候选序列(即候选匹配点的重力值)，h
g
为重力值相关滤波模板，p
ins
为惯性导航系统候选序列(即相邻时刻的候选匹配点之间的斜率)，h
ins
为惯性导航系统解算的轨迹形状模板，c1,c2为加权系数。
[0047]
为简便计算，此处对各个响应进行归一化处理，即
[0048][0049]
则为了得到最终匹配结果，需使式(7)最大，此时对应的候选匹配点即为相关滤波匹配位置。
[0050]
与重力值相关滤波模板卷积计算时相同，公式(6)和公式(7)中的卷积计算时也先进行快速傅里叶变换，将卷积变成点乘，减小运算量。
[0051]
此外，本实施例为了防止出现匹配位置误差过大的情况，还执行了步骤5：
[0052]
步骤5，误匹配检测；
[0053]
本实施例对步骤4得到的最终重力值匹配位置使用三角形法进行误匹配检测，防止出现匹配位置误差过大的情况。
[0054]
三角形法即利用三角形相似的原理来检测误匹配点，需要连续的3个ins轨迹点和3个匹配轨迹点来组成ins三角形和匹配三角形，根据三角形相似的判据边角边(sas)来判断2个三角形的相似程度。基本思想如图2所示，计算指标可由式(8)和式(9)表示，其中δang和δra为角度指标和边长指标，越小则表示两个三角形越相似。
[0055]
δang<|∠b
‑
∠e|
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0056][0057]
由于存在测量误差等误差，即使匹配三角形的3个匹配点完全正确也无法做到ins三角形和匹配三角形完全相似，且此算法的目的在于去除误差较大的点，因此可以适当降低2个三角形的相似程度，即
[0058]
δang<t
a
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0059]
δra<t
r
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0060]
其中t
a
,t
r
表示对角度差和边长差的阈值。同时满足式(10)和式(11)的f点即为正确的匹配点。
[0061]
由于三角形法则所使用的是连续的3个ins轨迹点和3个匹配轨迹点，当上一次误
匹配检测中出现误匹配点时，则不能将该误匹配点用作下次误匹配检测中三角形的顶点。因此，当每次检测后出现误匹配点时，不仅需要将该误匹配点从匹配序列中删除，还需要通过匹配三角形和ins三角形的特征构建出一个虚拟的匹配点，用作下一次误匹配检测。若图3中f点为误匹配点，则该虚拟匹配点f
v
的生成通过如下方法：
[0062][0063][0064][0065][0066][0067][0068][0069]
上述公式中，a点的坐标为(x
a
,y
a
)，b,c,d,e,f,f
v
点坐标同理。
[0070]
对于初始匹配的2个点，由于无法形成三角形，故该方法适用于对第3个匹配点之后的误匹配点进行检测。由于ins短时精度较高，故一般情况下匹配算法的第一个匹配点与ins点和真实轨迹点相差不大，此时可以认为匹配算法下第一个点不为误匹配点。对于第2个匹配点可以根据ins和匹配轨迹中1,2两点组成的ins向量和匹配向量之间的夹角大小来检测是否为误匹配点。
[0071]
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。