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

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

Info

Publication number
CN110542884B
CN110542884B CN201910800764.1A CN201910800764A CN110542884B CN 110542884 B CN110542884 B CN 110542884B CN 201910800764 A CN201910800764 A CN 201910800764A CN 110542884 B CN110542884 B CN 110542884B
Authority
CN
China
Prior art keywords
component
time
underwater
current position
inertial navigation
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
Application number
CN201910800764.1A
Other languages
English (en)
Other versions
CN110542884A (zh
Inventor
张海如
王海斌
汪俊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Institute of Acoustics CAS
Original Assignee
Institute of Acoustics CAS
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Institute of Acoustics CAS filed Critical Institute of Acoustics CAS
Priority to CN201910800764.1A priority Critical patent/CN110542884B/zh
Publication of CN110542884A publication Critical patent/CN110542884A/zh
Application granted granted Critical
Publication of CN110542884B publication Critical patent/CN110542884B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
    • G01S5/22—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
  • Navigation (AREA)

Abstract

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

Description

一种基于惯导修正的长基线导航定位方法
技术领域
本发明属于水声导航定位技术领域，具体涉及一种基于惯导修正的长基线导航定位方法。
背景技术
当前，水下航行器常采用多普勒计程仪与惯导系统进行组合导航定位技术，这种采用舰位推算进行导航定位的方法，短时间内具有较高的定位精度，但是，随着时间推移，定位误差不断积累。水下航行器在水下工作一定时间后，需要上浮接收GPS信号进行位置校正，该过程容易导致自身位置暴漏。
另外，现有的水声导航定位方法通常采用长基线水声导航定位技术，该技术具有导航误差不随时间累积的特性；受复杂海洋环境影响，导航信号从发射位置传输到接收位置的平均传播速度存在一定的估计误差；受海洋多途特性的影响，导航信号从发射位置传输到接收位置的时延估计存在一定的估计误差；导航信号远距离传输时，受局部传输信道突发的涡、流等水声物理现象干扰，使得平均声速估计值与真实值相比存在较大误差，导致声速估计值与传播时延测量值不匹配，进而造成个别点水声长基线定位结果误差突然变大。
将水声长基线导航定位与舰位推算类导航定位相结合，有利于提高导航定位精度、降低舰位推算类仪器设备精度的要求、增强水下航行器的隐蔽性。
发明内容
本发明的目的在于，为解决现有的导航定位方法存在的上述缺陷，本发明提出了一种基于惯导修正的长基线导航定位方法，充分利用了惯导系统在短时间内能高精度获得水下航行器的相对位移量
Figure GDA0002675599640000011
和
Figure GDA0002675599640000012
的优点，采用惯导相对位移量修正水声长基线定位结果，提高水声定位的准确性和定位精度。
为了实现上述目的，本发明提供了一种基于惯导修正的长基线导航定位方法，该方法包括：
根据水下航行器时刻ti的前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量，获取M个对水下航行器的当前位置的X分量的估计值；
对由获得的M个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值
Figure GDA0002675599640000021
根据水下航行器时刻ti的前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量，获取M个对水下航行器的当前位置的Y分量的估计值；
对由获得的M个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值
Figure GDA0002675599640000022
根据获得的水下航行器当前位置的X分量的最优估计值和获得的水下航行器当前位置的Y分量的最优估计值，确定水下航行器当前位置；实现对水下航行器的导航定位。
作为上述技术方案的改进之一，所述根据水下航行器前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量，获取M个对水下航行器的当前位置的X分量的估计值；具体包括：
根据公式(1)，获得水下航行器在时间点ti时的当前位置的坐标X分量xi：
Figure GDA0002675599640000023
其中，xj为水下航行器在时间点tj时的水下航行器的当前位置的坐标X分量；
Figure GDA0002675599640000024
为水下航行器从时刻tj运动到时刻ti的相对位移量；
其中，通过公式(2)，获得水下航行器从时刻tj运动到时刻ti的相对位移量
Figure GDA0002675599640000025
Figure GDA0002675599640000026
其中，
Figure GDA0002675599640000027
为在时间点ti，惯导组合定位结果的X分量；
Figure GDA0002675599640000028
为在时间点tj，惯导组合定位结果的X分量；
Figure GDA0002675599640000029
为时刻tj到时刻ti的惯导相对累计误差；
令
Figure GDA00026755996400000210
其中，
Figure GDA00026755996400000211
为惯导组合定位误差的X分量；根据惯导系统的物理特性，
Figure GDA0002675599640000031
为随着时间推移逐渐增大的累计误差；
Figure GDA0002675599640000032
为在时间点ti，惯导组合定位结果的X分量；
令
Figure GDA0002675599640000033
其中，
Figure GDA0002675599640000034
为惯导组合定位误差的X分量；根据惯导系统的物理特性，
Figure GDA0002675599640000035
为随着时间推移逐渐增大的累计误差；
Figure GDA0002675599640000036
为在时间点tj，惯导组合定位结果的X分量；
令
Figure GDA0002675599640000037
其中，
Figure GDA0002675599640000038
为水下航行器在时间点ti时的水下航行器的水声长基线定位结果的坐标X分量，xi为水下航行器在时间点ti时的水下航行器的当前真实位置的坐标X分量，
Figure GDA0002675599640000039
为水下航行器在时间点ti时的水下航行器的水声长基线定位误差的坐标X分量；
因此，根据公式(3)，获得水下航行器在时刻ti的当前位置的X分量的估计值
Figure GDA00026755996400000310
Figure GDA00026755996400000311
其中，
Figure GDA00026755996400000312
为时刻tj到时刻ti的惯导相对累计误差；
选用时刻ti前序M个时刻，1≤M≤i；获得M个水下航行器在时刻ti的当前位置的X分量的估计值。
作为上述技术方案的改进之一，所述对由获得的M个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值
Figure GDA00026755996400000313
具体包括：
对M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行降序或升序排序，去掉其中的最大的估计值和最小的估计值，完成对M+1个估计值的野点剔除后，对剩余的M-1个水下航行器在时刻ti的当前位置的X分量的估计值求均值，得到水下航行器当前位置的X分量的最优估计值
Figure GDA0002675599640000041
如公式(4)所示：
Figure GDA0002675599640000042
其中，
Figure GDA0002675599640000043
为水声长基线定位综合误差的X分量；
Figure GDA0002675599640000044
为惯导组合定位综合误差的X分量。
作为上述技术方案的改进之一，所述根据水下航行器前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量，获取M个对水下航行器的当前位置的Y分量的估计值；具体包括：
根据公式(5)，获得水下航行器在时间点ti时的当前位置的坐标Y分量yi：
Figure GDA0002675599640000045
其中，yj为水下航行器在时间点tj时的水下航行器的当前位置的坐标Y分量；
Figure GDA0002675599640000046
为水下航行器从时刻tj运动到时刻ti的相对位移量；
其中，通过公式(6)，获得水下航行器从时刻tj运动到时刻ti的相对位移量
Figure GDA0002675599640000047
Figure GDA0002675599640000048
其中，
Figure GDA0002675599640000049
为在时间点ti，惯导组合定位结果的Y分量；
Figure GDA00026755996400000410
为在时间点tj，惯导组合定位结果的Y分量；
Figure GDA00026755996400000411
为时刻tj到时刻ti的惯导相对累计误差；
令
Figure GDA00026755996400000412
其中，
Figure GDA00026755996400000413
为惯导组合定位误差的Y分量；根据惯导系统的物理特性，
Figure GDA00026755996400000414
为随着时间推移逐渐增大的累计误差；
Figure GDA00026755996400000415
为在时间点ti，惯导组合定位结果的Y分量；
令
Figure GDA00026755996400000416
其中，
Figure GDA00026755996400000417
为惯导组合定位误差的Y分量；根据惯导系统的物理特性，
Figure GDA00026755996400000418
为随着时间推移逐渐增大的累计误差；
Figure GDA00026755996400000419
为在时间点tj，惯导组合定位结果的Y分量；
令
Figure GDA0002675599640000051
其中，
Figure GDA0002675599640000052
为水下航行器在时间点ti时的水下航行器的水声长基线定位结果的坐标Y分量，yi为水下航行器在时间点ti时的水下航行器的当前真实位置的坐标Y分量，
Figure GDA0002675599640000053
为水下航行器在时间点ti时的水下航行器的水声长基线定位误差的坐标Y分量；
因此，根据公式(7)，获得水下航行器在时刻ti的当前位置的Y分量的估计值
Figure GDA0002675599640000054
Figure GDA0002675599640000055
其中，
Figure GDA0002675599640000056
为时刻tj到时刻ti的惯导相对累计误差；
选用时刻ti前序M个时刻，1≤M≤i；获得M个水下航行器在时刻ti的当前位置的Y分量的估计值。
作为上述技术方案的改进之一，所述对由获得的M个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值
Figure GDA0002675599640000057
具体包括：
M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行降序排序，去掉其中的最大的估计值和最小的估计值，完成对M+1个估计值的野点剔除后，对剩余的M-1个水下航行器在时刻ti的当前位置的Y分量的估计值求均值，得到水下航行器当前位置的Y分量的最优估计值
Figure GDA0002675599640000058
如公式(8)所示：
Figure GDA0002675599640000059
其中，
Figure GDA00026755996400000510
为水声长基线定位综合误差的Y分量；
Figure GDA00026755996400000511
为惯导组合定位综合误差的Y分量。
本发明相比于现有技术的有益效果在于：
本发明的方法充分利用了惯导系统在短时间内能高精度获得水下航行器相对位移量的优点，采用时间序列估计和数据融合的方法，获得组合导航定位结果的X分量和Y分量，并采用阶段修正的方法，消除惯导系统综合定位误差
Figure GDA0002675599640000061
和
Figure GDA0002675599640000062
对组合定位结果的X分量和Y分量的影响，以提高水声定位的准确性和定位精度。
附图说明
图1是本发明的一种基于惯导修正的长基线导航定位方法的流程图；
图2是利用本发明的一种基于惯导修正的长基线导航定位方法的一个实施例的发射船、水下航行器位置示意图；
图3是本发明的一种基于惯导修正的长基线导航定位方法的一个实施例中所选用的惯导系统随时间增长其累积定位误差增长曲线示意图；
图4是本发明的一种基于惯导修正的长基线导航定位方法的一个实施例中不同定位方法获得水下航行器坐标值与真实坐标值之间的距离误差对比示意图。
具体实施方式
现结合附图对本发明作进一步的描述。
如图1所示，本发明提出了一种基于惯导修正的长基线导航定位方法，该方法实施前所需要的条件如下：
假设T＝[t1 t2 … ti]为水下航行器前序时间序列，ti为水下航行器的时间点；[(x1,y1) (x2,y2) … (xi,yi)]为水下航行器的当前真实位置；
水下航行器的当前位置包括：水下航行器的当前真实位置、水声长基线定位误差和惯导组合定位误差；具体地，水下航行器的当前位置包括：水下航行器的当前位置的X分量和水下航行器的当前位置的Y分量；其中，水下航行器的当前位置的X分量包括：水下航行器的当前真实位置的X分量、水声长基线定位误差的X分量和惯导组合定位误差的X分量；水下航行器的当前位置的Y分量包括：水下航行器的当前真实位置的Y分量、水声长基线定位误差的Y分量和惯导组合定位误差的Y分量；
假设水声长基线定位结果分别为
Figure GDA0002675599640000071
惯导定位结果分别为
Figure GDA0002675599640000072
该方法包括：
步骤1)根据水下航行器前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量，获取M个对水下航行器的当前位置的X分量的估计值；
所述步骤1)具体包括：
令水下航行器在时间点ti时的水下航行器的当前真实位置的坐标X分量为X＝[x1x2 … xi]；则在时间点ti时，水声长基线定位结果的坐标X分量为
Figure GDA0002675599640000073
在时间点ti，惯导组合定位结果的坐标X分量为
Figure GDA0002675599640000074
令
Figure GDA0002675599640000075
其中，
Figure GDA0002675599640000076
为水声长基线定位误差的X分量；xi为水下航行器在时间点ti时的水下航行器的当前真实位置的X分量；
Figure GDA0002675599640000077
为在时间点ti时，水声长基线定位结果的X分量；其中，
Figure GDA0002675599640000078
为独立同分布随机变量；
因此，根据公式(1)，获得水下航行器在时间点ti时的当前位置的坐标X分量xi：
Figure GDA0002675599640000079
其中，xj为水下航行器在时间点tj时的水下航行器的当前位置的坐标X分量；
Figure GDA00026755996400000710
为水下航行器从时刻tj运动到时刻ti的相对位移量；
其中，通过公式(2)，获得水下航行器从时刻tj运动到时刻ti的相对位移量
Figure GDA00026755996400000711
Figure GDA00026755996400000712
其中，
Figure GDA00026755996400000713
为在时间点ti，惯导组合定位结果的X分量；
Figure GDA00026755996400000714
为在时间点tj，惯导组合定位结果的X分量；
Figure GDA00026755996400000715
为时刻tj到时刻ti的惯导相对累计误差；
令
Figure GDA00026755996400000716
其中，
Figure GDA0002675599640000081
为惯导组合定位误差的X分量；根据惯导系统的物理特性，
Figure GDA0002675599640000082
为随着时间推移逐渐增大的累计误差；
Figure GDA0002675599640000083
为在时间点ti，惯导组合定位结果的X分量；
令
Figure GDA0002675599640000084
其中，
Figure GDA0002675599640000085
为惯导组合定位误差的X分量；根据惯导系统的物理特性，
Figure GDA0002675599640000086
为随着时间推移逐渐增大的累计误差；
Figure GDA0002675599640000087
为在时间点tj，惯导组合定位结果的X分量；
令
Figure GDA0002675599640000088
其中，
Figure GDA0002675599640000089
为水下航行器在时间点ti时的水下航行器的水声长基线定位结果的坐标X分量，xi为水下航行器在时间点ti时的水下航行器的当前真实位置的坐标X分量，
Figure GDA00026755996400000810
为水下航行器在时间点ti时的水下航行器的水声长基线定位误差的坐标X分量；
因此，根据公式(3)，获得水下航行器在时刻ti的当前位置的X分量的估计值
Figure GDA00026755996400000811
Figure GDA00026755996400000812
其中，
Figure GDA00026755996400000813
为时刻tj到时刻ti的惯导相对累计误差；
根据
Figure GDA00026755996400000814
获得估计误差的X分量为
Figure GDA00026755996400000815
选用时刻ti前序M个时刻，1≤M≤i；获得M个水下航行器在时刻ti的当前位置的X分量的估计值。
步骤2)对由获得的M个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值
Figure GDA00026755996400000816
作为组合定位结果的X分量；
具体地，为了去掉突变点，对M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行降序或升序排序，去掉其中的最大的估计值和最小的估计值，完成对M+1个估计值的野点剔除后，对剩余的M-1个水下航行器在时刻ti的当前位置的X分量的估计值求均值，得到水下航行器当前位置的X分量的最优估计值
Figure GDA0002675599640000091
如公式(4)所示：
Figure GDA0002675599640000092
其中，
Figure GDA0002675599640000093
为水声长基线定位综合误差的X分量；由于ΔXS为独立同分布随机变量，M+1次测量取均值，与单次测量相比，误差有所降低，且M越大，综合估计误差越小；
Figure GDA0002675599640000094
为惯导组合定位综合误差的X分量；由于惯导具有累计误差，
Figure GDA0002675599640000095
值随着k的增大而增大。
实用中需要根据水声长基线定位系统与惯导系统的定位精度，综合考虑选取最优的M值。
由于短时间内，惯导相对累计误差较小，组合定位结果的X分量
Figure GDA0002675599640000096
的误差的X分量
Figure GDA0002675599640000097
比水声长基线定位误差的X分量
Figure GDA0002675599640000098
小，提高了估计精度。
因此，在获取下一个时刻ti+1的水下航行器当前位置的X分量的最优估计值
Figure GDA0002675599640000099
时，根据该公式：
Figure GDA00026755996400000910
采用
Figure GDA00026755996400000911
代替
Figure GDA00026755996400000912
具体见如下公式：
Figure GDA00026755996400000913
(采用
Figure GDA00026755996400000914
代替
Figure GDA00026755996400000915
)
有利于提高
Figure GDA00026755996400000916
的估计精度。其中，
Figure GDA00026755996400000917
为下一时刻ti+1的惯导组合定位综合误差的X分量。
其中，
Figure GDA00026755996400000918
与
Figure GDA00026755996400000919
相比，引入了较大的惯导组合定位综合误差的X分量，引入的惯导组合定位综合误差的X分量作用到组合定位结果的X分量中，该惯导组合定位综合误差的X分量具有时间累积特性。随着时间的推移，在组合定位结果的X分量中，惯导组合定位综合误差的X分量不断增大，水声长基线定位综合误差的X分量不断减小，当惯导组合定位综合误差的X分量超过单次水声长基线定位综合误差的X分量时，惯导系统将失去对水声长基线定位结果的修正作用。因此，为了有效提高组合定位结果的X分量的估计精度，采用阶段修正的方法，当组合定位结果的X分量中惯导系统的惯导组合定位综合误差的X分量达到预先设定的阈值后，需重新将惯导系统置为相对零点，则阶段性地采用水声长基线定位结果的X分量代替组合定位结果的X分量，对后续定位结果的X分量进行估计，从而消除组合定位结果的X分量中惯导组合定位综合误差的X分量，使组合定位结果的X分量的估计精度得到了提高。
步骤3)根据水下航行器前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量，获取M个对水下航行器的当前位置的Y分量的估计值；
所述步骤3)具体包括：
令水下航行器在时间点ti时的水下航行器的当前真实位置的坐标Y分量为Y＝[y1y2 … yi]；则在时间点ti时，水声长基线定位结果的坐标Y分量为
Figure GDA0002675599640000101
在时间点ti，惯导组合定位结果的坐标Y分量为
Figure GDA0002675599640000102
令
Figure GDA0002675599640000103
其中，
Figure GDA0002675599640000104
为水声长基线定位误差的Y分量；yi为水下航行器在时间点ti时的水下航行器的当前真实位置的Y分量；
Figure GDA0002675599640000105
为在时间点ti时，水声长基线定位结果的Y分量；其中，
Figure GDA0002675599640000106
为独立同分布随机变量；
因此，根据公式(5)，获得水下航行器在时间点ti时的当前位置的坐标Y分量yi：
Figure GDA0002675599640000107
其中，yj为水下航行器在时间点tj时的水下航行器的当前位置的坐标Y分量；
Figure GDA0002675599640000108
为水下航行器从时刻tj运动到时刻ti的相对位移量；
其中，通过公式(6)，获得水下航行器从时刻tj运动到时刻ti的相对位移量
Figure GDA0002675599640000109
Figure GDA00026755996400001010
其中，
Figure GDA00026755996400001011
为在时间点ti，惯导组合定位结果的Y分量；
Figure GDA00026755996400001012
为在时间点tj，惯导组合定位结果的Y分量；
Figure GDA0002675599640000111
为时刻tj到时刻ti的惯导相对累计误差；
令
Figure GDA0002675599640000112
其中，
Figure GDA0002675599640000113
为惯导组合定位误差的Y分量；根据惯导系统的物理特性，
Figure GDA0002675599640000114
为随着时间推移逐渐增大的累计误差；
Figure GDA0002675599640000115
为在时间点ti，惯导组合定位结果的Y分量；
令
Figure GDA0002675599640000116
其中，
Figure GDA0002675599640000117
为惯导组合定位误差的Y分量；根据惯导系统的物理特性，
Figure GDA0002675599640000118
为随着时间推移逐渐增大的累计误差；
Figure GDA0002675599640000119
为在时间点tj，惯导组合定位结果的Y分量；
令
Figure GDA00026755996400001110
其中，
Figure GDA00026755996400001111
为水下航行器在时间点ti时的水下航行器的水声长基线定位结果的坐标Y分量，yi为水下航行器在时间点ti时的水下航行器的当前真实位置的坐标Y分量，
Figure GDA00026755996400001112
为水下航行器在时间点ti时的水下航行器的水声长基线定位误差的坐标Y分量；
因此，根据公式(7)，获得水下航行器在时刻ti的当前位置的Y分量的估计值
Figure GDA00026755996400001113
Figure GDA00026755996400001114
其中，
Figure GDA00026755996400001115
为时刻tj到时刻ti的惯导相对累计误差；
根据
Figure GDA00026755996400001116
获得估计误差的Y分量为
Figure GDA00026755996400001117
选用时刻ti前序M个时刻，1≤M≤i；获得M个水下航行器在时刻ti的当前位置的Y分量的估计值。
步骤4)对由获得的M个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值
Figure GDA0002675599640000121
作为组合定位结果的Y分量；
具体地，为了去掉突变点，对M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行降序或升序排序，去掉其中的最大的估计值和最小的估计值，完成对M+1个估计值的野点剔除后，对剩余的M-1个水下航行器在时刻ti的当前位置的Y分量的估计值求均值，得到水下航行器当前位置的Y分量的最优估计值
Figure GDA0002675599640000122
如公式(8)所示：
Figure GDA0002675599640000123
其中，
Figure GDA0002675599640000124
为水声长基线定位综合误差的Y分量；由于ΔYS为独立同分布随机变量，M+1次测量取均值，与单次测量相比，误差有所降低，且M越大，综合估计误差越小；
Figure GDA0002675599640000125
为惯导组合定位综合误差的Y分量；由于惯导具有累计误差，
Figure GDA0002675599640000126
值随着k的增大而增大。
实用中需要根据水声长基线定位系统与惯导系统的定位精度，综合考虑选取最优的M值。
由于短时间内，惯导相对累计误差较小，组合定位结果的Y分量
Figure GDA0002675599640000127
的误差的Y分量
Figure GDA0002675599640000128
比水声长基线定位误差的Y分量
Figure GDA0002675599640000129
小，提高了估计精度。
因此，在获取下一个时刻ti+1的水下航行器当前位置的Y分量的最优估计值
Figure GDA00026755996400001210
时，根据该公式：
Figure GDA00026755996400001211
采用
Figure GDA00026755996400001212
代替
Figure GDA00026755996400001213
具体见如下公式：
Figure GDA00026755996400001214
有利于提高
Figure GDA00026755996400001215
的估计精度。其中，
Figure GDA00026755996400001216
为下一时刻ti+1的惯导组合定位综合误差的Y分量。
其中，
Figure GDA00026755996400001217
与
Figure GDA00026755996400001218
相比，引入了较大的惯导组合定位综合误差的Y分量，引入的惯导组合定位综合误差的Y分量作用到组合定位结果的Y分量中，该惯导组合定位综合误差的Y分量具有时间累积特性。随着时间的推移，在组合定位结果的Y分量中，惯导组合定位综合误差的Y分量不断增大，水声长基线定位综合误差的Y分量不断减小，当惯导组合定位综合误差的Y分量超过单次水声长基线定位综合误差的Y分量时，惯导系统将失去对水声长基线定位结果的修正作用。因此，为了有效提高组合定位结果的Y分量的估计精度，采用阶段修正的方法，当组合定位结果的Y分量中惯导系统的惯导组合定位综合误差的Y分量达到预先设定的阈值后，需重新将惯导系统置为相对零点，则阶段性地采用水声长基线定位结果的Y分量代替组合定位结果的Y分量，对后续定位结果的Y分量进行估计，从而消除组合定位结果的Y分量中惯导组合定位综合误差的Y分量，使组合定位结果的Y分量的估计精度得到了提高。
步骤5)根据步骤2)获得的水下航行器当前位置的X分量的最优估计值和步骤4)获得的水下航行器当前位置的Y分量的最优估计值，确定水下航行器当前位置，实现对水下航行器的导航定位；该位置要比水下航行器的当前水声长基线导航定位获得的位置更精确，误差更小。
具体地，如图2所示，为了提高仿真结果的可靠性，设定共有三艘发射船和一艘水下航行器A，其中，第一艘发射船位于站点1，第二艘发射船位于站点2，第三艘发射船由L1运动至L2，站点3为运动轨迹L1-L2上的任一点，水下航行器在时刻ti的真实位置为(xi,yi)。
在本实施例中，本发明提供了一种基于惯导修正的长基线导航定位方法，该方法具体包括：
步骤1)根据水下航行器前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量，获取M个对水下航行器的当前位置的X分量的估计值；在本实施例中，M为7；
步骤2)对由获得的7个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的8个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值
Figure GDA0002675599640000131
作为组合定位结果的X分量；具体地，去掉最大的估计值和最小的估计值，对剩下的6个估计值进行求平均；获得
Figure GDA0002675599640000132
步骤3)根据水下航行器前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量，获取7个对水下航行器的当前位置的Y分量的估计值；
步骤4)对由获得的7个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的8个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值，作为组合定位结果的Y分量；具体地，去掉最大的估计值和最小的估计值，对剩下的6个估计值进行求平均；获得
Figure GDA0002675599640000141
步骤5)根据步骤2)获得的水下航行器当前位置的X分量的最优估计值和步骤4)获得的水下航行器当前位置的Y分量的最优估计值，确定水下航行器当前位置，实现对水下航行器的导航定位；
水下航行器在时刻ti，基于惯导修正的长基线导航定位获得的水下航行器当前位置坐标值
Figure GDA0002675599640000142
与真实坐标值(xi,yi)之间的距离误差为
Figure GDA0002675599640000143
其中，水下航行器A在时刻ti的水声长基线定位获得的位置坐标值
Figure GDA0002675599640000144
与真实坐标值(xi,yi)之间的距离误差为
Figure GDA0002675599640000145
因此，基于惯导修正的长基线导航定位方法获得的水下航行器位置与真实位置之间的距离误差
Figure GDA0002675599640000146
要比基于水声长基线导航定位获得的水下航行器位置与真实位置之间的距离误差要小，基于惯导修正的长基线导航定位方法获得的水下航行器位置要比水声长基线导航定位获得的位置更精确，误差更小。
水下航行器A的水声长基线定位结果的误差曲线如图4中虚线所示，其产生的距离误差
Figure GDA0002675599640000148
的最大值为129.76m、最小值为15.78m、平均值为49.47m。本申请采用的惯导修正结果的误差曲线如图3所示，通过该惯导系统对水声长基线定位结果进行修正，前序时间序列长度M为7，获得组合定位结果如图4中实线所示，其产生的距离误差
Figure GDA0002675599640000149
的最大值为57.26m、最小值为2.05m、平均值为23.75m。因此，本发明的定位方法产生的距离误差
Figure GDA00026755996400001410
取平均值23.75m，而直接采用水声长基线定位结果的距离误差
Figure GDA00026755996400001411
取平均值49.47m；通过数据比较可知，本申请中的定位方法的距离误差要更小一些，其定位精度得到了显著提高。
最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (5)

1.一种基于惯导修正的长基线导航定位方法，其特征在于，该方法包括：
根据水下航行器时刻ti的前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量，获取M个对水下航行器的当前位置的X分量的估计值；
对由获得的M个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值
Figure FDA0002661085870000011
根据水下航行器时刻ti的前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量，获取M个对水下航行器的当前位置的Y分量的估计值；
对由获得的M个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值
Figure FDA0002661085870000012
根据获得的水下航行器当前位置的X分量的最优估计值和获得的水下航行器当前位置的Y分量的最优估计值，确定水下航行器当前位置；实现对水下航行器的导航定位。
2.根据权利要求1所述的方法，其特征在于，所述根据水下航行器前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量，获取M个对水下航行器的当前位置的X分量的估计值；具体包括：
根据公式(1)，获得水下航行器在时间点ti时的当前位置的坐标X分量xi：
Figure FDA0002661085870000013
其中，xj为水下航行器在时间点tj时的水下航行器的当前位置的坐标X分量；
Figure FDA0002661085870000014
为水下航行器从时刻tj运动到时刻ti的相对位移量；
其中，通过公式(2)，获得水下航行器从时刻tj运动到时刻ti的相对位移量
Figure FDA0002661085870000015
Figure FDA0002661085870000016
其中，
Figure FDA0002661085870000021
为在时间点ti，惯导组合定位结果的X分量；
Figure FDA0002661085870000022
为在时间点tj，惯导组合定位结果的X分量；
Figure FDA0002661085870000023
为时刻tj到时刻ti的惯导相对累计误差；
令
Figure FDA0002661085870000024
其中，
Figure FDA0002661085870000025
为惯导组合定位误差的X分量；根据惯导系统的物理特性，
Figure FDA0002661085870000026
为随着时间推移逐渐增大的累计误差；
Figure FDA0002661085870000027
为在时间点ti，惯导组合定位结果的X分量；
令
Figure FDA0002661085870000028
其中，
Figure FDA0002661085870000029
为惯导组合定位误差的X分量；根据惯导系统的物理特性，
Figure FDA00026610858700000210
为随着时间推移逐渐增大的累计误差；
Figure FDA00026610858700000211
为在时间点tj，惯导组合定位结果的X分量；
令
Figure FDA00026610858700000212
其中，
Figure FDA00026610858700000213
为水下航行器在时间点ti时的水下航行器的水声长基线定位结果的坐标X分量，xi为水下航行器在时间点ti时的水下航行器的当前真实位置的坐标X分量，
Figure FDA00026610858700000214
为水下航行器在时间点ti时的水下航行器的水声长基线定位误差的坐标X分量；
因此，根据公式(3)，获得水下航行器在时刻ti的当前位置的X分量的估计值
Figure FDA00026610858700000215
Figure FDA00026610858700000216
其中，
Figure FDA00026610858700000217
为时刻tj到时刻ti的惯导相对累计误差；
选用时刻ti前序M个时刻，1≤M≤i；获得M个水下航行器在时刻ti的当前位置的X分量的估计值。
3.根据权利要求1所述的方法，其特征在于，所述对由获得的M个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的X分量的最优估计值
Figure FDA0002661085870000031
具体包括：
对M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行降序或升序排序，去掉其中的最大的估计值和最小的估计值，完成对M+1个估计值的野点剔除后，对剩余的M-1个水下航行器在时刻ti的当前位置的X分量的估计值求均值，得到水下航行器当前位置的X分量的最优估计值
Figure FDA0002661085870000032
如公式(4)所示：
Figure FDA0002661085870000033
其中，
Figure FDA0002661085870000034
为水声长基线定位综合误差的X分量；
Figure FDA0002661085870000035
为惯导组合定位综合误差的X分量；xi为水下航行器在时间点ti时的当前位置的坐标X分量。
4.根据权利要求1所述的方法，其特征在于，所述根据水下航行器前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量，获取M个对水下航行器的当前位置的Y分量的估计值；具体包括：
根据公式(5)，获得水下航行器在时间点ti时的当前位置的坐标Y分量yi：
Figure FDA0002661085870000036
其中，yj为水下航行器在时间点tj时的水下航行器的当前位置的坐标Y分量；
Figure FDA0002661085870000037
为水下航行器从时刻tj运动到时刻ti的相对位移量；
其中，通过公式(6)，获得水下航行器从时刻tj运动到时刻ti的相对位移量
Figure FDA0002661085870000038
Figure FDA0002661085870000039
其中，
Figure FDA00026610858700000310
为在时间点ti，惯导组合定位结果的Y分量；
Figure FDA00026610858700000311
为在时间点tj，惯导组合定位结果的Y分量；
Figure FDA00026610858700000312
为时刻tj到时刻ti的惯导相对累计误差；
令
Figure FDA00026610858700000313
其中，
Figure FDA00026610858700000314
为惯导组合定位误差的Y分量；根据惯导系统的物理特性，
Figure FDA00026610858700000315
为随着时间推移逐渐增大的累计误差；
Figure FDA00026610858700000316
为在时间点ti，惯导组合定位结果的Y分量；
令
Figure FDA0002661085870000041
其中，
Figure FDA0002661085870000042
为惯导组合定位误差的Y分量；根据惯导系统的物理特性，
Figure FDA0002661085870000043
为随着时间推移逐渐增大的累计误差；
Figure FDA0002661085870000044
为在时间点tj，惯导组合定位结果的Y分量；
令
Figure FDA0002661085870000045
其中，
Figure FDA0002661085870000046
为水下航行器在时间点ti时的水下航行器的水声长基线定位结果的坐标Y分量，yi为水下航行器在时间点ti时的水下航行器的当前真实位置的坐标Y分量，
Figure FDA0002661085870000047
为水下航行器在时间点ti时的水下航行器的水声长基线定位误差的坐标Y分量；
因此，根据公式(7)，获得水下航行器在时刻ti的当前位置的Y分量的估计值
Figure FDA0002661085870000048
Figure FDA0002661085870000049
其中，
Figure FDA00026610858700000410
为时刻tj到时刻ti的惯导相对累计误差；
选用时刻ti前序M个时刻，1≤M≤i；获得M个水下航行器在时刻ti的当前位置的Y分量的估计值。
5.根据权利要求1所述的方法，其特征在于，所述对由获得的M个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均，获得水下航行器当前位置的Y分量的最优估计值
Figure FDA00026610858700000411
具体包括：
M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行降序排序，去掉其中的最大的估计值和最小的估计值，完成对M+1个估计值的野点剔除后，对剩余的M-1个水下航行器在时刻ti的当前位置的Y分量的估计值求均值，得到水下航行器当前位置的Y分量的最优估计值
Figure FDA00026610858700000412
如公式(8)所示：
Figure FDA0002661085870000051
其中，
Figure FDA0002661085870000052
为水声长基线定位综合误差的Y分量；
Figure FDA0002661085870000053
为惯导组合定位综合误差的Y分量；yi为水下航行器在时间点ti时的当前位置的坐标Y分量。
CN201910800764.1A 2019-08-28 2019-08-28 一种基于惯导修正的长基线导航定位方法 Active CN110542884B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910800764.1A CN110542884B (zh) 2019-08-28 2019-08-28 一种基于惯导修正的长基线导航定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910800764.1A CN110542884B (zh) 2019-08-28 2019-08-28 一种基于惯导修正的长基线导航定位方法

Publications (2)

Publication Number Publication Date
CN110542884A CN110542884A (zh) 2019-12-06
CN110542884B true CN110542884B (zh) 2020-11-06

Family

ID=68710725

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910800764.1A Active CN110542884B (zh) 2019-08-28 2019-08-28 一种基于惯导修正的长基线导航定位方法

Country Status (1)

Country Link
CN (1) CN110542884B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104280026A (zh) * 2013-07-08 2015-01-14 中国科学院沈阳自动化研究所 基于aukf的深海机器人长基线组合导航方法
CN104457754A (zh) * 2014-12-19 2015-03-25 东南大学 一种基于sins/lbl紧组合的auv水下导航定位方法
CN105547289A (zh) * 2015-12-16 2016-05-04 南京信息工程大学 一种水下航行器组合导航系统及导航信息融合方法
CN106017467A (zh) * 2016-07-28 2016-10-12 中国船舶重工集团公司第七0七研究所 一种基于多水下应答器的惯性/水声组合导航方法
CN106767793A (zh) * 2017-01-19 2017-05-31 东南大学 一种基于sins/usbl紧组合的auv水下导航定位方法
CN107990891A (zh) * 2016-10-26 2018-05-04 中国科学院沈阳自动化研究所 基于长基线和信标在线标定的水下机器人组合导航方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104280026A (zh) * 2013-07-08 2015-01-14 中国科学院沈阳自动化研究所 基于aukf的深海机器人长基线组合导航方法
CN104457754A (zh) * 2014-12-19 2015-03-25 东南大学 一种基于sins/lbl紧组合的auv水下导航定位方法
CN105547289A (zh) * 2015-12-16 2016-05-04 南京信息工程大学 一种水下航行器组合导航系统及导航信息融合方法
CN106017467A (zh) * 2016-07-28 2016-10-12 中国船舶重工集团公司第七0七研究所 一种基于多水下应答器的惯性/水声组合导航方法
CN107990891A (zh) * 2016-10-26 2018-05-04 中国科学院沈阳自动化研究所 基于长基线和信标在线标定的水下机器人组合导航方法
CN106767793A (zh) * 2017-01-19 2017-05-31 东南大学 一种基于sins/usbl紧组合的auv水下导航定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Øyvind Hegrenæs 等.Underwater Transponder Positioning and Navigation of Autonomous Underwater Vehicles.《IN PROCEEDINGS OF THE MTS/IEEE OCEANS CONFERENCE AND EXHIBITION》.2009,1-7. *
基于SINS/DVL与LBL交互辅助的AUV水下定位系统;张涛 等;《中国惯性技术学报》;20151231;第23卷(第6期);第769-774页 *
无人水下航行器SINS/DVL组合导航方法研究;于旭;《辽东学院学报（自然科学版）》;20090331;第16卷(第1期);第64-67页 *

Also Published As

Publication number Publication date
CN110542884A (zh) 2019-12-06

Similar Documents

Publication Publication Date Title
CN109443379B (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
US5077700A (en) Doppler velocity profiler
CN108614258B (zh) 一种基于单水声信标距离量测的水下定位方法
CN109724599A (zh) 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN109738902B (zh) 一种基于同步信标模式的水下高速目标高精度自主声学导航方法
CN106643723B (zh) 一种无人艇安全航行船位推算方法
CN109579850B (zh) 基于对水速度辅助惯导的深水智能导航方法
EP1471365A1 (en) A method for determining the optimum observer heading change in bearings -only passive emitter tracking
US20080034871A1 (en) Test Method For A Method For Passively Obtaining Target Parameters
CN109883392A (zh) 一种基于相位补偿的捷联惯导升沉测量方法
CN103090884A (zh) 基于捷联惯导系统的多普勒计程仪测速误差抑制方法
CN114942428A (zh) 多目标跟踪的混合体制雷达数据关联算法
CN110555398A (zh) 一种基于滤波最优平滑确定故障首达时刻的故障诊断方法
CN108871379A (zh) 一种dvl测速误差在线标定方法
CN110865338B (zh) 一种基于水下机动平台的未知周期声信标高精度定位方法
CN110865333B (zh) 洋流影响下水下滑翔机单信标无源声学定位方法
CN111929676A (zh) 一种基于密度聚类的x波段雷达目标检测与跟踪方法
CN110333369B (zh) 基于水面gps校正的uuv的dvl测速系统及自适应去噪方法
CN108332756A (zh) 一种基于拓扑信息的水下航行器协同定位方法
CN110542884B (zh) 一种基于惯导修正的长基线导航定位方法
CN117146830B (zh) 一种自适应多信标航位推算和长基线的紧组合导航方法
CN113218421B (zh) 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法
CN112666519B (zh) 一种基于广义二阶时延差的水下目标高精度定位方法
CN113155134A (zh) 一种基于惯性信息辅助的水声信道跟踪与预测方法
CN113933526B (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