CN110542884A - 一种基于惯导修正的长基线导航定位方法 - Google Patents
一种基于惯导修正的长基线导航定位方法 Download PDFInfo
- Publication number
- CN110542884A CN110542884A CN201910800764.1A CN201910800764A CN110542884A CN 110542884 A CN110542884 A CN 110542884A CN 201910800764 A CN201910800764 A CN 201910800764A CN 110542884 A CN110542884 A CN 110542884A
- Authority
- CN
- China
- Prior art keywords
- component
- underwater
- time
- inertial navigation
- underwater vehicle
- 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.)
- Granted
Links
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分量的最优估计值获取M个对水下航行器的当前位置的Y分量的估计值;对M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均,获得水下航行器当前位置的Y分量的最优估计值;根据获得的水下航行器当前位置的X分量的最优估计值和获得的水下航行器当前位置的Y分量的最优估计值,确定水下航行器当前位置;实现对水下航行器的导航定位。
Description
技术领域
本发明属于水声导航定位技术领域,具体涉及一种基于惯导修正的长基线导航定位方法。
背景技术
当前,水下航行器常采用多普勒计程仪与惯导系统进行组合导航定位技术,这种采用舰位推算进行导航定位的方法,短时间内具有较高的定位精度,但是,随着时间推移,定位误差不断积累。水下航行器在水下工作一定时间后,需要上浮接收GPS信号进行位置校正,该过程容易导致自身位置暴漏。
另外,现有的水声导航定位方法通常采用长基线水声导航定位技术,该技术具有导航误差不随时间累积的特性;受复杂海洋环境影响,导航信号从发射位置传输到接收位置的平均传播速度存在一定的估计误差;受海洋多途特性的影响,导航信号从发射位置传输到接收位置的时延估计存在一定的估计误差;导航信号远距离传输时,受局部传输信道突发的涡、流等水声物理现象干扰,使得平均声速估计值与真实值相比存在较大误差,导致声速估计值与传播时延测量值不匹配,进而造成个别点水声长基线定位结果误差突然变大。
将水声长基线导航定位与舰位推算类导航定位相结合,有利于提高导航定位精度、降低舰位推算类仪器设备精度的要求、增强水下航行器的隐蔽性。
发明内容
本发明的目的在于,为解决现有的导航定位方法存在的上述缺陷,本发明提出了一种基于惯导修正的长基线导航定位方法,充分利用了惯导系统在短时间内能高精度获得水下航行器的相对位移量和的优点,采用惯导相对位移量修正水声长基线定位结果,提高水声定位的准确性和定位精度。
为了实现上述目的,本发明提供了一种基于惯导修正的长基线导航定位方法,该方法包括:
根据水下航行器时刻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分量的最优估计值,确定水下航行器当前位置;实现对水下航行器的导航定位。
作为上述技术方案的改进之一,所述根据水下航行器前序时间序列获得的水声长基线定位结果的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分量的估计值。
作为上述技术方案的改进之一,所述对由获得的M个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均,获得水下航行器当前位置的X分量的最优估计值具体包括:
对M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行降序或升序排序,去掉其中的最大的估计值和最小的估计值,完成对M+1个估计值的野点剔除后,对剩余的M-1个水下航行器在时刻ti的当前位置的X分量的估计值求均值,得到水下航行器当前位置的X分量的最优估计值如公式(4)所示:
其中,为水声长基线定位综合误差的X分量;为惯导组合定位综合误差的X分量。
作为上述技术方案的改进之一,所述根据水下航行器前序时间序列获得的水声长基线定位结果的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分量的估计值。
作为上述技术方案的改进之一,所述对由获得的M个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均,获得水下航行器当前位置的Y分量的最优估计值具体包括:
M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行降序排序,去掉其中的最大的估计值和最小的估计值,完成对M+1个估计值的野点剔除后,对剩余的M-1个水下航行器在时刻ti的当前位置的Y分量的估计值求均值,得到水下航行器当前位置的Y分量的最优估计值如公式(8)所示:
其中,为水声长基线定位综合误差的Y分量;为惯导组合定位综合误差的Y分量。
本发明相比于现有技术的有益效果在于:
本发明的方法充分利用了惯导系统在短时间内能高精度获得水下航行器相对位移量的优点,采用时间序列估计和数据融合的方法,获得组合导航定位结果的X分量和Y分量,并采用阶段修正的方法,消除惯导系统综合定位误差和对组合定位结果的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分量;
假设水声长基线定位结果分别为惯导定位结果分别为
该方法包括:
步骤1)根据水下航行器前序时间序列获得的水声长基线定位结果的X分量和惯导组合定位结果的X分量,获取M个对水下航行器的当前位置的X分量的估计值;
所述步骤1)具体包括:
令水下航行器在时间点ti时的水下航行器的当前真实位置的坐标X分量为X=[x1x2 … xi];则在时间点ti时,水声长基线定位结果的坐标X分量为在时间点ti,惯导组合定位结果的坐标X分量为
令
其中,为水声长基线定位误差的X分量;xi为水下航行器在时间点ti时的水下航行器的当前真实位置的X分量;为在时间点ti时,水声长基线定位结果的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的惯导相对累计误差;
根据获得估计误差的X分量为
选用时刻ti前序M个时刻,1≤M≤i;获得M个水下航行器在时刻ti的当前位置的X分量的估计值。
步骤2)对由获得的M个水下航行器在时刻ti的当前位置的X分量的估计值和当前水声长基线定位结果的X分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行野点剔除和算数平均,获得水下航行器当前位置的X分量的最优估计值作为组合定位结果的X分量;
具体地,为了去掉突变点,对M+1个水下航行器在时刻ti的当前位置的X分量的估计值进行降序或升序排序,去掉其中的最大的估计值和最小的估计值,完成对M+1个估计值的野点剔除后,对剩余的M-1个水下航行器在时刻ti的当前位置的X分量的估计值求均值,得到水下航行器当前位置的X分量的最优估计值如公式(4)所示:
其中,为水声长基线定位综合误差的X分量;由于ΔXS为独立同分布随机变量,M+1次测量取均值,与单次测量相比,误差有所降低,且M越大,综合估计误差越小;为惯导组合定位综合误差的X分量;由于惯导具有累计误差,值随着k的增大而增大。
实用中需要根据水声长基线定位系统与惯导系统的定位精度,综合考虑选取最优的M值。
由于短时间内,惯导相对累计误差较小,组合定位结果的X分量的误差的X分量比水声长基线定位误差的X分量小,提高了估计精度。
因此,在获取下一个时刻ti+1的水下航行器当前位置的X分量的最优估计值时,根据该公式:采用代替具体见如下公式:
(采用代替)
有利于提高的估计精度。
其中,与相比,引入了较大的惯导组合定位综合误差的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分量为在时间点ti,惯导组合定位结果的坐标Y分量为
令
其中,为水声长基线定位误差的Y分量;yi为水下航行器在时间点ti时的水下航行器的当前真实位置的Y分量;为在时间点ti时,水声长基线定位结果的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的惯导相对累计误差;
根据获得估计误差的Y分量为
选用时刻ti前序M个时刻,1≤M≤i;获得M个水下航行器在时刻ti的当前位置的Y分量的估计值。
步骤4)对由获得的M个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均,获得水下航行器当前位置的Y分量的最优估计值作为组合定位结果的Y分量;
具体地,为了去掉突变点,对M+1个水下航行器在时刻ti的当前位置的Y分量的估计值进行降序或升序排序,去掉其中的最大的估计值和最小的估计值,完成对M+1个估计值的野点剔除后,对剩余的M-1个水下航行器在时刻ti的当前位置的Y分量的估计值求均值,得到水下航行器当前位置的Y分量的最优估计值如公式(8)所示:
其中,为水声长基线定位综合误差的Y分量;由于ΔYS为独立同分布随机变量,M+1次测量取均值,与单次测量相比,误差有所降低,且M越大,综合估计误差越小;为惯导组合定位综合误差的Y分量;由于惯导具有累计误差,值随着k的增大而增大。
实用中需要根据水声长基线定位系统与惯导系统的定位精度,综合考虑选取最优的M值。
由于短时间内,惯导相对累计误差较小,组合定位结果的Y分量的误差的Y分量比水声长基线定位误差的Y分量小,提高了估计精度。
因此,在获取下一个时刻ti+1的水下航行器当前位置的Y分量的最优估计值时,根据该公式:采用代替具体见如下公式:
有利于提高的估计精度。
其中,与相比,引入了较大的惯导组合定位综合误差的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分量的最优估计值作为组合定位结果的X分量;具体地,去掉最大的估计值和最小的估计值,对剩下的6个估计值进行求平均;获得
步骤3)根据水下航行器前序时间序列获得的水声长基线定位结果的Y分量和惯导组合定位结果的Y分量,获取7个对水下航行器的当前位置的Y分量的估计值;
步骤4)对由获得的7个水下航行器在时刻ti的当前位置的Y分量的估计值和当前水声长基线定位结果的Y分量的估计值组成的8个水下航行器在时刻ti的当前位置的Y分量的估计值进行野点剔除和算数平均,获得水下航行器当前位置的Y分量的最优估计值,作为组合定位结果的Y分量;具体地,去掉最大的估计值和最小的估计值,对剩下的6个估计值进行求平均;获得
步骤5)根据步骤2)获得的水下航行器当前位置的X分量的最优估计值和步骤4)获得的水下航行器当前位置的Y分量的最优估计值,确定水下航行器当前位置,实现对水下航行器的导航定位;
水下航行器在时刻ti,基于惯导修正的长基线导航定位获得的水下航行器当前位置坐标值与真实坐标值(xi,yi)之间的距离误差为
其中,水下航行器A在时刻ti的水声长基线定位获得的位置坐标值与真实坐标值(xi,yi)之间的距离误差为
因此,基于惯导修正的长基线导航定位方法获得的水下航行器位置与真实位置之间的距离误差要比基于水声长基线导航定位获得的水下航行器位置与真实位置之间的距离误差要小,基于惯导修正的长基线导航定位方法获得的水下航行器位置要比水声长基线导航定位获得的位置更精确,误差更小。
水下航行器A的水声长基线定位结果的误差曲线如图4中虚线所示,其产生的距离误差的最大值为129.76m、最小值为15.78m、平均值为49.47m。本申请采用的惯导修正结果的误差曲线如图3所示,通过该惯导系统对水声长基线定位结果进行修正,前序时间序列长度M为7,获得组合定位结果如图4中实线所示,其产生的距离误差的最大值为57.26m、最小值为2.05m、平均值为23.75m。因此,本发明的定位方法产生的距离误差取平均值23.75m,而直接采用水声长基线定位结果的距离误差取平均值49.47m;通过数据比较可知,本申请中的定位方法的距离误差要更小一些,其定位精度得到了显著提高。
最后所应说明的是,以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明,本领域的普通技术人员应当理解,对本发明的技术方案进行修改或者等同替换,都不脱离本发明技术方案的精神和范围,其均应涵盖在本发明的权利要求范围当中。
Claims (5)
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分量。
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分量。
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 true CN110542884A (zh) | 2019-12-06 |
CN110542884B 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)
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 | 中国科学院沈阳自动化研究所 | 基于长基线和信标在线标定的水下机器人组合导航方法 |
-
2019
- 2019-08-28 CN CN201910800764.1A patent/CN110542884B/zh active Active
Patent Citations (6)
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)
Title |
---|
ØYVIND HEGRENÆS 等: "Underwater Transponder Positioning and Navigation of Autonomous Underwater Vehicles", 《IN PROCEEDINGS OF THE MTS/IEEE OCEANS CONFERENCE AND EXHIBITION》 * |
于旭: "无人水下航行器SINS/DVL组合导航方法研究", 《辽东学院学报(自然科学版)》 * |
张涛 等: "基于SINS/DVL与LBL交互辅助的AUV水下定位系统", 《中国惯性技术学报》 * |
Also Published As
Publication number | Publication date |
---|---|
CN110542884B (zh) | 2020-11-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN108614258B (zh) | 一种基于单水声信标距离量测的水下定位方法 | |
CN110207695B (zh) | 一种适用于深海auv的无速度辅助单信标定位方法 | |
CN106643723B (zh) | 一种无人艇安全航行船位推算方法 | |
CN112432644A (zh) | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 | |
CN107918139B (zh) | 一种角速度辅助的Kalman滤波定位方法 | |
CN110132308A (zh) | 一种基于姿态确定的usbl安装误差角标定方法 | |
CN107101631A (zh) | 一种基于自适应滤波技术的舰船升沉测量方法 | |
CN110865333B (zh) | 洋流影响下水下滑翔机单信标无源声学定位方法 | |
CN106527454B (zh) | 一种无稳态误差的远程水下航行器定深控制方法 | |
CN109582026B (zh) | 基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法 | |
CN113218421B (zh) | 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法 | |
CN110542884A (zh) | 一种基于惯导修正的长基线导航定位方法 | |
CN112684453B (zh) | 一种基于无人潜航器双基地声纳系统的定位误差修正方法 | |
Yuan et al. | Adaptive turn rate estimation using range rate measurements | |
Wang et al. | SINS/USBL integrated navigation fault-tolerant method with chi-square test | |
CN110007298B (zh) | 一种目标超前预测跟踪方法 | |
JP7366582B2 (ja) | Isarレーダ装置及びisarレーダ信号処理方法 | |
CN111757242B (zh) | 一种基于ais信息计算的海域通信波束控制方法及装置 | |
CN109799480B (zh) | 一种水下通信节点基于多途信道约束的长基线定位方法 | |
CN113434806A (zh) | 一种抗差自适应多模型滤波方法 | |
CN113514823B (zh) | 一种基于伪谱的多模型机动目标检测前跟踪方法 | |
CN115392117B (zh) | 一种水下高速机动平台高帧率无模糊声学导航方法 | |
CN118111436A (zh) | 基于卡尔曼滤波的无人艇航机信号修正补偿方法和设备 | |
CN115372956B (zh) | 一种基于正逆逻辑Hough变换的混合体制雷达航迹起始方法 |
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 |