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

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

A terrain aided navigation method for UAV Based on ant colony algorithm path planning

【技术实现步骤摘要】
一种基于蚁群算法路径规划的无人机地形辅助导航方法
本专利技术涉及无人机地形辅助导航方法领域，具体是一种基于蚁群算法路径规划的无人机地形辅助导航方法。
技术介绍
惯性导航系统不需要任何外来信息，也不向外辐射任何信息，仅依靠自身就能够在全天候条件下，在全球范围内和任何介质环境里进行连续的定位与导航，这种同时具备自主性、隐蔽性和能获取载体完备信息的独特优点是其他导航系统无法比拟的。但是，惯导系统有着系统误差随时间积累的原理性缺陷，为了实现无人机长航时高精度的导航目标，需要利用外界位置信息对其进行周期性调整和校正。地形辅助导航是一种利用地形高程特征来进行辅助定位的方法，它具有自主、隐蔽、连续、全天候工作、导航定位误差不随时间积累等优点，是理想的辅助导航定位手段。然而，地形辅助导航要求地形高程有明显的变化，对于地形变化过于平缓、地形特征不明显的区域，用地形辅助导航方法来降低惯导系统的定位误差是不可行的。衡量地形信息量的主要特征参数包括地形标准差、地形相关系数、地形粗糙度和地形熵等。基于上述特征参数，可以有效地将地形划分为地形适配区和地形非适配区。应用于路径规划的现代智能算法主要有遗传算法、粒子群算法以及蚁群算法等。相较粒子群算法，蚁群算法寻找全局最优的能力更强；且蚁群算法采用了正反馈机制和启发式贪婪策略使搜索时间明显短于遗传算法；同时，蚁群算法的环境建模和实现简单，不需要遗传算法和粒子群算法复杂的编码机制。目前，蚁群算法的研究和应用更为成熟和广泛，其参数的选择和确定有更多的文献和理论支持。综上所述，根据地形划分情况，结合蚁群算法对路径进行规划，使无人机每隔一段时间飞经地形适配区，完成对惯导误差的修正，这对于真正实现长航时高精度导航定位具有非常重要的现实意义。
技术实现思路
为了解决上述问题，本专利技术提出一种基于蚁群算法路径规划的无人机地形辅助导航方法。一种基于蚁群算法路径规划的无人机地形辅助导航方法,其具体步骤如下：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：假设蚂蚁下一步可以进入的网格为[g1，g2，g3]，按照公式计算得到进入每一个网格的概率为[ξ1，ξ2，ξ3](0≤ξ1，ξ2，ξ3≤1，且ξ1+ξ2+ξ3＝1)，轮赌法按照下述方式进行：首先对进入每一个网格做累积概率统计，得到[ξ1ξ1+ξ2ξ1+ξ2+ξ3]＝[ξ1ξ1+ξ2]，然后产生一个0到1之间的随机数，若产生的随机数位于0和ξ1之间，则蚂蚁往网格g1前进，若位于ξ1和ξ1+ξ2之间，则蚂蚁往网格g2前进，若位于ξ1+ξ2和1之间，则蚂蚁往网格g3前进；d：更新路径和路径长度；e：重复步骤a和d，直到蚂蚁都到达终点或是陷入到死区，若蚂蚁未到达终点则至路径长度为0；f：重复步骤a到e，直至所有本波蚂蚁都到达了终点或是陷入了死区；g：更新信息素矩阵：当本波所有蚂蚁完成了路径选择之后，若蚂蚁到达了终点，对信息素进行更新；h：若所有波次蚂蚁均已进行了路径寻找，则输出所有路径中的最短路径及路径长度，寻径结束，否则，返回执行步骤a；i：依据步骤S2.3中规划的路径，采用等值线最近点迭代(ICCP)算法完成地形匹配，对惯导位置误差进行修正；j：获取惯导指示序列并进行初始变换：采用随机旋转和平移的方法进行初始变换，旋转和平移的大小在惯导系统误差方差的3倍范围内随意取值，取旋转偏移量为θrand，纬度方向的平移量为tL_rand，经度方向的随机平移量为tλ_rand，得到初始变换后的序列Pirand；k：提取高程等值线，寻找刚性变换求取最近点：通过机载气压高度表、无线电高度表获取初始变换后的位置序列Pirand处相应的高程值Hi，并从已知的数字地图中提取相应的高程等值线Ci；假设Pirand到相应等值线的最近点为Yi，寻找一个包含旋转矩阵R和平移矢量t的刚性变换T，得到最小目标函数d；l：迭代地进行刚性变换直至收敛：根据步骤3.2中获得的刚性变换T，对Pirand应用刚性变换可获得Pirand＝T·Pirand；此时，若迭代次数k大于最大迭代次数kmax，则表明收敛速度过慢，舍弃此次迭代地结果，并且返回执行步骤j；若迭代次数k小于最大迭代次数kmax并且|dk-dk-1|>τ，则返回步骤3.2；若迭代次数k小于最大迭代次数kmax并且|dk-dk-1|≤τ，则迭代终止，确定最终匹配结果为[LICCPλICCP]T；S5：用ICCP匹配所得位置信息对惯性导航系统输出位置信息进行校正：将惯性导航系统输出的位置信息LSINS、λSINS与ICCP匹配得到的位置信息LICCP、λICCP的差值LSINS-LICCP、λSINS-λICCP作为观测量进行Kalman滤波，并利用滤波所得的位置信息反馈到惯性导航系统中对惯导输出的位置进行校正。所述的步骤S1中的地形高程采用网格矩阵的方式存储。所述的步骤S2的S2.2中假设某一地形的经纬度跨度为m×n网格，网格点坐标为(i，j)处的地形高程值为h(i，j)，且有i＝1，2，…，m，j＝1，2，…，n；可以计算得到地形信息量的主要特征参数包括地形标准差σ、地形相关系数R、地形粗糙度r和地形高度熵Hf，它们的具体定义如下:其中，为地形高程平均值；为经度方向相关系数；为纬度方向相关系数；为经度方向的粗糙度；为纬度方向的粗糙度；为归一化高程值。所述本文档来自技高网...

【技术保护点】
1.一种基于蚁群算法路径规划的无人机地形辅助导航方法,其特征在于：其具体步骤如下：/nS1：载入地形高程数据；/nS2：根据地形信息量，判断地形是否可匹配：/nS2.1：将路径规划中的地形区域划分为L块候选区域，L的值根据匹配定位精度和载体计算机存储容量确定；/nS2.2：根据无人机定位精度要求确定地形适配区与地形非适配区；/nS2.3：结合适配区划分结果，基于蚁群算法进行导航路径规划：/n(1)适配性矩阵，若网格所在地形为地形适配区则置0，非适配区则置去；/n(2)可达性矩阵，若网格i到网格j是可达的，则用LEN(i，j)记录距离，若不可达，则LEN(i，j)＝0；规定每一个网格可能到达的网格为其四周相邻的八个网格，及上、下、左、右、左上、左下、右上、右下方向的八个网格，若适配性矩阵是N维方阵，则可达性矩阵是N×N方阵；/nS3：导入适配性地图，设置初始参数：设置路径规划的起点网格为S,终点网格为E，设置出动蚁群的波数为K，每一波出动的蚂蚁只数为M；设置信息素启发因子为α，自启发因子为β，信息素蒸发系数为ρ，信息素增强系数为Q，设置对应于可达性矩阵的信息素矩阵的初始信息素浓度为c，c为一常数，用ROUTES记录每一波蚂蚁中的每只蚂蚁行进的路径，用PL记录每一波蚂蚁中的每一只蚂蚁行进路径的长度；/nS4：依据蚁群算法对路径进行规划：/na：首先判断当前的网格是否是终点网格，若是则终止本只蚂蚁的寻径，启动下一只蚂蚁寻径；/nb：若不是则按照轮赌法选择下一个可前进的网格，通过轮赌法求出蚂蚁下一步可走的每一个网格的概率；/nc：假设蚂蚁下一步可以进入的网格为[g...

【技术特征摘要】
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：假设蚂蚁下一步可以进入的网格为[g1，g2，g3]，按照公式计算得到进入每一个网格的概率为[ξ1，ξ2，ξ3](0≤ξ1，ξ2，ξ3≤1，且ξ1+ξ2+ξ3＝1)，轮赌法按照下述方式进行：首先对进入每一个网格做累积概率统计，得到[ξ1ξ1+ξ2ξ1+ξ2+ξ3]＝[ξ1ξ1+ξ2]，然后产生一个0到1之间的随机数，若产生的随机数位于0和ξ1之间，则蚂蚁往网格g1前进，若位于ξ1和ξ1+ξ2之间，则蚂蚁往网格g2前进，若位于ξ1+ξ2和1之间，则蚂蚁往网格g3前进；
d：更新路径和路径长度；
e：重复步骤a和d，直到蚂蚁都到达终点或是陷入到死区，若蚂蚁未到达终点则至路径长度为0；
f：重复步骤a到e，直至所有本波蚂蚁都到达了终点或是陷入了死区；
g：更新信息素矩阵：当本波所有蚂蚁完成了路径选择之后，若蚂蚁到达了终点，对信息素进行更新；
h：若所有波次蚂蚁均已进行了路径寻找，则输出所有路径中的最短路径及路径长度，寻径结束，否则，返回执行步骤a；
i：依据步骤S2.3中规划的路径，采用等值线最近点迭代ICCP算法完成地形匹配，对惯导位置误差进行修正；
j：获取惯导指示序列并进行初始变换：
采用随机旋转和平移的方法进行初始变换，旋转和平移的大小在惯导系统误差方差的3倍范围内随意取值，取旋转偏移量为θrand，纬度方向的平移量为tL_rand，经度方向的随机平移量为tλ_rand，得到初始变换后的序列Pirand；
k：提取高程等值线，寻找刚性变换求取最近点：
通过机载气压高度表、无线电高度表获取初始变换后的位置序列Pirand处相应的高程值Hi，并从已知的数字地图中提取相应的高程等值线Ci；假设Pirand到相应等值线的最近点为Yi，寻找一个包含旋转矩阵R和平移矢量t的刚性变换T，得到最小目标函数d；
l：迭代地进行刚性变换直至收敛：
根据步骤3.2中获得的刚性变换T，对Pirand应用刚性变换可获得Pirand＝T·Pirand；此时，若迭代次数k大于最大迭代次数kmax，则表明收敛速度过慢，舍弃此次迭代地结果，并且返回执行步骤j；若迭代次数k小于最大迭代次数kmax并且|dk-dk-1|>τ，则返回...

【专利技术属性】
技术研发人员：汤郡郡，单奕萌，李金猛，
申请(专利权)人：国营芜湖机械厂，
类型：发明
国别省市：安徽;34