一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法

一种基于强跟踪kalman滤波的级联式惯性/卫星深组合方法
技术领域
1.本发明属于惯性/卫星组合导航技术领域，具体涉及一种基于强跟踪kalman滤波的级联式惯性/卫星深组合方法。


背景技术：

2.随着科技技术的发展，需要导航定位的应用场景越来越多，对定位的精度和鲁棒性要求越来越高，不仅局限于信号良好、视野开阔的场景，还可能处于一些复杂的电磁环境中，多变的气候，建筑物遮挡、电磁干扰都可能导致载体难以接受到稳定的外源信息，导航定位进度下降，将影响载体的任务完成情况，因此，如何实现导航定位系统在复杂电磁环境下的高精度导航与定位是一个值得研究的问题。
3.全球卫星导航系统(global navigation satellite system，gnss)作为一种空间上的无线定位系统，为用户实时提供三维位置、速度和时间信息的精确信息，但是卫星接收机天线容易受到周围复杂电磁环境的干扰导致定位精度恶化甚至无法定位。捷联惯导系统(sins)具有自主性、隐蔽性等特点，能够连续提供位置、速度、姿态信息，常用于与gnss进行组合保证卫星拒止条件下的导航定位精度，但是在缺乏外源信息校正的情况下捷联惯导系统的定位误差将随着时间积累。
4.因此，设计一种能够解决传统惯性/卫星组合导航方法在强电磁干扰下定位精度差的问题是十分必要的。


技术实现要素：

5.为解决上述问题，本发明公开了一种基于强跟踪kalman滤波的级联式惯性/卫星深组合方法，建立基于强跟踪kalman滤波的预滤波算法，引入基于平滑窗口的的χ2故障检测模型，排除故障量测值，同时基于各通道载噪比值自适应的调整量测方差，建立惯性/卫星紧组合导航算法，用组合定位结果复现本地码相位和载波频率反馈到码nco和载波nco。本发明有效提高了卫星接收机跟踪环路抗干扰能力，最终提高了导航定位的精度。
6.为达到上述目的，本发明的技术方案如下：
7.一种基于强跟踪kalman滤波的级联式惯性/卫星深组合方法，包括以下步骤：
8.(1)推导卫星导航接收机跟踪环路实现步骤，引入强跟踪kalman滤波(stkf)，对码相位误差、载波频率误差进行预滤波处理；
9.(2)推导基于平滑窗口的χ2故障检测模型和基于载噪比的量测噪声计算方程。并应用于步骤(1)模型；
10.(3)由步骤(1)的预滤波输出和捷联惯导系统接收的数据信息推导惯性/卫星紧组合导航模型；
11.(4)在每次步骤(3)滤波结束后对系统误差状态进行校正，输出校正后的组合导航的姿态、速度、位置等信息并将其反馈到载波nco和码nco。
12.进一步地，所述步骤(1)中，推导卫星导航接收机跟踪环路实现步骤，引入强跟踪
kalman滤波。卫星信号跟踪过程需要解决的核心问题是对码相位和载波频率的精确跟踪，卫星信号经过射频前端处理后得到中频信号，中频信号在完成载波分离和伪码分离后产生同相i支路信号和正交相q支路信号，i支路信号和q支路信号的数学表达式如下：
[0013][0014][0015]
其中，a是gps信号幅度；n是相关器中的信号采样点数量；r(
·
)是伪随机码的自相关函数；δτ是本地码相位误差；δω是本地载波的角频率误差；t是相干积分时间；是本地载波相位误差平均值，其计算公式可以表示为：
[0016][0017]
设超前滞后相关器超前码、滞后码与即时码的间隔为0.5码片，则码相位误差δτ的计算公式为：
[0018][0019]
其中ie、qe、i
l
、q
l
分别为超前码、滞后码相关积分输出的i、q分量。以上运算通过鉴相器完成。
[0020]
卫星跟踪环路预滤波器的状态方程和量测方程为：
[0021][0022]
其中，φ
k,k-1
为t
k-1
时刻到tk时刻的一步状态状态转移矩阵；xk为状态向量；γ
k-1
为系统噪声矩阵；w
k-1
为系统激励噪声序列,其对应方差为qk；zk为量测向量；vk为量测噪声序列，其对应方差为rk。kalman滤波更新方程如下：
[0023][0024][0025][0026][0027]
pk＝(i-k
khk
)p
k,k-1
[0028]
其中pk为状态均方误差，kk为滤波增益矩阵。在实际工程中，为了便于处理，会对卡尔曼滤波的观测、状态模型进行简化，且实际工程中我们很难得到准确的噪声统计特性，因此上述模型存在不确定性，为了解决这个问题，此处以强跟踪kalman滤波进行卫星跟踪环路设计。强跟踪成立需要满足的条件为，选择一个合适的增益矩阵kk使得
[0029]
e[(γ
k j
)γ
kt
]＝0,k＝0,1,2...,j＝1,2,...
[0030]
其中γk＝z
k-hx
k,k-1
为观测量残差。我们可以对增益矩阵kk进行调整，强迫残差序列保持正交，使上式始终成立，从而达到强迫滤波器对系统实际状态保持跟踪的目的。将一步预测方差矩阵改为
[0031][0032]
其中，λk为时变渐消因子，
[0033][0034][0035][0036][0037][0038]
其中，0《ρ≤1为遗忘因子，一般取ρ＝0.95；为量测方差估计。
[0039]
选取卫星中频信号的和码相位误差δτ、载波相位误差载波角频率误差δω和载波角频率变化率误差δa作为stkf的状态向量，载波相位可视为载波频率的一次函数，载波频率可视为载波频率变化率的一次函数，载波频率变化率可模型化为缓慢变化的偏差，码相位误差也可视为载波频率的一次函数，具体形式如下：
[0040][0041]
其中，对于gps l1 c/a信号来说，α＝β＝1/1540。w
mp
、w
clock
、w
drift
、w
acc
分别为各个状态量对应的过程噪声，其协方差矩阵如下：
[0042][0043]
其中c为光速，qb、qd、qa分别为载波相位、载波频率、载波频率变化率的噪声功率谱密度，大小与接收机射频前端振荡器噪声特性有关。由于stkf是一个线性滤波器，可以将鉴
相器输出的码相位和载波相位作为观测量，则观测方程如下：
[0044][0045]
进一步地，所述步骤(2)中，推导基于平滑窗口的χ2故障检测模型和基于载噪比的量测噪声计算方程，具体如下：
[0046]
量测方差可以近似表示为长度为μ的滑动窗口内各个历元新息方差估计的平均值，
[0047][0048]
窗口宽度μ不宜设置过大，否则会产生较大的时延，故障检测能力将变弱，一般取3～5适宜。定义为故障检测函数，则εk服从自由度为m的χ2分布即
[0049]
εk～χ2(m)
[0050]
式中，m为量测向量的维度。当卫星伪距/伪距率量测信息异常时，故障检测函数不再服从上述分布，可以通过χ2分布上分为点进行故障检测，上分为点与量测维度有关，例如，当量测维度m＝6时，也就是说在量测信息正常时，认为的概率只有0.5％，而当m＝3时，该故障检测函数能够准确判断量测信息是否出现异常。
[0051]
量测噪声主要来源于跟踪环路的相位抖动和动态响应误差，因此可以通过各个卫星通道的载噪比值来计算当前时刻量测噪声：
[0052][0053]
其中，载噪比可通过窄带宽带功率比方法求得。
[0054]
进一步地，所述步骤(3)中，由步骤(1)的预滤波输出和捷联惯导系统接收的数据信息推导惯性/卫星紧组合导航模型，具体如下：
[0055]
将预滤波器输出的码相位和载波频率经过一定的运算之后可以得到各个卫星通道的伪距、伪距率信息，由卫星的广播星历文件可以计算得到卫星在地心地固坐标系(ecef)下的位置速度信息，结合捷联惯导系统接收的包括三轴陀螺仪的角增量或角速度数据和三轴加速度计的比力或比力积分增量数据可以算得另一组伪距、伪距率信息，二者作差作为紧组合的量测值。
[0056]
组合导航滤波的状态方程为：
[0057][0058]
其中，f
tc
为状态转移矩阵，g
tc
为系统噪声分配矩阵，w
tc
为系统噪声，x
tc
为状态变量，表示为：
[0059]
x
tc
＝[(φn)
t (δvn)
t (δpn)
t (εb)
t (
△b)
t cδt
u cδt
ru
]
t
[0060]
其中φn为姿态误差角向量，δvn、δpn为东北天坐标系(enu)下的速度误差向量、位置误差向量，εb、
△b分别是陀螺仪和加速度计的常值零偏，cδtu、cδt
ru
分别是卫星导航接收机钟差等效距离误差和钟漂等效距离误差；
[0061]wtc
＝[(ω
bg
)
t (ω
ba
)
t ω
tu ω
tru
]
t
[0062]
其中ω
bg
、ω
ba
、ω
tu
、ω
tru
分别表示陀螺零偏噪声、加速度计零偏噪声、钟差等效距离误差的白噪声、钟漂等效距离误差的白噪声；
[0063]
量测方程为z
tc
＝h
tc
x
tc
v
tc
，其中量测值z
tc
为卫星接收机输出伪距、伪距率数据和sins推出的各个卫星相应的伪距、伪距率数据之差，h
tc
为量测更新矩阵，v
tc
为量测噪声。
[0064][0065][0066]
量测噪声方差的大小可以由预滤波器的状态噪声方差来确定，即伪距、伪距率误差方差可以通过各个通道的预滤波器中码相位误差、载波频率误差对应的状态方差量来调整：
[0067]rρ,tc
＝p
τ
·
(λ
carr
/α)2[0068][0069]
其中，r
ρ,tc
、分别为伪距量测量、伪距率量测量对应的噪声方差矩阵，p
τ
、为对应卫星通道预滤波器码相位误差和载波频率误差状态方差矩阵，λ
carr
为载波波长，对于gps l1 c/a信号来说，α＝1/1540。
[0070]
进一步地，所述步骤(4)中，对系统误差状态进行校正，输出校正后的组合导航的姿态、速度、位置等信息并将其反馈到载波nco和码nco，具体如下：
[0071]
在步骤(4)一个组合导航滤波周期结束后得到各误差量的估计值，并用估计值对导航参数进行补偿，得到各导航参数的“精确值”，补偿公式为：
[0072]
位置：
[0073]
速度：
[0074]
钟差：
[0075]
钟漂：
[0076]
码相位：
[0077]
载波频率：
[0078]
其中a
j,k
从接收机指向各通道卫星方向的单位矢量。通过更新的位置速度信息复现的本地码相位和载波频率反馈到码nco和载波nco，就构成了闭合的跟踪环路，同时输出的位置速度信息也能够在复杂电磁干扰环境下实现高精度。
[0079]
本发明的有益效果为：
[0080]
本发明所述的一种基于强跟踪kalman滤波的级联式惯性/卫星深组合方法，首先推导卫星导航接收机跟踪环路实现步骤，引入自适应强跟踪kalman滤波(astkf)，对码相位误差、载波相位误差进行预滤波处理；其次，推导基于平滑窗口的χ2故障检测模型和基于载噪比的量测噪声计算方程，并应用于与滤波算法中；然后由上述预滤波地输出和捷联惯导系统接收的数据信息推导惯性/卫星紧组合导航模型；最后，在每个组合导航滤波周期结束后对系统误差状态进行校正，输出校正后的组合导航的姿态、速度、位置等信息并将其反馈到载波nco和码nco，从而提高卫星接收机在复杂电磁环境下的定位精度。
附图说明
[0081]
图1为本发明基于强跟踪kalman滤波的级联式惯性/卫星深组合流程图；
[0082]
图2为惯导辅助卫星跟踪环路传递函数框图；
[0083]
图3为惯导辅助与无辅助的卫星跟踪环路传递函数幅频响应曲线比较图。
具体实施方式
[0084]
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
[0085]
如图1所示，本发明实施提出的一种基于强跟踪kalman滤波的级联式惯性/卫星深组合，用自适应地强跟踪kalman滤波对卫星接收机跟踪环路中鉴相器输出的码相位、载波相位等进行预滤波，滤波过程中引入基于平滑窗口的χ2故障检测算法，将与滤波的结果和惯导定位信息结合得到最终的位置速度修正量，修正量反馈到载波nco、码nco构成回路。图2和图3显示惯导辅助锁相环的等效传递函数模型，以及幅频响应曲线，可以定性地判断惯导的辅助能够有效提高卫星跟踪锁相环的动态响应特性和噪声抑制能力。下面进行详细的分析：
[0086]
步骤1：推导卫星导航接收机跟踪环路实现步骤，引入强跟踪kalman滤波作为预滤波器。
[0087]
卫星信号跟踪过程需要解决的核心问题是对码相位和载波频率的精确跟踪。gps信号在l1波段可按如下形式表示：
[0088][0089]
其中，s(t)是波段上的gps信号；pc是c/a码信号的功率；p
pl1
是l1波段上p码的信号功率，c(t)是c/a码的相位；d(t)表示数据码；p(t)是p码相位；f
l1
是l1波段的频率。
[0090]
卫星信号经过射频前端处理后得到中频信号，中频信号在完成载波分离和伪码分离后产生同相i支路信号和正交相q支路信号，i支路信号和q支路信号的数学表达式如下：
[0091][0092][0093]
其中，a是gps信号幅度；n是相关器中的信号采样点数量；r(
·
)是伪随机码的自相
关函数；δτ是本地码相位误差；δω是本地载波的角频率误差；t是相干积分时间；是本地载波相位误差平均值，由上述公式设计鉴相器或鉴频器可以算得载波相位误差或码相位误差。载波环中使用的鉴相器和鉴频器主要有以下几种，其中比较常用的是两象限的反正切函数arctan[q(t)/i(t)]。
[0094][0095]
其中cross＝i(t1)
×
q(t2)-i(t2)
×
q(t1),dot＝i(t1)
×
q(t2) i(t2)
×
q(t1)。
[0096]
码相位误差则通过超前码、滞后码算得。由伪码生成器生成三个移位版本的本地伪码，分别是超前、滞后和即时码，相干间隔为d/2个码片，超前和滞后码的四路相关积分输出分别如下：
[0097][0098][0099][0100][0101]
设超前滞后相关器超前码、滞后码与即时码的间隔为0.5码片，采用非相干超前滞后包络法计算码相位误差δτ公式如下：
[0102][0103]
其中ie、qe、i
l
、q
l
分别为超前码、滞后码相关积分输出的i、q分量。
[0104]
卫星跟踪环路预滤波器的状态方程和量测方程为：
[0105][0106]
其中，φ
k,k-1
为t
k-1
时刻到tk时刻的一步状态状态转移矩阵；xk为状态向量；γ
k-1
为系统噪声矩阵；w
k-1
为系统激励噪声序列,其对应方差为qk；zk为量测向量；vk为量测噪声序列，其对应方差为rk。kalman滤波更新方程如下：
[0107]
[0108][0109][0110][0111]
pk＝(i-k
khk
)p
k,k-1
[0112]
其中pk为状态均方误差，kk为滤波增益矩阵。在实际工程中，为了便于处理，会对卡尔曼滤波的观测、状态模型进行简化，且实际工程中我们很难得到准确的噪声统计特性，因此上述模型存在不确定性，为了解决这个问题，此处以强跟踪kalman滤波进行卫星跟踪环路设计。强跟踪成立需要满足的条件为，选择一个合适的增益矩阵kk使得
[0113]
e[(γ
k j
)γ
kt
]＝0,k＝0,1,2...,j＝1,2,...
[0114]
其中γk＝z
k-hx
k,k-1
为观测量残差。我们可以对增益矩阵kk进行调整，强迫残差序列保持正交，使上式始终成立，从而达到强迫滤波器对系统实际状态保持跟踪的目的。将一步预测方差矩阵改为
[0115][0116]
其中，λk为时变渐消因子，
[0117][0118][0119][0120][0121][0122]
其中，0《ρ≤1为遗忘因子，一般取ρ＝0.95；为量测方差估计。
[0123]
选取卫星中频信号的和码相位误差δτ、载波相位误差载波角频率误差δω和载波角频率变化率误差δa作为stkf的状态向量，载波相位可视为载波频率的一次函数，载波频率可视为载波频率变化率的一次函数，载波频率变化率可模型化为缓慢变化的偏差，码相位误差也可视为载波频率的一次函数，具体形式如下：
[0124][0125]
其中，对于gps l1 c/a信号来说，α＝β＝1/1540。w
mp
、w
clock
、w
drift
、w
acc
分别为各个状态量对应的过程噪声，其协方差矩阵如下：
[0126][0127]
其中c为光速，qb、qd、qa分别为载波相位、载波频率、载波频率变化率的噪声功率谱密度，大小与接收机射频前端振荡器噪声特性有关。由于stkf是一个线性滤波器，可以将鉴相器输出的码相位和载波相位作为观测量，则观测方程如下：
[0128][0129]
步骤2：推导基于平滑窗口的χ2故障检测模型和基于载噪比的量测噪声计算方程，具体如下：
[0130]
量测方差可以近似表示为长度为μ的滑动窗口内各个历元新息方差估计的平均值，
[0131][0132]
窗口宽度μ不宜设置过大，否则会产生较大的时延，故障检测能力将变弱，一般取3～5适宜。定义为故障检测函数，则εk服从自由度为m的χ2分布即
[0133]
εk～χ2(m)
[0134]
式中，m为量测向量的维度。当卫星伪距/伪距率量测信息异常时，故障检测函数不再服从上述分布，可以通过χ2分布上分为点进行故障检测，上分为点与量测维度有关，例如，当量测维度m＝6时，也就是说在量测信息正常时，认为的概率只有0.5％，而当m＝3时，该故障检测函数能够准确判断量测信息是否出现异常。
[0135]
量测噪声主要来源于跟踪环路的相位抖动和动态响应误差，因此可以通过各个卫星通道的载噪比值来计算当前时刻量测噪声：
[0136][0137]
其中，载噪比可通过窄带宽带功率比方法求得：
[0138][0139][0140][0141][0142]
其中wbp、nbp分别为窄带信号能量和宽带信号能量，t1为相干积分时间。
[0143]
步骤3：由步骤(1)的预滤波输出和捷联惯导系统接收的数据信息推导惯性/卫星紧组合导航模型，具体如下：
[0144]
将预滤波器输出的码相位和载波频率经过一定的运算之后可以得到各个卫星通道的伪距、伪距率信息，由卫星的广播星历文件可以计算得到卫星在地心地固坐标系(ecef)下的位置速度信息，结合捷联惯导系统接收的包括三轴陀螺仪的角增量或角速度数据和三轴加速度计的比力或比力积分增量数据可以算得另一组伪距、伪距率信息，二者作差作为紧组合的量测值。
[0145]
组合导航滤波的状态方程为：
[0146][0147]
其中，f
tc
为状态转移矩阵，g
tc
为系统噪声分配矩阵，w
tc
为系统噪声，x
tc
为状态变量，表示为：
[0148]
x
tc
＝[(φn)
t (δvn)
t (δpn)
t (εb)
t (
△b)
t cδt
u cδt
ru
]
t
[0149]
其中φn为姿态误差角向量，δvn、δpn为东北天坐标系(enu)下的速度误差向量、位置误差向量，εb、
△b分别是陀螺仪和加速度计的常值零偏，cδtu、cδt
ru
分别是卫星导航接收机钟差等效距离误差和钟漂等效距离误差；
[0150]wtc
＝[(ω
bg
)
t (ω
ba
)
t ω
tu ω
tru
]
t
[0151]
其中ω
bg
、ω
ba
、ω
tu
、ω
tru
分别表示陀螺零偏噪声、加速度计零偏噪声、钟差等效距离误差的白噪声、钟漂等效距离误差的白噪声；
[0152][0153]
其中，
[0171][0172]
其中，r
ρ,tc
、分别为伪距量测量、伪距率量测量对应的噪声方差矩阵，p
τ
、p
fcarr
为对应卫星通道预滤波器码相位误差和载波频率误差状态方差矩阵，λ
carr
为载波波长，对于gps l1 c/a信号来说，α＝1/1540。
[0173]
步骤4：对系统误差状态进行校正，输出校正后的组合导航的姿态、速度、位置等信息并将其反馈到载波nco和码nco，具体如下：
[0174]
在步骤(3)一个组合导航滤波周期结束后得到各误差量的估计值，并用估计值对导航参数进行补偿，得到各导航参数的“精确值”，补偿公式为：
[0175]
位置：
[0176]
速度：
[0177]
钟差：
[0178]
钟漂：
[0179]
码相位：
[0180]
载波频率：
[0181]
其中a
j,k
从接收机指向各通道卫星方向的单位矢量。通过更新的位置速度信息复现的本地码相位和载波频率反馈到码nco和载波nco，就构成了闭合的跟踪环路，同时输出的位置速度信息也能够在复杂电磁干扰环境下实现高精度。