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

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

【技术实现步骤摘要】
基于粒子群算法的航迹规划地形辅助导航方法
本专利技术涉及一种地形辅助导航方法，特别是涉及一种基于粒子群算法的航迹规划地形辅助导航方法，属于导航、制导与控制领域。
技术介绍
地形辅助导航系统是近年来受到广泛重视并已成功使用的组合导航系统，地形辅助导航是一种利用地形高程特征来进行辅助定位的方法，它具有自主、隐蔽、连续、全天候工作、导航定位误差不随时间积累等优点，是理想的辅助导航定位手段。然而，地形辅助导航要求地形高程有明显的变化，对于地形变化过于平缓、地形特征不明显的区域，用地形辅助导航方法来降低惯导系统的定位误差是不可行的。《一种适用于无人机的地形匹配辅助导航算法》(姚军,纪红霞,王乃甲，《光学与光电技术》，2019年第2期)一文提出了一种适用于无人机的地形辅助导航算法。但是由于地形辅助导航的结果与地形变化密切相关，甚至无法在地形平坦区域使用，所以地形辅助导航前必须进行引入地形信息的航迹规划，为无人机规划一条符合航行任务、机动性能、威胁规避、有利于实现地形辅助导航的航线。
技术实现思路
要解决的技术问题由于地形辅助导航受地形因素影响大，无法在地形平坦区域使用，本专利技术设计了一种基于粒子群算法的航迹规划地形辅助导航方法，用于解决现有地形辅助导航方法在地形平坦区域精度较差的技术问题。该方法在航迹规划时引入地形信息，利用航迹规划算法为无人机规划一条在地形起伏较大区域的航线，以提升地形辅助导航的精度。技术方案一种基于粒子群算法的航迹规划地形辅助导航方法，其特征在于步骤如下：步骤1：基于地形差异熵的地形适配区选择确定无人机飞行任务后，将任务区间的数字高程地图划分为一定大小的网格，采用地形差异熵信息表示各网格地形起伏的复杂程度，地形差异熵Hmean定义为：式中，hi表示高程值；M为该区域高程点总数；表示平均高程值；Ci表示高程差异值；Pi表示某一高程差异值出现的概率；步骤2：将地形适配区加入无人机航迹约束建立航迹适应度函数在二维航迹规划中，其代价函数包括航迹的长度代价、威胁代价以及水平转角代价；所述的航迹的长度代价函数为：式中，Xi为航迹路径；di,0，di,n+1分别为第i条航迹的起点到第1个节点的距离和最后一个节点到目标点的距离；n为第i条航迹的节点总数；xi,j为第i条航迹的第j个节点。所述的航迹的水平转角代价函数为：式中，α为无人机当前的水平转角；αmax为无人机预设的最大转角，由无人机型号确定；将步骤1选出的地形非适配区视为威胁圈，所述的航迹的威胁代价函数为：式中，sk为威胁强度系数；Jk,j为第k个威胁圈对第j段航迹段的威胁度，且式中，dk为第k个威胁圈到第j段航迹段之间的垂线距离；综上所述，无人机航迹规划的适应度函数为：J＝w1×Jlength+w2×Jthreat+w3×Jturn_angle(6)式中，w1,w2,w3分别为权重系数，须满足w1+w2+w3＝1；步骤3：基于粒子群优化的航迹规划应用粒子群算法求解无人机航迹规划问题时，每一条可行的航路就是一个“粒子”；假设航路控制点有D个，则粒子的维数为D维；第i个粒子的位置表示为xi＝(xi1，xi2,…,xiD)，该粒子对应位置速度的变化量为vi＝(vi1,vi2,…,viD)，粒子的速度和位置更新公式为：式中，ω为惯性权重，可以动态调整粒子运动速度；为第i个粒子的d维分量在第t次迭代中的速度和位置；c1，c2为加速因子，取非负值；Pbest为粒子本身所找到的最好解；Gbest为整个种群目前找到的最好解；r1，r2为随机数，服从区间[0,1]上的均匀分布；粒子群算法初始化生成一群随机粒子，在每一次迭代中，根据式(6)评价粒子的适应度值，粒子通过跟踪Pbest和Gbest更新：如果适应度值优于该粒子当前的个体极值，则将Pbest设置为该粒子的当前位置，且更新个体极值；如果所有粒子的最优值的优于当前的全局极值，则将Gbest设置为该最优粒子的位置，记录该粒子的序号，且更新全局极值，直到迭代至预先设置的迭代次数，则停止迭代，输出最优解；步骤4：基于轨迹规划的地形轮廓匹配地形轮廓匹配TERCOM的步骤为：测得实时高程后，首先确定搜索区域范围；其次在基准图上搜索范围内采用序贯相似度算法匹配得到基准子图；最后利用相似度分析算法选出与实测图最相似的基准子图即为最佳匹配位置；步骤5：SINS/TERCOM组合导航得到TERCOM提供的最佳匹配位置后，选取SINS的误差为SINS/TERCOM组合导航系统的状态变量构建状态方程；选择TERCOM最佳匹配位置和SINS输出的位置信息的差值作为量测值；对建立的状态方程和量测方程使用卡尔曼滤波器进行滤波更新，并反馈修正SINS，得到组合导航的导航参数。步骤3中所述的ω取0.9-1.2之间。一种计算机系统，其特征在于包括：一个或多个处理器，计算机可读存储介质，用于存储一个或多个程序，其中，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现上述的方法。一种计算机可读存储介质，其特征在于存储有计算机可执行指令，所述指令在被执行时用于实现上述的方法。一种计算机程序，其特征在于包括计算机可执行指令，所述指令在被执行时用于实现上述的方法。有益效果针对现有地形辅助导航方法在地形平坦区域精度较差的问题，本专利技术提出了一种基于粒子群算法的航迹规划地形辅助导航方法，该方法将地形信息量引入航迹规划，结合无人机飞行约束条件建立航迹代价函数，利用粒子群算法得到在地形起伏较大区域的航路，在此基础上进行地形高度匹配。本专利技术航迹规划中引入地形信息量，使无人机始终飞行在地形起伏较大区域，提高了地形高度匹配的精度。附图说明附图仅用于示出具体实施例的目的，而并不认为是对本专利技术的限制，在整个附图中，相同的参考符号表示相同的部件。图1数字高程地图网格图。图2本专利技术方法流程图。具体实施方式为了使本专利技术的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本专利技术进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本专利技术，并不用于限定本专利技术。此外，下面描述的本专利技术各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。参照图2，本专利技术基于粒子群算法的航迹规划地形辅助导航方法，包括：得到无人机飞行任务后，首先将任务区间的数字高程地图划分为一定大小的网格，并采用地形差异熵信息表示各网格地形起伏的复杂程度，选择地形差异熵小的区域作为地形适配区；其次，基于地形适配区建立无人机航迹适应度函数，并利用粒子群算法得到优化航迹，从而保证无人机的航迹均位于地形适配区内；之后，利用无人机飞行路径正下方的地形高度和存储的参考高程地图进行比较，得到无人机的位置信息；最后利用TERCOM最佳匹本文档来自技高网...

【技术保护点】
1.一种基于粒子群算法的航迹规划地形辅助导航方法，其特征在于步骤如下：/n步骤1：基于地形差异熵的地形适配区选择/n确定无人机飞行任务后，将任务区间的数字高程地图划分为一定大小的网格，采用地形差异熵信息表示各网格地形起伏的复杂程度，地形差异熵H

【技术特征摘要】
1.一种基于粒子群算法的航迹规划地形辅助导航方法，其特征在于步骤如下：
步骤1：基于地形差异熵的地形适配区选择
确定无人机飞行任务后，将任务区间的数字高程地图划分为一定大小的网格，采用地形差异熵信息表示各网格地形起伏的复杂程度，地形差异熵Hmean定义为：



式中，hi表示高程值；M为该区域高程点总数；表示平均高程值；Ci表示高程差异值；Pi表示某一高程差异值出现的概率；
步骤2：将地形适配区加入无人机航迹约束建立航迹适应度函数
在二维航迹规划中，其代价函数包括航迹的长度代价、威胁代价以及水平转角代价；
所述的航迹的长度代价函数为：



式中，Xi为航迹路径；di,0，di,n+1分别为第i条航迹的起点到第1个节点的距离和最后一个节点到目标点的距离；n为第i条航迹的节点总数；xi，j为第i条航迹的第j个节点。
所述的航迹的水平转角代价函数为：



式中，α为无人机当前的水平转角；αmax为无人机预设的最大转角，由无人机型号确定；
将步骤1选出的地形非适配区视为威胁圈，所述的航迹的威胁代价函数为：



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



式中，dk为第k个威胁圈到第j段航迹段之间的垂线距离；
综上所述，无人机航迹规划的适应度函数为：
J＝w1×Jlength+w2×Jthreat+w3×Jturn_angle(6)
式中，w1,w2,w3分别为权重系数，须满足w1+w2+w3＝1；
步骤3：基于粒子群优化的航迹规划
应用粒子群算法求解无人机航迹规划问题时，每一条可行的航路就是一个“粒子”；假设航路控制点有D个，则粒子的维数为D维；第i个粒子的位置表示为xi＝(xi1,xi2，…，xiD)，该粒子对应位置速度的变化量为vi＝(vi1，vi2,…,viD)，粒子的速度和位置更新公式为：



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

【专利技术属性】
技术研发人员：许斌，张睿，李万睿，孔红华，黄锐，王瑞，冯小珂，
申请(专利权)人：西北工业大学，中国航空工业集团公司成都飞机设计研究所，
类型：发明
国别省市：陕西;61