CN103278837A - 基于自适应滤波的sins/gnss多级容错组合导航方法 - Google Patents
基于自适应滤波的sins/gnss多级容错组合导航方法 Download PDFInfo
- Publication number
- CN103278837A CN103278837A CN201310186549XA CN201310186549A CN103278837A CN 103278837 A CN103278837 A CN 103278837A CN 201310186549X A CN201310186549X A CN 201310186549XA CN 201310186549 A CN201310186549 A CN 201310186549A CN 103278837 A CN103278837 A CN 103278837A
- Authority
- CN
- China
- Prior art keywords
- gnss
- sins
- subsystem
- error
- 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.)
- Granted
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 63
- 238000000034 method Methods 0.000 title claims abstract description 57
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 30
- 238000005259 measurement Methods 0.000 claims abstract description 35
- 238000012937 correction Methods 0.000 claims abstract description 17
- 230000002159 abnormal effect Effects 0.000 claims abstract description 15
- 230000004927 fusion Effects 0.000 claims abstract description 11
- 238000013178 mathematical model Methods 0.000 claims abstract description 8
- 238000012545 processing Methods 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 44
- 238000005070 sampling Methods 0.000 claims description 14
- 230000000694 effects Effects 0.000 claims description 7
- 230000001133 acceleration Effects 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 5
- 238000012546 transfer Methods 0.000 claims description 3
- 230000006978 adaptation Effects 0.000 claims description 2
- 239000000654 additive Substances 0.000 claims description 2
- 230000000996 additive effect Effects 0.000 claims description 2
- 238000012360 testing method Methods 0.000 description 14
- 230000008859 change Effects 0.000 description 3
- 230000035772 mutation Effects 0.000 description 3
- 230000002411 adverse Effects 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 1
- 238000007429 general method Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 230000003313 weakening effect Effects 0.000 description 1
Images
Landscapes
- Navigation (AREA)
Abstract
本发明公开了一种基于自适应滤波的SINS/GNSS多级容错组合导航方法,步骤如下:一、建立SINS/GNSS组合导航系统数学模型:建立SINS误差方程,利用自适应卡尔曼滤波方法对SINS子系统和GNSS子系统进行信息融合,估计SINS的各误差量并对其进行反馈校正;二、子系统状态判断:根据SINS的陀螺采样值和加速度计采样值判断SINS子系统的工作状态,根据GNSS输出量测值判断GNSS子系统的工作状态;三、导航决策匹配:根据SINS子系统和GNSS子系统的工作状态进行导航决策匹配处理、输出导航信息,并循环执行步骤二、三。该方法在SINS异常、GNSS丢星失锁、噪声突变等动态情况下有效地提高组合系统的容错性能、可靠性和导航精度,在地面车辆、飞机、导弹及舰船领域有广泛的应用前景。
Description
技术领域
本发明涉及一种基于自适应滤波的SINS/GNSS多级容错组合导航方法,可用于提高地面车辆、飞机、导弹或者舰船用SINS/GNSS组合导航系统的容错性、可靠性及导航精度。
背景技术
捷联惯性导航系统(SINS)是一种不依赖于任何外部信息、也不向外辐射能量的自主式导航系统,具有体积小、数据更新率高、短时精度高及不受气候条件限制等优点,被广泛应用于航空航天及民用等领域,然而其误差随工作时间积累,即其精度具有时间相关性,不适于长时间导航;卫星导航定位系统具有与时间无关、定位输出稳定等特点,但定位条件苛刻,极易受到遮挡等外部因素的干扰。因此常将两者结合以构成SINS/GNSS组合导航系统。
在基于速度、位置组合的SINS/GNSS组合导航系统中,通常方法是通过建立组合导航系统的数学模型,以SINS的速度、位置、姿态及惯性测量器件的误差量作为状态量,以GNSS输出的位置、速度为观测量,通过卡尔曼滤波最优估计算法将SINS及GNSS子系统进行信息融合,估计出SINS误差后进行反馈校正,提高导航定位精度。然而,对于以SINS为主、GNSS辅助定位的组合导航系统而言,从本质上SINS及GNSS子系统仍然是独立工作的,并且卡尔曼滤波是量测驱动的,在高动态及恶劣环境等情况下各子系统出现的极短异常均会降低组合系统的导航精度,使系统稳定性变差;同时,系统数学模型和噪声统计不准确、噪声突变等情况均可能导致常规卡尔曼滤波的收敛性变差、精度下降,甚至导致滤波发散,同样极大地影响了组合系统的导航精度。
发明内容
本发明的目的在于提供一种基于自适应滤波的SINS/GNSS多级容错组合导航方法,该方法在SINS异常、GNSS丢星失锁、噪声突变等动态情况下有效地提高组合系统的容错性能、可靠性和导航精度。
本发明的技术解决方案为:一种基于自适应滤波的SINS/GNSS多级容错组合导航方法,包括下列步骤:
步骤1,建立SINS/GNSS组合导航系统数学模型:建立SINS误差方程,利用自适应卡尔曼滤波方法对SINS子系统和GNSS子系统进行信息融合,估计SINS的各误差量并对其进行反馈校正;
步骤2,子系统状态判断:根据SINS的陀螺采样值和加速度计采样值判断SINS子系统的工作状态,根据GNSS输出量测值判断GNSS子系统的工作状态;
步骤3,导航决策匹配:根据SINS子系统和GNSS子系统的工作状态进行导航决策匹配处理、输出导航信息,并循环执行步骤2~3。
本发明与现有技术相比其显著效果是:(1)本发明采用的多级容错组合导航方法,对组合系统SINS及GNSS子系统工作状态进行判断,匹配了不同状态下的导航策略,安全可靠;(2)将自适应滤波作为信息融合算法,可在SINS、GNSS异常及噪声突变、高动态等情况下减小由SINS及GNSS工作异常带来的不利影响;(3)有效提高了地面车辆、飞机、导弹或者舰船用SINS/GNSS组合导航系统的容错性、可靠性及导航精度。
附图说明
图1是本发明基于自适应滤波的SINS/GNSS多级容错组合导航方法的原理框图。
图2是本发明基于自适应滤波的SINS/GNSS多级容错组合导航方法的导航流程。
图3是实施例中组合导航系统的地面跑车试验轨迹曲线图。
图4是实施例中组合导航系统的地面跑车试验位置曲线图。
图5是实施例中组合导航系统的飞行试验轨迹曲线图。
图6是实施例中组合导航系统的飞行试验使用常规滤波CKF法与基于自适应滤波AKF的SINS/GNSS多级容错组合导航法的速度对比曲线图。
图7是实施例中组合导航系统的飞行试验使用常规滤波CKF法与基于自适应滤波AKF的SINS/GNSS多级容错组合导航法的位置对比曲线图。
图8是实施例中组合导航系统的飞行试验使用常规滤波CKF法与基于自适应滤波AKF的SINS/GNSS多级容错组合导航法的姿态对比曲线图。
具体实施方式
本发明通过建立组合导航系统的数学模型,以SINS的速度、位置、姿态及惯性测量器件的误差量作为状态量,以GNSS输出的位置、速度为观测量,通过卡尔曼滤波最优估计算法将SINS及GNSS子系统进行信息融合,估计出SINS误差后进行反馈校正,提高导航定位精度。
然而,对于以SINS为主、GNSS辅助定位的组合导航系统而言,从本质上SINS及GNSS子系统仍然是独立工作的,并且卡尔曼滤波是量测驱动的,在高动态及恶劣环境等情况下各子系统出现的极短异常均会降低组合系统的导航精度,使系统稳定性变差;同时,系统数学模型和噪声统计不准确、噪声突变等情况均可能导致常规卡尔曼滤波的收敛性变差、精度下降,甚至导致滤波发散,同样极大地影响了组合系统的导航精度。因此,需要根据SINS和GNSS子系统工作状态的不同,采用不同的导航策略,同时采用自适应滤波算法,提高组合导航系统的导航精度和系统可靠性、容错性能。
因此,所述的一种基于自适应滤波的SINS/GNSS多级容错组合导航方法的基本原理是:首先,通过多级容错组合导航方法对SINS及GNSS子系统的工作状态进行判断;然后,根据子系统工作状态的不同分别匹配不同的组合导航策略。其中,当SINS及GNSS子系统均正常工作时,建立以SINS/GNSS组合系统误差为状态量、GNSS提供的速度位置信息为观测量的系统状态方程,采用自适应卡尔曼滤波算法进行信息融合,估计出状态误差并进行反馈校正,提高了组合导航系统的容错性能、可靠性及导航精度。
下面结合附图与具体实施例对本发明做进一步详细描述。
如图1、2所示,本发明基于自适应滤波的SINS/GNSS多级容错组合导航方法,包括下列步骤:
步骤1,建立SINS/GNSS组合导航系统数学模型:建立SINS误差方程,利用自适应卡尔曼滤波方法对SINS子系统和GNSS子系统进行信息融合,估计SINS的各误差量并对其进行反馈校正;
(1.1)建立SINS误差方程,具体如下:
(a)系统状态方程:
X=[ψE ψN ψU δVE δVN δVU δL δλ δh εx εy εz ▽x ▽y ▽z]T
其中ψE、ψN、ψU分别为东、北、天方向的平台失准角误差;δVE、δVN、δVU分别为东、北、天方向的速度误差;δL、δλ、δh分别为纬度、经度及高度误差;εx、εy、εz分别为陀螺常值漂移在x、y、z轴上的分量;▽x、▽y、▽z分别为加速度计常值偏置在x、y、z轴上的分量;FN为对应9个基本导航参数的系统阵;为姿态矩阵; 分别为陀螺噪声在x、y、z轴上的分量;分别为加表噪声在x、y、z轴上的分量;
(b)系统量测方程:
Z=HX+V
其中,Z为观测矢量,H为观测矩阵,V为观测噪声矩阵,具体如下:
Z=[δL δλ δh δVE δVN δVU]T,H=[HP HV]T
HP=[03×6 diag[Rm Rn cosL 1]03×6]
HV=[03×3 I3×3 03×9]
其中Rm为椭球子午圈上各点的曲率半径,Rn为卯酉圈(它所在的平面与子午面垂直)上各点的曲率半径,L、λ、h分别表示纬度、经度及高度,VE、VN、VU分别为东、北、天方向的速度,VL、Vλ、Vh分别表示GNSS的纬度、经度、高度的观测误差,分别表示GNSS的东、北、天方向上的速度观测误差。
(1.2)利用自适应卡尔曼滤波方法对SINS子系统和GNSS子系统进行信息融合,估计SINS各误差量并对其进行反馈校正,具体为:
如图2所示,根据系统状态方程和系统量测方程,采用基于单步量测新息矢量rk的自适应卡尔曼滤波信息融合算法,其算法公式编排具体如下:
①状态一步预测方程:
②一步预测均方误差方程:
其中,Pk,k-1为k-1时刻到k时刻的系统状态协方差阵,Pk-1为k-1时刻的系统状态协方差阵,Sk为自适应因子,Qk-1为k-1时刻系统噪声矩阵、Γk-1为k-1时刻系统噪声驱动矩阵;
③卡尔曼滤波增益方程:
其中,Kk为k时刻系统增益矩阵,Hk为k时刻系统量测矩阵,Rk为k时刻系统量测噪声矩阵;
④状态估计值更新方程:
为k时刻系统状态估计值,rk为新息矢量,Kk为k时刻系统增益矩阵,Zk为k时刻量测向量;
⑤估计均方误差方程:
Pk为k时刻的系统状态协方差阵,I是单位阵;
基于单步量测新息rk的滤波发散判据为:
其中λ为常值系数,取1~5,若该式成立,则滤波发散;反之不发散。在滤波发散情况下,组合导航系统的各误差增大,降低了滤波精度。所以根据滤波发散判据,如果判断滤波发散,则通过求出自适应权重因子Sk以扩大Pk,k-1的作用,增大滤波增益,从而增强新量测值的修正作用,进而抑制滤波发散;如果判断滤波不发散,即Sk=1,则可以直接使用常规的卡尔曼滤波算法进行修正。
步骤2,子系统状态判断:根据SINS的陀螺采样值和加速度计采样值判断SINS子系统的工作状态,根据GNSS输出量测值判断GNSS子系统的工作状态;
子系统状态判断的具体步骤为:
(2.1)根据SINS的陀螺仪采样值和加速度计采样值判断SINS子系统的工作状态,设A(axis)max为加速度阈值、ω(axis)max为角速度阈值(通常根据惯性测量器件或载体的动态极限设定),判断加速度计采样值Aaxis和陀螺采样值ωaxis是否满足以下条件:
|Aaxis|<A(axis)max
|ωaxis|<ω(axis)max
当满足上式时,则SINS子系统的工作状态正常,否则SINS子系统的工作状态异常;
(2.2)根据GNSS输出量测值判断GNSS子系统的工作状态,先后进行外层判断和内层判断:
(a)外层判断为收星条件判断:设定n为最小收星数、dop为精度因子门限,判断收星数Nsats和精度因子xDOP是否满足以下条件:
Nsats≥n
xDOP≤dop
当满足上式时,继续内层判断,否则认为GNSS子系统的工作状态异常;
(b)内层判断主要依靠SINS数据短时间内具有较高的稳定性和精度这一特性,内层判断对GNSS量测粗大误差进行判断,分为位置粗大误差判断、速度粗大误差判断以及GNSS误差带判断;设定下标GNSS、SINS分别代表GNSS接收机输出值和惯导输出值,下标1、0分别代表当前时刻值和前一时刻值,具体判断方法如下:
①位置粗大误差判断:
位置粗大误差derr判断方法具体公式如下,其中,L、λ、H分别代表载体在地理系下的纬度、经度及高度,Rm为椭球子午圈上各点的曲率半径,Rn为卯酉圈(它所在的平面与子午面垂直)上各点的曲率半径:
derr={[Rm((LGNSS1-LGNSS0)-(LSINS1-LSINS0))]2
+[Rncos(LSINS1)·((λGNSS1-λGNSS0)-(λSINS1-λSINS0))]2
+[(HGNSS1-HGNSS0)-(HSINS1-HSINS0)]2}1/2
当derr大于所设定的位置误差阀值(如100m)时,则GNSS定位信息受到干扰,位置数据异常;否则,GNSS位置数据正常;
②速度粗大误差判断:
速度粗大误差Verr判断方法具体公式如下,其中V表示速度数据,下标E、N、U代表东、北、天方向:
当Verr大于设定的速度误差阈值(如10m/s)时,则认为GNSS测速信息受到干扰,速度数据异常;否则,GNSS速度数据正常;
③GNSS误差带判断:
采用下列公式判断当前时刻GNSS输出的位置数据是否处于SINS误差范围内,具体公式如下:
dperr={[Rm(LGNSS1-LSINS1)]2+[Rncos(LSINS1)·(λGNSS1-λSINS1)]2+(HGNSS1-HSINS1)2}1/2
设eGNSS为GNSS位置误差阈值、eSINS为SINS位置误差阈值(如均设定为50m);若dperr≤eGNSS+eSINS成立,GNSS数据处于SINS误差范围内,GNSS数据有效;否则,GNSS数据无效;
根据上述判断方法,当位置、速度数据均正常并且GNSS数据处于SINS误差范围内时,GNSS子系统正常;否则,GNSS子系统工作异常,数据存在突变。
步骤3,导航决策匹配:根据SINS子系统和GNSS子系统的工作状态进行导航决策匹配处理,并返回步骤2,根据GNSS、IMU工作状态的不同匹配相应导航策略,以增强系统适应性和鲁棒性,具体如下:
(3.1)当SINS子系统、GNSS子系统工作状态均正常时,采用SINS/GNSS组合导航:将SINS和GNSS进行位置速度误差组合得到误差方程,经卡尔曼滤波估计出载体的位置、速度和姿态误差,对SINS的位置、速度、横滚角和俯仰角进行反馈校正,其中滤波算法采用自适应滤波,滤波周期为1s;
(3.2)当SINS子系统工作状态异常、GNSS子系统工作状态正常时,放弃当前时刻SINS中陀螺仪和加速度计的量测值,用前一时刻的量测值替代:
ω(k)axis=ω(k-1)axis
A(k)axis=A(k-1)axis
其中ω(k)axis为k时刻的角速度,ω(k-1)axis为k-1时刻的角速度,A(k)axis为k时刻的加速度,A(k-1)axis为k-1时刻的加速度;
(3.3)当SINS子系统工作状态正常、GNSS子系统工作状态异常时,采用丢星算法处理;从丢星状态恢复收星时,利用状态误差转移矩阵F估计导航误差并对导航输出进行修正。
(a)在捷联解算后,采用照常进行状态及均方误差的时间更新,省略量测更新的方法处理短时间丢星情况,方法如下:
Pk=Pk,k-1
其中Xk为k时刻的n维状态向量,Xk-1为k-1时刻的n维状态向量,为状态Xk的卡尔曼滤波估计值,为状态Xk-1的卡尔曼滤波估计值,为利用计算得到的对Xk的一步预测;Φk,k-1为k-1到k时刻的系统一步转移矩阵(n×n阶),为Φk,k-1的转置矩阵;Pk为估值的均方误差阵,Pk-1为估值的均方误差阵,Pk,k-1为估值的均方误差阵;Γk-1为系统噪声矩阵(n×r阶),它表征由k-1到k时刻的各个系统噪声分别影响k时刻各个状态的程度,为Γk-1的转置矩阵;Qk-1为k-1时刻的系统噪声的方差矩阵。
(b)但是当量测缺失时间过长或者由此导致的状态估计均方误差过大时,应当否定滤波结果的有效性,甚至重置卡尔曼滤波器。
当GNSS从长时间异常恢复时,用GNSS给出的位置、速度作为当前时刻组合导航的位置、速度值;同时当采用的惯性器件稳定性较好时,可采用误差状态转移阵估计出导航误差并进行修正。首先,在丢星期间计算与滤波周期相应的Φk,k-1并连乘,得到丢星前一时刻t0到当前时刻t1状态转移矩阵下式所示:
其中,T为滤波周期;然后由此及t0时刻的误差状态量推算出当前t1时刻的误差状态量,并修正导航输出,如下式所示:
其中为t0到t1时刻的系统一步转移矩阵(n×n阶), ε、▽分别为t0时刻的平台失准角误差、速度误差、位置误差、陀螺漂移、加速度计零偏, ε、▽分别为t1时刻的平台失准角误差、速度误差、位置误差、陀螺漂移、加速度计零偏。
(3.4)当SINS子系统、GNSS子系统工作状态均异常时,采用机动目标的轨迹预测方法对载体运动状态进行估计,对载体当前时刻运动状态进行估计,削弱系统无法正常工作带来的不利影响。
实施例1
本发明基于自适应滤波的SINS/GNSS多级容错组合导航方法,已应用于SINS/GNSS组合导航系统,并先后经过多次试验,主要包括地面跑车试验及飞行试验:
(1)地面跑车试验主要验证低动态下组合导航的位置重复性,轨迹曲线及位置曲线分别如图3~4所示,其位置均方差结果如表1所示:
表1位置均方差结果
从场地跑车试验结果看出,其位置重复性较好,表明该组合导航系统在低动态下具有良好的稳定性和较高的导航定位精度。
(2)飞行试验进一步验证动态环境下组合导航的导航精度,图5为飞行轨迹曲线,图6~8分别为常规滤波(CKF)与采用本发明的容错导航方法及自适应滤波(AKF)时的速度、位置、姿态对比曲线。从试验结果看出,在飞行环境恶劣的情况下,CKF由于不具备发散在线判断、发散抑制措施以及缺乏噪声变化时的自适应能力等,其滤波效果和收敛性均较差,并且出现了滤波发散,导致滤波结果偏差过大,速度、姿态误差均超出指标,无法满足组合导航的要求;当使用AKF并采用多级容错组合导航方法时,效果较好,无论滤波收敛性、滤波精度还是稳定性均要优于CKF。
上述试验表明,本发明的一种基于自适应滤波的SINS/GNSS多级容错组合导航方法可以有效提高SINS/GNSS组合导航系统的导航精度及容错性、可靠性。
Claims (5)
1.一种基于自适应滤波的SINS/GNSS多级容错组合导航方法,其特征在于,包括下列步骤:
步骤1,建立SINS/GNSS组合导航系统数学模型:建立SINS误差方程,利用自适应卡尔曼滤波方法对SINS子系统和GNSS子系统进行信息融合,估计SINS的各误差量并对其进行反馈校正;
步骤2,子系统状态判断:根据SINS的陀螺采样值和加速度计采样值判断SINS子系统的工作状态,根据GNSS输出量测值判断GNSS子系统的工作状态;
步骤3,导航决策匹配:根据SINS子系统和GNSS子系统的工作状态进行导航决策匹配处理、输出导航信息,并循环执行步骤2~3。
2.根据权利要求1所述的基于自适应滤波的SINS/GNSS多级容错组合导航方法,其特征在于,步骤1所述的建立SINS误差方程,具体如下:
(a)系统状态方程:
X=[ψE ψN ψU δVE δVN δVU δL δλ δh εx εy εz ▽x ▽y ▽z]T
其中ψE、ψN、ψU分别为东、北、天方向的平台失准角误差;δVE、δVN、δVU分别为东、北、天方向的速度误差;δL、δλ、δh分别为纬度、经度及高度误差;εx、εy、εz分别为陀螺常值漂移在x、y、z轴上的分量;▽x、▽y、▽z分别为加速度计常值偏置在x、y、z轴上的分量;FN为对应9个基本导航参数的系统阵;为姿态矩阵; 分别为陀螺噪声在x、y、z轴上的分量;分别为加表噪声在x、y、z轴上的分量;
(b)系统量测方程:
Z=HX+V
其中,Z为观测矢量,H为观测矩阵,V为观测噪声矩阵,具体如下:
Z=[δL δλ δh δVE δVN δVU]T,H=[HP HV]T
HP=[03×6 diag[Rm Rn cosL 1] 03×6]
HV=[03×3 I3×3 03×9]
3.根据权利要求1或2所述的基于自适应滤波的SINS/GNSS多级容错组合导航方法,其特征在于,步骤1中所述利用自适应卡尔曼滤波方法对SINS子系统和GNSS子系统进行信息融合,估计SINS各误差量并对其进行反馈校正,具体为:
根据系统状态方程和系统量测方程,采用基于单步量测新息矢量rk的自适应卡尔曼滤波信息融合算法,其算法公式编排具体如下:
①状态一步预测方程:
②一步预测均方误差方程:
其中,Pk,k-1为k-1时刻到k时刻的系统状态协方差阵,Pk-1为k-1时刻的系统状态协方差阵,Sk为自适应因子,Qk-1为k-1时刻系统噪声矩阵,Γk-1为k-1时刻系统噪声驱动矩阵;
③卡尔曼滤波增益方程:
其中,Kk为k时刻系统增益矩阵,Hk为k时刻系统量测矩阵,Rk为k时刻系统量测噪声矩阵;
④状态估计值更新方程:
为k时刻系统状态估计值,rk为新息矢量,Kk为k时刻系统增益矩阵,Zk为k时刻量测向量;
⑤估计均方误差方程:
Pk为k时刻的系统状态协方差阵,I是单位阵;
基于单步量测新息rk的滤波发散判据为:
其中λ为常值系数,取1~5,若该式成立,则滤波发散;否则滤波不发散:
如果判断滤波发散,则通过求出自适应权重因子Sk以扩大Pk,k-1的作用,增大滤波增益,从而增强新量测值的修正作用,进而抑制滤波发散;如果判断滤波不发散,即Sk=1,则可以直接使用常规的卡尔曼滤波算法进行修正;
4.根据权利要求1所述的基于自适应滤波的SINS/GNSS多级容错组合导航方法,其特征在于,步骤2中所述子系统状态判断的具体步骤为:
(2.1)根据SINS的陀螺仪采样值和加速度计采样值判断SINS子系统的工作状态,设A(axis)max为加速度阈值、ω(axis)max为角速度阈值(通常根据惯性测量器件或载体的动态极限设定),判断加速度计采样值Aaxis和陀螺采样值ωaxis是否满足以下条件:
|Aaxis|<A(axis)max
|ωaxis|<ω(axis)max
当满足上式时,则SINS子系统的工作状态正常,否则SINS子系统的工作状态异常;
(2.2)根据GNSS输出量测值判断GNSS子系统的工作状态,先后进行外层判断和内层判断:
(a)外层判断为收星条件判断:设定n为最小收星数、dop为精度因子门限,判断收星数Nsats和精度因子xDOP是否满足以下条件:
Nsats≥n
xDOP≤dop
当满足上式时,继续内层判断,否则认为GNSS子系统的工作状态异常;
(b)内层判断对GNSS量测粗大误差进行判断,分为位置粗大误差判断、速度粗大误差判断以及GNSS误差带判断;设定下标GNSS、SINS分别代表GNSS接收机输出值和惯导输出值,下标1、0分别代表当前时刻值和前一时刻值,具体判断方法如下:
①位置粗大误差判断:
位置粗大误差derr判断方法具体公式如下,其中,L、λ、H分别代表载体在地理系下的纬度、经度及高度,Rm为椭球子午圈上各点的曲率半径,Rn为卯酉圈上各点的曲率半径:
derr={[Rm((LGNSS1-LGNSS0)-(LSINS1-LSINS0))]2
+[Rncos(LSINS1)·((λGNSS1-λGNSS0)-(λSINS1-λSINS0))]2
+[(HGNSS1-HGNSS0)-(HSINS1-HSINS0)]2}1/2
当derr大于所设定的位置误差阀值时,则GNSS位置数据异常;否则,GNSS位置数据正常;
②速度粗大误差判断:
速度粗大误差Verr判断方法具体公式如下,其中V表示速度数据,下标E、N、U代表东、北、天方向:
当Verr大于设定的速度误差阈值时,GNSS速度数据异常;否则,GNSS速度数据正常;
③GNSS误差带判断:
采用下列公式判断当前时刻GNSS输出的位置数据是否处于SINS误差范围内,具体公式如下:
dperr={[Rm(LGNSS1-LSINS1)]2+[Rncos(LSINS1)·(λGNSS1-λSINS1)]2+(HGNSS1-HSINS1)2}1/2
设eGNSS为GNSS位置误差阈值、eSINS为SINS位置误差阈值,若dperr≤eGNSS+eSINS成立,GNSS数据处于SINS误差范围内,GNSS数据有效;否则,GNSS数据无效;
根据上述判断方法,当位置、速度数据均正常并且GNSS数据处于SINS误差范围内时,GNSS子系统正常;否则,GNSS子系统工作异常。
5.根据权利要求1所述的基于自适应滤波的SINS/GNSS多级容错组合导航方法,其特征在于,步骤3中所述导航决策匹配的具体方法如下:
(3.1)当SINS子系统、GNSS子系统工作状态均正常时,采用SINS/GNSS组合导航:将SINS和GNSS进行位置速度误差组合得到误差方程,经卡尔曼滤波估计出载体的位置、速度和姿态误差,对SINS的位置、速度、横滚角和俯仰角进行反馈校正;
(3.2)当SINS子系统工作状态异常、GNSS子系统工作状态正常时,放弃当前时刻SINS中陀螺仪和加速度计的量测值,用前一时刻的量测值替代:
ω(k)axis=ω(k-1)axis
A(k)axis=A(k-1)axis
其中ω(k)axis为k时刻的角速度,ω(k-1)axis为k-1时刻的角速度,A(k)axis为k时刻的加速度,A(k-1)axis为k-1时刻的加速度;
(3.3)当SINS子系统工作状态正常、GNSS子系统工作状态异常时,采用丢星算法处理;从丢星状态恢复收星时,利用状态误差转移矩阵F估计导航误差并对导航输出进行修正;
(3.4)当SINS子系统、GNSS子系统工作状态均异常时,采用机动目标的轨迹预测方法对载体运动状态进行估计。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310186549.XA CN103278837B (zh) | 2013-05-17 | 2013-05-17 | 基于自适应滤波的sins/gnss多级容错组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310186549.XA CN103278837B (zh) | 2013-05-17 | 2013-05-17 | 基于自适应滤波的sins/gnss多级容错组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103278837A true CN103278837A (zh) | 2013-09-04 |
CN103278837B CN103278837B (zh) | 2015-04-15 |
Family
ID=49061410
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310186549.XA Active CN103278837B (zh) | 2013-05-17 | 2013-05-17 | 基于自适应滤波的sins/gnss多级容错组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103278837B (zh) |
Cited By (28)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103675880A (zh) * | 2013-11-29 | 2014-03-26 | 航天恒星科技有限公司 | 一种卫星信号阻塞情况下的持续导航方法 |
CN104181572A (zh) * | 2014-05-22 | 2014-12-03 | 南京理工大学 | 一种弹载惯性/卫星紧组合导航方法 |
CN104459747A (zh) * | 2014-12-19 | 2015-03-25 | 北京临近空间飞艇技术开发有限公司 | 一种gnss/ins组合导航方法及系统 |
CN104913783A (zh) * | 2015-07-09 | 2015-09-16 | 上海华测导航技术股份有限公司 | 基于自适应滤波算法实现车辆自动导航的方法 |
CN104977002A (zh) * | 2015-06-12 | 2015-10-14 | 同济大学 | 基于sins/双od的惯性组合导航系统及其导航方法 |
CN105371838A (zh) * | 2014-08-06 | 2016-03-02 | 航天恒星科技有限公司 | 基于ins辅助gnss单天线测姿的组合导航方法及系统 |
CN105698790A (zh) * | 2016-03-17 | 2016-06-22 | 孙红星 | 一种gnss-ins组合中的桥接方法 |
CN105737828A (zh) * | 2016-05-09 | 2016-07-06 | 郑州航空工业管理学院 | 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法 |
CN106323276A (zh) * | 2015-06-19 | 2017-01-11 | 中船航海科技有限责任公司 | 一种便携式舰船综合导航系统及导航方法 |
US9593570B2 (en) | 2014-07-15 | 2017-03-14 | Caterpillar Inc. | Drill positioning system utilizing drill operation state |
CN106595669A (zh) * | 2016-12-27 | 2017-04-26 | 南京理工大学 | 一种旋转体姿态解算方法 |
CN106767788A (zh) * | 2017-01-04 | 2017-05-31 | 北京航天自动控制研究所 | 一种组合导航方法和系统 |
CN106772517A (zh) * | 2016-12-29 | 2017-05-31 | 华南农业大学 | 基于双天线gnss接收机/陀螺仪信息融合的农机横滚角测试方法 |
CN107544536A (zh) * | 2016-06-29 | 2018-01-05 | 通用电气公司 | 用于基于性能的到达以及排序和间距的方法和系统 |
CN107588770A (zh) * | 2017-10-26 | 2018-01-16 | 北京正安维视科技股份有限公司 | 一种基于惯性导航的室内定位方法及设备 |
WO2018059532A1 (zh) * | 2016-09-30 | 2018-04-05 | 华为技术有限公司 | 观测时滞系统的组合导航数据解算方法、装置及导航设备 |
CN109000643A (zh) * | 2018-06-01 | 2018-12-14 | 深圳市元征科技股份有限公司 | 导航参数获取方法、车辆急速转弯判断方法、系统、装置 |
CN109357674A (zh) * | 2018-12-07 | 2019-02-19 | 上海机电工程研究所 | 一种gnss和ins组合导航观测量引入方法 |
CN109974695A (zh) * | 2019-04-09 | 2019-07-05 | 东南大学 | 基于Krein空间的水面舰艇导航系统的鲁棒自适应滤波方法 |
CN109990789A (zh) * | 2019-03-27 | 2019-07-09 | 广东工业大学 | 一种飞行导航方法、装置及相关设备 |
CN110189421A (zh) * | 2019-05-10 | 2019-08-30 | 山东大学 | 一种北斗gnss/dr组合导航出租车计程计时系统及其运行方法 |
CN111089580A (zh) * | 2018-10-23 | 2020-05-01 | 北京自动化控制设备研究所 | 一种基于协方差交叉的无人战车同时定位与地图构建方法 |
CN112105961A (zh) * | 2019-07-26 | 2020-12-18 | 深圳市大疆创新科技有限公司 | 基于多数据融合的定位方法、可移动平台及存储介质 |
CN112325875A (zh) * | 2020-10-11 | 2021-02-05 | 中国运载火箭技术研究院 | 容错处理方法及装置、存储介质、电子装置 |
CN113607176A (zh) * | 2021-10-11 | 2021-11-05 | 智道网联科技(北京)有限公司 | 组合导航系统轨迹输出方法及装置 |
CN114295122A (zh) * | 2021-12-02 | 2022-04-08 | 河北汉光重工有限责任公司 | 用于嵌入式系统的sins_gnss时间同步方法及系统 |
CN114488224A (zh) * | 2021-12-24 | 2022-05-13 | 西南交通大学 | 一种用于卫星集中式自主导航的自适应滤波方法 |
CN117452448A (zh) * | 2023-12-25 | 2024-01-26 | 合众新能源汽车股份有限公司 | 高精定位跳变联合故障诊断方法和车辆定位控制系统 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20020026283A1 (en) * | 2000-05-30 | 2002-02-28 | Tucker Terry A. | Method and apparatus for improving performance of an inertial navigation system having global positioning system corrections |
WO2005071431A1 (en) * | 2004-01-23 | 2005-08-04 | Novatel Inc. | Inertial gps navigation system with modified kalman filter |
CN1763475A (zh) * | 2005-11-04 | 2006-04-26 | 北京航空航天大学 | 一种sins/gps组合导航系统的空中机动对准方法 |
CN1869630A (zh) * | 2006-04-19 | 2006-11-29 | 吉林大学 | 完备汽车运动状态测量系统 |
CN1945212A (zh) * | 2006-11-03 | 2007-04-11 | 北京航空航天大学 | 一种sins/gps组合导航系统的自适应加权反馈校正滤波方法 |
CN201751861U (zh) * | 2010-04-19 | 2011-02-23 | 重庆星熠导航设备有限公司 | 一种惯性辅助卫星导航系统的接收机 |
-
2013
- 2013-05-17 CN CN201310186549.XA patent/CN103278837B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20020026283A1 (en) * | 2000-05-30 | 2002-02-28 | Tucker Terry A. | Method and apparatus for improving performance of an inertial navigation system having global positioning system corrections |
WO2005071431A1 (en) * | 2004-01-23 | 2005-08-04 | Novatel Inc. | Inertial gps navigation system with modified kalman filter |
CN1763475A (zh) * | 2005-11-04 | 2006-04-26 | 北京航空航天大学 | 一种sins/gps组合导航系统的空中机动对准方法 |
CN1869630A (zh) * | 2006-04-19 | 2006-11-29 | 吉林大学 | 完备汽车运动状态测量系统 |
CN1945212A (zh) * | 2006-11-03 | 2007-04-11 | 北京航空航天大学 | 一种sins/gps组合导航系统的自适应加权反馈校正滤波方法 |
CN201751861U (zh) * | 2010-04-19 | 2011-02-23 | 重庆星熠导航设备有限公司 | 一种惯性辅助卫星导航系统的接收机 |
Non-Patent Citations (3)
Title |
---|
陈帅: "精确制导炸弹低成本惯导-卫星组合导航方法研究", 《中国博士学位论文全文数据库 信息科技辑 I136-35》, vol. 2010, no. 1, 15 January 2010 (2010-01-15) * |
陈帅等: "弹载SINS-GPS组合导航信息同步与融合技术", 《火力与指挥控制》, vol. 34, no. 10, 31 October 2009 (2009-10-31), pages 2 * |
陈帅等: "机载多传感器组合导航系统设计", 《第三十届中国控制会议论文集》, 24 July 2011 (2011-07-24) * |
Cited By (43)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103675880B (zh) * | 2013-11-29 | 2016-01-13 | 航天恒星科技有限公司 | 一种卫星信号阻塞情况下的持续导航方法 |
CN103675880A (zh) * | 2013-11-29 | 2014-03-26 | 航天恒星科技有限公司 | 一种卫星信号阻塞情况下的持续导航方法 |
CN104181572A (zh) * | 2014-05-22 | 2014-12-03 | 南京理工大学 | 一种弹载惯性/卫星紧组合导航方法 |
US9593570B2 (en) | 2014-07-15 | 2017-03-14 | Caterpillar Inc. | Drill positioning system utilizing drill operation state |
CN105371838A (zh) * | 2014-08-06 | 2016-03-02 | 航天恒星科技有限公司 | 基于ins辅助gnss单天线测姿的组合导航方法及系统 |
CN104459747A (zh) * | 2014-12-19 | 2015-03-25 | 北京临近空间飞艇技术开发有限公司 | 一种gnss/ins组合导航方法及系统 |
CN104977002A (zh) * | 2015-06-12 | 2015-10-14 | 同济大学 | 基于sins/双od的惯性组合导航系统及其导航方法 |
CN106323276B (zh) * | 2015-06-19 | 2019-04-09 | 中船航海科技有限责任公司 | 一种便携式舰船综合导航系统及导航方法 |
CN106323276A (zh) * | 2015-06-19 | 2017-01-11 | 中船航海科技有限责任公司 | 一种便携式舰船综合导航系统及导航方法 |
CN104913783A (zh) * | 2015-07-09 | 2015-09-16 | 上海华测导航技术股份有限公司 | 基于自适应滤波算法实现车辆自动导航的方法 |
CN104913783B (zh) * | 2015-07-09 | 2018-01-23 | 上海华测导航技术股份有限公司 | 基于自适应滤波算法实现车辆自动导航的方法 |
CN105698790B (zh) * | 2016-03-17 | 2019-01-29 | 武汉际上导航科技有限公司 | 一种gnss-ins组合中的桥接方法 |
CN105698790A (zh) * | 2016-03-17 | 2016-06-22 | 孙红星 | 一种gnss-ins组合中的桥接方法 |
CN105737828A (zh) * | 2016-05-09 | 2016-07-06 | 郑州航空工业管理学院 | 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法 |
CN105737828B (zh) * | 2016-05-09 | 2018-07-31 | 郑州航空工业管理学院 | 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法 |
CN107544536A (zh) * | 2016-06-29 | 2018-01-05 | 通用电气公司 | 用于基于性能的到达以及排序和间距的方法和系统 |
CN107884800A (zh) * | 2016-09-30 | 2018-04-06 | 华为技术有限公司 | 观测时滞系统的组合导航数据解算方法、装置及导航设备 |
WO2018059532A1 (zh) * | 2016-09-30 | 2018-04-05 | 华为技术有限公司 | 观测时滞系统的组合导航数据解算方法、装置及导航设备 |
CN107884800B (zh) * | 2016-09-30 | 2020-06-26 | 华为技术有限公司 | 观测时滞系统的组合导航数据解算方法、装置及导航设备 |
CN106595669B (zh) * | 2016-12-27 | 2023-04-11 | 南京理工大学 | 一种旋转体姿态解算方法 |
CN106595669A (zh) * | 2016-12-27 | 2017-04-26 | 南京理工大学 | 一种旋转体姿态解算方法 |
CN106772517A (zh) * | 2016-12-29 | 2017-05-31 | 华南农业大学 | 基于双天线gnss接收机/陀螺仪信息融合的农机横滚角测试方法 |
CN106767788B (zh) * | 2017-01-04 | 2019-07-19 | 北京航天自动控制研究所 | 一种组合导航方法和系统 |
CN106767788A (zh) * | 2017-01-04 | 2017-05-31 | 北京航天自动控制研究所 | 一种组合导航方法和系统 |
CN107588770A (zh) * | 2017-10-26 | 2018-01-16 | 北京正安维视科技股份有限公司 | 一种基于惯性导航的室内定位方法及设备 |
CN109000643A (zh) * | 2018-06-01 | 2018-12-14 | 深圳市元征科技股份有限公司 | 导航参数获取方法、车辆急速转弯判断方法、系统、装置 |
CN109000643B (zh) * | 2018-06-01 | 2021-11-12 | 深圳市元征科技股份有限公司 | 导航参数获取方法、车辆急速转弯判断方法、系统、装置 |
CN111089580A (zh) * | 2018-10-23 | 2020-05-01 | 北京自动化控制设备研究所 | 一种基于协方差交叉的无人战车同时定位与地图构建方法 |
CN109357674A (zh) * | 2018-12-07 | 2019-02-19 | 上海机电工程研究所 | 一种gnss和ins组合导航观测量引入方法 |
CN109990789A (zh) * | 2019-03-27 | 2019-07-09 | 广东工业大学 | 一种飞行导航方法、装置及相关设备 |
CN109974695A (zh) * | 2019-04-09 | 2019-07-05 | 东南大学 | 基于Krein空间的水面舰艇导航系统的鲁棒自适应滤波方法 |
CN110189421A (zh) * | 2019-05-10 | 2019-08-30 | 山东大学 | 一种北斗gnss/dr组合导航出租车计程计时系统及其运行方法 |
CN112105961A (zh) * | 2019-07-26 | 2020-12-18 | 深圳市大疆创新科技有限公司 | 基于多数据融合的定位方法、可移动平台及存储介质 |
WO2021016749A1 (zh) * | 2019-07-26 | 2021-02-04 | 深圳市大疆创新科技有限公司 | 基于多数据融合的定位方法、可移动平台及存储介质 |
CN112105961B (zh) * | 2019-07-26 | 2024-06-25 | 深圳市卓驭科技有限公司 | 基于多数据融合的定位方法、可移动平台及存储介质 |
CN112325875A (zh) * | 2020-10-11 | 2021-02-05 | 中国运载火箭技术研究院 | 容错处理方法及装置、存储介质、电子装置 |
CN113607176B (zh) * | 2021-10-11 | 2021-12-10 | 智道网联科技(北京)有限公司 | 组合导航系统轨迹输出方法及装置 |
CN113607176A (zh) * | 2021-10-11 | 2021-11-05 | 智道网联科技(北京)有限公司 | 组合导航系统轨迹输出方法及装置 |
CN114295122A (zh) * | 2021-12-02 | 2022-04-08 | 河北汉光重工有限责任公司 | 用于嵌入式系统的sins_gnss时间同步方法及系统 |
CN114295122B (zh) * | 2021-12-02 | 2024-06-11 | 河北汉光重工有限责任公司 | 用于嵌入式系统的sins_gnss时间同步方法及系统 |
CN114488224A (zh) * | 2021-12-24 | 2022-05-13 | 西南交通大学 | 一种用于卫星集中式自主导航的自适应滤波方法 |
CN117452448A (zh) * | 2023-12-25 | 2024-01-26 | 合众新能源汽车股份有限公司 | 高精定位跳变联合故障诊断方法和车辆定位控制系统 |
CN117452448B (zh) * | 2023-12-25 | 2024-04-09 | 合众新能源汽车股份有限公司 | 高精定位跳变联合故障诊断方法和车辆定位控制系统 |
Also Published As
Publication number | Publication date |
---|---|
CN103278837B (zh) | 2015-04-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103278837B (zh) | 基于自适应滤波的sins/gnss多级容错组合导航方法 | |
US11754400B2 (en) | Motion constraint-aided underwater integrated navigation method employing improved Sage-Husa adaptive filtering | |
WO2020062791A1 (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN104181572B (zh) | 一种弹载惯性/卫星紧组合导航方法 | |
CN110779521A (zh) | 一种多源融合的高精度定位方法与装置 | |
CN104457754B (zh) | 一种基于sins/lbl紧组合的auv水下导航定位方法 | |
CN102508275B (zh) | 多天线gps/gf-ins深度组合定姿方法 | |
CN102830414B (zh) | 一种基于sins/gps的组合导航方法 | |
CN104422948A (zh) | 一种嵌入式组合导航系统及其方法 | |
CN101900573B (zh) | 一种实现陆用惯性导航系统运动对准的方法 | |
Stančić et al. | The integration of strap-down INS and GPS based on adaptive error damping | |
CN105091907B (zh) | Sins/dvl组合中dvl方位安装误差估计方法 | |
CN103792561B (zh) | 一种基于gnss通道差分的紧组合降维滤波方法 | |
CN102506857A (zh) | 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法 | |
CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN102636798A (zh) | 基于环路状态自检测的sins/gps深组合导航方法 | |
CN101825467A (zh) | 捷联惯性导航系统与天文导航系统实现组合导航的方法 | |
CN104457748A (zh) | 一种嵌入式瞄准吊舱测姿系统及其传递对准方法 | |
CN102393204B (zh) | 一种基于sins/cns的组合导航信息融合方法 | |
CN103697894A (zh) | 基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法 | |
CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
CN105928515A (zh) | 一种无人机导航系统 | |
CN108151765A (zh) | 一种在线实时估计补偿磁强计误差的定位测姿方法 | |
CN104634348A (zh) | 组合导航中的姿态角计算方法 | |
CN108279025B (zh) | 一种基于重力信息的光纤陀螺罗经快速精对准方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant |