一种动态序列重力匹配算法

2023-05-31徐孝萍蔡体菁倪江生

压电与声光 2023年2期

徐孝萍,蔡体菁, 倪江生

( 东南大学仪器科学与工程学院,江苏南京 210096)

0 引言

海洋中蕴藏着丰富的物质资源和战略资源,水下载体导航技术在海洋的探测和开发中必不可少,在当前海洋强国的发展趋势背景下具有相当重要的应用前景[1]。普遍采用的水下导航系统为惯性导航系统(INS)[2]。惯性导航具有很强的屏蔽性和抗干扰能力等[3],但其最大的缺点是精度随着导航时间增长而降低[4],所以,需要采用重力匹配辅助惯性导航。重力匹配导航不依赖外界信息交换,只借助地球物理特性完成潜艇水下长航的导航技术[5]。

重力匹配算法按照采样方式分为单点匹配和序列匹配[6]。单点匹配主要为基于扩展卡尔曼滤波的桑迪亚惯性地形辅助导航算法(SITAN)[6],序列匹配主要有迭代最近等值线算法(ICCP)和地形轮廓匹配算法(TERCOM)[7]。

本文提出了一种动态序列重力匹配算法。首先根据惯导系统提供位置信息,对航迹按距离进行多起点采样构建轨迹序列,然后对每个起点轨迹序列进行相关极值匹配,最后动态选择一条最优估计输出。该算法减小了序列匹配算法中由于构造观测序列起点选取不同而导致的匹配结果存在差异,提高了重力匹配的精度和稳定性。

1 动态序列重力匹配算法

1.1 动态序列匹配思想

图2 动态序列匹配

图3 动态匹配算法流程

图4 设置搜索范围

1.2 多起点构造观测轨迹

计算出一个网格的长度记为S,一条轨迹采样点个数为n。具体步骤如下:

表1 二维数组arrins(b,n)表格

2) 将b个起点对应的b条观测轨迹,即表1中数组的每行对应的轨迹进行相关极值匹配。

1.3 相关极值匹配

1.3.1 搜索范围设置

在惯性导航系统中,由于陀螺仪和加速度计的累计误差,使得潜航器的惯导在输出此时刻位置的经纬度时具有漂移现象。实际潜航器的位置应该在预定的航行区域和以惯导输出位置为中心的一定范围内,随着航行位置移动而移动。

一般认为同一条轨迹序列,每个采样点之间的距离间隔为d,则需要d≥S。轨迹长度为S′=n×d。根据起始点的初始经纬度设定搜索区域范围(L′×L′)(见图2)。要确保真实位置轨迹在重力基准图搜索区域范围内,如此可确保匹配的实时性和准确性,随着水下潜航器的位置改变,重力基准图上的搜索范围也随着航迹不断改变。

1.3.2 刚性变换

如果能够获得构建的惯导轨迹序列S′相对于实际航迹的平移量(dx,dy)和偏转角β,可得实际的航行轨迹。因此,利用旋转和平移对INS输出的航迹点的位置进行刚性变换,可得到多条待匹配轨迹。

本文以2个采样点为例。设构建的惯导航迹序列第一个采样点的位置为A,第二个采样点的位置为B,用二维矩阵旋转得到B′点,然后平移到点B″,将该点假设为可能的真实位置,即待匹配轨迹点,具体旋转平移示意图如图5所示。

图5 二维矩阵旋转与平移示意图

以第k个起点构建的待匹配轨迹为例,具体步骤如下:

1) 为了保证匹配的实时性,当二维数组中,每行数据达到m个开始匹配。

2) 将这m个数据中第2个点至第m个点(同上述B点)的位置相对第1个点A,利用二维矩阵进行旋转与平移,在重力基准图中构建待匹配轨迹。刚性变换构建待匹配轨迹过程如下:

(1)

(2)

b.以x轴为0°参考线,第i点相对于第1个点的角度为α,设需要旋转的角度为β′=β/180×π。

c.旋转后,B′相对于A点经纬度方向上的距离:

(3)

(4)

d.设x轴方向平移dx距离,y轴方向平移dy距离。平移和旋转后,构建的待匹配的序列坐标B″相对于A点经纬度方向上的距离:

(5)

(6)

3) 之后航迹数据每增加1个,重复步骤2),直到每行数据数达到n个。使待匹配轨迹的采样点逐渐增多,重力变化特征更明显,匹配效果越好。

4) 为了提高实时性和匹配精度,快速启动匹配,使用了滑动窗口机制维护数组。当每行数据个数达到n个,删去该行第1个点的数据,将其后每个点向前移动一位,新输入点的数据放在此行的最后,数组向前滑动,然后重复步骤2)。

1.3.3 待匹配轨迹点插值

1.3.4 相关极值匹配

(7)

式中:j=1～a;n为一条轨迹序列的采样点个数。

1.4 动态选择最优估计

2 试验结果

试验数据来自某船在某海域实测的激光捷联惯导系统与重力仪数据。船上有GPS接收机,其位置指示作为船的真实位置,对重力匹配精度进行核验。重力异常图分辨率为1′×1′。轨迹采样点个数n=20。每条轨迹序列相邻采样点的间隔d≥1′。50条轨迹每完成一次匹配,通过相关极值匹配,选择一条最优估计的轨迹最后一点作为动态序列匹配算法的输出。

图6、7分别为两段航行区域的GPS真实轨迹、惯导输出轨迹、MSD匹配算法曲线和动态匹配算法曲线轨迹图。图8、9分别为上述两段航行区域的GPS真实轨迹与惯导输出轨迹、MSD匹配算法曲线和动态匹配算法误差曲线图。