CN110617819A - 一种基于蚁群算法路径规划的无人机地形辅助导航方法 - Google Patents

Abstract

本发明涉及无人机地形辅助导航方法领域，具体是一种基于蚁群算法路径规划的无人机地形辅助导航方法，其具体步骤如下：S1：载入地形高程数据；S2：根据地形信息量，判断地形是否可匹配；S3：导入适配性地图，设置初始参数；S4：依据蚁群算法对路径进行规划；S5：用ICCP匹配所得位置信息对惯性导航系统输出位置信息进行校正；针对惯导误差随时间积累的问题，采用基于ICCP算法的地形辅助导航方法对惯导误差进行修正，以实现无人机长航时高精度的定位要求；针对地形辅助导航在地形变化不明显的区域无法有效修正惯导积累误差的问题，通过对导航区域地形信息量的计算，利用熵值法赋权灰色关联决策将地形划分为地形适配区与地形非适配区。

Description

一种基于蚁群算法路径规划的无人机地形辅助导航方法

技术领域

本发明涉及无人机地形辅助导航方法领域，具体是一种基于蚁群算法路径规划的无人机地形辅助导航方法。

背景技术

惯性导航系统不需要任何外来信息，也不向外辐射任何信息，仅依靠自身就能够在全天候条件下，在全球范围内和任何介质环境里进行连续的定位与导航，这种同时具备自主性、隐蔽性和能获取载体完备信息的独特优点是其他导航系统无法比拟的。但是，惯导系统有着系统误差随时间积累的原理性缺陷，为了实现无人机长航时高精度的导航目标，需要利用外界位置信息对其进行周期性调整和校正。

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

衡量地形信息量的主要特征参数包括地形标准差、地形相关系数、地形粗糙度和地形熵等。基于上述特征参数，可以有效地将地形划分为地形适配区和地形非适配区。

应用于路径规划的现代智能算法主要有遗传算法、粒子群算法以及蚁群算法等。相较粒子群算法，蚁群算法寻找全局最优的能力更强；且蚁群算法采用了正反馈机制和启发式贪婪策略使搜索时间明显短于遗传算法；同时，蚁群算法的环境建模和实现简单，不需要遗传算法和粒子群算法复杂的编码机制。目前，蚁群算法的研究和应用更为成熟和广泛，其参数的选择和确定有更多的文献和理论支持。

综上所述，根据地形划分情况，结合蚁群算法对路径进行规划，使无人机每隔一段时间飞经地形适配区，完成对惯导误差的修正，这对于真正实现长航时高精度导航定位具有非常重要的现实意义。

发明内容

为了解决上述问题，本发明提出一种基于蚁群算法路径规划的无人机地形辅助导航方法。

一种基于蚁群算法路径规划的无人机地形辅助导航方法,其具体步骤如下：

S1：载入地形高程数据；

S2：根据地形信息量，判断地形是否可匹配：

S2.1：将路径规划中的地形区域划分为L块候选区域，L的值根据匹配定位精度和载体计算机存储容量确定；

S2.2：根据无人机定位精度要求确定地形适配区与地形非适配区；

S2.3：结合适配区划分结果，基于蚁群算法进行导航路径规划：

(1)适配性矩阵，若网格所在地形为地形适配区则置0，非适配区则置去；

(2)可达性矩阵，若网格i到网格j是可达的，则用LEN(i，j)记录距离，若不可达，则LEN(i，j)＝0；规定每一个网格可能到达的网格为其四周相邻的八个网格，及上、下、左、右、左上、左下、右上、右下方向的八个网格，若适配性矩阵是N维方阵，则可达性矩阵是N×N方阵；

S3：导入适配性地图，设置初始参数：设置路径规划的起点网格为S,终点网格为E，设置出动蚁群的波数为K，每一波出动的蚂蚁只数为M；设置信息素启发因子为α，自启发因子为β，信息素蒸发系数为ρ，信息素增强系数为Q，设置对应于可达性矩阵的信息素矩阵的初始信息素浓度为c，c为一常数，用ROUTES记录每一波蚂蚁中的每只蚂蚁行进的路径，用PL记录每一波蚂蚁中的每一只蚂蚁行进路径的长度；

S4：依据蚁群算法对路径进行规划：

a：首先判断当前的网格是否是终点网格，若是则终止本只蚂蚁的寻径，启动下一只蚂蚁寻径；

b：若不是则按照轮赌法选择下一个可前进的网格，通过轮赌法求出蚂蚁下一步可走的每一个网格的概率；

c：假设蚂蚁下一步可以进入的网格为[g₁，g₂，g₃]，按照公式计算得到进入每一个网格的概率为[ξ₁，ξ₂，ξ₃](0≤ξ₁，ξ₂，ξ₃≤1，且ξ₁+ξ₂+ξ₃＝1)，轮赌法按照下述方式进行：首先对进入每一个网格做累积概率统计，得到[ξ₁ξ₁+ξ₂ξ₁+ξ₂+ξ₃]＝[ξ₁ξ₁+ξ₂]，然后产生一个0到1之间的随机数，若产生的随机数位于0和ξ₁之间，则蚂蚁往网格g₁前进，若位于ξ₁和ξ₁+ξ₂之间，则蚂蚁往网格g₂前进，若位于ξ₁+ξ₂和1之间，则蚂蚁往网格g₃前进；

d：更新路径和路径长度；

e：重复步骤a和d，直到蚂蚁都到达终点或是陷入到死区，若蚂蚁未到达终点则至路径长度为0；

f：重复步骤a到e，直至所有本波蚂蚁都到达了终点或是陷入了死区；

g：更新信息素矩阵：当本波所有蚂蚁完成了路径选择之后，若蚂蚁到达了终点，对信息素进行更新；

h：若所有波次蚂蚁均已进行了路径寻找，则输出所有路径中的最短路径及路径长度，寻径结束，否则，返回执行步骤a；

i：依据步骤S2.3中规划的路径，采用等值线最近点迭代(ICCP)算法完成地形匹配，对惯导位置误差进行修正；

j：获取惯导指示序列并进行初始变换：

采用随机旋转和平移的方法进行初始变换，旋转和平移的大小在惯导系统误差方差的3倍范围内随意取值，取旋转偏移量为θ_rand，纬度方向的平移量为t_{L_rand}，经度方向的随机平移量为t_{λ_rand}，得到初始变换后的序列P_irand；

k：提取高程等值线，寻找刚性变换求取最近点：

通过机载气压高度表、无线电高度表获取初始变换后的位置序列P_irand处相应的高程值H_i，并从已知的数字地图中提取相应的高程等值线C_i；假设P_irand到相应等值线的最近点为Y_i，寻找一个包含旋转矩阵R和平移矢量t的刚性变换T，得到最小目标函数d；

l：迭代地进行刚性变换直至收敛：

根据步骤3.2中获得的刚性变换T，对P_irand应用刚性变换可获得P_irand＝T·P_irand；此时，若迭代次数k大于最大迭代次数k_max，则表明收敛速度过慢，舍弃此次迭代地结果，并且返回执行步骤j；若迭代次数k小于最大迭代次数k_max并且|d_k-d_k-1|>τ，则返回步骤3.2；若迭代次数k小于最大迭代次数k_max并且|d_k-d_k-1|≤τ，则迭代终止，确定最终匹配结果为[L_ICCPλ_ICCP]^T；

S5：用ICCP匹配所得位置信息对惯性导航系统输出位置信息进行校正：

将惯性导航系统输出的位置信息L_SINS、λ_SINS与ICCP匹配得到的位置信息L_ICCP、λ_ICCP的差值L_SINS-L_ICCP、λ_SINS-λ_ICCP作为观测量进行Kalman滤波，并利用滤波所得的位置信息反馈到惯性导航系统中对惯导输出的位置进行校正。

所述的步骤S1中的地形高程采用网格矩阵的方式存储。

所述的步骤S2的S2.2中假设某一地形的经纬度跨度为m×n网格，网格点坐标为(i，j)处的地形高程值为h(i，j)，且有i＝1，2，…，m，j＝1，2，…，n；可以计算得到地形信息量的主要特征参数包括地形标准差σ、地形相关系数R、地形粗糙度r和地形高度熵H_f，它们的具体定义如下:

其中，

为地形高程平均值；

为经度方向相关系数；

为纬度方向相关系数；

为经度方向的粗糙度；

为纬度方向的粗糙度；

为归一化高程值。

所述的步骤S2的S2.2通过熵值法赋权灰色关联决策法对地形进行划分。

所述的步骤S3将蚂蚁安放在起点网格S，并将起点网格加入到禁忌表TABU中，其中，TABU表示禁忌表，当地形网格为N维方正时，禁忌表为1行N²列矩阵，用以表示是否已经经过了某个网格，若已经经过了则对应于网格序号的列置0，为防止蚂蚁回溯，需要根据蚂蚁经过的路径动态调整禁忌表。

所述的步骤S4的b中蚂蚁选择下一个可行进的网格采用公式(5)进行计算：

其中，allowed代表禁忌表未经过的网格，τ_i，j为网格i到网格j的路径上的信息素浓度，η_i，g为自启发函数，η_i，g＝1/d_ig，d_ig代表网格i到目标网格的距离；

所述的步骤S4中的g信息素按公式(6)进行更新，同时，路径上的信息素浓度随着时间推移逐渐蒸发：

τ_i，j←(1-ρ)·τ_i，j+Δτ_i，j (6)

其中：Δτ_i，j为增加的信息素部分，Q为信息素增强系数，L为蚂蚁到达终点的路径长度。

所述的步骤S4中的j对惯导系统指示航迹序列P_i按公式(8)作随机旋转和平移，可得到初始变换后的序列P_irand：

所述的步骤S4中的k通过公式(11)得到最小目标函数d：

其中，k表示迭代次数，D(P_irand，Y_i)表示P_irand和Y之间的距离，D_max表示P_irand和Y之间距离的最大值。

本发明的有益效果是：针对惯导误差随时间积累的问题，采用基于ICCP算法的地形辅助导航方法对惯导误差进行修正，以实现无人机长航时高精度的定位要求；针对地形辅助导航在地形变化不明显的区域无法有效修正惯导积累误差的问题，通过对导航区域地形信息量的计算，利用熵值法赋权灰色关联决策将地形划分为地形适配区与地形非适配区；针对无人机无法准确经过地形适配区的问题，采用基于蚁群算法的路径规划方法，规划基于地形适配区的行进路径，确保地形辅助导航对惯导误差的全程有效修正。

附图说明

下面结合附图和实施例对本发明进一步说明。

图1为本发明的流程结构示意图；

图2为本发明的规则格网地形结构示意图；

图3为本发明的适配性矩阵结构示意图；

图4为本发明的可达性示意图结构示意图；

图5为本发明的地形三维示意图结构示意图；

图6为本发明的地形适配区示意图；

图7为本发明的第1、2、3、4、5波蚂蚁所寻得的最优路径；

图8为本发明的第10、20、50、100、300、500波蚂蚁所寻得的最优路径；

图9为本发明的规划路径匹配结果图。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面对本发明进一步阐述。

如图1至图9所示，一种基于蚁群算法路径规划的无人机地形辅助导航方法,其具体步骤如下：

S1：载入地形高程数据：如图5所示，区域的最大高程值为251.3940m，最小高程值为182.0500m，平均高程值为223.5656m，其经纬度起始于(118°E，38°N)，网格数为44×44，网格间距为0.00125°，为139m；

S2：根据地形信息量，判断地形是否可匹配：

S2.1：将路径规划中的地形区域划分为L块候选区域，L的值根据匹配定位精度和载体计算机存储容量确定；

S2.2：根据无人机定位精度要求确定地形适配区与地形非适配区；

S2.3：结合适配区划分结果，基于蚁群算法进行导航路径规划，如图6所示为地形适配区示意图，其中阴影部分为地形非适配区域：

(1)适配性矩阵，若网格所在地形为地形适配区则置0，非适配区则置去，如图2为规则格网地形，阴影部分代表地形非适配区，图4为该区域的适配性矩阵；

(2)可达性矩阵，若网格i到网格j是可达的，则用LEN(i，j)记录距离，若不可达，则LEN(i，j)＝0；规定每一个网格可能到达的网格为其四周相邻的八个网格，及上、下、左、右、左上、左下、右上、右下方向的八个网格，如图3是对应于图1各个网格的可达性示意图，箭头表示可以前进的网格，若适配性矩阵是N维方阵，则可达性矩阵是N×N方阵；

S3：导入适配性地图，设置初始参数：设置路径规划的起点网格即初始点为(118.01°E，38.00875°N)，终点为(118.0475°E，38.04625°N)，设置出动蚁群波数的K为500次，每一波出动的蚂蚁只数M为50只；设置信息素启发因子α为1，自启发因子β为7，信息素蒸发系数ρ为0.3，信息素增强系数Q为1，设置对应于可达性矩阵的信息素矩阵的初始信息素浓度c为1，c为一常数，用ROUTES记录每一波蚂蚁中的每只蚂蚁行进的路径，用PL记录每一波蚂蚁中的每一只蚂蚁行进路径的长度；

根据上述参数设置，得到第1、2、3、4、5波次蚂蚁所寻得的最优路径如图7所示，第10、20、50、100、300、500波次蚁群算法计算所得最优路径如图8所示；

以图8中第500代蚂蚁所规划的路径作为无人机真实航迹，利用ICCP算法进行航迹匹配，匹配结果如图8所示：

如图9，匹配误差统计结果如表1：

表1匹配误差统计

从匹配结果中可以看出：基于蚁群算法路径规划的无人机地形辅助导航在无人机地形辅助导航应用中是有效的，当无人机沿着所规划的路径航行时，利用基于ICCP算法的地形辅助导航可以得到较高的定位精度，实现无人机长航时高精度导航定位；

S4：依据蚁群算法对路径进行规划：

a：首先判断当前的网格是否是终点网格，若是则终止本只蚂蚁的寻径，启动下一只蚂蚁寻径；

b：若不是则按照轮赌法选择下一个可前进的网格，通过轮赌法求出蚂蚁下一步可走的每一个网格的概率；

c：假设蚂蚁下一步可以进入的网格为[g₁，g₂，g₃]，按照公式计算得到进入每一个网格的概率为[ξ₁，ξ₂，ξ₃](0≤ξ₁，ξ₂，ξ₃≤1，且ξ₁+ξ₂+ξ₃＝1)，轮赌法按照下述方式进行：首先对进入每一个网格做累积概率统计，得到[ξ₁ξ₁+ξ₂ξ₁+ξ₂+ξ₃]＝[ξ₁ξ₁+ξ₂]，然后产生一个0到1之间的随机数，若产生的随机数位于0和ξ₁之间，则蚂蚁往网格g₁前进，若位于ξ₁和ξ₁+ξ₂之间，则蚂蚁往网格g₂前进，若位于ξ₁+ξ₂和1之间，则蚂蚁往网格g₃前进；

d：更新路径和路径长度；

e：重复步骤a和d，直到蚂蚁都到达终点或是陷入到死区，若蚂蚁未到达终点则至路径长度为0；

f：重复步骤a到e，直至所有本波蚂蚁都到达了终点或是陷入了死区；

g：更新信息素矩阵：当本波所有蚂蚁完成了路径选择之后，若蚂蚁到达了终点，对信息素进行更新；

h：若所有波次蚂蚁均已进行了路径寻找，则输出所有路径中的最短路径及路径长度，寻径结束，否则，返回执行步骤a；

i：依据步骤S2.3中规划的路径，采用等值线最近点迭代(ICCP)算法完成地形匹配，对惯导位置误差进行修正；

j：获取惯导指示序列并进行初始变换：

采用随机旋转和平移的方法进行初始变换，旋转和平移的大小在惯导系统误差方差的3倍范围内随意取值，取旋转偏移量为θ_rand，纬度方向的平移量为t_{L_rand}，经度方向的随机平移量为t_{λ_rand}，得到初始变换后的序列P_irand；

k：提取高程等值线，寻找刚性变换求取最近点：

通过机载气压高度表、无线电高度表获取初始变换后的位置序列P_irand处相应的高程值H_i，并从已知的数字地图中提取相应的高程等值线C_i；假设P_irand到相应等值线的最近点为Y_i，寻找一个包含旋转矩阵R和平移矢量t的刚性变换T，得到最小目标函数d；

l：迭代地进行刚性变换直至收敛：

根据步骤3.2中获得的刚性变换T，对P_irand应用刚性变换可获得P_irand＝T·P_irand；此时，若迭代次数k大于最大迭代次数k_max，则表明收敛速度过慢，舍弃此次迭代地结果，并且返回执行步骤j；若迭代次数k小于最大迭代次数k_max并且|d_k-d_k-1|>τ，则返回步骤3.2；若迭代次数k小于最大迭代次数k_max并且|d_k-d_k-1|≤τ，则迭代终止，确定最终匹配结果为[L_ICCPλ_ICCP]^T；

S5：用ICCP匹配所得位置信息对惯性导航系统输出位置信息进行校正：

将惯性导航系统输出的位置信息L_SINS、λ_SINS与ICCP匹配得到的位置信息L_ICCP、λ_ICCP的差值L_SINS-L_ICCP、λ_SINS-λ_ICCP作为观测量进行Kalman滤波，并利用滤波所得的位置信息反馈到惯性导航系统中对惯导输出的位置进行校正。

所述的步骤S1中的地形高程采用网格矩阵的方式存储。

所述的步骤S2的S2.2中假设某一地形的经纬度跨度为m×n网格，网格点坐标为(i，j)处的地形高程值为h(i，j)，且有i＝1，2，…，m，j＝1，2，…，n；可以计算得到地形信息量的主要特征参数包括地形标准差σ、地形相关系数R、地形粗糙度r和地形高度熵H_f，它们的具体定义如下:

其中，

为地形高程平均值；

为经度方向相关系数；

为纬度方向相关系数；

为经度方向的粗糙度；

为纬度方向的粗糙度；

为归一化高程值。

针对惯导误差随时间积累的问题，采用基于ICCP算法的地形辅助导航方法对惯导误差进行修正，以实现无人机长航时高精度的定位要求；针对地形辅助导航在地形变化不明显的区域无法有效修正惯导积累误差的问题，通过对导航区域地形信息量的计算，利用熵值法赋权灰色关联决策将地形划分为地形适配区与地形非适配区；针对无人机无法准确经过地形适配区的问题，采用基于蚁群算法的路径规划方法，规划基于地形适配区的行进路径，确保地形辅助导航对惯导误差的全程有效修正。

所述的步骤S2的S2.2按照参考文献《基于熵值法赋权灰色关联决策的地形辅助导航适配区选择》中所述熵值法赋权灰色关联决策法对地形进行划分，如图6所示，为地形适配区示意图，其中阴影部分为地形非适配区域。

所述的步骤S3将蚂蚁安放在起点网格S，并将起点网格加入到禁忌表TABU中，其中，TABU表示禁忌表，当地形网格为N维方正时，禁忌表为1行N²列矩阵，用以表示是否已经经过了某个网格，若已经经过了则对应于网格序号的列置0，为防止蚂蚁回溯，需要根据蚂蚁经过的路径动态调整禁忌表。

所述的步骤S4的b中蚂蚁选择下一个可行进的网格采用公式(5)进行计算：

其中，allowed代表禁忌表未经过的网格，τ_i，j为网格i到网格j的路径上的信息素浓度，η_i，g为自启发函数，η_i，g＝1/d_ig，d_ig代表网格i到目标网格的距离；

所述的步骤S4中的g信息素按公式(6)进行更新，同时，路径上的信息素浓度随着时间推移逐渐蒸发：

τ_i，j←(1-ρ)·τ_i，j+Δτ_i，j (6)

其中：Δτ_i，j为增加的信息素部分，Q为信息素增强系数，L为蚂蚁到达终点的路径长度。

所述的步骤S4中的j对惯导系统指示航迹序列P_i按公式(8)作随机旋转和平移，可得到初始变换后的序列P_irand：

所述的步骤S4中的k通过公式(11)得到最小目标函数d：

其中，k表示迭代次数，D(P_irand，Y_i)表示P_irand和Y之间的距离，D_max表示P_irand和Y之间距离的最大值。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (9)

1.一种基于蚁群算法路径规划的无人机地形辅助导航方法,其特征在于：其具体步骤如下：

S1：载入地形高程数据；

S2：根据地形信息量，判断地形是否可匹配：

S2.1：将路径规划中的地形区域划分为L块候选区域，L的值根据匹配定位精度和载体计算机存储容量确定；

S2.2：根据无人机定位精度要求确定地形适配区与地形非适配区；

S2.3：结合适配区划分结果，基于蚁群算法进行导航路径规划：

(1)适配性矩阵，若网格所在地形为地形适配区则置0，非适配区则置去；

(2)可达性矩阵，若网格i到网格j是可达的，则用LEN(i，j)记录距离，若不可达，则LEN(i，j)＝0；规定每一个网格可能到达的网格为其四周相邻的八个网格，及上、下、左、右、左上、左下、右上、右下方向的八个网格，若适配性矩阵是N维方阵，则可达性矩阵是N×N方阵；

S3：导入适配性地图，设置初始参数：设置路径规划的起点网格为S,终点网格为E，设置出动蚁群的波数为K，每一波出动的蚂蚁只数为M；设置信息素启发因子为α，自启发因子为β，信息素蒸发系数为ρ，信息素增强系数为Q，设置对应于可达性矩阵的信息素矩阵的初始信息素浓度为c，c为一常数，用ROUTES记录每一波蚂蚁中的每只蚂蚁行进的路径，用PL记录每一波蚂蚁中的每一只蚂蚁行进路径的长度；

S4：依据蚁群算法对路径进行规划：

a：首先判断当前的网格是否是终点网格，若是则终止本只蚂蚁的寻径，启动下一只蚂蚁寻径；

b：若不是则按照轮赌法选择下一个可前进的网格，通过轮赌法求出蚂蚁下一步可走的每一个网格的概率；

c：假设蚂蚁下一步可以进入的网格为[g₁，g₂，g₃]，按照公式计算得到进入每一个网格的概率为[ξ₁，ξ₂，ξ₃](0≤ξ₁，ξ₂，ξ₃≤1，且ξ₁+ξ₂+ξ₃＝1)，轮赌法按照下述方式进行：首先对进入每一个网格做累积概率统计，得到[ξ₁ξ₁+ξ₂ξ₁+ξ₂+ξ₃]＝[ξ₁ξ₁+ξ₂]，然后产生一个0到1之间的随机数，若产生的随机数位于0和ξ₁之间，则蚂蚁往网格g₁前进，若位于ξ₁和ξ₁+ξ₂之间，则蚂蚁往网格g₂前进，若位于ξ₁+ξ₂和1之间，则蚂蚁往网格g₃前进；

d：更新路径和路径长度；

e：重复步骤a和d，直到蚂蚁都到达终点或是陷入到死区，若蚂蚁未到达终点则至路径长度为0；

f：重复步骤a到e，直至所有本波蚂蚁都到达了终点或是陷入了死区；

g：更新信息素矩阵：当本波所有蚂蚁完成了路径选择之后，若蚂蚁到达了终点，对信息素进行更新；

h：若所有波次蚂蚁均已进行了路径寻找，则输出所有路径中的最短路径及路径长度，寻径结束，否则，返回执行步骤a；

i：依据步骤S2.3中规划的路径，采用等值线最近点迭代ICCP算法完成地形匹配，对惯导位置误差进行修正；

j：获取惯导指示序列并进行初始变换：

采用随机旋转和平移的方法进行初始变换，旋转和平移的大小在惯导系统误差方差的3倍范围内随意取值，取旋转偏移量为θ_rand，纬度方向的平移量为t_{L_rand}，经度方向的随机平移量为t_{λ_rand}，得到初始变换后的序列P_irand；

k：提取高程等值线，寻找刚性变换求取最近点：

通过机载气压高度表、无线电高度表获取初始变换后的位置序列P_irand处相应的高程值H_i，并从已知的数字地图中提取相应的高程等值线C_i；假设P_irand到相应等值线的最近点为Y_i，寻找一个包含旋转矩阵R和平移矢量t的刚性变换T，得到最小目标函数d；

l：迭代地进行刚性变换直至收敛：

根据步骤3.2中获得的刚性变换T，对P_irand应用刚性变换可获得P_irand＝T·P_irand；此时，若迭代次数k大于最大迭代次数k_max，则表明收敛速度过慢，舍弃此次迭代地结果，并且返回执行步骤j；若迭代次数k小于最大迭代次数k_max并且|d_k-d_k-1|>τ，则返回步骤3.2；若迭代次数k小于最大迭代次数k_max并且|d_k-d_k-1|≤τ，则迭代终止，确定最终匹配结果为[L_ICCP λ_ICCP]^T；

S5：用ICCP匹配所得位置信息对惯性导航系统输出位置信息进行校正：

将惯性导航系统输出的位置信息L_SINS、λ_SINS与ICCP匹配得到的位置信息L_ICCP、λ_ICCP的差值L_SINS-L_ICCP、λ_SINS-λ_ICCP作为观测量进行Kalman滤波，并利用滤波所得的位置信息反馈到惯性导航系统中对惯导输出的位置进行校正。

2.根据权利要求1所述的一种基于蚁群算法路径规划的无人机地形辅助导航方法,其特征在于：所述的步骤S1中的地形高程采用网格矩阵的方式存储。

3.根据权利要求1所述的一种基于蚁群算法路径规划的无人机地形辅助导航方法,其特征在于：所述的步骤S2的S2.2中假设某一地形的经纬度跨度为m×n网格，网格点坐标为(i，j)处的地形高程值为h(i，j)，且有i＝1，2，…，m，j＝1，2，…，n；可以计算得到地形信息量的主要特征参数包括地形标准差σ、地形相关系数R、地形粗糙度r和地形高度熵H_f，它们的具体定义如下:

其中，

为地形高程平均值；

为经度方向相关系数；

为纬度方向相关系数；

为经度方向的粗糙度；

为纬度方向的粗糙度；

为归一化高程值。

4.根据权利要求1所述的一种基于蚁群算法路径规划的无人机地形辅助导航方法,其特征在于：所述的步骤S2的S2.2通过熵值法赋权灰色关联决策法对地形进行划分。

5.根据权利要求1所述的一种基于蚁群算法路径规划的无人机地形辅助导航方法,其特征在于：所述的步骤S3将蚂蚁安放在起点网格S，并将起点网格加入到禁忌表TABU中，其中，TABU表示禁忌表，当地形网格为N维方正时，禁忌表为1行N²列矩阵，用以表示是否已经经过了某个网格，若已经经过了则对应于网格序号的列置0，为防止蚂蚁回溯，需要根据蚂蚁经过的路径动态调整禁忌表。

6.根据权利要求1所述的一种基于蚁群算法路径规划的无人机地形辅助导航方法,其特征在于：所述的步骤S4的b中蚂蚁选择下一个可行进的网格采用公式(5)进行计算：

其中，allowed代表禁忌表未经过的网格，τ_i，j为网格i到网格j的路径上的信息素浓度，η_i，g为自启发函数，η_i，g＝1/d_ig，d_ig代表网格i到目标网格的距离。

7.根据权利要求1所述的一种基于蚁群算法路径规划的无人机地形辅助导航方法,其特征在于：所述的步骤S4中的g信息素按公式(6)进行更新，同时，路径上的信息素浓度随着时间推移逐渐蒸发：

τ_i，j←(1-ρ)·τ_i，j+Δτ_i，j (6)

其中：Δτ_i，j为增加的信息素部分，Q为信息素增强系数，L为蚂蚁到达终点的路径长度。

8.根据权利要求1所述的一种基于蚁群算法路径规划的无人机地形辅助导航方法,其特征在于：所述的步骤S4中的j对惯导系统指示航迹序列P_i按公式(8)作随机旋转和平移，可得到初始变换后的序列P_irand：

9.根据权利要求1所述的一种基于蚁群算法路径规划的无人机地形辅助导航方法,其特征在于：所述的步骤S4中的k通过公式(11)得到最小目标函数d：

其中，k表示迭代次数，D(P_irand，Y_i)表示P_irand和Y之间的距离，D_max表示P_irand和Y之间距离的最大值。