CN113252039B - 面向地形辅助导航的粒子群快速匹配方法 - Google Patents

Abstract

本发明涉及一种面向地形辅助导航的粒子群快速匹配方法，用于解决现有地形辅助导航方法实时性较差的技术问题。该方法首先建立无人机任务区间的环境模型，并使用稀疏A*算法进行航迹规划，在此基础上采用一种优化的粒子群算法进行地形高度匹配，该算法在粒子群算法的基础上引入分布估计思想，增强了粒子群优化算法的全局寻优能力，提高了TERCOM的实时性。得到TERCOM最佳位置后，将TERCOM最佳匹配位置和SINS输出的位置信息的差值作为量测值，利用Kalman滤波估计SINS的误差并校正SINS。本发明在地形辅助导航中使用改进的粒子群算法进行地形高度匹配，避免了传统TERCOM由于采取遍历的方式而导致的实时性差的问题，提升了地形辅助导航的实时性。

Description

面向地形辅助导航的粒子群快速匹配方法

技术领域

本发明涉及一种地形辅助导航方法，特别是涉及一种面向地形辅助导航的粒子群快速匹配方法，属于导航、制导与控制领域。

背景技术

地形辅助导航TAN(Terrain-Aided Navigation)技术是近年来受到广泛重视并已成功使用的组合导航***，地形辅助导航是一种利用地形高程特征来进行辅助定位的方法，它具有自主、隐蔽、连续、全天候工作、导航定位误差不随时间积累等优点，是理想的辅助导航定位手段。然而，地形辅助导航要求地形高程有明显的变化，对于地形变化过于平缓、地形特征不明显的区域，用地形辅助导航方法来降低惯导***的定位误差是不可行的。

提高匹配效率是地形辅助导航的关键，《基于粒子群优化的飞行器地形匹配新算法》(何艳萍，刘新学，蔡艳平，李亚雄，朱昱，《红外与激光工程》，2016年第5期第45卷)一文将粒子群(Particle Swarm Optimization，PSO)优化算法引入到地形辅助导航问题中，以解决传统地形匹配算法精度低实时性差等问题。但是粒子群优化算法粒子群优化算法存在早熟收敛的问题，容易陷入局部极值点，从而不能快速收敛到全局极值点。

发明内容

要解决的技术问题

针对粒子群算法存在早熟收敛而导致地形辅助导航实时性较差的问题，本发明设计了一种面向地形辅助导航的粒子群快速匹配方法，该方法在粒子群算法的搜索过程中引入分布估计算法(Estimate Distribution Algorithms，EDA)思想，增强粒子群优化算法的全局寻优能力以提高地形辅助导航的实时性。

技术方案

一种面向地形辅助导航的粒子群快速匹配方法，其特征在于步骤如下：

步骤1：基于地形差异熵的地形适配区选择

在无人机进行航迹规划之前，需要获取执行任务所途径区域的环境信息，并将地形适配区与非适配区区分开来，以保证飞行航迹均位于地形适配区；地形辅助导航所利用的是地形高程信息，地形差异熵可以用来描述地形的起伏的复杂程度，所述的地形差异熵为：

式中，h_i表示高程值；M为该区域高程点总数；

表示平均高程值；C_i表示高程差异值；P_i表示某一高程差异值出现的概率；地形高程变化越急剧，计算出的熵值越小，越有利于匹配计算；

步骤2：基于稀疏A*算法的航迹规划

在规划环境建模的基础上，使用稀疏A*算法在任务起点与终点间规划出一条最优航迹；该算法在搜索中加入无人机飞行约束条件，能够减少搜索空间中的无效节点，从而提高搜索效率；由于地形辅助导航航迹规划在二维平面内进行，故只考虑最大拐弯角约束与最小航迹段约束；

以当前航线在水平面投影为对称轴，以最小航迹段长度R为半径，两倍最大拐弯角Ψ为圆心角构造扇形区域作为待搜索区，扇形弧线与数字地图网格相交点为待扩展结点，不在数字地图网格上的交点扩展为网格的顶点位置，如果待扩展结点位于非适配区内或与当前节点距离小于最小航迹段距离，则将该节点从待扩展节点中排除，待当前节点与目标点的距离小于最小航迹段时，则终止搜索；

步骤3：基于优化粒子群算法的地形高度匹配

地形高度匹配TERCOM通过比较飞行器飞行路径正下方的地形高度和存储的参考高程地图，得出飞行器的位置信息；包括以下步骤：

1.高程信息采集

捷联惯导***SINS提供水平位置，气压高度计可得到运载体的海拔高度，雷达高度计测量运载体的离地高度，两者相减即为地形高度；

2.确定搜索区域

搜索范围的大小与SINS精度有关，假设SINS的漂移量为σ，则基准图上的搜索范围以SINS推算定位为中心，将±5σ的范围内的高程点作为匹配搜索的起点，在基准图上进行遍历搜索；

3.匹配搜索

飞机测得其正下方的地形高程后，则与数字地图的高程数据进行匹配；匹配搜索采用优化粒子群算法，考虑PSO算法全局寻优能力不足，在其基础上引入EDA思想，即粒子通过PSO算法与EDA算法同时更新，设R为搜索范围内基准子图的总数，每一代将从中选出M个基准子图由粒子群算法移动到下一位置，选出N个基准子图在EDA中进化到下一代；由于EDA中的种群是根据基准子图的适应度值由最优到最劣排序的，所以PSO-EDA组合方法可以避免搜索方向向局部最优快速收敛；

在地形高度匹配搜索中，每一个与实测图平行的基准子图即为一个粒子，假设实测图有D个高程点，则粒子的维数为D维；第i个粒子的位置表示为x_i＝(x_i1,x_i2,…,x_iD)，该粒子对应位置速度的变化量为v_i＝(v_i1,v_i2,…,v_iD)，粒子的速度和位置更新公式为：

式中，ω为惯性权重，可以动态调整粒子运动速度；

为第i粒子d维分量在第t次迭代中的速度和位置；c₁，c₂为加速因子，取非负值；P_best为粒子本身所找到的最好解；G_best为整个种群目前找到的最好解；r₁，r₂为随机数，服从区间[0,1]上的均匀分布；

采用PSO-EDA算法TERCOM的步骤如下：

(1)初始化粒子，在搜索区域内随机生成R个基准子图，得到种群D₀；

(2)初始化EDA算法的概率模型；在TERCOM中，基准子图的位置分布服从二维高斯分布，令x表示基准子图中节点的经度，y表示基准子图中节点的纬度，则基准子图的位置分布表示为：

式中，μ₁，μ₂，

分别为变量x，y的均值和方差；ρ为常数；设搜索范围的中心坐标为(x₀,y₀)，则令概率模型中μ₁＝x₀，μ₂＝y₀，

式中，X[i][j]，Y[i][j]分别为第i个基准子图中第j个节点的经、纬度；N为基准子图个数；

(3)确定粒子的适应度值；将基准子图与实测图之间的平均Hausdorff距离MHD作为粒子的适应度值，并按照适应度值由小到大对粒子进行排序；MHD定义为：

式中，A＝{a₁,a₂,…a_n}，B＝{b₁,b₂,…b_n}为两序列；P为A中的元素个数；||·||为定义在集合A和B之间的欧式范数；a_i为A中第i个点的位置；b_j为B中第j个点的位置；

(4)更新EDA算法中高斯概率模型的均值与方差；在所有基准子图中选择适应度值最优的前30％基准子图，设在第t代时，x_j对应的高斯分布均值和方差分别为

y_j对应的高斯分布均值和方差分别为

则有

式中，α为学习因子；

和

分别表示种群中最优个体、次优个体和最差个体所对应的x_j值；

分别表示种群中最优个体、次优个体和最差个体所对应的y_j值；K为选出的较优种群的个数；x_jk为种群按照适应度由小到大排序的第k-1个个体的x_j值；y_jk为种群按照适应度由小到大排序的第k-1个个体的y_j值；

为选出的K个较优种群的x_jk的均值；

为选出的K个较优种群的y_jk的均值；

(5)根据更新后的概率模型生成t+1代EDA子种群

(6)更新粒子群算法中每个粒子的G_best和P_best；比较各粒子的当前适应度值和个体极值P_best与全局极值G_best，若粒子当前适应度值小于P_best，则将P_best更新为粒子当前的适应度值；若当前适应度值小于G_best，则将G_best更新为粒子当前的适应度值；

(7)根据式(1)更新每个粒子的位置得到t+1代粒子群子种群

(8)建立新种群

(9)判断是否满足算法终止条件，若不满足则转到第(3)步进行t+1次迭代，否则输出全局极值G_best及其对应的位置，即最佳匹配位置；

步骤4：SINS/TERCOM组合导航

得到TERCOM提供的最佳匹配位置后，选取SINS的误差为SINS/TERCOM组合导航***的状态变量构建状态方程；选择TERCOM最佳匹配位置和SINS输出的位置信息的差值作为量测值；对建立的状态方程和量测方程使用卡尔曼滤波器进行滤波更新，并反馈修正SINS，得到组合导航后的导航参数。

步骤2中所述的R＝1.2km，Ψ＝48°。

一种计算机***，其特征在于包括：一个或多个处理器，计算机可读存储介质，用于存储一个或多个程序，其中，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现上述的方法。

一种计算机可读存储介质，其特征在于存储有计算机可执行指令，所述指令在被执行时用于实现上述的方法。

一种计算机程序，其特征在于包括计算机可执行指令，所述指令在被执行时用于实现上述的方法。

有益效果

针对传统地形辅助导航采用遍历算法搜索实时性较差的问题，本发明提出了一种面向地形辅助导航的粒子群快速匹配方法，该方法将地形信息量引入航迹规划，使用稀疏A*算法进行航迹规划，在此基础上采用一种优化的粒子群算法进行TERCOM，该算法在粒子群算法的基础上引入分布估计思想，增强了粒子群优化算法的全局寻优能力，提高了TERCOM的实时性。得到TERCOM最佳匹配位置后，则利用TERCOM最佳匹配位置和SINS输出的位置信息的差值作为量测值，使用Kalman滤波估计SINS的误差，并校正SINS。本发明采用一种改进的粒子群算法实现TERCOM，增强了粒子群优化算法的全局寻优能力，提高了地形辅助导航的实时性。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1待搜索区示意图。

图2本发明方法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

参照图2，本发明面向地形辅助导航的粒子群快速匹配方法的具体步骤如下：

步骤1：基于地形差异熵的地形适配区选择

由于地形辅助导航无法在地形平坦区域使用，故无人机进行航迹规划之前，需要获取执行任务所途径区域的环境信息并区分地形适配区与非适配区，以确保无人机的高效航行；

在无人机进行航迹规划之前，需要获取执行任务所途径区域的环境信息，并将地形适配区与非适配区区分开来，以保证飞行航迹均位于地形适配区。地形辅助导航所利用的是地形高程信息，由于地形辅助导航的精度受地形的影响，地形差异熵可以用来描述地形的起伏的复杂程度，地形差异熵为：

式中，h_i表示高程值，M为该区域高程点总数，

表示平均高程值，C_i表示高程差异值，P_i表示某一高程差异值出现的概率。地形高程变化越急剧，计算出的熵值越小，越有利于匹配计算。

相关的参数定义如下：

设无人机飞行任务为由东经109度北纬34度至东经110度北纬35度，即任务区间为东经109度-东经110度，北纬34度-北纬35度的矩形区域；数字高程地图精度为30m×30m；将任务区间划分为精度为900m×900m的网格区域，则任务区间的网格数量为14400个。

步骤2：基于稀疏A*算法的航迹规划

在规划环境建模的基础上，使用稀疏A*算法在任务起点与终点间规划出一条最优航迹。该算法在搜索中加入无人机飞行约束条件，能够减少搜索空间中的无效节点，从而提高搜索效率。由于地形辅助导航航迹规划在二维平面内进行，故只考虑最大拐弯角约束与最小航迹段约束。

以当前航线在水平面投影为对称轴，以最小航迹段长度R为半径，两倍最大拐弯角Ψ为圆心角构造扇形区域作为待搜索区，扇形弧线与数字地图网格相交点为待扩展结点，不在数字地图网格上的交点扩展为网格的顶点位置，如图1所示。如果待扩展结点位于非适配区内或与当前节点距离小于最小航迹段距离，则将该节点从待扩展节点中排除，若当前节点与目标点的距离小于最小航迹段，则终止搜索。

相关参数取值为：R＝1.2km，Ψ＝48°。

步骤3：基于优化粒子群算法的地形高度匹配

TERCOM通过比较飞行器飞行路径正下方的地形高度和存储的参考高程地图，得出飞行器的位置信息。包括以下步骤：

1.高程信息采集

惯性导航***提供水平位置，气压高度计可得到运载体的海拔髙度，雷达高度计测量运载体的离地高度，两者相减即为地形高度。

2.确定搜索区域

设SINS的漂移量为500m，那么在基准图上的搜索范围以SINS推算定位为中心，将±2500m的范围内的高程点作为匹配搜索的起点，在基准图上进行遍历搜索。

3.匹配搜索

当飞机测得其正下方的地形高程后，便可与数字地图的高程数据进行匹配，匹配搜索采用优化粒子群算法，考虑PSO算法全局寻优能力不足，在其基础上引入EDA思想，即在该算法中粒子通过PSO算法与EDA算法同时更新。设R为搜索范围内基准子图的总数，每一代将从中选出M个基准子图由粒子群算法移动到下一位置，选出N个基准子图在EDA中进化到下一代。由于EDA中的种群是根据基准子图的适应度值由最优到最劣排序的，所以PSO-EDA组合方法可以避免搜索方向向局部最优快速收敛。

在地形高度匹配搜索中，每一个与实测图平行的基准子图即为一个粒子，假设实测图有D高程点个，则粒子的维数为D维。第i个粒子的位置表示为x_i＝(x_i1,x_i2,…,x_iD)，该粒子对应位置速度的变化量为v_i＝(v_i1,v_i2,…,v_iD)，粒子的速度和位置更新公式为：

式中，ω＝0.9为惯性权重，可以动态调整粒子运动速度；

为第i个粒子的d维分量在第t次迭代中的速度和位置；c₁＝2，c₂＝2为加速因子；P_best为粒子本身所找到的最优解；G_best为整个种群目前找到的最好解；r₁，r₂为随机数，服从区间[0,1]上的均匀分布。

PSO-EDA算法进行TERCOM的步骤如下：

(1)初始化粒子，在搜索区域内随机生成200个基准子图，得到种群D₀；

(2)初始化EDA算法的概率模型。在地形高度匹配中，基准子图的位置分布服从二维高斯分布，用x表示基准子图中节点的经度，y表示基准子图中节点的纬度，则其分布表示为；

式中，μ₁，μ₂，

分别为变量x，y的均值和方差，ρ为常数。设搜索范围的中心坐标为(x₀,y₀)，则令概率模型中μ₁＝x₀，μ₂＝y₀，

式中，X[i][j]，Y[i][j]分别为第i个基准子图中第j个节点的经、纬度，N为基准子图个数。

(3)确定粒子的适应度值。将基准子图与实测地形高程之间的平均Hausdorff距离作为粒子的适应度值，并按照适应度值由小到大对粒子进行排序。MHD定义为：

式中，A＝{a₁,a₂,…a_n}，B＝{b₁,b₂,…b_n}为两序列；P为A中的元素个数；||·||为定义在集合A和B之间的欧式范数；a_i为A中第i个点的位置；b_j为B中第j个点的位置。

(4)更新EDA算法中高斯概率模型的均值与方差。在所有基准子图中选择适应度值最优的前30％基准子图，设在第t代时，x_j对应的高斯分布均值和方差分别为

y_j对应的高斯分布均值和方差分别为

则有

式中，α＝0.5为学习因子；

和

分别表示种群中最优个体、次优个体和最差个体所对应的x_j值；

分别表示种群中最优个体、次优个体和最差个体所对应的y_j值；K为选出的较优种群的个数；x_jk为种群按照适应度由小到大排序的第k-1个个体的x_j值；y_jk为种群按照适应度由小到大排序的第k-1个个体的y_j值；

为选出的K个较优种群的x_jk的均值；

为选出的K个较优种群的y_jk的均值。

(5)根据更新后的概率模型生成t+1代EDA子种群

(6)更新粒子群算法中每个粒子的G_best和P_best；比较各粒子的当前适应度值和个体极值P_best与全局极值G_best，若粒子当前适应度值小于P_best，则将P_best更新为粒子当前的适应度值；若当前适应度值小于G_best，则将G_best更新为粒子当前的适应度值；

(7)根据式(1)更新每个粒子的位置得到t+1代粒子群子种群

(8)建立新种群

(9)判断是否满足算法终止条件，若不满足则转到第(2)步进行下一次迭代，否则输出全局极值G_best及其对应的位置，即最佳匹配位置。

步骤4：SINS/TERCOM组合导航

得到TERCOM最佳匹配位置后，则将TERCOM最佳匹配位置和SINS输出的位置信息的差值作为量测值，利用Kalman滤波估计惯导***的误差，并校正SINS。

选择SINS的位置误差、速度误差、姿态误差、陀螺漂移和加速度计零偏作为状态量：

式中，φ_E、φ_N和φ_U分别为东向失准角、北向失准角和方位失准角；δV_E、δV_N和δV_U分别为东向、北向和天向速度误差；δL、δλ和δh分别为经度、纬度和高度误差；ε_bx、ε_by和ε_bz分别为机体坐标系下x轴、y轴和z轴陀螺漂移误差；

和

分别为机体坐标系下x轴、y轴和z轴加速度计零偏。

选取SINS的误差为SINS/TERCOM组合导航***的状态变量，根据SINS的误差方程，可得***的状态方程为

式中，A_15×15的非零元为：

A_1,10＝-(cosγcosΨ+sinΨsinθ),A_1,11＝cosγsinΨ-sinγcosΨsinθ,A_1,12＝sinγcosθ

A_6,13＝sinγcosΨ-cosγsinΨsinθ,A_6,15＝cosγcosθ,A_6,14＝-sinγsinΨ-cosγcosΨsinθ,

A_9,3＝1，

式中，Ψ为运载体航向角；γ为运载体横滚角；θ为运载体俯仰角；ω_ie为地球自转角速率；V_E、V_N和V_U分别是导航计算所得运载体沿东向、北向和天向的速度最新值；f_E、f_N和f_U为东向、北向和天向的加速度计测量值；R_M和R_N分别为运载体所在点子午圈和卯酉圈的曲率半径；L为运载体所在点的纬度；h为运载体所在点的高度；τ_g为马尔科夫过程相关时间；

为***噪声分布阵；***的白噪声矢量为W_6×1＝[ω_gx,ω_gy,ω_gz,ω_ax,ω_ay,ω_az]^T，ω_gx、ω_gy和ω_gz为陀螺随机白噪声漂移；ω_ax、ω_ay和ω_az为加速度计马尔可夫驱动白噪声。

选择经、纬度作为量测量，SINS***的经、纬度可表示为：

式中，L_SINS，λ_SINS为SINS测量的经、纬度；L，λ表示载体真实经、纬度；δL_SINS、δλ_SINS为SINS经、纬度的测量误差。

TERCOM***的经、纬度可表示为：

式中，L_TER、λ_TER为TERCOM测量的经、纬度；δL_TER、δλ_TER为TERCOM经、纬度的测量误差。

SINS/TERCOM组合导航***的组合观测矢量为：

式中，量测阵H_2×15＝[0_2×6 I_2×2 0_2×7]；量测噪声矢量V_2×1＝[N_L N_λ]^T；N_L、N_λ为TERCOM经坐标转换后输出的经、纬度误差。

对建立的状态方程和量测方程使用卡尔曼滤波器进行滤波更新，并反馈修正SINS，则可得到组合导航后的导航参数。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明公开的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。

Claims (4)

1.一种面向地形辅助导航的粒子群快速匹配方法，其特征在于步骤如下：

步骤1：基于地形差异熵的地形适配区选择

在无人机进行航迹规划之前，需要获取执行任务所途径区域的环境信息，并将地形适配区与非适配区区分开来，以保证飞行航迹均位于地形适配区；地形辅助导航所利用的是地形高程信息，地形差异熵可以用来描述地形的起伏的复杂程度，所述的地形差异熵为：

式中，h_i表示高程值；M为该区域高程点总数；

表示平均高程值；C_i表示高程差异值；P_i表示某一高程差异值出现的概率；地形高程变化越急剧，计算出的熵值越小，越有利于匹配计算；

步骤2：基于稀疏A*算法的航迹规划

在规划环境建模的基础上，使用稀疏A*算法在任务起点与终点间规划出一条最优航迹；该算法在搜索中加入无人机飞行约束条件，能够减少搜索空间中的无效节点，从而提高搜索效率；由于地形辅助导航航迹规划在二维平面内进行，故只考虑最大拐弯角约束与最小航迹段约束；

以当前航线在水平面投影为对称轴，以最小航迹段长度R为半径，两倍最大拐弯角Ψ为圆心角构造扇形区域作为待搜索区，扇形弧线与数字地图网格相交点为待扩展结点，不在数字地图网格上的交点扩展为网格的顶点位置，如果待扩展结点位于非适配区内或与当前节点距离小于最小航迹段距离，则将该节点从待扩展节点中排除，待当前节点与目标点的距离小于最小航迹段时，则终止搜索；

步骤3：基于优化粒子群算法的地形高度匹配

地形高度匹配TERCOM通过比较飞行器飞行路径正下方的地形高度和存储的参考高程地图，得出飞行器的位置信息；包括以下步骤：

A.高程信息采集

捷联惯导***SINS提供水平位置，气压高度计可得到运载体的海拔高度，雷达高度计测量运载体的离地高度，两者相减即为地形高度；

B.确定搜索区域

搜索范围的大小与SINS精度有关，假设SINS的漂移量为σ，则基准图上的搜索范围以SINS推算定位为中心，将±5σ的范围内的高程点作为匹配搜索的起点，在基准图上进行遍历搜索；

C.匹配搜索

飞机测得其正下方的地形高程后，则与数字地图的高程数据进行匹配；匹配搜索采用优化粒子群算法，考虑PSO算法全局寻优能力不足，在其基础上引入EDA思想，即粒子通过PSO算法与EDA算法同时更新，设R为搜索范围内基准子图的总数，每一代将从中选出M个基准子图由粒子群算法移动到下一位置，选出N个基准子图在EDA中进化到下一代；由于EDA中的种群是根据基准子图的适应度值由最优到最劣排序的，所以PSO-EDA组合方法可以避免搜索方向向局部最优快速收敛；

在地形高度匹配搜索中，每一个与实测图平行的基准子图即为一个粒子，假设实测图有D个高程点，则粒子的维数为D维；第i个粒子的位置表示为x_i＝(x_i1,x_i2,…,x_iD)，该粒子对应位置速度的变化量为v_i＝(v_i1,v_i2,…,v_iD)，粒子的速度和位置更新公式为：

式中，ω为惯性权重，可以动态调整粒子运动速度；

为第i粒子d维分量在第t次迭代中的速度和位置；c₁，c₂为加速因子，取非负值；P_best为粒子本身所找到的最好解；G_best为整个种群目前找到的最好解；r₁，r₂为随机数，服从区间[0,1]上的均匀分布；

采用PSO-EDA算法TERCOM的步骤如下：

(1)初始化粒子，在搜索区域内随机生成R个基准子图，得到种群D₀；

(2)初始化EDA算法的概率模型；在TERCOM中，基准子图的位置分布服从二维高斯分布，令x表示基准子图中节点的经度，y表示基准子图中节点的纬度，则基准子图的位置分布表示为：

式中，μ₁，μ₂，

分别为变量x，y的均值和方差；ρ为常数；设搜索范围的中心坐标为(x₀,y₀)，则令概率模型中μ₁＝x₀，μ₂＝y₀，

式中，X[i][j]，Y[i][j]分别为第i个基准子图中第j个节点的经、纬度；N为基准子图个数；

(3)确定粒子的适应度值；将基准子图与实测图之间的平均Hausdorff距离MHD作为粒子的适应度值，并按照适应度值由小到大对粒子进行排序；MHD定义为：

式中，A＝{a₁,a₂,…a_n}，B＝{b₁,b₂,…b_n}为两序列；P为A中的元素个数；||·||为定义在集合A和B之间的欧式范数；a_i为A中第i个点的位置；b_j为B中第j个点的位置；

(4)更新EDA算法中高斯概率模型的均值与方差；在所有基准子图中选择适应度值最优的前30％基准子图，设在第t代时，x_j对应的高斯分布均值和方差分别为

y_j对应的高斯分布均值和方差分别为

则有

式中，α为学习因子；

和

分别表示种群中最优个体、次优个体和最差个体所对应的x_j值；

分别表示种群中最优个体、次优个体和最差个体所对应的y_j值；K为选出的较优种群的个数；x_jk为种群按照适应度由小到大排序的第k-1个个体的x_j值；y_jk为种群按照适应度由小到大排序的第k-1个个体的y_j值；

为选出的K个较优种群的x_jk的均值；

为选出的K个较优种群的y_jk的均值；

(5)根据更新后的概率模型生成t+1代EDA子种群

(6)更新粒子群算法中每个粒子的G_best和P_best；比较各粒子的当前适应度值和个体极值P_best与全局极值G_best，若粒子当前适应度值小于P_best，则将P_best更新为粒子当前的适应度值；若当前适应度值小于G_best，则将G_best更新为粒子当前的适应度值；

(7)根据式(2)更新每个粒子的位置得到t+1代粒子群子种群

(8)建立新种群

(9)判断是否满足算法终止条件，若不满足则转到第(3)步进行t+1次迭代，否则输出全局极值G_best及其对应的位置，即最佳匹配位置；

步骤4：SINS/TERCOM组合导航

得到TERCOM提供的最佳匹配位置后，选取SINS的误差为SINS/TERCOM组合导航***的状态变量构建状态方程；选择TERCOM最佳匹配位置和SINS输出的位置信息的差值作为量测值；对建立的状态方程和量测方程使用卡尔曼滤波器进行滤波更新，并反馈修正SINS，得到组合导航后的导航参数。

2.根据权利要求1所述的一种面向地形辅助导航的粒子群快速匹配方法，其特征在于步骤2中所述的R＝1.2km，Ψ＝48°。

3.一种计算机***，其特征在于包括：一个或多个处理器，计算机可读存储介质，用于存储一个或多个程序，其中，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现权利要求1所述的方法。

4.一种计算机可读存储介质，其特征在于存储有计算机可执行指令，所述指令在被执行时用于实现权利要求1所述的方法。

Images