CN113252038A - 基于粒子群算法的航迹规划地形辅助导航方法 - Google Patents

Abstract

本发明涉及一种基于粒子群算法的航迹规划地形辅助导航方法，用于解决现有地形辅助导航方法在地形平坦区域精度较差的技术问题。该方法的步骤为：得到无人机飞行任务后，首先将任务区间的数字高程地图划分为一定大小的网格，并采用地形差异熵信息表示各网格地形起伏的复杂程度，选择地形差异熵小的区域作为地形适配区；其次，基于地形适配区建立无人机航迹适应度函数，并利用粒子群算法得到优化航迹，从而保证无人机的航迹均位于地形适配区内；之后，利用无人机飞行路径正下方的地形高度和存储的参考高程地图进行比较，得到无人机的位置信息；最后利用TERCOM最佳匹配位置和SINS输出的位置信息的差值作为量测值，利用Kalman滤波估计SINS的误差，并校正SINS。

Description

基于粒子群算法的航迹规划地形辅助导航方法

技术领域

本发明涉及一种地形辅助导航方法，特别是涉及一种基于粒子群算法的航迹规划地形辅助导航方法，属于导航、制导与控制领域。

背景技术

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

《一种适用于无人机的地形匹配辅助导航算法》(姚军,纪红霞,王乃甲，《光学与光电技术》，2019年第2期)一文提出了一种适用于无人机的地形辅助导航算法。但是由于地形辅助导航的结果与地形变化密切相关，甚至无法在地形平坦区域使用，所以地形辅助导航前必须进行引入地形信息的航迹规划，为无人机规划一条符合航行任务、机动性能、威胁规避、有利于实现地形辅助导航的航线。

发明内容

要解决的技术问题

由于地形辅助导航受地形因素影响大，无法在地形平坦区域使用，本发明设计了一种基于粒子群算法的航迹规划地形辅助导航方法，用于解决现有地形辅助导航方法在地形平坦区域精度较差的技术问题。该方法在航迹规划时引入地形信息，利用航迹规划算法为无人机规划一条在地形起伏较大区域的航线，以提升地形辅助导航的精度。

技术方案

一种基于粒子群算法的航迹规划地形辅助导航方法，其特征在于步骤如下：

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

确定无人机飞行任务后，将任务区间的数字高程地图划分为一定大小的网格，采用地形差异熵信息表示各网格地形起伏的复杂程度，地形差异熵H_mean定义为：

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

表示平均高程值；C_i表示高程差异值；P_i表示某一高程差异值出现的概率；

步骤2：将地形适配区加入无人机航迹约束建立航迹适应度函数

在二维航迹规划中，其代价函数包括航迹的长度代价、威胁代价以及水平转角代价；

所述的航迹的长度代价函数为：

式中，X_i为航迹路径；d_i,0，d_i,n+1分别为第i条航迹的起点到第1个节点的距离和最后一个节点到目标点的距离；n为第i条航迹的节点总数；x_i,j为第i条航迹的第j个节点。

所述的航迹的水平转角代价函数为：

式中，α为无人机当前的水平转角；α_max为无人机预设的最大转角，由无人机型号确定；

将步骤1选出的地形非适配区视为威胁圈，所述的航迹的威胁代价函数为：

式中，s_k为威胁强度系数；J_k,j为第k个威胁圈对第j段航迹段的威胁度，且

式中，d_k为第k个威胁圈到第j段航迹段之间的垂线距离；

综上所述，无人机航迹规划的适应度函数为：

J＝w₁×J_length+w₂×J_threat+w₃×J_{turn_angle} (6)

式中，w₁,w₂,w₃分别为权重系数，须满足w₁+w₂+w₃＝1；

步骤3：基于粒子群优化的航迹规划

应用粒子群算法求解无人机航迹规划问题时，每一条可行的航路就是一个“粒子”；假设航路控制点有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]上的均匀分布；

粒子群算法初始化生成一群随机粒子，在每一次迭代中，根据式(6)评价粒子的适应度值，粒子通过跟踪P_best和G_best更新：如果适应度值优于该粒子当前的个体极值，则将P_best设置为该粒子的当前位置，且更新个体极值；如果所有粒子的最优值的优于当前的全局极值，则将G_best设置为该最优粒子的位置，记录该粒子的序号，且更新全局极值，直到迭代至预先设置的迭代次数，则停止迭代，输出最优解；

步骤4：基于轨迹规划的地形轮廓匹配

地形轮廓匹配TERCOM的步骤为：测得实时高程后，首先确定搜索区域范围；其次在基准图上搜索范围内采用序贯相似度算法匹配得到基准子图；最后利用相似度分析算法选出与实测图最相似的基准子图即为最佳匹配位置；

步骤5：SINS/TERCOM组合导航

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

步骤3中所述的ω取0.9-1.2之间。

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

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

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

有益效果

针对现有地形辅助导航方法在地形平坦区域精度较差的问题，本发明提出了一种基于粒子群算法的航迹规划地形辅助导航方法，该方法将地形信息量引入航迹规划，结合无人机飞行约束条件建立航迹代价函数，利用粒子群算法得到在地形起伏较大区域的航路，在此基础上进行地形高度匹配。本发明航迹规划中引入地形信息量，使无人机始终飞行在地形起伏较大区域，提高了地形高度匹配的精度。

附图说明

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

图1数字高程地图网格图。

图2本发明方法流程图。

具体实施方式

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

参照图2，本发明基于粒子群算法的航迹规划地形辅助导航方法，包括：得到无人机飞行任务后，首先将任务区间的数字高程地图划分为一定大小的网格，并采用地形差异熵信息表示各网格地形起伏的复杂程度，选择地形差异熵小的区域作为地形适配区；其次，基于地形适配区建立无人机航迹适应度函数，并利用粒子群算法得到优化航迹，从而保证无人机的航迹均位于地形适配区内；之后，利用无人机飞行路径正下方的地形高度和存储的参考高程地图进行比较，得到无人机的位置信息；最后利用TERCOM最佳匹配位置和SINS输出的位置信息的差值作为量测值，利用Kalman滤波估计SINS的误差，并校正SINS。本发明在航迹规划中引入地形信息量，避免了无人机飞行过程中地形辅助导航由于地形平坦而失效的问题，提高了地形辅助导航的精度。具体步骤如下：

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

确定无人机飞行任务后，将任务区间的数字高程地图划分为一定大小的网格，采用地形差异熵信息表示各网格地形起伏的复杂程度，地形差异熵H_mean定义为：

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

表示平均高程值，C_i表示高程差异值，P_i表示某一高程差异值出现的概率。

相关的参数定义如下：

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

根据式(1)计算出各网格的地形差异熵，并由大到小排序，将前30％的网格区域视为威胁圈。

地形高程变化越急剧，熵值越小，匹配精度越高。将地形变化剧烈的区域视为地形适配区，将地形平坦的区域视为地形非适配区。如图1所示，图中白色网格为地形适配区，黑色网格为地形非适配区。

步骤2：将地形适配区加入无人机航迹约束建立航迹适应度函数

在二维航迹规划中，其代价函数只考虑航迹的长度代价、威胁代价以及水平转角代价等。

航迹的长度代价函数为：

式中，X_i为航迹路径；d_i,0，d_i,n+1分别为第i条航迹的起点到第1个节点的距离和最后一个节点到目标点的距离；n为第i条航迹的节点总数(由无人机任务航程决定)；x_i,j为第i条航迹的第j个节点。

航迹的水平转角代价函数为：

式中，α为无人机当前的水平转角，α_max＝50°为无人机预设的最大转角。

根据步骤1选出的威胁圈，航迹的威胁代价函数为：

式中，s_k为威胁强度系数，取

为第k个威胁圈的地形差异熵；J_k,j为第k个威胁圈对第j段航迹段的威胁度，且

式中，d_k为第k个威胁圈到第j段航迹段之间的垂线距离。

综上所述，无人机航迹规划的适应度函数为：

J＝w₁×J_length+w₂×J_threat+w₃×J_{turn_angle} (13)

式中，w₁,w₂,w₃分别为权重系数，取w₁＝0.4，w₂＝0.3，w₃＝0.3。

步骤3：基于粒子群优化的航迹规划

应用粒子群算法求解无人机航迹规划问题时，每一条可行的航路就是一个“粒子”。假设航路控制点有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]上的均匀分布。

粒子群算法初始化生成一群随机粒子，在每一次迭代中，根据式(6)评价粒子的适应度值，粒子通过跟踪P_best和G_best来更新自己，如果适应度值优于该粒子当前的个体极值，则将P_best设置为该粒子的当前位置，且更新个体极值。如果所有粒子的最优值的优于当前的全局极值，则将G_best设置为该最优粒子的位置，记录该粒子的序号，且更新全局极值，直到迭代至预先设置的迭代次数，则停止迭代，输出最优解。

步骤4：基于航迹规划的地形轮廓匹配

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

1.高程信息采集

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

2.确定搜索区域

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

3.匹配搜索

当飞机测得其正下方的地形高程后，便可与数字地图的高程数据进行匹配，匹配搜索采用序贯相似度检测算法，即：

式中，绝对差值ε_s为实测高程h_r与基准图高程h_m之差，以及附加噪声，其中不匹配位置的ε_s远大于匹配位置的ε_s；在搜索计算中，通过对ε_s进行累加，并设定门限值T，经过N次累加，当ε_s之和E大于T时，就停止该位置的搜索计算，可大幅减少计算量。动态门限曲线为：

式中，n为累加次数；取g＝4，匹配概率为99.9％；r＝5为测量噪声绝对值均值。

4.相关性分析

利用平均平方差(Mean Square Deviation，MSD)相关性算法判断实测图与基准子图是否匹配。设基准子图为X＝(x₁,x₂,...,x_N)^T，实测图为Y＝(y₁,y₂,...,y_N)^T，其中N表示高度序列的个数，x_i和y_i，i＝1,2,...,N分别表示基准子图和实测图中的高度元素，则MSD相关性为：

最佳匹配结果J_MSD(X,Y)的最小值给出。

步骤5：SINS/TERCOM组合导航

得到TERCOM提供的最佳匹配位置后，选取SINS的误差为SINS/TERCOM组合导航系统的状态变量构建状态方程；选择TERCOM最佳匹配位置和SINS输出的位置信息的差值作为量测值；对建立的状态方程和量测方程使用卡尔曼滤波器进行滤波更新，并反馈修正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_2,7＝-ω_ie sinL，

A_2,10＝-sinΨcosθ,A_2,11＝-cosΨcosθ,A_2,12＝-sinθ，

A_3，10＝-sinγcosΨ+cosγsinΨsinθ,A_3,11＝sinγsinΨ+cosγcosΨsinθ,A_3,12＝-cosγcosθε_bz,A_4,2＝-f_U,A_4,3＝f_N,

A_4,13＝cosγcosΨ+sinΨsinθ,A_4，14＝-cosγsinΨ+sinγcosΨsinθ，A_4，15＝-sinγcosθ，A_5，1＝f_U，A_5，3＝-f_E,

A_5,13＝sinΨcosθ，A_5,14＝cosΨcosθ,A_5,15＝sinθ,A_6,1＝-f_N,A_6,2＝f_E,

A_6,7＝-2V_Eω_ie sinL,

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

式中，Ψ为运载体航向角；γ为运载体横滚角；θ为运载体俯仰角；ω_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为惯导经、纬度的测量误差。

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 (5)

1.一种基于粒子群算法的航迹规划地形辅助导航方法，其特征在于步骤如下：

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

确定无人机飞行任务后，将任务区间的数字高程地图划分为一定大小的网格，采用地形差异熵信息表示各网格地形起伏的复杂程度，地形差异熵H_mean定义为：

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

表示平均高程值；C_i表示高程差异值；P_i表示某一高程差异值出现的概率；

步骤2：将地形适配区加入无人机航迹约束建立航迹适应度函数

在二维航迹规划中，其代价函数包括航迹的长度代价、威胁代价以及水平转角代价；

所述的航迹的长度代价函数为：

式中，X_i为航迹路径；d_i,0，d_i,n+1分别为第i条航迹的起点到第1个节点的距离和最后一个节点到目标点的距离；n为第i条航迹的节点总数；x_i，j为第i条航迹的第j个节点。

所述的航迹的水平转角代价函数为：

式中，α为无人机当前的水平转角；α_max为无人机预设的最大转角，由无人机型号确定；

将步骤1选出的地形非适配区视为威胁圈，所述的航迹的威胁代价函数为：

式中，s_k为威胁强度系数；J_k,j为第k个威胁圈对第j段航迹段的威胁度，且

式中，d_k为第k个威胁圈到第j段航迹段之间的垂线距离；

综上所述，无人机航迹规划的适应度函数为：

J＝w₁×J_length+w₂×J_threat+w₃×J_{turn_angle} (6)

式中，w₁,w₂,w₃分别为权重系数，须满足w₁+w₂+w₃＝1；

步骤3：基于粒子群优化的航迹规划

应用粒子群算法求解无人机航迹规划问题时，每一条可行的航路就是一个“粒子”；假设航路控制点有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]上的均匀分布；

粒子群算法初始化生成一群随机粒子，在每一次迭代中，根据式(6)评价粒子的适应度值，粒子通过跟踪P_best和G_best更新：如果适应度值优于该粒子当前的个体极值，则将P_best设置为该粒子的当前位置，且更新个体极值；如果所有粒子的最优值的优于当前的全局极值，则将G_best设置为该最优粒子的位置，记录该粒子的序号，且更新全局极值，直到迭代至预先设置的迭代次数，则停止迭代，输出最优解；

步骤4：基于轨迹规划的地形轮廓匹配

地形轮廓匹配TERCOM的步骤为：测得实时高程后，首先确定搜索区域范围；其次在基准图上搜索范围内采用序贯相似度算法匹配得到基准子图；最后利用相似度分析算法选出与实测图最相似的基准子图即为最佳匹配位置；

步骤5：SINS/TERCOM组合导航

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

2.根据权利要求1所述的一种基于粒子群算法的航迹规划地形辅助导航方法，其特征在于步骤3中所述的ω取0.9-1.2之间。

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

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

5.一种计算机程序，其特征在于包括计算机可执行指令，所述指令在被执行时用于实现权利要求1所述的方法。

Images