一种用于极区水下航行器数据库辅助导航路径规划方法与流程

1.本发明涉及一种用于极区水下航行器数据库辅助导航路径规划方法，属于极区水下航行器导航、路径规划领域。


背景技术：

2.两极是全球气候变化的驱动器，且两极地区蕴含丰富的资源，两极地区在未来海战以及非战争领域都具有重要地位。随着科学技术和经济的发展，两极科考任务势在必行，而极区水下航行器的精确定位、导航与路径规划则是重中之重。
3.现有的极区导航手段主要是卫星导航和惯性导航，但由于极区上空卫星分布较少，同时频繁出现磁暴等现象，卫星信号容易受到影响，所以惯性导航技术是目前极区导航最为重要的方法，但无论是平台式还是捷联式惯性导航系统都存在难以精确定位的问题。因此将地球物理信息、路径规划算法和惯性导航技术结合，利用地球地理物理信息数据库辅助惯导，具有广阔的应用前景。
4.路径规划最初广泛用于移动机器人的研究，指机器人按照开始设置的性能指标搜索出一条从初始位置到目标位置的最优路径。极区数据库辅助导航主要通过极区重力值和深度数据来辅助惯性导航技术，但在极区，重力和深度数据并不是完全可知的，且不完全具有梯度，这些重力深度未知点和梯度不大点为非良好适配区，如何通过合适的路径规划算法，在保证设计性能指标的基础上，适当穿过这些非良好适配区，加快航行速度的同时保证精确定位，对极区水下航行器快速安全隐蔽执行极区任务有着重要意义。
5.专利cc112799414a公布了一种松弛轨迹规划方法，将全局规划按照设计路径长度划分为多个子航段，利用自适应差分进化粒子群优化算法完成局部航迹规划。
6.专利cn112967450a公布了一种基于a*算法实现对运输装置的路径进行有效的规划，进而实现对运输装置路径进行确定，完成路径选择。
7.2020年12月欧阳明达的《基于改进a*算法的水下重力匹配导航路径规划》中使用改进a*算法实现了水下重力匹配导航路径规划的仿真实验。仿真实验表面该方法能在一般地区进行重力匹配导航路径规划，但从文章和仿真结果看该算法路径规划无泛化能力，只能选择走有重力数据的地方，而在极区环境下，这种方法不能达到快速有效的结果。
8.路径规划算法主要有图搜索法、人工势场法、rrt算法、bug算法、增量式启发算法等。本发明涉及使用图搜索算法中的dijkstra算法，进行路径规划。全局路径规划算法用于首先找到一条具有较好导航能力的路径，局部路径规划用于当水下航行器测得地球地理信息数据与原始数据库发生较大误差或遇到随机漂移水下冰山时，重新寻找一条具有较好导航能力的路径。
9.综上所述，目前的路径规划算法在和地球地理信息数据库辅助导航结合时，泛化能力很低，只能在数据充足、数据变化明显的条件下运用。对于极区的某些重要战略性区域，存在地球地理数据不足，存在大量水下冰山等危险航线环境的情况，目前的方法在极区均不能很好的运用。本发明基于dijkstra算法，结合重力和深度数据，提高了路径规划的泛
化能力，能较好的规划出极区水下航行器的隐蔽安全准确快速的可行路径，实现路径规划在极区水下航行器上的应用。


技术实现要素：

10.本发明的目的是为了克服在极区环境下惯性导航系统难以精确定位、难以快速安全隐蔽执行任务的问题，而提供一种数据库辅助导航路径规划方法，可以使极区水下航行器在航行过程中，按照指定性能指标，尽可能多的航行在数据库辅助导航可匹配区，从而提高惯导系统在极区定位的精确性，确保极区水下航行器能在某些战略性区域隐蔽安全准确的航行，执行相关任务。
11.本发明的目的是这样实现的：步骤如下：
12.步骤一：获取极区重力值和深度数据，并对部分稀疏区间进行精准性插值；
13.步骤二：对其进行网格化，构建网格地图，绘制重力和深度等值线；
14.步骤三：叠加融合重力和深度网格图，并计算每个规定大小网格窗口的可匹配度；
15.步骤四：将所有格网点分为五类，并将这五类格网点按性质划分区域；
16.步骤五：设置匹配优先级，优先级依次为第一类》第二类》第三类》第四类》第五类，第五类点为严格禁止规划为航线区域类；
17.步骤六：建立有向图模型，给所有的有向边加上权值；
18.步骤七：根据极区水下航行器当前位置、目标位置进行路径规划，确定某次极区水下航行器航行路线；
19.步骤八：根据路径规划算法规划的路径，避开水下冰山等危险航行环境，并在航行过程中通过匹配算法，使用数据库导航辅助惯性导航，抑制惯导系统长时间使用误差累积效应，使得极区水下航行器安全、快速、精准的到达目标点。
20.本发明还包括这样一些结构特征：
21.1.步骤三所述计算每个规定大小网格窗口的可匹配度的具体为：
22.σ＝(r
long
r
lat
)/2
[0023][0024][0025]
式中，r
long
和r
lat
分别是计算某一选定窗口内重力值和深度值在沿经线方向和纬线方向的可匹配度，σ为某一选定窗口内重力值和深度值在沿经线方向和纬线方向可匹配度的平均值，反映该窗口内数据的变化幅度，可以作为评估该位置窗口是否可为可匹配位置的一个因素；m和n表示选定窗口格网地图的尺寸大小，g(i，j)为(i，j)点的重力值，h(i，j)为(i，j)点的深度值；若该窗口格网内某相邻位置(i，j)和(i，j 1)或(i，j)和(i 1，j)间数据梯度越大，即该窗口相邻位置的数据变化更明显，则该窗口的σ值越大，即该窗口越适合用来进行匹配辅助惯性导航系统。
[0026]
2.步骤四所述分类为：第一类为既有重力值又有深度值的点，第二类为只有重力值的点，第三类为只有深度值的点，第四类为既没有重力值也没有深度值的点，第五类为存
在水下冰山等危险航线环境的点；其中，将第四类网格点的可匹配度设置为第二类和第三类点可匹配度和的五分之一，将第五类存在水下冰山等危险航线环境的网格点的可匹配度置为0。
[0027]
3.步骤四所述分类为：第一类为既有重力值又有深度值的点，第二类为只有重力值的点，第三类为只有深度值的点，第四类为既没有重力值也没有深度值的点，第五类为存在水下冰山等危险航线环境的点；其中，将第四类网格点的可匹配度设置为第二类和第三类点可匹配度和的五分之一，将第五类存在水下冰山等危险航线环境的网格点的可匹配度置为0。
[0028]
4.步骤五所述优先级通过权值k来控制，格网窗口可匹配度参与权值k计算。
[0029]
5.步骤七中路径规划算法使用dijkstra算法，具体为：
[0030]
首先创建三个集合，open集合、close集合和距离集合s，open集合存储未确定最短路径的节点，初始化时为所有节点，close集合存储已确定最短路径的节点，初始化时为空，s集合存储所有节点间的最短距离，初始化时，第一个值置为0，其余置为无穷；
[0031]
将起点从open集合中取出，加入close集合中，计算在open集合中的各节点到起点的距离，并将起点设为open集合中各节点的父节点；
[0032]
选取到起点计算值最小的路径的节点，将其从open集合中取出，加入close集合中；
[0033]
更新距离集合s，将该最小路径计算值记录到距离集合s中，其他与起点的最短路径值置为无穷大；
[0034]
从选出的close集合中的节点出发，计算open集合中其他点到该点的距离，并将该点设置为open集合中剩余节点的父节点；
[0035]
选取到该点计算值最小的节点，将其从open集合中取出，加入close集合中；
[0036]
更新距离集合s，将该最小路径计算值与原距离集合s中的值进行比较，如果存在更小值，则更新保存最小值到距离集合中，如果更大，则保持；
[0037]
维持循环，直到目标点从open集合中被取出加入到close集合中，算法迭代终止，从目标点开始依次寻找其父节点直到出发点，将其连成线，得到起点到目标终点的最短路径，计算距离集合s中相关节点对于值即可的得到最短路径值。
[0038]
6.步骤八中数据库匹配算法使用改进iccp算法，具体为：
[0039]
通过极区水下航行器搭载的imu数据解算出位姿信息，同时测量相同时间戳位置的重力和深度值，以航向为约束，通过计算距离评估价值函数δ，来快速确定匹配点；
[0040][0041]
δ＝10 50*k
[0042]
其中：l
imu
为短时间内imu数据解算距离，l
cal
为同时间戳重力和深度在对应等值线上两点的计算距离，k为权力要求4所述权值，δ为容忍距离阈值，取值范围为10m～60m。
[0043]
与现有技术相比，本发明的有益效果是：本发明将极区地理地图转换为格网地图，并将格网地图的格网点按性质分成5类，评估这5类点的可匹配度，结合可匹配度值计算相应的计算权值，建立有向图模型，合理的利用dijkstra算法进行路径规划，提高了路径规划的泛化能力，在确保极区水下航行器不触碰水下冰山等危险点的基础上，规划极区水下航
行器走地理信息纹理多的地方，也不避免航行器完全避开无重力和无深度数据的地方。使用改进iccp算法，加快实时匹配速度，确保惯导信息能被实时修正，安全快捷有效的到达指定目标点，从而确保极区水下航行器能顺利开展极区任务。
附图说明
[0044]
图1是本发明流程图；
[0045]
图2是本发明路径规划算法的流程图。
具体实施方式
[0046]
下面结合附图与具体实施方式对本发明作进一步详细描述。显然，所描述的实施例仅仅是本发明一部分实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他的所有其他实施例，都属于本发明保护的范围。
[0047]
结合图1和图2，本发明在获取极区重力和深度的基础上，将极区重力和深度按照数据纹理度划分为若干个可匹配区，建立有向图模型，利用dijkstra算法实现全局路径规划，从而避免极区航行器行驶至无重力或深度数据的地方，无法辅助校正惯导的误差，且避开水下冰山等危险区域，进而通过改进的iccp算法，加快实时匹配速度，以一个安全、准确、快速的方式到达指定位置，进行极区任务。
[0048]
所述的一种用于极区航行器数据库辅助导航路径规划方法，具体包括以下几个步骤：
[0049]
步骤一：测量极区重力和深度数据，并对部分稀疏区间进行精准性插值；
[0050]
步骤二：对其进行网格化，构建网格地图，绘制重力和深度等值线；
[0051]
步骤三：叠加融合重力和深度格网图，并计算每个规定大小网格窗口的可匹配度；
[0052]
因为极区局部数据稀疏，梯度不明显，故通过比较各个选定窗口的可匹配度来进行，即块匹配法。
[0053]
σ＝(r
long
r
lat
)/2
[0054][0055][0056]
式中，r
long
和r
lat
分别是计算某一选定窗口内重力值和深度值在沿经线方向和纬线方向的可匹配度，σ为某一选定窗口内重力值和深度值在沿经线方向和纬线方向可匹配度的平均值，反映该窗口内数据的变化幅度，可以作为评估该位置窗口是否可为可匹配位置的一个因素。m和n表示选定窗口格网地图的尺寸大小，g(i，j)为(i，j)点的重力值，h(i，j)为(i，j)点的深度值。若该窗口格网内某相邻位置(i，j)和(i，j 1)或(i，j)和(i 1，j)间数据梯度越大，即该窗口相邻位置的数据变化更明显，则该窗口的σ值越大，即该窗口越适合用来进行匹配辅助惯性导航系统。
[0057]
步骤四：将所有格网点分为五类，第一类为既有重力值又有深度值的点，第二类为只有重力值的点，第三类为只有深度值的点，第四类为既没有重力值也没有深度值的点，第
五类为存在水下冰山等危险航线环境的点，并将这五类格网点按性质划分区域。其中，将第四类网格点的可匹配度设置为第二类和第三类点可匹配度和的五分之一，将第五类存在水下冰山等危险航线环境的格网点可匹配度置为0；
[0058]
步骤五：设置匹配优先级，优先级依次为第一类》第二类》第三类》第四类》第五类，第五类点为严格禁止规划为航线区域类，其中优先级通过权值来控制，步骤三计算的可匹配度参与权值计算；
[0059]
步骤六：建立有向图模型，给所有的有向边加上权值；
[0060]
步骤七：根据极区水下航行器当前位置、目标位置进行路径规划，确定某次极区水下航行器航行路线；
[0061]
步骤八：根据路径规划算法规划的路径，避开水下冰山等危险航行环境，并在航行过程中通过匹配算法，使用数据库导航辅助惯性导航，抑制惯导系统长时间使用误差累积效应，使得极区水下航行器安全、快速、精准的到达目标点。
[0062]
实施例二：在实施例一的基础上，步骤七中路径规划算法使用dijkstra算法，具体为：
[0063]
首先创建三个集合，open集合、close集合和距离集合s。open集合存储未确定最短路径的节点，初始化时为所有节点。close集合存储已确定最短路径的节点，初始化时为空。s集合存储所有节点间的最短距离，初始化时，第一个值置为0，其余置为无穷。
[0064]
将起点从open集合中取出，加入close集合中，计算在open集合中的各节点到起点的距离，并将起点设为open集合中各节点的父节点。
[0065]
选取到起点计算值最小的路径的节点，将其从open集合中取出，加入close集合中。
[0066]
更新距离集合s。将该最小路径计算值记录到距离集合s中，其他与起点的最短路径值置为无穷大。
[0067]
从上一步选出的close集合中的节点出发，计算open集合中其他点到该点的距离，并将该点设置为open集合中剩余节点的父节点。
[0068]
选取到该点计算值最小的节点，将其从open集合中取出，加入close集合中。
[0069]
更新距离集合s。将该最小路径计算值与原距离集合s中的值进行比较，如果存在更小值，则更新保存最小值到距离集合中，如果更大，则保持。
[0070]
维持循环，直到目标点从open集合中被取出加入到close集合中，算法迭代终止，从目标点开始依次寻找其父节点直到出发点，将其连成线，得到起点到目标终点的最短路径，计算距离集合s中相关节点对于值即可的得到最短路径值。
[0071]
实施例二：在实施例一和二的基础上，步骤八中数据库匹配算法使用改进iccp算法，iccp算法需要求取每次迭代的旋转和平移变换t，使当前迭代点集x经变换后与x在等值线c上的最近点点集y的距离平方价值函数m最小。改进iccp算法，首先通过极区水下航行器搭载的imu数据解算出位姿信息，同时测量相同时间戳位置的重力和深度。以航向为约束，通过计算距离评估价值函数δ，来快速确定匹配点。
[0072][0073]
δ＝10 50*k
[0074]
其中l
imu
为短时间内imu数据解算距离，l
cal
为同时间戳重力和深度在对应等值线上两点的计算距离，k为权力要求4所述权值，δ为容忍距离阈值，取值范围为10m～60m。