[发明专利]一种提高水下航行器惯性地形匹配导航平均精度的方法

权利要求书

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)。