CN107525502B - 一种提高水下航行器惯性地形匹配导航平均精度的方法 - Google Patents
一种提高水下航行器惯性地形匹配导航平均精度的方法 Download PDFInfo
- Publication number
- CN107525502B CN107525502B CN201710532849.7A CN201710532849A CN107525502B CN 107525502 B CN107525502 B CN 107525502B CN 201710532849 A CN201710532849 A CN 201710532849A CN 107525502 B CN107525502 B CN 107525502B
- Authority
- CN
- China
- Prior art keywords
- underwater vehicle
- course
- matching
- navigation
- spherical
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 81
- 238000012937 correction Methods 0.000 claims abstract description 50
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 40
- 230000000737 periodic effect Effects 0.000 claims abstract description 34
- 238000005259 measurement Methods 0.000 claims description 5
- 238000010586 diagram Methods 0.000 description 18
- 238000005516 engineering process Methods 0.000 description 8
- 230000008901 benefit Effects 0.000 description 6
- 238000012876 topography Methods 0.000 description 5
- 238000004364 calculation method Methods 0.000 description 4
- 230000008859 change Effects 0.000 description 4
- 238000011161 development Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 4
- 230000005484 gravity Effects 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000011160 research Methods 0.000 description 4
- 238000009825 accumulation Methods 0.000 description 3
- 238000001914 filtration Methods 0.000 description 3
- 238000012935 Averaging Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000010219 correlation analysis Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000007123 defense Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- VAOCPAMSLUNLGC-UHFFFAOYSA-N metronidazole Chemical compound CC1=NC=C([N+]([O-])=O)N1CCO VAOCPAMSLUNLGC-UHFFFAOYSA-N 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 239000000571 coke Substances 0.000 description 1
- 230000002349 favourable effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000005358 geomagnetic field Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/0205—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric not using a model or a simulator of the controlled system
- G05B13/024—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric not using a model or a simulator of the controlled system in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种提高水下航行器惯性地形匹配导航平均精度的方法，包括：根据水下航行器的初始位置和目标位置，进行航向规划；从初始位置开始，每隔设定周期，根据水下航行器的当前位置与目标位置之间的相对方位，基于球面最短弧周期性航向控制法对航向进行修正；在第N次基于球面最短弧周期性航向控制法对航向进行修正之后，若确定水下航行器已驶入预设匹配区域，则根据搜索范围半径R，通过TERCOM算法进行一次地形匹配修正，并根据匹配修正结果对水下航行器的当前位置进行修正；将修正后的当前位置作为新初始位置，重复执行上述步骤，直至所述水下航行器到达目标位置。通过本发明，提高了水下航行器惯性地形匹配导航平均精度。
Description
技术领域
本发明属于航天航海技术领域，尤其涉及一种提高水下航行器惯性地形匹配导航平均精度的方法。
背景技术
天空海一体化海洋导航(融合天基、空基、海基等多源信息及技术手段)是水下航行器导航系统发展的主要趋势，如图1，示出了现有惯性导航系统的导航示意图。目前水下航行器的导航系统主要由惯性导航系统(INS，Inertial Navigation System)组成，存在误差随时间累积的问题，时间越长，误差累积越大。因此，为抑制惯性导航系统的累积，确保其安全航行和武器的精准打击，必须利用外界信息手段对其进行周期性重调与校正。
自导航技术发展至今，各种各样的导航技术也随着历史的进步而出现，它们已被广泛应用于军事、经济、社会等方面。目前，提高水下航行器安全航行和武器精准打击主要有两个途径：一是尽可能提高导航仪器本身的精度；二是采用优化的导航技术。目前，可用于水下长时间隐蔽导航的技术有地形辅助、重力辅助方案、地磁辅助方案。然而，现有技术存在诸多问题：
地磁辅助方案：地磁辅助方案由于地磁场本身存在长期和短期变化，导致地磁辅助方案的精度达不到较高的要求，且测磁手段存在磁干扰等局限性。
重力辅助方案：重力场和海底地形都是辅助导航的主要技术手段，而且地形场的研究开展较早，特别是陆地上的地形辅助导航技术经过了30多年的发展，已经比较成熟且运用于飞行器的导航。水下地形辅助导航发展较晚，但国内外加大了此技术的研究，“2000-2035年美国海军技术”发展战略研究中就提出了主要采用地形匹配技术提高水下航行器导航精度的目标。地形辅助导航根据算法原理可分为相关分析方法、扩展的递推卡尔曼滤波方法和直接概率准则方法三种，但它们都有各自的局限性。相关分析的匹配算法往往采用全局遍历的搜索策略，运算量较大，且对载体航迹要求较高，当航向存在较大偏差时误差将急剧增大。递推卡尔曼滤波方法的匹配算法需要较精确的初始位置误差，而且反复的全局变换运算将较大程度提高计算量，影响匹配的实时性。直接概率准则方法出现较晚，目前多为仿真结果。
发明内容
本发明的技术解决问题：克服现有技术的不足，提供了一种提高水下航行器惯性地形匹配导航平均精度的方法，旨在提高水下航行器惯性地形匹配导航平均精度。
为了解决上述技术问题，本发明公开了一种提高水下航行器惯性地形匹配导航平均精度的方法，包括：
步骤1，根据水下航行器的初始位置和目标位置，进行航向规划；
步骤2，从初始位置开始，每隔设定周期，根据所述水下航行器的当前位置与目标位置之间的相对方位，基于球面最短弧周期性航向控制法对航向进行修正；
步骤3，在第N次基于球面最短弧周期性航向控制法对航向进行修正之后，判断所述水下航行器是否已驶入预设匹配区域；
步骤4，若确定所述水下航行器已驶入预设匹配区域，则根据搜索范围半径R，通过TERCOM算法进行一次地形匹配修正，并根据匹配修正结果对水下航行器的当前位置进行修正；
步骤5，将修正后的当前位置作为新初始位置，重复执行上述步骤2-4，直至所述水下航行器到达所述目标位置。
在上述提高水下航行器惯性地形匹配导航平均精度的方法中，还包括：
若确定所述水下航行器未驶入预设匹配区域，则进行下一次基于球面最短弧周期性航向控制法的航向修正，直至水下航行器驶入预设匹配区域。
在上述提高水下航行器惯性地形匹配导航平均精度的方法中，还包括：
根据规划的航向，结合所规划的航向的路径上的水下地理环境属性信息，确定若干个预设匹配区域；其中，所述若干个预设匹配区域处于所述规划的航向上。
在上述提高水下航行器惯性地形匹配导航平均精度的方法中，还包括：
当所述水下航行器位于所述预设匹配区域之外、或、所述水下航行器位于所述预设匹配区域之内且已完成地形匹配修正时，根据惯性导航系统进行定位，确定所述水下航行器的当前位置。
在上述提高水下航行器惯性地形匹配导航平均精度的方法中，所述若确定所述水下航行器已驶入预设匹配区域，则根据搜索范围半径R，通过TERCOM算法进行一次地形匹配修正，并根据匹配修正结果对水下航行器的当前位置进行修正，包括：
若确定所述水下航行器已驶入预设匹配区域，则当所述水下航行器继续航行设定时间T后，根据搜索范围半径R，通过TERCOM算法进行一次地形匹配修正，并根据匹配修正结果对水下航行器的当前位置进行修正；其中，所述设定时间T满足如下条件：当所述水下航行器继续航行设定时间T后，通过惯性导航系统获取的高程测量序列满足所述TERCOM算法所需的修正条件。
在上述提高水下航行器惯性地形匹配导航平均精度的方法中，所述搜索范围半径R通过如下步骤确定：
确定所述水下航行器处于当前位置时所对应的航行时间；
根据确定的航行时间，结合惯性导航系统的属性信息，确定所述惯性导航系统在所述航行时间时所对应的误差估计值；
将所述误差估计值作为本次地形匹配修正所对应的搜索范围半径R。
在上述提高水下航行器惯性地形匹配导航平均精度的方法中，所述基于球面最短弧周期性航向控制法对航向进行修正的步骤如下：
取北极点C，并确定水下航行器的当前位置为点A、目标位置为点B；其中，点A的坐标为[lat_A，lon_A]，点B的坐标为[lat_B，lon_B]；
确定球面三角四元素公式：
sin∠ACBcot∠CAB＝cotasinb-cos∠ACBcosb···公式(1)
其中，∠CAB表示最优航向角，a表示圆心角∠COB对应的弧段，b表示圆心角∠COA对应的弧段；点O表示地球球心；
对上述公式(1)进行整理，得到最优航向角∠CAB的求解公式：
根据上述公式(2)，求解得到最优航向角∠CAB；
根据求解得到的最优航向角∠CAB，对规划的航向进行修正。
在上述提高水下航行器惯性地形匹配导航平均精度的方法中，还包括：
根据如下公式(3)确定∠ACB：
∠ACB＝(lon_B-lon_A)pi/180···公式(3)
根据公式(4)确定a：
a＝(90-lat_B)pi/180···公式(4)
根据公式(5)确定b：
b＝(90-lat_A)pi/180···公式(5)。
本发明具有以下优点：
本发明公开了一种提高水下航行器惯性地形匹配导航平均精度的方法，基于球面最短弧周期性航向控制法对航向进行周期性修正，并在此基础上判断水下航行器是否已驶入预设匹配区域，在确定水下航行器驶入预设匹配区域时，根据搜索范围半径R，通过TERCOM算法进行地形匹配修正，球面最短弧周期性航向控制法与TERCOM算法的结合，提高了水下航行器惯性地形匹配导航的平均精度，具有重要的科学价值和社会效益，同时有利于国防安全和领海完整。
其次，本发明所述的一种提高水下航行器惯性地形匹配导航平均精度的方法，具有运算量小，可靠性高、匹配实时性高、计算速度快、搜索范围半径小以及匹配过程物理含义明确等优点。
附图说明
图1是现有惯性导航系统的导航示意图；
图2是本发明实施例中一种提高水下航行器惯性地形匹配导航平均精度的方法的步骤流程图；
图3是本发明实施例中一种球面最短弧算法的原理示意图；
图4是本发明实施例中一种南海范围内的预设匹配区的卫星图；
图5是本发明实施例中一种误差变化曲线示意图；
图6是本发明实施例中一种静基座条件下陀螺仪漂移和加速度计零偏对经度误差的影响示意图；
图7是本发明实施例中一种惯导轨迹与真实轨迹的误差对比图；
图8是本发明实施例中一种惯导轨迹与真实轨迹的累积误差对比图；
图9是本发明实施例中又一种惯导轨迹与真实轨迹的误差对比图；
图10是本发明实施例中一种基于TERCOM算法进行地形匹配修正时，匹配位置与真实位置之间的对比示意图；
图11是本发明实施例中一种基于球面最短弧周期性航向控制法进行航向修正、并结合TERCOM算法进行地形匹配修正时，匹配位置与真实位置之间的对比示意图；
图12是本发明实施例中一种导航平均精度对比图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明公共的实施方式作进一步详细描述。
地形匹配精度主要由匹配算法和地形区域特性决定。现有地形匹配算法中，TERCOM(Terrain Contour Matching)、ICCP(Iterative Closest Contour Point)、PDAF(Probability Data Associate Filtering)等均为批处理的相关类算法，SITAN(SandiaInertial Terrain-Aided Navigation)和PF(Particle Filter)算法分别是基于扩展卡尔曼滤波(EKF)和基于直接概率准则的连续匹配算法。
本发明公开的一种提高水下航行器惯性地形匹配导航平均精度的方法，法联合几何学中的球面最短距离法则和航天/航海学中的姿态控制原理，可减小水下导航系统的误差，同时有利于提高匹配速度。该方法导航精度高，计算速度快，计算机性能要求低，有利于克服TERCOM算法对航向误差较敏感特点的不足，通过精确修正航向偏差，提高天空海一体化水下地形辅助导航的平均精度。
参照图2，示出了本发明实施例中一种提高水下航行器惯性地形匹配导航平均精度的方法的步骤流程图。在本实施例中，所述提高水下航行器惯性地形匹配导航平均精度的方法，包括：
步骤101，根据水下航行器的初始位置和目标位置，进行航向规划。
步骤102，从初始位置开始，每隔设定周期，根据所述水下航行器的当前位置与目标位置之间的相对方位，基于球面最短弧周期性航向控制法对航向进行修正。
在平面上，点A到点B的最短距离为二者之间的直线距离，但在球面上，点A到点B的最短距离为连接两点的大圆弧。如图3所示，示出了本发明实施例中一种球面最短弧算法的原理示意图。如图3，A、B两点之间的最短距离应当是圆心角∠AOB对应的弧段AOB，而非纬度线圈上的弧段AO′B。
在本实施例中，考虑到地球球形因素，基于球面最短弧周期性航向控制法对航向进行修正。优选的，基于球面最短弧周期性航向控制法对航向进行修正的步骤如下：
取北极点C，并确定水下航行器的当前位置为点A、目标位置为点B；其中，点A的坐标为[lat_A，lon_A]，点B的坐标为[lat_B，lon_B]；
确定球面三角四元素公式：
sin∠ACBcot∠CAB＝cotasinb-cos∠ACBcosb···公式(1)
其中，∠CAB表示最优航向角，a表示圆心角∠COB对应的弧段，b表示圆心角∠COA对应的弧段；点O表示地球球心。
对上述公式(1)进行整理，得到最优航向角∠CAB的求解公式：
根据上述公式(2)，求解得到最优航向角∠CAB。其中，∠CAB表示北偏东的角度。
根据求解得到的最优航向角∠CAB，对规划的航向进行修正。
进一步优选的，在本实施例中，可以根据如下公式(3)确定∠ACB：
∠ACB＝(lon_B-lon_A)pi/180···公式(3)
根据如下公式(4)确定a：
a＝(90-lat_B)pi/180···公式(4)
根据如下公式(5)确定b：
b＝(90-lat_A)pi/180···公式(5)。
在本实施例中，根据公式(3)(4)和(5)可以对公式(2)求解。
步骤103，在第N次基于球面最短弧周期性航向控制法对航向进行修正之后，判断所述水下航行器是否已驶入预设匹配区域。
步骤104，若确定所述水下航行器已驶入预设匹配区域，则根据搜索范围半径R，通过TERCOM算法进行一次地形匹配修正，并根据匹配修正结果对水下航行器的当前位置进行修正。
如前所述，TERCOM算法是一种批处理算法，最佳匹配位置是在测得一定长度的地形高程序列后，通过无遗漏地搜索位置不确定区域内的每个网格位置得到的。在本实施例中，所选择的地形区域(预设匹配区域)经过了预先验证，且适配性较好。其中，地形区域特性通常利用地形特征参数来描述.依据不同的理论，研究人员提出了包括地形标准差、信息熵、粗糙度、相关系数等众多的地形特征参数，总体上从地形的宏观起伏、微观破碎和自相似性三个方面对地形区的适配性进行描述，并寻求利用特征参数建立地形适配性判别的模型。
优选的，可以根据规划的航向，结合所规划的航向的路径上的水下地理环境属性信息，确定若干个预设匹配区域；其中，所述若干个预设匹配区域处于所述规划的航向上。
在本发明的一优选实施例中，若确定所述水下航行器已驶入预设匹配区域，则当所述水下航行器继续航行设定时间T后，根据搜索范围半径R，通过TERCOM算法进行一次地形匹配修正，并根据匹配修正结果对水下航行器的当前位置进行修正。其中，所述设定时间T满足如下条件：当所述水下航行器继续航行设定时间T后，通过惯性导航系统获取的高程测量序列满足所述TERCOM算法所需的修正条件。
优选的，所述搜索范围半径R通过如下步骤确定：
确定所述水下航行器处于当前位置时所对应的航行时间；根据确定的航行时间，结合惯性导航系统的属性信息，确定所述惯性导航系统在所述航行时间时所对应的误差估计值；将所述误差估计值作为本次地形匹配修正所对应的搜索范围半径R。其中，惯性导航系统的属性信息是已知的，也即，航行时间与惯性导航系统的误差估计值之间的对应关系是已知的，根据航行时间可以直接查表确定当前时刻对应的误差估计值。
步骤105，将修正后的当前位置作为新初始位置，重复执行上述步骤102-104，直至所述水下航行器到达所述目标位置。
在本实施例中，若确定所述水下航行器未驶入预设匹配区域，则可以返回执行步骤102，进行下一次基于球面最短弧周期性航向控制法的航向修正，直至水下航行器驶入预设匹配区域。
其中，需要说明的是，在本实施例中，当所述水下航行器位于所述预设匹配区域之外、或、所述水下航行器位于所述预设匹配区域之内且已完成地形匹配修正时，根据惯性导航系统进行定位，确定所述水下航行器的当前位置。换而言之，在进行基于球面最短弧周期性航向控制法对航向进行修正时，采用惯性导航系统进行定位。
下面结合实验过程对本发明实施例所述的提高水下航行器惯性地形匹配导航平均精度的方法进行详细说明。
本发明实验采用的原始海底数字地形图的分辨率为0.5′×0.5′，经过插值加密可达0.25′×0.25′，为利用地形数据进行辅助导航提供了较好条件。本实验研究数据源为全球海陆数据库(GEBCO，General Bathymetric Chart of the Oceans)数据，选取南海地区数据进行研究，数据经纬度取值范围为：纬度10°～14°N，经度112°～116°E。
经地形区域适配性的预先验证，西北方与南方区域地形起伏剧烈，水深变化值多在1000m内，此区域海底浅焦分布密集，地形变化大，有利于实验模拟路线的设定。图4，表示本发明实施例中一种南海范围内的预设匹配区的卫星图。其中，在本实施例中，实验选取的预设匹配区为：纬度10°～11°N，经度114°～115°E。其中，小方框区域为所选择的预设匹配区域。
假设水下航行器沿着图1所示的AB路线行驶。其中，水下航行器基于陀螺仪漂移0.01°/h、加速度计零偏10-3m/s2、纬度10.25°、运行48h时，东向/北向速度误差、经/纬度误差和东/北/天向姿态误差变化曲线如图5所示，其中，图5，示出了本发明实施例中一种误差变化曲线示意图。可见，对于惯性导航系统而言，纬度误差成周期性变化，其累积误差主要是经度误差。
进一步的，如图6，示出了本发明实施例中一种静基座条件下陀螺仪漂移和加速度计零偏对经度误差的影响示意图。其中，εx、εy、εz表示陀螺仪漂移，
表示加速度计零偏，δλ表示经度误差。结合图6的曲线，陀螺仪漂移和加速度计零偏对经度误差的影响分析如下：
(1)陀螺仪的常值漂移引起的系统经度误差随时间累积。其累积主要由北向陀螺漂移εy和天向陀螺漂移εz产生，而东向陀螺漂移εx不引起随时间积累的经度误差。
(2)加速度计的零偏不引起随时间积累的经度误差。
在本发明的一优选实施例中，参照图7，示出了本发明实施例中一种惯导轨迹与真实轨迹的误差对比图。其中，虚线表示惯导轨迹，实线表示真实轨迹。图7a，为基于INS进行导航时，惯导轨迹与真实轨迹的误差对比图；图7b，为基于球面最短弧周期性航向控制法进行导航修正时，惯导轨迹与真实轨迹的误差对比图。其中，参数设置如下：陀螺仪漂移0.01°/h、加速度计零偏10-3m/s2、航速10m/s、初始位置误差0、球面最短弧周期性航向控制法修正周期T＝180s。进一步的，参照图8，示出了本发明实施例中一种惯导轨迹与真实轨迹的累积误差对比图。结合图7和图8可见，在陀螺仪漂移为0.01°/h和加速度计零偏为10-3m/s2情况下，INS的惯导轨迹与真实轨迹的实时误差较大；在陀螺仪漂移和加速度计零偏不变情况下，基于球面最短弧周期性航向控制法进行导航修正可以较大程度减小惯导轨迹与真实轨迹的实时误差。
在本发明的一优选实施例中，参照图9，示出了本发明实施例中又一种惯导轨迹与真实轨迹的误差对比图。其中，虚线表示惯导轨迹，实线表示真实轨迹。图9a，为基于TERCOM算法进行地形匹配修正时，惯导轨迹与真实轨迹的误差对比图；图9b，为基于球面最短弧周期性航向控制法进行航向修正、并结合TERCOM算法进行地形匹配修正时，惯导轨迹与真实轨迹的误差对比图。其中，a1(a2)、b1(b2)、c1(c2)和d1(d2)表示四次地形匹配修正点。
如图9a，基于TERCOM算法进行地形匹配修正时，惯导轨迹与真实轨迹误差较大；与此同时，在两个匹配点之间仅依靠惯性导航，导航误差累积发散较快，据图8(虚线)可知，位置误差发散约2km/h。如图9b，球面最短弧周期性航向控制法与TERCOM算法的结合，惯导轨迹与真实轨迹误差较小；与此同时，在两个匹配点之间，每隔周期时间T，依靠惯性导航与球面最短弧周期性航向控制法进行航向控制，据图8(实线)可知，位置误差发散速度得到了一定的抑制，同时也有利于减小匹配区的搜索范围半径R。
在本发明的一优选实施例中，参照图10，示出了本发明实施例中一种基于TERCOM算法进行地形匹配修正时，匹配位置与真实位置之间的对比示意图。其中，图10(a)、10(b)、10(c)和10(d)分别表示四次地形匹配修正时，匹配位置与真实位置的对比图。图10(a)、10(b)、10(c)和10(d)分别对应四个地形匹配修正点：a1(a2)、b1(b2)、c1(c2)和d1(d2)。根据图10可知，在进行地形匹配修正之前，惯导指示位置与真实位置之间的误差较大，达到1km以上；在进行地形匹配修正之后，匹配位置均优于惯导指示位置。其中，图10(b)、10(c)和10(d)所示的三次地形匹配修正后，匹配位置与真实位置之间的误差约为100m内，匹配修正效果较好。图10(d)匹配修正效果最好，但图10(a)匹配修正后的匹配位置与真实位置之间的误差仍相对较大，达到310.2m，这表明在地形匹配修正点a1(a2)附近，匹配区域的适配性要稍差于其他3个位置，匹配区域内沿轨迹方向可能存在多个相似性较高的水深序列，从而导致在测量噪声作用下，出现较大偏差。
在本发明的一优选实施例中，参照图11，示出了本发明实施例中一种基于球面最短弧周期性航向控制法进行航向修正、并结合TERCOM算法进行地形匹配修正时，匹配位置与真实位置之间的对比示意图。据图11可知，匹配前惯导指示位置与真实位置之间误差较小，约为200m，这是利用基于球面最短弧周期性航向控制法对航向进行修正后，减小了惯导指示位置与真实位置之间的误差；在此基础上再基于TERCOM算法进行地形匹配修正，地形匹配修正后的匹配位置均优于惯导指示位置，匹配位置与真实位置之间的误差约为100m内，且稳定度较好。
综上，据图10和图11可知，由于预设匹配区域的搜索范围半径R的减小，降低了预设匹配区域内出现相似性航迹的可能，因而提高了匹配概率；且，基于球面最短弧周期性航向控制法一定程度上降低了航向误差，改善了TERCOM算法的应用环境。
在本发明的一优选实施例中，参照图12，示出了本发明实施例中一种导航平均精度对比图。其中，虚线表示：采用INS+TERCOM算法时的导航平均精度；实线表示：采用INS+TERCOM算法+球面最短弧周期性航向控制法时的导航平均精度。由图10、11和12可知，采用INS+TERCOM算法时的导航平均精度约为504m；采用INS+TERCOM算法+球面最短弧周期性航向控制法时的导航平均精度约为93m，导航平均精度提高约5倍。因此，基于球面最短弧周期性航向控制法，有利于提高天空海一体化水下航行器惯性/地形匹配导航平均精度。
综上所述，本发明所述的一种提高水下航行器惯性地形匹配导航平均精度的方法，基于球面最短弧周期性航向控制法对航向进行周期性修正，并在此基础上判断水下航行器是否已驶入预设匹配区域，在确定水下航行器驶入预设匹配区域时，根据搜索范围半径R，通过TERCOM算法进行地形匹配修正，球面最短弧周期性航向控制法与TERCOM算法的结合，提高了水下航行器惯性地形匹配导航的平均精度，具有重要的科学价值和社会效益，同时有利于国防安全和领海完整。其次，本发明还具有运算量小，可靠性高、匹配实时性高、计算速度快、搜索范围半径小以及匹配过程物理含义明确等优点。
本说明中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。
以上所述，仅为本发明最佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。
本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。
Claims (4)
1.一种提高水下航行器惯性地形匹配导航平均精度的方法，其特征在于，包括：
步骤1，根据水下航行器的初始位置和目标位置，进行航向规划；
步骤2，从初始位置开始，每隔设定周期，根据所述水下航行器的当前位置与目标位置之间的相对方位，基于球面最短弧周期性航向控制法对航向进行修正；其中，在进行基于球面最短弧周期性航向控制法对航向进行修正时，采用惯性导航系统进行定位；
步骤3，在第N次基于球面最短弧周期性航向控制法对航向进行修正之后，判断所述水下航行器是否已驶入预设匹配区域；其中，预设匹配区域为根据规划的航向，结合所规划的航向的路径上的水下地理环境属性信息，确定的若干个预设匹配区域，所述若干个预设匹配区域处于所述规划的航向上；
步骤4，若确定所述水下航行器已驶入预设匹配区域，则当所述水下航行器继续航行设定时间T后，根据搜索范围半径R，通过TERCOM算法进行一次地形匹配修正，并根据匹配修正结果对水下航行器的当前位置进行修正；其中，所述设定时间T满足如下条件：当所述水下航行器继续航行设定时间T后，通过惯性导航系统获取的高程测量序列满足所述TERCOM算法所需的修正条件；其中，搜索范围半径R通过如下步骤确定：确定所述水下航行器处于当前位置时所对应的航行时间；根据确定的航行时间，结合惯性导航系统的属性信息，确定所述惯性导航系统在所述航行时间时所对应的误差估计值；将所述误差估计值作为本次地形匹配修正所对应的搜索范围半径R；
步骤5，将修正后的当前位置作为新初始位置，重复执行上述步骤2-4，直至所述水下航行器到达所述目标位置。
2.根据权利要求1所述的方法，其特征在于，还包括：
若确定所述水下航行器未驶入预设匹配区域，则进行下一次基于球面最短弧周期性航向控制法的航向修正，直至水下航行器驶入预设匹配区域。
3.根据权利要求1所述的方法，其特征在于，所述基于球面最短弧周期性航向控制法对航向进行修正的步骤如下：
取北极点C，并确定水下航行器的当前位置为点A、目标位置为点B；其中，点A的坐标为[lat_A，lon_A]，点B的坐标为[lat_B，lon_B]；
确定球面三角四元素公式：
sin∠ACBcot∠CAB＝cotasinb-cos∠ACBcosb…公式(1)
其中，∠CAB表示最优航向角，a表示圆心角∠COB对应的弧段，b表示圆心角∠COA对应的弧段；点O表示地球球心；
对上述公式(1)进行整理，得到最优航向角∠CAB的求解公式：
根据上述公式(2)，求解得到最优航向角∠CAB；
根据求解得到的最优航向角∠CAB，对规划的航向进行修正。
4.根据权利要求3所述的方法，其特征在于，还包括：
根据如下公式(3)确定∠ACB：
∠ACB＝(lon_B-lon_A)pi/180…公式(3)
根据公式(4)确定a：
a＝(90-lat_B)pi/180…公式(4)
根据公式(5)确定b：
b＝(90-lat_A)pi/180…公式(5)。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710532849.7A CN107525502B (zh) | 2017-07-03 | 2017-07-03 | 一种提高水下航行器惯性地形匹配导航平均精度的方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710532849.7A CN107525502B (zh) | 2017-07-03 | 2017-07-03 | 一种提高水下航行器惯性地形匹配导航平均精度的方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107525502A CN107525502A (zh) | 2017-12-29 |
| CN107525502B true CN107525502B (zh) | 2020-10-23 |
Family
ID=60748222
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710532849.7A Active CN107525502B (zh) | 2017-07-03 | 2017-07-03 | 一种提高水下航行器惯性地形匹配导航平均精度的方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107525502B (zh) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108318034B (zh) * | 2018-01-09 | 2020-05-22 | 浙江大学 | 一种基于声呐地图的auv回坞导航方法 |
| CN110081884B (zh) * | 2019-05-21 | 2021-02-12 | 北京理工大学 | 基于虚拟航向的重力辅助惯性导航区域适配性评价方法 |
| CN112066991A (zh) * | 2020-09-07 | 2020-12-11 | 常州微亿智造科技有限公司 | 多约束条件下智能飞行器航迹快速规划方法、装置 |
| CN114199238B (zh) * | 2021-12-17 | 2024-05-03 | 中国空间技术研究院 | 基于软间隔局部半圆搜索提高水下导航效率和可靠性方法 |
| CN114734435B (zh) * | 2022-03-24 | 2023-09-19 | 苏州艾利特机器人有限公司 | 一种基于超球面的编码器校准方法、装置及系统 |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7522090B2 (en) * | 2006-10-31 | 2009-04-21 | Honeywell International Inc. | Systems and methods for a terrain contour matching navigation system |
| CN101339036B (zh) * | 2008-08-20 | 2010-06-23 | 北京航空航天大学 | 地形辅助导航方法和设备 |
| CN106643729A (zh) * | 2015-12-22 | 2017-05-10 | 中国电子科技集团公司第二十研究所 | 一种海用卫星导航设备大圆航线航路点划分与航程计算方法 |
| CN105760688B (zh) * | 2016-03-04 | 2018-07-10 | 李清林 | 用假设经纬度法测定天体位置或者球面目标位置的方法 |
| CN106643714B (zh) * | 2017-03-15 | 2019-06-07 | 北京航空航天大学 | 一种自主实时机载地形辅助惯性导航方法和系统 |
-
2017
- 2017-07-03 CN CN201710532849.7A patent/CN107525502B/zh active Active
Non-Patent Citations (2)
| Title |
|---|
| 导航系统航迹仿真及应用;邢靓等;《弹箭与制导学报》;20071231;第27卷(第2期);第11-13页 * |
| 水下地形辅助导航方法研究;辛廷慧;《中国优秀博硕士学位论文全文数据库 工程科技Ⅱ辑》;20040915;第30-36页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107525502A (zh) | 2017-12-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107525502B (zh) | 一种提高水下航行器惯性地形匹配导航平均精度的方法 | |
| CN107314768B (zh) | 水下地形匹配辅助惯性导航定位方法及其定位系统 | |
| Georgy et al. | Enhanced MEMS-IMU/odometer/GPS integration using mixture particle filter | |
| CN110487301A (zh) | 一种雷达辅助机载捷联惯性导航系统初始对准方法 | |
| CN106052688B (zh) | 基于地形轮廓匹配的惯性导航系统速度累积误差修正方法 | |
| CN113252038B (zh) | 基于粒子群算法的航迹规划地形辅助导航方法 | |
| EP3350628A1 (en) | Measurement of magnetic field gradients | |
| CN111751852A (zh) | 基于点云配准的无人车gnss定位可靠性评价方法 | |
| CN111829512B (zh) | 一种基于多传感器数据融合的auv导航定位方法及系统 | |
| CN102829777A (zh) | 自主式水下机器人组合导航系统及方法 | |
| CN107860399A (zh) | 一种基于地图匹配的车载捷联惯导行进间精对准方法 | |
| CN109724592B (zh) | 一种基于进化梯度搜索的auv地磁仿生导航方法 | |
| CN111176298B (zh) | 一种无人车轨迹录制与轨迹跟踪方法 | |
| CN109738902B (zh) | 一种基于同步信标模式的水下高速目标高精度自主声学导航方法 | |
| CN110057354A (zh) | 一种基于磁偏角修正的地磁匹配导航方法 | |
| Guo et al. | Algorithm for geomagnetic navigation and its validity evaluation | |
| Liu et al. | Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter | |
| CN113932806B (zh) | 高速飞行器惯性/地磁匹配搜索区域自适应组合导航方法 | |
| CN113503892A (zh) | 一种基于里程计和回溯导航的惯导系统动基座初始对准方法 | |
| Wang et al. | Land vehicle navigation using odometry/INS/vision integrated system | |
| CN112798016A (zh) | 一种基于sins与dvl组合的auv行进间快速初始对准方法 | |
| CN114088098A (zh) | 一种用于极区水下航行器数据库辅助导航路径规划方法 | |
| CN113252039B (zh) | 面向地形辅助导航的粒子群快速匹配方法 | |
| CN111189446A (zh) | 一种基于无线电的组合导航方法 | |
| CN108106597A (zh) | 全捷联激光导引头在目标出线性视场情况下角度测量方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |