一种基于惯导修正的长基线导航定位方法

摘要

本发明涉及水声导航定位技术领域，具体涉及一种基于惯导修正的长基线导航定位方法，包括：获取M个对水下航行器的当前位置的X分量的估计值；对M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值获取M个对水下航行器的当前位置的Y分量的估计值；对M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值；根据获得的水下航行器当前位置的X分量的最优估计值和获得的水下航行器当前位置的Y分量的最优估计值，确定水下航行器当前位置；实现对水下航行器的导航定位。

权利要求

1.一种基于惯导修正的长基线导航定位方法，其特征在于，该方法包括：根据水下航行器时刻ti的前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量，获取M个对水下航行器的当前位置的X分量的估计值；

对由获得的M个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值根据水下航行器时刻ti的前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量，获取M个对水下航行器的当前位置的Y分量的估计值；

对由获得的M个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值根据获得的水下航行器当前位置的X分量的最优估计值和获得的水下航行器当前位置的Y分量的最优估计值，确定水下航行器当前位置；实现对水下航行器的导航定位。

2.根据权利要求1所述的方法，其特征在于，所述根据水下航行器前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量，获取M个对水下航行器的当前位置的X分量的估计值；具体包括：根据公式(1)，获得水下航行器在时间点ti时的当前位置的坐标X分量xi：其中，xj为水下航行器在时间点tj时的水下航行器的当前位置的坐标X分量； 为水下航行器从时刻tj运动到时刻ti的相对位移量；

其中，通过公式(2)，获得水下航行器从时刻tj运动到时刻ti的相对位移量其中， 为在时间点ti，惯导组合定位结果的X分量； 为在时间点tj，惯导组合定位结果的X分量； 为时刻tj到时刻ti的惯导相对累计误差；

令

其中， 为惯导组合定位误差的X分量；根据惯导系统的物理特性，为随着时间推移逐渐增大的累计误差； 为在时间点ti，惯导

组合定位结果的X分量；

令

其中， 为惯导组合定位误差的X分量；根据惯导系统的物理特性，为随着时间推移逐渐增大的累计误差； 为在时间点tj，惯导

组合定位结果的X分量；

令

其中， 为水下航行器在时间点ti时的水下航行器的水声长基线定位结果的坐标X分量，xi为水下航行器在时间点ti时的水下航行器的当前真实位置的坐标X分量， 为水下航行器在时间点ti时的水下航行器的水声长基线定位误差的坐标X分量；

因此，根据公式(3)，获得水下航行器在时刻ti的当前位置的X分量的估计值其中， 为时刻tj到时刻ti的惯导相对累计误差；

选用时刻ti前序M个时刻，1≤M≤i；获得M个水下航行器在时刻ti的当前位置的X分量的估计值。

3.根据权利要求1所述的方法，其特征在于，所述对由获得的M个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值 具体包括：对M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行降序或升序排序，去掉其中的最大的估计值和最小的估计值，完成对M+1个估计值的野点剔除后，对剩余的M-1个水下航行器在时刻ti的当前位置的X分量的估计值求均值，得到水下航行器当前位置的X分量的最优估计值 如公式(4)所示：其中， 为水声长基线定位综合误差的X分量； 为惯导组合定位综合误差的X分量；xi为水下航行器在时间点ti时的当前位置的坐标X分量。

4.根据权利要求1所述的方法，其特征在于，所述根据水下航行器前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量，获取M个对水下航行器的当前位置的Y分量的估计值；具体包括：根据公式(5)，获得水下航行器在时间点ti时的当前位置的坐标Y分量yi：其中，yj为水下航行器在时间点tj时的水下航行器的当前位置的坐标Y分量； 为水下航行器从时刻tj运动到时刻ti的相对位移量；

其中，通过公式(6)，获得水下航行器从时刻tj运动到时刻ti的相对位移量其中， 为在时间点ti，惯导组合定位结果的Y分量； 为在时间点tj，惯导组合定位结果的Y分量； 为时刻tj到时刻ti的惯导相对累计误差；

令

其中， 为惯导组合定位误差的Y分量 ；根据惯导系统的物理特性，为随着时间推移逐渐增大的累计误差； 为在时间点ti，惯导

组合定位结果的Y分量；

令

其中， 为惯导组合定位误差的Y分量；根据惯导系统的物理特性，为随着时间推移逐渐增大的累计误差； 为在时间点tj，惯导

组合定位结果的Y分量；

令

其中， 为水下航行器在时间点ti时的水下航行器的水声长基线定位结果的坐标Y分量，yi为水下航行器在时间点ti时的水下航行器的当前真实位置的坐标Y分量， 为水下航行器在时间点ti时的水下航行器的水声长基线定位误差的坐标Y分量；

因此，根据公式(7)，获得水下航行器在时刻ti的当前位置的Y分量的估计值其中， 为时刻tj到时刻ti的惯导相对累计误差；

选用时刻ti前序M个时刻，1≤M≤i；获得M个水下航行器在时刻ti的当前位置的Y分量的估计值。

5.根据权利要求1所述的方法，其特征在于，所述对由获得的M个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值 具体包括：M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行降序排序，去掉其中的最大的估计值和最小的估计值，完成对M+1个估计值的野点剔除后，对剩余的M-1个水下航行器在时刻ti的当前位置的Y分量的估计值求均值，得到水下航行器当前位置的Y分量的最优估计值 如公式(8)所示：其中， 为水声长基线定位综合误差的Y分量； 为惯导组合定位综合误差的Y分量；yi为水下航行器在时间点ti时的当前位置的坐标Y分量。

说明书

一种基于惯导修正的长基线导航定位方法

技术领域

[0001] 本发明属于水声导航定位技术领域，具体涉及一种基于惯导修正的长基线导航定位方法。

背景技术

[0002] 当前，水下航行器常采用多普勒计程仪与惯导系统进行组合导航定位技术，这种采用舰位推算进行导航定位的方法，短时间内具有较高的定位精度，但是，随着时间推移，定位误差不断积累。水下航行器在水下工作一定时间后，需要上浮接收GPS信号进行位置校正，该过程容易导致自身位置暴漏。

[0003] 另外，现有的水声导航定位方法通常采用长基线水声导航定位技术，该技术具有导航误差不随时间累积的特性；受复杂海洋环境影响，导航信号从发射位置传输到接收位置的平均传播速度存在一定的估计误差；受海洋多途特性的影响，导航信号从发射位置传输到接收位置的时延估计存在一定的估计误差；导航信号远距离传输时，受局部传输信道突发的涡、流等水声物理现象干扰，使得平均声速估计值与真实值相比存在较大误差，导致声速估计值与传播时延测量值不匹配，进而造成个别点水声长基线定位结果误差突然变大。

[0004] 将水声长基线导航定位与舰位推算类导航定位相结合，有利于提高导航定位精度、降低舰位推算类仪器设备精度的要求、增强水下航行器的隐蔽性。

发明内容

[0005] 本发明的目的在于，为解决现有的导航定位方法存在的上述缺陷，本发明提出了一种基于惯导修正的长基线导航定位方法，充分利用了惯导系统在短时间内能高精度获得水下航行器的相对位移量 和 的优点，采用惯导相对位移量修正水声长基线定位结果，提高水声定位的准确性和定位精度。

[0006] 为了实现上述目的，本发明提供了一种基于惯导修正的长基线导航定位方法，该方法包括：

[0007] 根据水下航行器时刻ti的前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量，获取M个对水下航行器的当前位置的X分量的估计值；

[0008] 对由获得的M个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值[0009] 根据水下航行器时刻ti的前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量，获取M个对水下航行器的当前位置的Y分量的估计值；

[0010] 对由获得的M个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值[0011] 根据获得的水下航行器当前位置的X分量的最优估计值和获得的水下航行器当前位置的Y分量的最优估计值，确定水下航行器当前位置；实现对水下航行器的导航定位。

[0012] 作为上述技术方案的改进之一，所述根据水下航行器前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量，获取M个对水下航行器的当前位置的X分量的估计值；具体包括：

[0013] 根据公式(1)，获得水下航行器在时间点ti时的当前位置的坐标X分量xi：

[0014]

[0015] 其中，xj为水下航行器在时间点tj时的水下航行器的当前位置的坐标X分量；为水下航行器从时刻tj运动到时刻ti的相对位移量；

[0016] 其中，通过公式(2)，获得水下航行器从时刻tj运动到时刻ti的相对位移量[0017]

[0018] 其中， 为在时间点ti，惯导组合定位结果的X分量； 为在时间点tj，惯导组合定位结果的X分量； 为时刻tj到时刻ti的惯导相对累计误差；

[0019] 令

[0020] 其中， 为惯导组合定位误差的X分量；根据惯导系统的物理特性，为随着时间推移逐渐增大的累计误差； 为在时间点ti，惯导组合定位结果的X分量；

[0021] 令

[0022] 其中， 为惯导组合定位误差的X分量；根据惯导系统的物理特性，为随着时间推移逐渐增大的累计误差； 为在时间点tj，惯导组合定位结果的X分量；

[0023] 令

[0024] 其中， 为水下航行器在时间点ti时的水下航行器的水声长基线定位结果的坐标X分量，xi为水下航行器在时间点ti时的水下航行器的当前真实位置的坐标X分量， 为水下航行器在时间点ti时的水下航行器的水声长基线定位误差的坐标X分量；

[0025] 因此，根据公式(3)，获得水下航行器在时刻ti的当前位置的X分量的估计值[0026]

[0027] 其中， 为时刻tj到时刻ti的惯导相对累计误差；

[0028] 选用时刻ti前序M个时刻，1≤M≤i；获得M个水下航行器在时刻ti的当前位置的X分量的估计值。

[0029] 作为上述技术方案的改进之一，所述对由获得的M个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值 具体包括：

[0030] 对M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行降序或升序排序，去掉其中的最大的估计值和最小的估计值，完成对M+1个估计值的野点剔除后，对剩余的M-1个水下航行器在时刻ti的当前位置的X分量的估计值求均值，得到水下航行器当前位置的X分量的最优估计值 如公式(4)所示：

[0031]

[0032] 其中， 为水声长基线定位综合误差的X分量； 为惯导组合定位综合误差的X分量。

[0033] 作为上述技术方案的改进之一，所述根据水下航行器前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量，获取M个对水下航行器的当前位置的Y分量的估计值；具体包括：

[0034] 根据公式(5)，获得水下航行器在时间点ti时的当前位置的坐标Y分量yi：

[0035]

[0036] 其中，yj为水下航行器在时间点tj时的水下航行器的当前位置的坐标Y分量；为水下航行器从时刻tj运动到时刻ti的相对位移量；

[0037] 其中，通过公式(6)，获得水下航行器从时刻tj运动到时刻ti的相对位移量[0038]

[0039] 其中， 为在时间点ti，惯导组合定位结果的Y分量； 为在时间点tj，惯导组合定位结果的Y分量； 为时刻tj到时刻ti的惯导相对累计误差；

[0040] 令

[0041] 其中， 为惯导组合定位误差的Y分量；根据惯导系统的物理特性，为随着时间推移逐渐增大的累计误差； 为在时间点ti，惯导组合定位结果的Y分量；

[0042] 令

[0043] 其中， 为惯导组合定位误差的Y分量；根据惯导系统的物理特性，为随着时间推移逐渐增大的累计误差； 为在时间点tj，惯导组合定位结果的Y分量；

[0044] 令

[0045] 其中， 为水下航行器在时间点ti时的水下航行器的水声长基线定位结果的坐标Y分量，yi为水下航行器在时间点ti时的水下航行器的当前真实位置的坐标Y分量， 为水下航行器在时间点ti时的水下航行器的水声长基线定位误差的坐标Y分量；

[0046] 因此，根据公式(7)，获得水下航行器在时刻ti的当前位置的Y分量的估计值[0047]

[0048] 其中， 为时刻tj到时刻ti的惯导相对累计误差；

[0049] 选用时刻ti前序M个时刻，1≤M≤i；获得M个水下航行器在时刻ti的当前位置的Y分量的估计值。

[0050] 作为上述技术方案的改进之一，所述对由获得的M个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值 具体包括：

[0051] M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行降序排序，去掉其中的最大的估计值和最小的估计值，完成对M+1个估计值的野点剔除后，对剩余的M-1个水下航行器在时刻ti的当前位置的Y分量的估计值求均值，得到水下航行器当前位置的Y分量的最优估计值 如公式(8)所示：

[0052]

[0053] 其中， 为水声长基线定位综合误差的Y分量； 为惯导组合定位综合误差的Y分量。

[0054] 本发明相比于现有技术的有益效果在于：

[0055] 本发明的方法充分利用了惯导系统在短时间内能高精度获得水下航行器相对位移量的优点，采用时间序列估计和数据融合的方法，获得组合导航定位结果的X分量和Y分量，并采用阶段修正的方法，消除惯导系统综合定位误差 和 对组合定位结果的X分量和Y分量的影响，以提高水声定位的准确性和定位精度。

附图说明

[0056] 图1是本发明的一种基于惯导修正的长基线导航定位方法的流程图；

[0057] 图2是利用本发明的一种基于惯导修正的长基线导航定位方法的一个实施例的发射船、水下航行器位置示意图；

[0058] 图3是本发明的一种基于惯导修正的长基线导航定位方法的一个实施例中所选用的惯导系统随时间增长其累积定位误差增长曲线示意图；

[0059] 图4是本发明的一种基于惯导修正的长基线导航定位方法的一个实施例中不同定位方法获得水下航行器坐标值与真实坐标值之间的距离误差对比示意图。

具体实施方式

[0060] 现结合附图对本发明作进一步的描述。

[0061] 如图1所示，本发明提出了一种基于惯导修正的长基线导航定位方法，该方法实施前所需要的条件如下：

[0062] 假设T＝[t1 t2 … ti]为水下航行器前序时间序列，ti为水下航行器的时间点；[(x1,y1) (x2,y2) … (xi,yi)]为水下航行器的当前真实位置；

[0063] 水下航行器的当前位置包括：水下航行器的当前真实位置、水声长基线定位误差和惯导组合定位误差；具体地，水下航行器的当前位置包括：水下航行器的当前位置的X分量和水下航行器的当前位置的Y分量；其中，水下航行器的当前位置的X分量包括：水下航行器的当前真实位置的X分量、水声长基线定位误差的X分量和惯导组合定位误差的X分量；水下航行器的当前位置的Y分量包括：水下航行器的当前真实位置的Y分量、水声长基线定位误差的Y分量和惯导组合定位误差的Y分量；

[0064] 假设水声长基线定位结果分别为 惯导定位结果分别为

[0065] 该方法包括：

[0066] 步骤1)根据水下航行器前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量，获取M个对水下航行器的当前位置的X分量的估计值；

[0067] 所述步骤1)具体包括：

[0068] 令水下航行器在时间点ti时的水下航行器的当前真实位置的坐标X分量为X＝[x1 x2 … xi]；则在时间点ti时，水声长基线定位结果的坐标X分量为在时间点ti，惯导组合定位结果的坐标X分量为

[0069] 令

[0070] 其中， 为水声长基线定位误差的X分量；xi为水下航行器在时间点ti时的水下航行器的当前真实位置的X分量； 为在时间点ti时，水声长基线定位结果的X分量；其中，为独立同分布随机变量；

[0071] 因此，根据公式(1)，获得水下航行器在时间点ti时的当前位置的坐标X分量xi：

[0072]

[0073] 其中，xj为水下航行器在时间点tj时的水下航行器的当前位置的坐标X分量；为水下航行器从时刻tj运动到时刻ti的相对位移量；

[0074] 其中，通过公式(2)，获得水下航行器从时刻tj运动到时刻ti的相对位移量[0075]

[0076] 其中， 为在时间点ti，惯导组合定位结果的X分量； 为在时间点tj，惯导组合定位结果的X分量； 为时刻tj到时刻ti的惯导相对累计误差；

[0077] 令

[0078] 其中， 为惯导组合定位误差的X分量；根据惯导系统的物理特性，为随着时间推移逐渐增大的累计误差； 为在时间点ti，惯导组合定位结果的X分量；

[0079] 令

[0080] 其中， 为惯导组合定位误差的X分量；根据惯导系统的物理特性，为随着时间推移逐渐增大的累计误差； 为在时间点tj，惯导组合定位结果的X分量；

[0081] 令

[0082] 其中， 为水下航行器在时间点ti时的水下航行器的水声长基线定位结果的坐标X分量，xi为水下航行器在时间点ti时的水下航行器的当前真实位置的坐标X分量， 为水下航行器在时间点ti时的水下航行器的水声长基线定位误差的坐标X分量；

[0083] 因此，根据公式(3)，获得水下航行器在时刻ti的当前位置的X分量的估计值[0084]

[0085] 其中， 为时刻tj到时刻ti的惯导相对累计误差；

[0086] 根据 获得估计误差的X分量为

[0087] 选用时刻ti前序M个时刻，1≤M≤i；获得M个水下航行器在时刻ti的当前位置的X分量的估计值。

[0088] 步骤2)对由获得的M个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值作为组合定位结果的X分量；

[0089] 具体地，为了去掉突变点，对M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行降序或升序排序，去掉其中的最大的估计值和最小的估计值，完成对M+1个估计值的野点剔除后，对剩余的M-1个水下航行器在时刻ti的当前位置的X分量的估计值求均值，得到水下航行器当前位置的X分量的最优估计值 如公式(4)所示：

[0090]

[0091] 其中， 为水声长基线定位综合误差的X分量；由于ΔXS为独立同分布随机变量，M+1次测量取均值，与单次测量相比，误差有所降低，且M越大，综合估计误差越小； 为惯导组合定位综合误差的X分量；由于惯导具有累计误差， 值随着k的增大而增大。

[0092] 实用中需要根据水声长基线定位系统与惯导系统的定位精度，综合考虑选取最优的M值。

[0093] 由于短时间内，惯导相对累计误差较小，组合定位结果的X分量 的误差的X分量比水声长基线定位误差的X分量 小，提高了估计精度。

[0094] 因此，在获取下一个时刻ti+1的水下航行器当前位置的X分量的最优估计值 时，根据该公式： 采用 代替 具体见如下公式：

[0095] (采用 代替 )

[0096] 有利于提高 的估计精度。其中， 为下一时刻ti+1的惯导组合定位综合误差的X分量。

[0097] 其中， 与 相比，引入了较大的惯导组合定位综合误差的X分量，引入的惯导组合定位综合误差的X分量作用到组合定位结果的X分量中，该惯导组合定位综合误差的X分量具有时间累积特性。随着时间的推移，在组合定位结果的X分量中，惯导组合定位综合误差的X分量不断增大，水声长基线定位综合误差的X分量不断减小，当惯导组合定位综合误差的X分量超过单次水声长基线定位综合误差的X分量时，惯导系统将失去对水声长基线定位结果的修正作用。因此，为了有效提高组合定位结果的X分量的估计精度，采用阶段修正的方法，当组合定位结果的X分量中惯导系统的惯导组合定位综合误差的X分量达到预先设定的阈值后，需重新将惯导系统置为相对零点，则阶段性地采用水声长基线定位结果的X分量代替组合定位结果的X分量，对后续定位结果的X分量进行估计，从而消除组合定位结果的X分量中惯导组合定位综合误差的X分量，使组合定位结果的X分量的估计精度得到了提高。

[0098] 步骤3)根据水下航行器前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量，获取M个对水下航行器的当前位置的Y分量的估计值；

[0099] 所述步骤3)具体包括：

[0100] 令水下航行器在时间点ti时的水下航行器的当前真实位置的坐标Y分量为Y＝[y1 y2 … yi]；则在时间点ti时，水声长基线定位结果的坐标Y分量为在时间点ti，惯导组合定位结果的坐标Y分量为

[0101] 令

[0102] 其中， 为水声长基线定位误差的Y分量；yi为水下航行器在时间点ti时的水下航行器的当前真实位置的Y分量； 为在时间点ti时，水声长基线定位结果的Y分量；其中，为独立同分布随机变量；

[0103] 因此，根据公式(5)，获得水下航行器在时间点ti时的当前位置的坐标Y分量yi：

[0104]

[0105] 其中，yj为水下航行器在时间点tj时的水下航行器的当前位置的坐标Y分量；为水下航行器从时刻tj运动到时刻ti的相对位移量；

[0106] 其中，通过公式(6)，获得水下航行器从时刻tj运动到时刻ti的相对位移量[0107]

[0108] 其中， 为在时间点ti，惯导组合定位结果的Y分量； 为在时间点tj，惯导组合定位结果的Y分量； 为时刻tj到时刻ti的惯导相对累计误差；

[0109] 令

[0110] 其中， 为惯导组合定位误差的Y分量；根据惯导系统的物理特性，为随着时间推移逐渐增大的累计误差； 为在时间点ti，惯导组合定位结果的Y分量；

[0111] 令

[0112] 其中， 为惯导组合定位误差的Y分量；根据惯导系统的物理特性，为随着时间推移逐渐增大的累计误差； 为在时间点tj，惯导组合定位结果的Y分量；

[0113] 令

[0114] 其中， 为水下航行器在时间点ti时的水下航行器的水声长基线定位结果的坐标Y分量，yi为水下航行器在时间点ti时的水下航行器的当前真实位置的坐标Y分量， 为水下航行器在时间点ti时的水下航行器的水声长基线定位误差的坐标Y分量；

[0115] 因此，根据公式(7)，获得水下航行器在时刻ti的当前位置的Y分量的估计值[0116]

[0117] 其中， 为时刻tj到时刻ti的惯导相对累计误差；

[0118] 根据 获得估计误差的Y分量为

[0119] 选用时刻ti前序M个时刻，1≤M≤i；获得M个水下航行器在时刻ti的当前位置的Y分量的估计值。

[0120] 步骤4)对由获得的M个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值作为组合定位结果的Y分量；

[0121] 具体地，为了去掉突变点，对M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行降序或升序排序，去掉其中的最大的估计值和最小的估计值，完成对M+1个估计值的野点剔除后，对剩余的M-1个水下航行器在时刻ti的当前位置的Y分量的估计值求均值，得到水下航行器当前位置的Y分量的最优估计值 如公式(8)所示：

[0122]

[0123] 其中， 为水声长基线定位综合误差的Y分量；由于ΔYS为独立同分布随机变量，M+1次测量取均值，与单次测量相比，误差有所降低，且M越大，综合估计误差越小； 为惯导组合定位综合误差的Y分量；由于惯导具有累计误差， 值随着k的增大而增大。

[0124] 实用中需要根据水声长基线定位系统与惯导系统的定位精度，综合考虑选取最优的M值。

[0125] 由于短时间内，惯导相对累计误差较小，组合定位结果的Y分量 的误差的Y分量比水声长基线定位误差的Y分量 小，提高了估计精度。

[0126] 因此，在获取下一个时刻ti+1的水下航行器当前位置的Y分量的最优估计值 时，根据该公式： 采用 代替 具体见如下公式：

[0127]

[0128] 有利于提高 的估计精度。其中， 为下一时刻ti+1的惯导组合定位综合误差的Y分量。

[0129] 其中， 与 相比，引入了较大的惯导组合定位综合误差的Y分量，引入的惯导组合定位综合误差的Y分量作用到组合定位结果的Y分量中，该惯导组合定位综合误差的Y分量具有时间累积特性。随着时间的推移，在组合定位结果的Y分量中，惯导组合定位综合误差的Y分量不断增大，水声长基线定位综合误差的Y分量不断减小，当惯导组合定位综合误差的Y分量超过单次水声长基线定位综合误差的Y分量时，惯导系统将失去对水声长基线定位结果的修正作用。因此，为了有效提高组合定位结果的Y分量的估计精度，采用阶段修正的方法，当组合定位结果的Y分量中惯导系统的惯导组合定位综合误差的Y分量达到预先设定的阈值后，需重新将惯导系统置为相对零点，则阶段性地采用水声长基线定位结果的Y分量代替组合定位结果的Y分量，对后续定位结果的Y分量进行估计，从而消除组合定位结果的Y分量中惯导组合定位综合误差的Y分量，使组合定位结果的Y分量的估计精度得到了提高。

[0130] 步骤5)根据步骤2)获得的水下航行器当前位置的X分量的最优估计值和步骤4)获得的水下航行器当前位置的Y分量的最优估计值，确定水下航行器当前位置，实现对水下航行器的导航定位；该位置要比水下航行器的当前水声长基线导航定位获得的位置更精确，误差更小。

[0131] 具体地，如图2所示，为了提高仿真结果的可靠性，设定共有三艘发射船和一艘水下航行器A，其中，第一艘发射船位于站点1，第二艘发射船位于站点2，第三艘发射船由L1运动至L2，站点3为运动轨迹L1-L2上的任一点，水下航行器在时刻ti的真实位置为(xi,yi)。

[0132] 在本实施例中，本发明提供了一种基于惯导修正的长基线导航定位方法，该方法具体包括：

[0133] 步骤1)根据水下航行器前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量，获取M个对水下航行器的当前位置的X分量的估计值；在本实施例中，M为7；

[0134] 步骤2)对由获得的7个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的8个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值 作为组合定位结果的X分量；具体地，去掉最大的估计值和最小的估计值，对剩下的6个估计值进行求平均；获得

[0135] 步骤3)根据水下航行器前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量，获取7个对水下航行器的当前位置的Y分量的估计值；

[0136] 步骤4)对由获得的7个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的8个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值，作为组合定位结果的Y分量；具体地，去掉最大的估计值和最小的估计值，对剩下的6个估计值进行求平均；获得

[0137] 步骤5)根据步骤2)获得的水下航行器当前位置的X分量的最优估计值和步骤4)获得的水下航行器当前位置的Y分量的最优估计值，确定水下航行器当前位置，实现对水下航行器的导航定位；

[0138] 水下航行器在时刻ti，基于惯导修正的长基线导航定位获得的水下航行器当前位置坐标值 与真实坐标值(xi,yi)之间的距离误差为

[0139] 其中，水下航行器A在时刻ti的水声长基线定位获得的位置坐标值 与真实坐标值(xi,yi)之间的距离误差为

[0140] 因此，基于惯导修正的长基线导航定位方法获得的水下航行器位置与真实位置之间的距离误差 要比基于水声长基线导航定位获得的水下航行器位置与真实位置之间的距离误差 要小，基于惯导修正的长基线导航定位方法获得的水下航行器位置要比水声长基线导航定位获得的位置更精确，误差更小。

[0141] 水下航行器A的水声长基线定位结果的误差曲线如图4中虚线所示，其产生的距离误差 的最大值为129.76m、最小值为15.78m、平均值为49.47m。本申请采用的惯导修正结果的误差曲线如图3所示，通过该惯导系统对水声长基线定位结果进行修正，前序时间序列长度M为7，获得组合定位结果如图4中实线所示，其产生的距离误差 的最大值为57.26m、最小值为2.05m、平均值为23.75m。因此，本发明的定位方法产生的距离误差 取平均值23.75m，而直接采用水声长基线定位结果的距离误差 取平均值49.47m；通过数据比较可知，本申请中的定位方法的距离误差要更小一些，其定位精度得到了显著提高。

[0142] 最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。