CN110006427B - 一种低动态高振动环境下的bds/ins紧组合导航方法 - Google Patents
一种低动态高振动环境下的bds/ins紧组合导航方法 Download PDFInfo
- Publication number
- CN110006427B CN110006427B CN201910416605.1A CN201910416605A CN110006427B CN 110006427 B CN110006427 B CN 110006427B CN 201910416605 A CN201910416605 A CN 201910416605A CN 110006427 B CN110006427 B CN 110006427B
- Authority
- CN
- China
- Prior art keywords
- data
- ins
- bds
- rts
- kalman filtering
- 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 47
- 238000009499 grossing Methods 0.000 claims abstract description 48
- 238000001914 filtration Methods 0.000 claims abstract description 45
- 238000012545 processing Methods 0.000 claims abstract description 26
- 230000008569 process Effects 0.000 claims abstract description 13
- 230000001360 synchronised effect Effects 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 57
- 239000013598 vector Substances 0.000 claims description 49
- 238000005259 measurement Methods 0.000 claims description 23
- 238000000354 decomposition reaction Methods 0.000 claims description 22
- 238000011084 recovery Methods 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 5
- 238000012887 quadratic function Methods 0.000 claims description 5
- 238000012216 screening Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 230000002087 whitening effect Effects 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 2
- 238000011156 evaluation Methods 0.000 claims description 2
- 238000012880 independent component analysis Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 5
- 230000002159 abnormal effect Effects 0.000 description 3
- 238000013459 approach Methods 0.000 description 3
- 238000007667 floating Methods 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000004873 anchoring Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 239000005433 ionosphere Substances 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000011160 research 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
- G01C21/165—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 combined with non-inertial navigation instruments
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/35—Constructional details or hardware or software details of the signal processing chain
- G01S19/37—Hardware or software details of the signal processing chain
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
- G01S19/44—Carrier phase ambiguity resolution; Floating ambiguity; LAMBDA [Least-squares AMBiguity Decorrelation Adjustment] method
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明提出一种低动态高振动环境下的BDS/INS紧组合方法,该方法获取低动态高振动环境下的移动载具的INS原始数据以及同步的BDS原始数据;首先对INS原始数据进行EMD‑ICA去噪处理,得到去噪后的INS数据;将EMD‑ICA去噪后的INS数据与BDS原始数据进行紧组合Kalman滤波解算;将Kalman滤波解算后的数据做RTS平滑处理,得到整个导航过程中移动载具在当地地理坐标系中的位置、当地地理坐标系下三方向的速度和姿态信息,从而获得移动载具的具体定位信息,结合目的地的地理信息从而实现对移动载具的精确导航。
Description
技术领域
本发明涉及一种导航方法,尤其适用于一种导航领域使用的低动态高振动环境下的BDS/INS紧组合导航方法。
背景技术
随着卫星定位技术的发展,GPS/INS组合导航用于动态定位的研究非常广泛,通过GPS/INS紧组合提高***的定位性能。目前,我国BDS***与GPS***具有相同的信号调制方式,并且BDS已具备全球导航定位服务能力。BDS/INS紧组合方法具有更高的导航精度,能进行复杂环境的导航定位。目前,国内外多数论文和专利主要针对INS原始数据降噪提出解决方案,且大多采用小波阈值降噪、EMD降噪方法以及EMD-小波降噪方法,小波阈值降噪方法不能抑制异常噪声的干扰,且需事先确定基函数和分解层数,不具有自适应能力,同时降噪结果受阈值和阈值函数的影响较大;对于EMD降噪,当信号的信噪较低且存在脉冲干扰或异常噪声时,信号的极值点分布不均匀,从而产生模态混叠现象,导致相邻2个IMF分量混叠,难以区分,从而影响EMD降噪效果;EMD-小波降噪模型中,需确定高频IMF进行小波降噪,故降噪效果受高频IMF数量的影响,同样地,当信号的信噪较低且存在脉冲干扰或异常噪声时,EMD-小波降噪的降噪效果不理想。
目前的低动态高振动环境是指:搭载BDS和INS的移动载具行驶速度缓慢,但该移动载具却处于高频不规则振动中,这种低动态高振动环境将同时影响BDS和INS定位解算:对于BDS,快速振动会导致BDS卫星信号跟踪失锁,从而难以固定整周模糊度;对于INS,低动态高振动会使惯性器件的观测信号淹没在高频噪声中,大幅降低BDS/INS紧组合导航定位精度。因此,有必要提出一种技术手段,解决上述问题。
发明内容
发明目的:针对上述技术的不足之处,提供一种利用EMD-ICA算法对INS原始数据进行去噪处理,有效提高整周模糊度解算的速度,再利用KF-RTS平滑算法对数据去噪处理,提高运移动载具在低动态高振动环境下导航定位精度的低动态高振动环境下的BDS/INS紧组合导航方法。
技术方案:
为实现上述技术效果,本发明低动态高振动环境下的BDS/INS紧组合导航方法,包括以下步骤:
步骤1:获取低动态高振动环境下的移动载具的INS(惯性导航***)原始数据以及同步的BDS(北斗卫星导航***)原始数据,并对INS原始数据进行EMD-ICA(经验模态分解-独立分量分析)去噪处理,得到去噪后的INS数据;
步骤2:将INS原始数据与BDS原始数据进行紧组合中的Kalman滤波解算,得到紧组合处理后移动载具在当地地理坐标系中的位置、当地地理坐标系三轴方向的速度和姿态信息;
步骤3:对Kalman滤波解算后的数据进行RTS(Rauch-Tung-Striebel)平滑处理削弱噪声信号的影响,得到移动载具平滑后在当地地理坐标系中更精确的位置、当地地理坐标系三轴方向的速度和姿态信息,从而获得移动载具的具体定位信息,结合目的地的地理信息从而实现对移动载具的精确导航。
所述步骤1的去噪处理包括将原始INS数据通过小波滤波去噪,减去其均值,将结果作为INS数据误差模型的参考信号rt:步骤1.5中求出陀螺仪数据误差信号后,以此作为误差改正模型,对INS原始观测数据进行改正,即用原始数据减去误差信号数据得到去噪后的数据。
步骤1.1:利用陀螺仪与加速度计采集移动载具的信息,包括陀螺仪的x、y、z轴三轴角速度信息与加速度计的x、y、z轴的三轴加速度信息,将所有陀螺仪与加速度计归入观测历元,对观测历元内的上述六种信息进行去噪处理;
步骤1.2:首先处理陀螺仪x轴数据,将原始陀螺仪x轴数据进行EMD(经验模态分解)分解,得到n个IMF(本征模态函数)分量:
找出原始陀螺仪x轴数据q(t)的所有极大值点序列和极小值点序列,将这两个序列分别用三次样条差值拟合成上、下包络线;
计算上、下包络线对应的均值序列mi,将所有原始陀螺仪x轴数据构成序列q并利用公式:hi=q(t)-mi与得到hi,hi为中间变量;
利用公式:ri(t)=ri-1(t)-IMFi(t)计算剩余信号的残余分量ri(t);IMFi(t)为第i个本征模态函数,t表示本征模态函数是时间t的函数;
检测残余分量ri(t)是否为单调数,若ri(t)极值点个数不少于2个,则判断ri(t)不为单调函数,则i=i+1,继续以上步骤;否则,分解结束;
首先对虚拟观测数据QL×n去均值并进行白化,得到矩阵Z,初始化惩罚因子γ,选择一个随机的初始向量w0并标准化;选择拉格朗日乘系数u和λ的初值;计算ρ=E{G(y)}-E{G(v)},其中ρ为中间变量,无具体含义;E表示求函数的期望;G为任意非二次函数,自变量y表示与具有相同方差的变量,v为一个零均值单位方差的高斯变量。利用公式:uk+1=max{0,uk+γg(wk)}和λk+1=λk+γh(wk)更新u和λ;利用公式:更新w,
标准化w直到|w(k+1)Tw(k)-1|趋近于0,式中y为与具有相同方差的变量,μ为拉格朗日乘子,η为比例系数,取值范围在0~1之间,求取q的协方差矩阵月q=E{qqT},对求得的正常数ρ取符号函数Gy(y)、和gy(y)、分别为Gy和gy关于y的一阶导数和二阶导数,其中G与g为两不同的任意实际非二次函数,最后求取迭代分量et=wZ;
步骤1.5:利用公式:s1,s1=a·et求得期望的陀螺仪x轴数据误差信号s1:通过将误差信号s1从原始数据中剥离从而获得陀螺仪数据的有用信号,从而完成对原始INS数据去噪处理;式中et为期望信号;
步骤1.6:重复上述步骤从而完成陀螺仪的x、y、z轴三轴角速度信息与加速度计的x、y、z轴的三轴加速度信息的去噪处理。
所述步骤2包括以下步骤:
步骤2.1:构建BDS/INS紧组合的状态方程:包括INS的误差状态方程;
步骤2.2:构建BDS/INS紧组合的量测方程:包括双差载波相位定位方程和多普勒测速方程;
步骤2.3:使用INS数据辅助BDS数据解算整周模糊度;
步骤2.4:将BDS/INS紧组合的状态方程和BDS/INS紧组合的量测方程进行Kalman滤波解算,得到当前时刻移动载具在当地地理坐标系中的位置、当地地理坐标系三轴方向的速度和姿态信息;
将BDS/INS紧组合的状态方程和BDS/INS紧组合的量测方程进行Kalman滤波解算,得到解如下式所示:
其中,为Xk的Kalman滤波估值,为滤波增益矩阵,为预测的状态向量协方差阵,Rk为量测噪声Vk的对称正定方差阵,Qk为噪声向量Wk的非负定方差阵,为当前时刻状态向量的协方差阵;时刻的量测方程系数矩阵,Hk T为Hk的转置;为前一时刻的状态估值;
步骤2.5:通过Kalman滤波解算得到的状态估值作为下一时刻的状态估值重复步骤2.1至步骤2.4,再构建新的BDS/1NS紧组合的状态方程和量测方程进行Kalman滤波解算得到新的状态估值,直至所有的INS原始数据与BDS原始数据迭代完成,则可以得到整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系三轴方向的速度和姿态信息。
所述步骤3包括以下步骤:
步骤3.1:将步骤2中Kalman滤波解算后的数据作为RTS平滑的输入值:经过上一步骤中的Kalman滤波解算后的数据继续进行一次Kalman滤波解算,得到k+1时刻状态向量Xk的Kalman滤波估值以及k+1时刻预测的状态向量协方差阵
步骤3.2:对最后一个观测历元的数据进行RTS平滑处理:首先利用公式:通过Kalman滤波解算k时刻的数据计算出k时刻的RTS平滑增益,式中:Pk为k时刻的状态向量的协方差矩阵,中k+1.k为k+1时刻的BDS/1NS紧组合的状态转移矩阵,利用RTS平滑得到的k时刻的RTS平滑增益Ck,计算得到RTS平滑处理后的k时刻的状态向量其中RTS平滑过程的初值等于Kalman滤波过程中最后一个状态估计向量xk;由k+1时刻的RTS平滑后状态向量的协方差阵计算得到k时刻的RTS平滑后状态向量的协方差阵式中:为k时刻的RTS平滑后状态向量的协方差阵,为k+1时刻的RTS平滑后状态向量的协方差阵,式中“+”和“-”分别表示预报值与滤波值;
步骤3.3:从k时刻对所有历元依次进行前向RTS平滑处理,k时刻通过RTS平滑处理得到的状态向量作为k-1时刻的己知量,重复步骤3.1至3.2,再构建新的RTS平滑增益方程解算得到新的RTS平滑处理的状态向量及其协方差阵,直至所有时刻的Kalman滤波估值迭代完成,则可以得到RTS平滑处理后的整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系三轴方向的速度和姿态信息。
有益效果:本发明改善了在低动态高振动环境中常规BDS/INS紧组合导航结果精度低的问题,定位精度达到厘米级,有效提高了导航定位的精度,满足高精度的导航应用要求;利用INS原始数据与BDS原始数据进行KF-RTS处理,计算获得移动载具的位置信息、速度信息与姿态信息,有效提高了低动态高振动环境下紧组合导航结果的可靠性;低动态高振动环境将同时影响BDS和INS定位解算:对于BDS,快速振动会导致BDS卫星信号跟踪失锁,从而难以固定整周模糊度;对于INS,低动态高振动会使惯性器件的观测信号淹没在高频噪声中,大幅降低BDS/INS紧组合导航定位精度。对原始INS数据进行EMD-ICA进行去噪处理,提高紧组合导航定位解算精度;对紧组合解算后的数据进行KF-RTS去噪,有效提高导航定位解算精度。
附图说明:
图1为本发明低动态高振动环境下的BDS/INS紧组合导航方法流程图;
图2为本发明原始数据EMD-ICA去噪方法流程图;
具体实施方式:
下面结合附图对本发明作更进一步的说明。
如图1所示为本发明的流程图,主要步骤为:首先对INS原始数据进行EMD-ICA去噪处理,利用去噪后的INS数据构建误差状态方程,同时结合INS数据、BDS数据构建BDS/INS紧组合的量测方程(包括双差载波相位载波相位定位方程和多普勒测速方程);其次进行Kalman滤波解算,将解算得到的状态向量及其协方差矩阵作为RTS平滑的初值进行RTS平滑处理,最后输出移动载具的位置信息、速度信息与姿态信息。
如图2所示为INS原始数据EMD-ICA去噪方法流程图,主要步骤为:首先对首个历元的INS原始数据进行去噪处理,作为后续处理的参考信号;其次将第二历元的INS原始数据进行EMD分解,得到IMF分量;利用ICA-R方法提取出剩余历元的期望信号,经过幅值恢复,将INS数据噪声信号分离,得到去噪后的INS数据。
具体实施步骤如下:
步骤1:在移动载具上装载陀螺仪与加速度计,获取低动态高振动环境下的移动载具的INS原始数据以及同步的BDS(北斗卫星导航***)原始数据,并对INS(惯性导航***)原始数据进行EMD-ICA(经验模态分解-独立分量分析)去噪处理,得到去噪后的INS数据。
步骤1.1:利用陀螺仪与加速度计采集移动载具的信息,包括陀螺仪的x、y、z轴三轴角速度信息与加速度计的x、y、z轴的三轴加速度信息,将所有陀螺仪与加速度计归入观测历元,对观测历元内的上述六种信息进行去噪处理;
对INS原始数据进行小波分解:构造变换矩阵,确定分解层数,对INS原始数据进行小波分解将不同数据进行不同层数分解,得到各层小波分解系数,分解系数包括高频系数和低频系数,对各层小波分解的高频系数进行阈值处理,去除高频部分的噪声,分别对陀螺仪三轴数据与加速度计三轴数据进行小波去噪;
步骤1.2:首先处理陀螺仪x轴数据,将原始陀螺仪x轴数据进行EMD(经验模态分解)分解,得到n个IMF(本征模态函数)分量:
找出原始陀螺仪x轴数据q(t)的所有极大值点序列和极小值点序列,将这两个序列分别用三次样条差值拟合成上、下包络线;
计算上、下包络线对应的均值序列mi,将所有原始陀螺仪x轴数据构成序列q并利用公式:hi=q(t)-mi与得到hi,hi为中间变量;
计算剩余信号ri(t)=ri-1(t)-IMFi(t);若ri(t)不为单调函数,即其极值点个数不少于2个,则i=i+1,继续以上步骤;否则,分解结束,ri(t)是残余分量,
即通过EMD分解,将原始数据分解成一系列IMF分量和一个残余分量的和:
首先对虚拟观测数据QL×n去均值并进行白化,得到矩阵Z,初始化惩罚因子γ,选择一个随机的初始向量w0,并标准化;选择拉格朗日乘系数u和λ的初值;计算ρ=E{G(y)}-E{G(v)};按式uk+1=max{0,uk+γg(wk)}和λk+1=λk+γh(wk)更新u和λ;按式下式更新w,标准化w直到|w(k+1)Tw(k)-1|趋近于0。
标准化w直到|w(k+1)Tw(k)-1|趋近于0,式中y为与具有相同方差的变量,μ为拉格朗日乘子,η为比例系数,取值范围在0~1之间,求取q的协方差矩阵Rq=E(qqT},对求得的正常数ρ取符号函数Gy(y)、和gy(y)、分别为Gy和gy关于y的一阶导数和二阶导数,其中G与g为两不同的任意实际非二次函数,最后求取迭代分量et=wZ;
步骤1.4:求解陀螺仪数据幅值恢复的比例系数a,a=avg(at),(t=1,2,…n),n为观测历元数;a是at的平均值,at表示历元t的幅值恢复系数;以陀螺仪x轴数据为例,利用每一历元的参考信号rt与EMD-ICA提取的期望信号et,计算陀螺仪数据幅值恢复的比例系数a,a=avg(at),
步骤1.5:求得期望的陀螺仪数据误差信号s1,s1=a·et。本专利中以陀螺仪x轴数据为例,其他数据包括陀螺仪y轴、z轴以及加速度计xyz轴数据也采用相同的方式进行去噪处理,通过将误差信号s1从原始数据中剥离从而获得陀螺仪数据的有用信号,从而完成对原始INS数据去噪处理;式中et为期望信号;
利用步骤1.4中求解的陀螺仪数据幅值恢复的比例系数a与EMD-ICA提取的期望信号et,求得期望的陀螺仪x轴数据误差信号s1。步骤2:将INS原始数据与BDS原始数据进行紧组合处理,得到整个导航过程中移动载具在当地地理坐标系中的位置、当地地理坐标系三轴方向的速度和姿态信息;
步骤2:将INS原始数据与BDS原始数据进行紧组合中的Kalman滤波解算,得到紧组合处理后移动载具在当地地理坐标系中的位置、当地地理坐标系三轴方向的速度和姿态信息;
步骤2.1:构建BDS/INS紧组合的状态方程:包括INS的误差状态方程。
计算INS的误差状态包括当地地理坐标系下三轴的位置误差、速度误差、姿态角误差、陀螺漂移和加速度计漂移。状态方程的目的是估算出15个状态量的初值
其中,x=[δr δV φ d b]T,δr=[δX δY δZ]分别为地固系的X、Y、Z坐标误差,δV=[δVX δVY δVZ]为速度误差,φ=[φX φY φZ]为平台角误差,d=[εrx εry εrz]为陀螺一阶马尔科夫过程漂移,为加速度计一阶马尔科夫过程零偏。
上述内容构造状态方程的系数矩阵,如若详细展开叙述每个元素的含义,会占用过大篇幅,且此系数矩阵是组合导航中最基本、最经典的构造方式,因此不详细叙述各个元素含义。其中,表示利用位置误差计算正常重力误差时的系数矩阵,表示坐标转换矩阵,ωe为地心地固系下地球自转角速度,fx、fy、fz表示三轴下的比力,α、β分别是陀螺与加速度计误差中的相关时间倒数;
步骤2.2:构建BDS/INS紧组合的量测方程:包括双差载波相位定位方程和多普勒测速方程;
量测方程根据双差载波相位定位方程与多普勒量测方程建立的,具体为:
Z(t)=H(t)x(t)+V(t)
其中,x=[δr δV φ d b]T,V(t)为量测噪声,
步骤2.3:使用INS数据辅助BDS数据解算整周模糊度。
根据紧组合模式,INS子***作为参考子***,INS***导航出的位置就是参考位置,以此形成虚拟观测值,得到如下观测方程:eINS表示INS***虚拟观测值的观测噪声,I3表示3×3维单位矩阵。
结合以上三个公式,可以得出直接解算模糊度浮点解方程:将上述公式组合,即双差码伪距定位、双差载波相位定位与双差虚拟观测值定位方程的组合;
设C/A码伪距观测值、载波相位观测值、INS导航结果的权阵为:
对解算模糊度的浮点解方程进行最小二乘估计,求出模糊度的浮点解;
下面利用双频相关法求解整周模糊度的整数解。
BDS载波相位双差观测方程可写成如下形式:
对于双频BDS接收机载波相位观测值,可以得到:
上式为任何BDS双频载波相位观测值都满足的线性关系。其中u为误差项,若不顾及查分残差,并假设两种载波相位的观测精度相同,则u将服从正态分布,根据误差传播率,其方差为:
Du=2×2×2×DL
其中DL为载波相位的观测方差,其值约为万分之一。根据统计理论,取u的三倍中误差,双频相位不等式:
依概率99.74%成立。由此形成一条线性误差带,将落入误差带内的整数值一一进行ratio值检验,从而确定整周模糊度,对落入误差带内的整数值进行ratio检验后,最终只有一组整数值能通过检验,该整数值即为整周模糊度的整数解
步骤2.4:将BDS/INS紧组合的状态方程和BDS/INS紧组合的量测方程进行Kalman滤波解算,得到当前时刻移动载具在当地地理坐标系中的位置、当地地理坐标系三轴方向的速度和姿态信息。
将BDS/INS紧组合的状态方程和BDS/INS紧组合的量测方程进行Kalman滤波解算,得到解如下式所示:
步骤2.5:重复步骤2.1至步骤2.4,得到整个导航过程中移动载具在当地地理坐标系中的位置、当地地理坐标系三轴方向的速度和姿态信息:通过Kalman滤波解算得到的状态估值作为下一时刻的状态估值重复步骤2.1至步骤2.4,再构建新的BDS/INS紧组合的状态方程和量测方程进行Kalman滤波解算得到新的状态估值,直至所有的INS原始数据与BDS原始数据迭代完成,则可以得到整个导航过程中移动载具在当地地理坐标系中的位置、当地地理坐标系三轴方向的速度和姿态信息。
步骤3:对Kalman滤波解算后的数据进行RTS(Rauch-Tung-Striebel)平滑处理削弱噪声信号的影响,得到移动载具平滑后在当地地理坐标系中更精确的位置、当地地理坐标系三轴方向的速度和姿态信息,从而获得移动载具的具体定位信息,结合目的地的地理信息从而实现对移动载具的精确导航。
步骤3.1:将步骤2中Kalman滤波解算后的数据作为RTS平滑的输入值。
步骤3.2:对最后一个观测历元的数据进行RTS平滑处理:首先利用公式:通过Kalman滤波解算k时刻的数据计算出k时刻的RTS平滑增益,式中:Pk为k时刻的状态向量的协方差矩阵,Φk+1.k为k+1时刻的BDS/INS紧组合的状态转移矩阵,利用RTS平滑得到的k时刻的RTS平滑增益Ck,计算得到RTS平滑处理后的k时刻的状态向量其中RTS平滑过程的初值等于Kalman滤波过程中最后一个状态估计向量xk;由k+1时刻的RTS平滑后状态向量的协方差阵计算得到k时刻的RTS平滑后状态向量的协方差阵式中:为k时刻的RTS平滑后状态向量的协方差阵,为k+1时刻的RTS平滑后状态向量的协方差阵,式中“+”和“-”分别表示预报值与滤波值;
步骤3.3:从k时刻对所有历元依次进行前向RTS平滑处理,k时刻通过RTS平滑处理得到的状态向量作为k-1时刻的已知量,重复步骤3.1至3.2,再构建新的RTS平滑增益方程解算得到新的RTS平滑处理的状态向量及其协方差阵,直至所有时刻的Kalman滤波估值迭代完成,则可以得到RTS平滑处理后的整个导航过程中载体在当地地理坐标系中的位置、当地地理坐标系三轴方向的速度和姿态信息。
Claims (3)
1.一种低动态高振动环境下的BDS/INS紧组合导航方法,其特征在于,包括以下步骤:
步骤1:获取低动态高振动环境下的移动载具的INS原始数据以及同步的BDS原始数据,并对INS原始数据进行EMD-ICA去噪处理,得到去噪后的INS数据;
步骤2:将INS原始数据与BDS原始数据进行紧组合中的Kalman滤波解算,得到紧组合处理后移动载具在当地地理坐标系中的位置、当地地理坐标系三轴方向的速度和姿态信息;
步骤3:对Kalman滤波解算后的数据进行RTS平滑处理,削弱噪声信号的影响,得到移动载具平滑后在当地地理坐标系中更精确的位置、当地地理坐标系三轴方向的速度和姿态信息,从而获得移动载具的具体定位信息,结合目的地的地理信息从而实现对移动载具的精确导航;
所述步骤1的去噪处理包括将原始INS数据通过小波滤波去噪,减去其均值,将结果作为INS数据误差模型的参考信号rt:步骤1.5中求出陀螺仪数据误差信号后,以此作为误差改正模型,对INS原始观测数据进行改正,即用原始数据减去误差信号数据得到去噪后的数据:
步骤1.1:利用陀螺仪与加速度计采集移动载具的信息,包括陀螺仪的E、N、U轴三轴角速度信息与加速度计的E、N、U轴的三轴加速度信息,将所有陀螺仪与加速度计归入观测历元,对观测历元内的上述六种信息进行去噪处理;
步骤1.2:首先处理陀螺仪E轴数据,将原始陀螺仪E轴数据进行EMD分解,得到n个IMF分量:
找出原始陀螺仪E轴数据q的所有极大值点序列和极小值点序列,将这两个序列分别用三次样条差值拟合成上、下包络线;
计算上、下包络线对应的均值序列mi,并利用公式:hi(t)=q-mi与得到hi(t),hi(t)为中间变量;
利用公式:ri(t)=ri-1(t)-IMFi(t)计算剩余信号的残余分量ri(t);IMFi(t)为第i个本征模态关于时间t的函数;
检测残余分量ri(t)是否为单调数,若ri(t)极值点个数不少于2个,则判断ri(t)不为单调函数,则i=i+1,继续以上步骤;否则,分解结束;
首先对虚拟观测数据QL×n去均值并进行白化,得到矩阵Z,初始化惩罚因子γ,选择一个随机的初始向量w0并标准化;选择拉格朗日乘系数μ和拉格朗日乘系数λ的初值;计算ρ=E{G(y)}-E{G(v)},其中ρ为中间变量,无具体含义;E表示求函数的期望;G为任意非二次函数,自变量y表示与具有相同方差的变量,v为一个零均值单位方差的高斯变量;利用公式:μk+1=max{0,μk+γg(wk)}和λk+1=λk+γh(wk)更新μ和λ,h为约束条件,μk+1为第k+1时刻的拉格朗日乘系数,wk为第k时刻的随机解混向量;利用公式:更新w,wk+1为第k+1时刻的随机解混向量;标准化w直到趋近于0,式中y为与具有相同方差的变量,μ为拉格朗日乘系数,η为比例系数,取值范围在0~1之间,求取q的协方差矩阵Rq=E{qqT},对求得的中间变量ρ取符号函数G′y(y)、和g′y(y)、分别为Gy和gy关于y的一阶导数和二阶导数,其中G与g为两不同的任意实际非二次函数,最后求取期望信号et=wZ;
步骤1.5:利用公式:s1=a·et求得期望的陀螺仪E轴数据误差信号s1:通过将误差信号s1从原始数据中剥离从而获得陀螺仪数据的有用信号,从而完成对原始INS数据去噪处理;式中et为期望信号;
步骤1.6:重复上述步骤从而完成陀螺仪的E、N、U轴三轴角速度信息与加速度计的E、N、U轴的三轴加速度信息的去噪处理。
2.根据权利要求1所述的低动态高振动环境下的BDS/INS紧组合导航方法,其特征在于所述步骤2包括以下步骤:
步骤2.1:构建BDS/INS紧组合的状态方程:包括INS的误差状态方程;
步骤2.2:构建BDS/INS紧组合的量测方程:包括双差载波相位定位方程和多普勒测速方程;
步骤2.3:使用INS数据辅助BDS数据解算整周模糊度;
步骤2.4:将BDS/INS紧组合的状态方程和BDS/INS紧组合的量测方程进行Kalman滤波解算,得到当前时刻移动载具在当地地理坐标系中的位置、当地地理坐标系三轴方向的速度和姿态信息;
将BDS/INS紧组合的状态方程和BDS/INS紧组合的量测方程进行Kalman滤波解算,得到解如下式所示:
其中,为Xk的Kalman滤波估值,为滤波增益矩阵,为预测的状态向量协方差阵,Rk为量测噪声Vk的对称正定方差阵,Qk为噪声向量Wk的非负定方差阵,为当前时刻状态向量的协方差阵;Hk为k时刻的量测方程系数矩阵,Hk T为Hk的转置;为前一时刻的状态估值;Zk为k时刻紧组合量测矩阵;
3.根据权利要求1所述的低动态高振动环境下的BDS/INS紧组合导航方法,其特征在于:所述步骤3包括以下步骤:
步骤3.1:将步骤2中Kalman滤波解算后的数据作为RTS平滑的输入值:经过步骤2.4中的Kalman滤波解算后的数据继续进行一次Kalman滤波解算,得到k+1时刻状态向量Xk的Kalman滤波估值 以及k+1时刻预测的状态向量协方差阵
步骤3.2:对最后一个观测历元的数据进行RTS平滑处理:首先利用公式:通过Kalman滤波解算k时刻的数据计算出k时刻的RTS平滑增益,式中:Pk为k时刻的状态向量的协方差矩阵,Φk+1,k为k+1时刻的BDS/INS紧组合的状态转移矩阵,利用RTS平滑得到的k时刻的RTS平滑增益Ck,计算得到RTS平滑处理后的k时刻的状态向量其中RTS平滑过程的初值等于Kalman滤波过程中最后一个状态估计向量Xk;由k+1时刻的RTS平滑后状态向量的协方差阵计算得到k时刻的RTS平滑后状态向量的协方差阵式中:为k时刻的RTS平滑后状态向量的协方差阵,为k+1时刻的RTS平滑后状态向量的协方差阵;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910416605.1A CN110006427B (zh) | 2019-05-20 | 2019-05-20 | 一种低动态高振动环境下的bds/ins紧组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910416605.1A CN110006427B (zh) | 2019-05-20 | 2019-05-20 | 一种低动态高振动环境下的bds/ins紧组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110006427A CN110006427A (zh) | 2019-07-12 |
CN110006427B true CN110006427B (zh) | 2020-10-27 |
Family
ID=67177532
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910416605.1A Active CN110006427B (zh) | 2019-05-20 | 2019-05-20 | 一种低动态高振动环境下的bds/ins紧组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110006427B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111694040B (zh) * | 2020-04-08 | 2023-02-07 | 中国人民解放军战略支援部队信息工程大学 | 一种卫星/惯性组合导航***定位方法与装置 |
CN112415558B (zh) * | 2021-01-25 | 2021-04-16 | 腾讯科技(深圳)有限公司 | 行进轨迹的处理方法及相关设备 |
CN112924996B (zh) * | 2021-01-28 | 2023-11-03 | 湖南北斗微芯产业发展有限公司 | 一种增强北斗时序分析可靠性的方法、设备及存储介质 |
CN112526573B (zh) * | 2021-02-07 | 2021-05-14 | 腾讯科技(深圳)有限公司 | 对象定位方法和装置、存储介质及电子设备 |
CN113503879B (zh) * | 2021-07-09 | 2023-02-07 | 北京航空航天大学 | 一种基于集合经验模态分解的动态自适应卡尔曼滤波器方法 |
CN113654554B (zh) * | 2021-08-13 | 2024-06-14 | 北京维思韦尔航空电子技术有限公司 | 一种快速自适应的动态惯性导航实时解算去噪方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102608642A (zh) * | 2011-01-25 | 2012-07-25 | 北京七维航测科技股份有限公司 | 北斗/惯性组合导航*** |
CN103389095A (zh) * | 2013-07-24 | 2013-11-13 | 哈尔滨工程大学 | 一种用于捷联惯性/多普勒组合导航***的自适应滤波方法 |
CN105425261A (zh) * | 2015-11-03 | 2016-03-23 | 中原智慧城市设计研究院有限公司 | 基于GPS/Beidou2/INS的组合导航与定位方法 |
CN105526932A (zh) * | 2015-12-29 | 2016-04-27 | 中国矿业大学 | 基于伪卫星技术的无人机群定位方法及定位*** |
CN107505642A (zh) * | 2017-10-23 | 2017-12-22 | 中国矿业大学 | 一种ins辅助的实时bds单频周跳探测方法 |
WO2018059532A1 (zh) * | 2016-09-30 | 2018-04-05 | 华为技术有限公司 | 观测时滞***的组合导航数据解算方法、装置及导航设备 |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102096086B (zh) * | 2010-11-22 | 2012-09-05 | 北京航空航天大学 | 一种基于gps/ins组合导航***不同测量特性的自适应滤波方法 |
US8589072B2 (en) * | 2011-04-13 | 2013-11-19 | Honeywell International, Inc. | Optimal combination of satellite navigation system data and inertial data |
CN102353970B (zh) * | 2011-06-10 | 2013-06-12 | 北京航空航天大学 | 一种高抗干扰性能gps/sins组合导航***及实现方法 |
CN103941271B (zh) * | 2014-03-11 | 2016-12-07 | 哈尔滨工程大学 | 一种时间-空间差分的gps/sins超紧组合导航方法 |
CN105180935B (zh) * | 2015-10-30 | 2018-02-06 | 东南大学 | 一种适用于gnss微弱信号的组合导航数据融合方法 |
CN105737823B (zh) * | 2016-02-01 | 2018-09-21 | 东南大学 | 一种基于五阶ckf的gps/sins/cns组合导航方法 |
CN105891863B (zh) * | 2016-06-16 | 2018-03-20 | 东南大学 | 一种基于高度约束的扩展卡尔曼滤波定位方法 |
CN106597507B (zh) * | 2016-11-28 | 2019-03-19 | 武汉大学 | Gnss/sins紧组合滤波平滑的高精度快速算法 |
CN108226980B (zh) * | 2017-12-23 | 2022-02-08 | 北京卫星信息工程研究所 | 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法 |
-
2019
- 2019-05-20 CN CN201910416605.1A patent/CN110006427B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102608642A (zh) * | 2011-01-25 | 2012-07-25 | 北京七维航测科技股份有限公司 | 北斗/惯性组合导航*** |
CN103389095A (zh) * | 2013-07-24 | 2013-11-13 | 哈尔滨工程大学 | 一种用于捷联惯性/多普勒组合导航***的自适应滤波方法 |
CN105425261A (zh) * | 2015-11-03 | 2016-03-23 | 中原智慧城市设计研究院有限公司 | 基于GPS/Beidou2/INS的组合导航与定位方法 |
CN105526932A (zh) * | 2015-12-29 | 2016-04-27 | 中国矿业大学 | 基于伪卫星技术的无人机群定位方法及定位*** |
WO2018059532A1 (zh) * | 2016-09-30 | 2018-04-05 | 华为技术有限公司 | 观测时滞***的组合导航数据解算方法、装置及导航设备 |
CN107505642A (zh) * | 2017-10-23 | 2017-12-22 | 中国矿业大学 | 一种ins辅助的实时bds单频周跳探测方法 |
Non-Patent Citations (2)
Title |
---|
"一种惯性/北斗自合导航***的鲁棒滤波算法";杨琦;《中国优秀硕士学位论文全文数据库 信息科技辑》;20160415(第4期);正文第11-28、45-46页 * |
"基于EMD-小波随机消噪模型的GPS /INS 组合导航";王坚 等;《东南大学学报》;20120531;第42卷(第3期);正文第407-408页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110006427A (zh) | 2019-07-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110006427B (zh) | 一种低动态高振动环境下的bds/ins紧组合导航方法 | |
CN108226980B (zh) | 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法 | |
EP1585939B1 (en) | Attitude change kalman filter measurement apparatus and method | |
Bryne et al. | Nonlinear observers for integrated INS\/GNSS navigation: implementation aspects | |
CN103675861B (zh) | 一种基于星载gnss多天线的卫星自主定轨方法 | |
EP0856747A1 (en) | Method and apparatus for attitude determination utilizing an inertial measurement unit and a plurality of satellite transmitters | |
CN106597507B (zh) | Gnss/sins紧组合滤波平滑的高精度快速算法 | |
CN108827310A (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
CN111965685B (zh) | 一种基于多普勒信息的低轨卫星/惯性组合导航定位方法 | |
CN108562290B (zh) | 导航数据的滤波方法、装置、计算机设备及存储介质 | |
Leonard et al. | Absolute orbit determination and gravity field recovery for 433 eros using satellite-to-satellite tracking | |
CN114002725A (zh) | 一种车道线辅助定位方法、装置、电子设备及存储介质 | |
CN101122637A (zh) | 一种sar运动补偿用sins/gps组合导航自适应降维滤波方法 | |
KR20020080829A (ko) | 오차보정시스템을 구비하는 관성측정유닛-지피에스통합시스템과 미지정수 검색범위 축소방법 및 사이클 슬립검출방법, 및 그를 이용한 항체 위치, 속도,자세측정방법 | |
CN116007620A (zh) | 一种组合导航滤波方法、***、电子设备及存储介质 | |
CN110208843B (zh) | 一种基于增广伪距信息辅助的容错导航方法 | |
Hasan et al. | Comparative study on wavelet filter and thresholding selection for GPS/INS data fusion | |
CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
CN108981691B (zh) | 一种天空偏振光组合导航在线滤波与平滑方法 | |
CN114167472A (zh) | Ins辅助gnss ppp精密动态导航定位方法及*** | |
CN110873577B (zh) | 一种水下快速动基座对准方法及装置 | |
CN112304309B (zh) | 一种基于心动阵列的高超飞行器组合导航信息解算方法 | |
CN114018262B (zh) | 一种改进的衍生容积卡尔曼滤波组合导航方法 | |
Crassidis et al. | Attitude determination using combined gps and three-axis magnetometer data | |
Shaaban et al. | MARG Sensor-Based Attitude Estimation On SO (3) Under Unknown External Acceleration |
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 |