CN118209113B - 基于进化差分算法的单信标测距匹配辅助惯性导航方法 - Google Patents

Abstract

本发明公开了一种基于进化差分算法的单信标测距匹配辅助惯性导航方法，属于水下辅助导航研究技术领域，包括以下步骤：构建单信标测距解析基准图；基于固定序列长度对相关极值匹配辅助导航算法进行改进；在所述单信标测距解析基准图上引入改进后的相关极值匹配辅助导航算法，构建目标函数模型；基于进化差分算法对所述目标函数模型中的刚性变换参数进行解算寻优，获得最优刚性变换参数；基于所述最优刚性变换参数，获得优化后的目标函数模型；将惯导航迹点输入到所述优化后的目标函数模型中进行处理，输出匹配位置。本发明提出的方法具备易于实施、计算量小、精度高、实时性好的优点。

Description

基于进化差分算法的单信标测距匹配辅助惯性导航方法

技术领域

本发明属于水下辅助导航研究技术领域，尤其涉及一种基于进化差分算法的单信标测距匹配辅助惯性导航方法。

背景技术

目前，水下导航定位主要采用惯性导航方式，但仅依靠惯性导航方式进行定位解算，其定位误差随时间积累发散，难以保证水下载体长时间保持高精度导航定位。考虑到这一局限问题，通常采用基于某种地球物理场进行匹配辅助导航的方法实时或定期地校正INS，常见方式有地形匹配辅助导航、重力匹配辅助导航及地磁匹配辅助导航等。

现有文献一将经典的TERCOM算法引入水下地形匹配辅助导航，并通过仿真详细分析了载体运行速度、测深误差、初始位置误差、匹配区域地形特征、地图分辨率等因素对匹配定位精度的影响，较好地为地形匹配辅助导航工程实践应用提供了相关参考，但该算法依旧采用离散搜索形式，未能从本质上解决匹配定位精度与算法效率矛盾这一根本问题。现有文献二基于二维高斯函数对局部离散重力异常基准图进行逼近，由此建立了基准图解析模型，在此基础上对传统匹配算法进行重新建模，并采用拟牛顿BFGS寻优算法对新的匹配定位模型进行解算，较好地解决了匹配精度受限于基准图分辨率的问题。但是，上述基于地球物理场的匹配辅助导航方法都存在一个共性问题，即需要预先获得匹配定位区域大范围、高精度、高分辨率的物理场基础数据，且该基础数据需要具备较明显的变化特征，这使得这类匹配方式在实际应用中受到了极大地限制。

而单信标测距定位只需在海底布放单只声信标即可实现局部海域水下定位，不需要获取地球物理场背景数据，因而逐渐发展成为水下辅助导航研究领域的热点方向。基于此，提出了一种基于进化差分算法的单信标测距匹配辅助惯性导航方法。

发明内容

为解决上述技术问题，本发明提出了一种基于进化差分算法的单信标测距匹配辅助惯性导航方法，将单信标测距与匹配辅助导航定位相融合，较好地解决了传统离散匹配算法受限于基准数据获取及基准图分辨率与定位精度之间影响的问题，解决了上述现有技术存在的问题。

为实现上述目的，本发明提供了一种基于进化差分算法的单信标测距匹配辅助惯性导航方法，包括以下步骤：

构建单信标测距解析基准图；

基于固定序列长度对相关极值匹配辅助导航算法进行改进；

在所述单信标测距解析基准图上引入改进后的相关极值匹配辅助导航算法，构建目标函数模型；

基于进化差分算法对所述目标函数模型中的刚性变换参数进行解算寻优，获得最优刚性变换参数；

基于所述最优刚性变换参数，获得优化后的目标函数模型；

将惯导航迹点输入到所述优化后的目标函数模型中进行处理，输出匹配位置。

优选的，构建单信标测距解析基准图的过程包括：以信标位置为原点建立直角坐标系，在所述直角坐标系中选取测点，获取测点位置至信标位置的欧氏距离；基于信标位置水深和测点位置水深，获得深度差值；基于所述测点位置至信标位置的欧氏距离与深度差值，获得基准面的测距信息，进而构建单信标测距解析基准图。

优选的，构建目标函数模型的过程包括：构建惯导航迹序列，对所述惯导航迹序列进行刚性变换，获得参考航迹序列；在所述参考航迹序列处，以单信标测距解析基准图为数据基础进行插值计算，获得对应的单信标测距基准面计算值序列；获取实际测距序列，基于均方差算法对所述单信标测距基准面计算值序列与实际测距序列进行分析，建立目标函数模型。

优选的，对所述惯导航迹序列进行刚性变换的过程包括：获取所述惯导航迹序列的序列质心，以所述序列质心为旋转中心进行第一次刚性变换；构建第一次刚性变换与第二次刚性变换的转化公式，基于所述转化公式进行以原点为旋转中心的第二次刚性变换。

优选的，所述第一次刚性变换与第二次刚性变换的转化公式如下所示：

其中，为第一次刚性变换中旋转变换角度，为第二次刚性变换中旋转变换角度，为第一次刚性变换中平移变换向量，为第二次刚性变换中平移变换向量，为、两向量之差，为惯导航迹序列质心，为对应的旋转矩阵，为单位矩阵。

优选的，所述目标函数模型的公式表示如下所示：

其中，为种群目标函数，、为惯导航迹沿东向、北向平移量，为惯导航迹序列绕航迹序列质心旋转量，为参考航迹点坐标，为参考航迹点，为对应的基准测距值，为单信标真实基准测距信息，为惯导航迹点坐标，为惯导航迹序列质心坐标。

优选的，对所述目标函数模型中的刚性变换参数进行解算寻优的过程包括：构建所述目标函数模型的三维置信空间，在所述三维置信空间中随机均匀的产生满足约束条件的若干个种群成员；从当前代的若干个种群成员中，随机选择三个互异个体进行变异运算，获得若干个种群试验个体；将所述种群试验个体与当前代对应的父代个体进行交叉运算产生新个体；对新个体进行排序，将最优的新个体作为下一代个体继续进行迭代，直至满足终止条件，迭代结束，获得最优刚性变换参数。

优选的，将惯导航迹点输入到所述优化后的目标函数模型之前还包括：基于旋转平移修正矩阵对所述惯导航迹点进行位置修正，加入到初始匹配序列中，然后将所述初始匹配序列作为输入。

优选的，所述旋转平移修正矩阵经过每次匹配的旋转平移矩阵迭代得到，迭代公式如下：

其中，为第k次匹配后惯导航迹点旋转修正矩阵，为惯导航迹点平移修正矩阵，为第k次匹配定位解算的最优旋转矩阵，为第k次匹配定位解算的最优平移矩阵。

与现有技术相比，本发明具有如下优点和技术效果：

本发明将相关极值匹配算法与单信标测距相结合，在单信标测距解析基准图基础上重构了目标函数模型，并应用差分进化寻优算法对该模型进行解算。同时，针对相关极值匹配算法实时性差的缺点，设计了固定匹配序列长度建模方式，推导了相关单点迭代公式，最终完成了基于单信标测距解析基准图的实时相关极值匹配辅助惯性导航算法设计，该方法具备易于实施、计算量小、精度高、实时性好的优点。

附图说明

构成本申请的一部分的附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1为本发明实施例的单信标测距解析基准图构建示意图；

图2为本发明实施例的基于单信标测距解析基准图的实时相关极值匹配算法流程图；

图3为本发明实施例的试验真实航迹及推算航迹对比示意图；

图4为本发明实施例的匹配结果示意图；

图5为本发明实施例的推算航迹与匹配航迹位置误差分析图，其中，（a）为推算航迹与匹配航迹东向位置误差分析图，（b）为推算航迹与匹配航迹北向位置误差分析图；

图6为本发明实施例的匹配航迹测距MSD及径向误差对比分析图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

实施例一

单信标测距定位是水下辅助导航研究领域的热点方向。本实施例将相关极值算法与单信标测距相融合进行匹配辅助导航重新建模；同时，采用差分进化寻优算法对该模型进行解算；最后，设计了固定匹配序列长度建模方式改进了传统算法的实时性，最终完成了基于单信标测距解析基准图的实时相关极值匹配辅助惯性导航算法设计，该算法直接基于解析基准图建模，匹配精度不再受限于离散基准图分辨率，同时也具备了单点迭代实时匹配定位能力。

相关极值匹配算法基本原理及主要缺点：

假定惯导航迹序列点上的某地球物理场量测序列为，根据惯导航迹序列在该物理场基准图中搜索若干与等序列长度的参考序列，最后通过量测序列与参考序列之间相关极值分析获取最优匹配位置。常用的相关极值分析算法包括互相关(COR，Cross Correlation)算法、平均绝对差(MAD，Mean Absolution Difference)算法以及均方差(MSD，Mean Square Difference)算法。考虑到MSD算法性能较为优越，因此本实施例采用MSD算法进行相关极值建模分析。

从上述基本原理可以看出，相关极值匹配算法的关键是以惯导航迹为基础在物理场基准图中获取一系列参考序列，而获取参考序列的核心是在基准图中搜索对应于量测序列的等值点序列。为了提高算法效率，一般都采用局部搜索区域法减少搜索量，即以惯导航迹点为中心，惯导概略漂移范围为半径确定真实航迹点对应的置信区间，然后在该置信区间内以一定的阈值搜索等值点。首先，由于传统相关极值匹配算法采用离散基准图，如何设置合适的等值点搜索阈值成为一个难点问题。若阈值设定过大，会导致算法计算量增大且虚假匹配几率增加；若阈值设定过小，则会导致有效匹配几率变小甚至出现匹配实效等情况。其次，由于传统相关极值匹配算法只能定位到基准图格网点，由此算法的匹配精度将直接受限于基准图分辨率。最后，传统相关极值匹配算法为序列匹配算法，算法实时性不高。

基于此，本实施例中提供一种基于进化差分算法的单信标测距匹配辅助惯性导航方法，包括以下步骤：

基于单信标测距的实时相关极值匹配算法设计：

为了克服传统相关极值匹配算法局限性，本实施例采样单信标测距方式获取量测信息，显然此时匹配背景场为一系列以信标坐标为中心的同心圆，因此该背景场具有简单、统一的解析表达式，可以在此基础上对相关极值匹配算法进行重新设计，建立连续的目标函数模型，并对其进行解算以获取匹配定位。

单信标测距解析基准图构建：

如图1所示，以信标位置为原点建立直角坐标系，设测点P坐标为，则测点至信标位置的欧式距离函数为：

(1)

考虑到匹配定位一般基于二维平面进行解算，因此需要将测距信息归算至统一基准面上，本实施例将所有测点测距信息归算至平面。实际上，信标所处深度是预先可知的，而水下载体可通过压力深度测量传感器获取测点位置水深,比如美国NovaSensor公司的高精度、高稳定性扩散硅压力传感器测量精度可达±0.25%，通过两者深度之差即可实现基准面测距信息归算，其计算公式如下：

(2)

即为对应于平面上的解析基准图，后续算法设计、建模及解算均在此基础上展开。

基于单信标测距的相关极值匹配算法建模：

设惯导航迹序列为，其质心为，对应的单信标测距基准面计算值序列为，参考航迹序列为，其质心为，对应的单信标测距基准面计算值序列为，真实航迹点上单信标测距基准面计算值序列即为实际测距序列，记为。考虑到惯性导航在短时间内相对漂移误差较小，因此可将匹配过程视为航迹序列刚性变换过程(其中为航迹序列平移向量，为绕航迹序列质心旋转量)，具体过程如下：首先，惯导航迹序列经过刚性变化得到参考航迹序列；然后，在处以基准面测距基准图为数据基础进行插值计算获得单信标测距基准面计算值序列；最后，通过与进行均方差算法分析建立目标函数模型（如式(3)所示），最小化求得此时的，对进行刚性变换即可获得最优匹配航迹。

(3)

其中，为种群目标函数，、为惯导航迹沿东向、北向平移量，为惯导航迹序列绕航迹序列质心旋转量，为参考航迹点坐标，为参考航迹点，为对应的基准测距值，为单信标真实基准测距信息，为惯导航迹点坐标，为惯导航迹序列质心坐标。

上述刚性变换建模过程是基于序列质心进行的，该方式的优点是刚性变换范围较小，目标函数变量寻优区间易于确定，但每次匹配迭代计算后都需要存储本次匹配序列质心用于下一次匹配定位惯导航迹点修正计算。实际上，刚性变换建模过程也可基于坐标系原点进行，即惯导航迹序列经过刚性变化得到参考航迹序列，两刚性变化之间存在如下转换关系：

(4)

其中，为第一次刚性变换中旋转变换角度，为第二次刚性变换中旋转变换角度，为第一次刚性变换中平移变换向量，为第二次刚性变换中平移变换向量，为、两向量之差，为惯导航迹序列质心，为对应的旋转矩阵，为单位矩阵。

该方式刚性变换采用统一的旋转中心，因此无需存储上一次匹配序列质心，便捷性得到大大提升，但由于一般较大，导致目标函数其寻优区间急剧增大，由此极大地增加了算法搜索计算量及虚假匹配定位概率。综上分析，本实施例采用两种刚性变换相结合的方式来实现算法建模，即在匹配定位解算时采用以序列质心为旋转中心的方式进行第一次刚性变换，然后通过转化公式获得对应的以原点为质心的第二次刚性变换，该刚性变换参数即可用来修正下一惯导航迹点。

基于进化差分算法的最优刚性变换解算：

考虑到进化差分算法(DE, Differential Evolution)具备超强的全局多维非线性寻优性能，为此本实施例将采用进化差分算法对式(3)模型进行解算。根据式(3)将置信区间设定为；；，则公式(3)最优刚性变换解算过程转化为在该三维置信区间内非线性寻优过程。由式(3)可知，模型中有三个刚性变换参数需要确定，由此将种群的解空间设置成三维空间，每个种群成员的坐标及分布区间分别为，种群目标函数为式(3)中的F，则优化模型可转化为如下非线性优化问题：

(5)

解算流程如下：

（1）种群初始化：

在三维空间内均匀随机地产生满足约束条件的个种群成员：

(6)

式中：为第0代第j个种群个体，为0到1之间均匀随机数，为种群规模。

（2）变异运算：

从当前代中随机选择三个互异个体进行变异运算，按式(7)产生个种群试验个体：

(7)

其中，为第k代互异随机的种群个体；为0到2之间定常变异因子。

（3）交叉运算：

将变异运算获得的种群实验个体与对应父代个体进行交叉运算产生新个体，公式如下：

(8)

式中：k为1至N随机正整数，为0到1之间交叉概率。

（4）选择运算：

选择较优的个体作为下一代个体进行迭代，公式如下：

(9)

（5）重复执行步骤（2）~（4），直至满足终止条件为止。

算法实时性改进：

现有文献中采用固定序列长度来实现单点迭代的ICCP改进算法，由于相关极值匹配算法与ICCP匹配算法同为序列相关匹配算法，因此该改进方法亦可以应用到相关极值匹配算法实时性改进中。由于新的INS指示航迹点不能直接作为初始匹配序列点，必须经过旋转平移修正矩阵修正后才能进入初始匹配序列中。经过修正后的新的INS指示航迹点进入到初始匹配序列后，从原初始匹配序列中删除第一个航迹点，由此构成新的初始匹配序列进行匹配定位解算。此时，匹配序列长度保持了不变，由此实现了具备单点迭代能力的实时序列匹配算法。旋转平移修正矩阵经过每次匹配的旋转平移矩阵迭代得到，迭代公式如下：

(10)

其中，为第k次匹配后惯导航迹点旋转修正矩阵，为惯导航迹点平移修正矩阵，为第k次匹配定位解算的最优旋转矩阵，为第k次匹配定位解算的最优平移矩阵。

综上所述，整个基于单信标测距解析基准图的实时相关极值匹配算法流程如图2所示。

实测试验分析：

本实施例选取某内陆湖水域进行了单信标测距匹配辅助导航水面试验，单信标固定布置于坐标（2800，-2000）处，易知匹配背景场为以单信标坐标为圆心的一同心圆系列组成；试验船上搭载有高精度GPS、计程仪及声波测距仪等试验设备，试验船出发点坐标为（0,0），整个航行时间为12600秒，声信标测距相对误差为0.1%。GPS航迹由高精度GPS测量得到，可视为真实航迹；计程仪推算航迹由计程仪测量数据根据舰位推算模型得到。图3为航行试验GPS航迹及计程仪推算航迹，由图3可以看出，由于传感器精度较高、航行试验时间较短等原因，计程仪推算航迹精度较高、航迹漂移误差较小，为更有效地验证匹配辅助导航算法精度，本实施例将计程仪推算航迹分别向北、向东平移1000m，同时截取1200秒至5600秒数据，每隔100秒对该数据进行采样，由此获得初始航迹数据，匹配序列点数设置为6，迭代次数设置为5次，匹配结果如图4所示。由图4可以看出，匹配航迹能够以较高精度实时动态跟踪真实航迹，匹配算法具备较高的精度及较好的实时性。

图5为推算航迹与匹配航迹位置误差分析结果，图6为匹配航迹测距MSD及径向误差对比分析结果，表1为推算航迹与匹配航迹位置误差及测距MSD对比分析表。误差分析表明，推算航迹定位径向误差均值为1529.4m，而单信标测距匹配辅助导航定位径向误差仅为111.8m，定位精度较计程仪推算航迹有大幅提升。从图6及表1可以看出，经过匹配定位后获得的匹配航迹测距信息MSD值与测距误差相当，这也从另一方面说明了本实施例提出的算法从最大程度上保证了匹配定位精度。

表1

为解决传统匹配辅助惯性导航离散方法定位精度受限于离散基准图数据分辨率且定位精度与算法效率矛盾等问题，本实施例将相关极值匹配算法与单信标测距相结合，在单信标测距解析基准图基础上重构了相关极值匹配辅助导航算法模型，并应用差分进化寻优算法对该模型进行解算。同时，针对该序列算法实时性差的缺点，设计了固定匹配序列长度建模方式，推导了相关单点迭代公式，最终完成了基于单信标测距解析基准图的实时相关极值匹配辅助惯性导航算法设计。通过相关实测试验表明，该方法具备易于实施、计算量小、精度高、实时性好等优点，可为后续相关研究提供参考。

以上，仅为本申请较佳的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应该以权利要求的保护范围为准。

Claims (6)

1.一种基于进化差分算法的单信标测距匹配辅助惯性导航方法，其特征在于，包括以下步骤：

构建单信标测距解析基准图；

基于固定序列长度对相关极值匹配辅助导航算法进行改进；

在所述单信标测距解析基准图上引入改进后的相关极值匹配辅助导航算法，构建目标函数模型；

基于进化差分算法对所述目标函数模型中的刚性变换参数进行解算寻优，获得最优刚性变换参数；

基于所述最优刚性变换参数，获得优化后的目标函数模型；

将惯导航迹点输入到所述优化后的目标函数模型中进行处理，输出匹配位置；

构建单信标测距解析基准图的过程包括：以信标位置为原点建立直角坐标系，在所述直角坐标系中选取测点，获取测点位置至信标位置的欧氏距离；基于信标位置水深和测点位置水深，获得深度差值；基于所述测点位置至信标位置的欧氏距离与深度差值，获得基准面的测距信息，进而构建单信标测距解析基准图；

基于固定序列长度对相关极值匹配辅助导航算法进行改进的过程包括：

新的INS指示航迹点经过旋转平移修正矩阵修正后进入初始匹配序列中，从原初始匹配序列中删除第一个航迹点，由此构成新的初始匹配序列进行匹配定位解算，此时，匹配序列长度保持不变，获得具备单点迭代能力的改进后的相关极值匹配辅助导航算法；

构建目标函数模型的过程包括：构建惯导航迹序列，对所述惯导航迹序列进行刚性变换，获得参考航迹序列；在所述参考航迹序列处，以单信标测距解析基准图为数据基础进行插值计算，获得对应的单信标测距基准面计算值序列；获取实际测距序列，基于均方差算法对所述单信标测距基准面计算值序列与实际测距序列进行分析，建立目标函数模型；

所述目标函数模型的公式表示如下所示：

其中，为种群目标函数，、为惯导航迹沿东向、北向平移量，为惯导航迹序列绕航迹序列质心旋转量，为参考航迹点坐标，为参考航迹点，为对应的基准测距值，为单信标真实基准测距信息，为惯导航迹点坐标，为惯导航迹序列质心坐标。

2.根据权利要求1所述的基于进化差分算法的单信标测距匹配辅助惯性导航方法，其特征在于，

对所述惯导航迹序列进行刚性变换的过程包括：获取所述惯导航迹序列的序列质心，以所述序列质心为旋转中心进行第一次刚性变换；构建第一次刚性变换与第二次刚性变换的转化公式，基于所述转化公式进行以原点为旋转中心的第二次刚性变换。

3.根据权利要求2所述的基于进化差分算法的单信标测距匹配辅助惯性导航方法，其特征在于，

所述第一次刚性变换与第二次刚性变换的转化公式如下所示：

其中，为第一次刚性变换中旋转变换角度，为第二次刚性变换中旋转变换角度，为第一次刚性变换中平移变换向量，为第二次刚性变换中平移变换向量，为、两向量之差，为惯导航迹序列质心，为对应的旋转矩阵，为单位矩阵。

4.根据权利要求1所述的基于进化差分算法的单信标测距匹配辅助惯性导航方法，其特征在于，

对所述目标函数模型中的刚性变换参数进行解算寻优的过程包括：构建所述目标函数模型的三维置信空间，在所述三维置信空间中随机均匀的产生满足约束条件的若干个种群成员；从当前代的若干个种群成员中，随机选择三个互异个体进行变异运算，获得若干个种群试验个体；将所述种群试验个体与当前代对应的父代个体进行交叉运算产生新个体；对新个体进行排序，将最优的新个体作为下一代个体继续进行迭代，直至满足终止条件，迭代结束，获得最优刚性变换参数。

5.根据权利要求1所述的基于进化差分算法的单信标测距匹配辅助惯性导航方法，其特征在于，

将惯导航迹点输入到所述优化后的目标函数模型之前还包括：基于旋转平移修正矩阵对所述惯导航迹点进行位置修正，加入到初始匹配序列中，然后将所述初始匹配序列作为输入。

6.根据权利要求5所述的基于进化差分算法的单信标测距匹配辅助惯性导航方法，其特征在于，

所述旋转平移修正矩阵经过每次匹配的旋转平移矩阵迭代得到，迭代公式如下：

其中，为第k次匹配后惯导航迹点旋转修正矩阵，为惯导航迹点平移修正矩阵，为第k次匹配定位解算的最优旋转矩阵，为第k次匹配定位解算的最优平移矩阵。