CN108226980A - 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法 - Google Patents

Abstract

本发明涉及一种基于惯性测量的差分GNSS与INS自适应紧耦合导航方法，将北斗导航系统(BD)和GPS导航系统的观测量数据进行选择和组合，共同作为量测量信息，进行组合滤波，观测卫星数量增多，改善了卫星的空间分布，且增加了数据的冗余度，有效提高定位的精度；增加了在“城市峡谷”等复杂环境导航系统可观测卫星的数量，有效提高了导航系统的有效性；BD/GPS信息融合技术降低了对单一系统的依赖性，摆脱了GPS选择失效情况的影响。

Description

基于惯性测量单元的差分GNSS与INS自适应紧耦合导航方法

技术领域

本发明涉及一种基于惯性测量单元的差分GNSS与INS自适应紧耦 合导航方法，属于联合导航技术领域。

背景技术

随着导航技术的快速发展，将自主性较强的捷联惯导系统和精度长 时间稳定性强的GNSS导航系统组合,不但能在高性能的精确制导领域 发挥重要作用，同时也能作为低成本的车载导航系统使用。在飞艇对接、 飞机进近着舰、车辆自动驾驶、精准农业、船舶进港等生命安全领域， 高精度高可靠性导航是它们实现远距离执行任务的重要保障。

目前常用的导航方式主要有单独INS导航、单独GNSS导航、差分 GNSS导航以及组合导航，但是这些导航方式都存在一定的不足。单独 INS导航长时导航累积误差会增大，且其导航精度需要高成本的IMU 器件来保障；单独GNSS导航的精度较低，且可靠性受周边环境影响较 大；差分GNSS导航在有遮挡或“城市峡谷”等复杂环境下的精度和可靠 性会降低；而组合导航的精度一定程度上取决于观测量的质量，观测量 质量的优劣势必会影响组合导航的精度。

发明内容

本发明所要解决的技术问题是提供一种能够降低硬件成本，以及保 证复杂环境下导航系统精度和可靠性的基于惯性测量单元的差分GNSS 与INS自适应紧耦合导航方法。

本发明为了解决上述技术问题采用以下技术方案：本发明设计了一 种基于惯性测量的差分GNSS与INS自适应紧耦合导航方法，包括如下 步骤：

步骤A.INS导航系统通过惯性测量模块捕获目标对象位置速度信 息；

步骤B.INS导航系统根据所获得的目标对象位置速度信息、以及卫 星星历信息，获得所对应的伪距和伪距率信息，作为参考伪距和伪距率 信息；

步骤C.获得差分GNSS卫星导航系统中各个卫星的伪距和伪距率 信息，分别与参考伪距和伪距率信息之间的差异，并选择小于预设阈值 的各个差异分别对应的卫星，作为各个可用卫星；

步骤D.将INS导航系统所获目标对象位置速度信息、惯性测量模 块误差、GPS导航系统钟差、钟漂、以及北斗导航系统与GPS导航系统 之间的钟差之差、钟漂之差，作为组合滤波的状态量，构建组合滤波器 状态方程；

步骤E.将各个可用卫星分别对应的伪距和伪距率信息，作为组合滤 波的观测量，构建组合滤波器观测方程；

步骤F.基于组合滤波器状态方程、组合滤波器观测方程，进行自适 应抗差滤波处理，获得惯性测量模块误差、以及钟差和钟漂，并采用所 获惯性测量模块误差针对INS导航系统进行反馈矫正，以及将所获钟差 和钟漂作为GNSS卫星导航系统的矫正参考值，进一步计算获得目标对 象的位置、速度和姿态，实现目标对象的导航。

作为本发明的一种优选技术方案，所述步骤B至步骤C包括如下过 程：

根据INS导航系统所获得的目标对象位置速度信息、以及卫星星历 信息，按如下公式：

其中,

且，

r_i＝ρ_{I_i}

解算出各个卫星通道一段时间内的伪距ρ_{I_i}和伪距率其中i代 表卫星号为自然数；INS导航系统提供接收机的位置(x,y,z)和位置 (v_x,v_y,v_z)，卫星的位置(x_si,y_si,z_si)和速度(v_xsi,v_ysi,v_zsi)可通过存储的星历和INS 提供的时间解算；

以ρ_{I_i}和为基准，判断接收机各个卫星通道中接收到的第i颗卫 星的伪距ρ_i和伪距率是否合理，若伪距不合理，则可以剔除转发式干 扰信号，若伪距率不合理，则可以提出生成的伪卫星信号，具体的判断 步骤如下步骤，获得各个符合条件的卫星，作为各个可用卫星；

步骤BC①利用式(4)和(5)分别计算出第i颗卫星的伪距和伪距率的 时间序列ρ_{I_i}(k)和其中，k＝1，2…N,k代表脉冲计时时刻，N为 当前脉冲计时时刻；

步骤BC②找出卫星号均为i的一对卫星通道，利用接收机测得的 伪距伪距率组成两组时间序列ρ_i1(k)，和ρ_i2(k)，其中，k＝1， 2…N；

步骤BC③将上述的各个序列进行野值剔除处理，其中，设C(k)为 一个长度为N的序列，则上述伪距、伪距率时间序列均可以用C(k)表示， C_k为C(k)中的第k个元素，利用宽度为M的窗口函数和剔除因子为η的 方差法对C(k)进行野值剔除，η一般取2.5～5，序列中需要剔除的元素 满足下列条件：

其中，

步骤BC④剔除伪距不合理的卫星信号，将野值剔除处理后的时间 序列ρ'_i1(k)、ρ'_i2(k)和ρ'_{I_i}(k)分别按照时间对准以后求两个序列的方差D_i,j， j＝1,2，i为卫星号；B(k)＝{B₁(k),B₂(k)}＝{ρ'_i1(k),ρ'_i2(k)}，其中，k＝1,2....P_ij，

P_ij＝min{length(ρ_i'_j(k)),length(ρ'_{I_i}(k))}

B_jk,ρ'_Ik分别为B_j(k)和ρ'_{I_i}(k)的第k个元素，则：

正常卫星的D_i,j应满足：

其中，λ为调节因子，经验值为1.7～2.5.不满足上式的通道作为欺 骗式信号剔除掉；由于转发式干扰会在码相位上有延时从而导致伪距解 算值和真实值差异过大，这一步可以将其剔除掉；

步骤BC⑤剔除伪距率不合理的卫星信号，利用类似上一步的方法 可以剔除伪距率不合理的卫星；

步骤BC⑥改变i的值，从步骤BC②开始重复以上步骤，直到通道 内没有重复的卫星号为至。

作为本发明的一种优选技术方案，所述步骤D包括如下：

针对差分GNSS/INS紧耦合系统采用直接法组合，构建状态方程如 下所示：

其中，X_I为惯性误差状态量，针对状态量X_I的系统变量，选取系统 的位置p(lat(L)、log(λ)、h)、速度v(v_n、v_e、v_d)和姿态pitch(θ)、heading(Ψ))， 惯性测量模块误差模型为陀螺仪和加速度计零偏(b_g、b_a)，以及陀螺仪和 加速度计比例因子误差(s_g、s_a)；W_I为惯性系统有关噪声，包含位置噪声 (w_p)、速度噪声(w_v)、姿态噪声(w_ε)、器件噪声(w_bg、w_ba、w_sg、w_sa)；

X_G为RNSS接收机误差状态量，基于所采用GPS导航系统/北斗导 航系统双系统同时滤波，因此，X_G包含GPS和北斗的钟差钟漂相关误 差，以及GPS与北斗导航系统的钟差和钟漂之间均有一个固定的误差 X_G选用GPS系统与时钟误差等效的距离误差δl_u，以及时钟频率误差等 效的距离率误差δl_ru，以及BD系统与GPS系统钟差和钟漂之差Δl_u、Δl_tu；W_G为RNSS接收机有关噪声，F_I为惯性系统状态转移矩阵，F_G为RNSS 接收机状态转移矩阵，具体的方程实现步骤如下：

步骤D①建立器件误差状态方程，由于我们选用的是低成本IMU， 其输出为角速率和比力f^b，换算成高级别惯性测量模块IMU输出的 角增量和速度增量

针对惯性测量模块IMU误差，采用随机游走模拟，如下：

b_a,k＝b_g,k-1+w_bg,k (10)

b_a,k＝b_a,k-1+w_ba,k (11)

s_g,k＝s_g,k-1+w_sg,k (12)

s_a,k＝s_a,k-1+w_sa,k (13)

当公式(10)-(13)中的噪声增加的时候，需要保证在滤波器进行较长 时间的滤波时仍然能够保证噪声协方差矩阵的正定县性，此时进行传感 器误差补偿为：

其中，(k-1/2)表示为k-1与k时刻的中间时刻；

步骤D②建立速度状态方程，根据捷联解算方程，速度更新如下：

其中，

其中，是重力场和地球自转引起来的速度增量，是n系到i系 的旋转矢量，是b系到n系的转换矩阵DCM，Δt_k＝t_k-t_k-1，(·×)是叉 乘反对称阵，和分别是地球角速率和位移角速率；

步骤D③建立位置状态方程为了更新位置，需要更新

其中，L为纬度，λ为经度；

其中，是从四元数中提取的，

高度更新如下：

h_k＝h_k-1-v_D,k-1/2Δt_k (24)

步骤D④建立姿态更新方程，采用四元数法更新姿态针对姿态进 行更新，其中，为了更新姿态，需要更新如下：

假设，

步骤D⑤建立GNSS变量更新方程，F_G和W_G的取值由GNSS系统 误差方程决定，考虑其为一阶马尔科夫过程，有其中，-1/τ_f为反相关时间；W_G选取均值为零的白噪声。

作为本发明的一种优选技术方案，所述步骤E包括如下：

RTD模式下组合滤波中，直接选用差分GNSS矫正误差侯的伪距 ρ_u，根据式(4)可以获得INS计算的伪距ρ_INS，将其做差可以得到 GNSS/INS组合滤波的观测量：

z＝ρ_INS-ρ_u (34)

RTK模式下组合滤波中，可根据RTK定位结果以及卫星位置做差 求得伪距观测量ρ_u，如下式所示：

将式(35)带入式(34)可以获得RTK模式下的组合滤波器观测 方程。

作为本发明的一种优选技术方案，所述步骤F中，基于组合滤波器 状态方程、组合滤波器观测方程，按如下过程，进行自适应抗差滤波处 理，获得惯性测量模块误差、以及钟差和钟漂，并采用所获惯性测量模 块误差针对INS导航系统进行反馈矫正，以及将所获钟差和钟漂作为 GNSS卫星导航系统的矫正参考值，进一步计算获得目标对象的位置、 速度和姿态，实现目标对象的导航；

根据稳健估计的准则求取权因子：

则观测噪声协方差阵R为一个加权后的误差值，实现卡尔曼滤波观 测噪声阵的自适应估计；

R＝{r_ka_k}

在卡尔曼滤波中，根据稳健M估计等价权原理，通过分析增益矩 阵K_k，选取适当的权函数代替观测噪声协方差阵，等价权选定之后，重 新利用广义最小二乘原理，可导出卡尔曼滤波的稳健估计的递推方程：

状态一步预测

一步预测误差方差阵

滤波增益矩阵

或

状态估计

估计误差方差阵

式(3.9e)可以写成

或

其中， 是观测权矩阵；

基于等价权要由观测残差确定，因此进行迭代计算，将参数估计与 上一步参数估值进行比较，若之间的差值小于指定的限差，则停止计算。

本发明所述一种基于惯性测量单元的差分GNSS与INS自适应紧耦 合导航方法及性能采集方法采用以上技术方案与现有技术相比，具有以 下技术效果：

(1)本发明设计技术方案将北斗导航系统(BD)和GPS导航系统 的观测量数据进行选择和组合，共同作为量测量信息，进行组合滤波， 观测卫星数量增多，改善了卫星的空间分布，且增加了数据的冗余度， 有效提高定位的精度；增加了在“城市峡谷”等复杂环境导航系统可观测 卫星的数量，有效提高了导航系统的有效性；BD/GPS信息融合技术降 低了对单一系统的依赖性，摆脱了GPS选择失效情况的影响；

(2)本发明设计技术方案组合滤波的状态方程设计中设计的系统 方程更新方式采用四元数法，期间误差模型选用了器件偏差和安装误差 因子，更适用于低成本IMU器件误差大的特点，使得系统状态模型描 述更符合低成本IMU误差较大的实际工作特性，通过提高组合滤波的 建模准确性提高了组合滤波器的精度。

(3)本发明设计技术方案的观测量选用差分GNSS测量信息，适 用于RTD和RTK两种模式下的组合导航，使得系统精度提高至差分 GNSS精度的同时，保障了系统高精度定位的连续性；

(4)本发明设计技术方案的组合滤波器采用抗差自适应UKF，能 够根据观测量的实时状况，只要有一颗卫星可用信息便可进行量测更 新，提高了系统的适用性；同时采用抗差法对量测信息的有效性进行权 值设置，最大限度地利用了已有的观测量信息，同等观测条件下提高了 组合导航系统的精度。

附图说明

图1是本发明所设计一种基于惯性测量单元的差分GNSS与INS自 适应紧耦合导航方法的示意图。

具体实施方式

下面结合说明书附图对本发明的具体实施方式作进一步详细的说 明。

本发明提出一种基于惯性测量单元的差分GNSS与INS自适应紧耦 合导航方法，克服现有导航技术的不足，在降低了系统硬件成本的同时， 保证了复杂环境下导航系统的精度和可靠性，具体特点如下：组合滤波 状态方程中的系统模型的姿态采用欧拉角，器件误差采用随机游走，适 用于器件误差大且器件误差特性呈现非零非稳定特性的低成本IMU；组 合导航滤波器采用UKF进行四元数形式的更新，可以对非线性系统方 差和期望描述更准确外，还适用于IMU横滚角和俯仰角的不确定性大 误差；采用差分GNSS处理以后的伪距和伪距率作为量测值，进行 BD/GPS双系统抗差自适应滤波，最大限度保障卫星导航测量信息可用 性的同时，提高进入组合滤波器的观测量精度，也提高了组合滤波器的 稳定性；采用INS测量值进行BD/GPS双系统自适应选星，保障进入组 合滤波器的观测量有效可用。

如图1所示，本发明设计了一种基于惯性测量单元的差分GNSS与 INS自适应紧耦合导航方法，实际应用中，具体包括包括如下步骤：

步骤A.INS导航系统通过惯性测量模块捕获目标对象位置速度信 息。

步骤B.INS导航系统根据所获得的目标对象位置速度信息、以及卫 星星历信息，获得所对应的伪距和伪距率信息，作为参考伪距和伪距率 信息。

步骤C.获得差分GNSS卫星导航系统中各个卫星的伪距和伪距率 信息，分别与参考伪距和伪距率信息之间的差异，并选择小于预设阈值 的各个差异分别对应的卫星，作为各个可用卫星。

上述步骤B至步骤C包括如下过程：

根据INS导航系统所获得的目标对象位置速度信息、以及卫星星历 信息，按如下公式：

其中,

且，

r_i＝ρ_{I_i}

解算出各个卫星通道一段时间内的伪距ρ_{I_i}和伪距率其中i代 表卫星号为自然数；INS导航系统提供接收机的位置(x,y,z)和位置 (v_x,v_y,v_z)，卫星的位置(x_si,y_si,z_si)和速度(v_xsi,v_ysi,v_zsi)可通过存储的星历和INS 提供的时间解算；

以ρ_{I_i}和为基准，判断接收机各个卫星通道中接收到的第i颗卫 星的伪距ρ_i和伪距率是否合理，若伪距不合理，则可以剔除转发式干 扰信号，若伪距率不合理，则可以提出生成的伪卫星信号，具体的判断 步骤如下步骤，获得各个符合条件的卫星，作为各个可用卫星；

步骤BC①利用式(4)和(5)分别计算出第i颗卫星的伪距和伪距率的 时间序列ρ_{I_i}(k)和其中，k＝1，2…N,k代表脉冲计时时刻，N为 当前脉冲计时时刻；

步骤BC②找出卫星号均为i的一对卫星通道，利用接收机测得的 伪距伪距率组成两组时间序列ρ_i1(k)，和ρ_i2(k)，其中，k＝1， 2…N；

步骤BC③将上述的各个序列进行野值剔除处理，其中，设C(k)为 一个长度为N的序列，则上述伪距、伪距率时间序列均可以用C(k)表示， C_k为C(k)中的第k个元素，利用宽度为M的窗口函数和剔除因子为η的 方差法对C(k)进行野值剔除，η一般取2.5～5，序列中需要剔除的元素 满足下列条件：

其中，

步骤BC④剔除伪距不合理的卫星信号，将野值剔除处理后的时间 序列ρ'_i1(k)、ρ'_i2(k)和ρ'_{I_i}(k)分别按照时间对准以后求两个序列的方差D_i,j， j＝1,2，i为卫星号；B(k)＝{B₁(k),B₂(k)}＝{ρ'_i1(k),ρ'_i2(k)}，其中，k＝1,2....P_ij，

P_ij＝min{length(ρ_i'_j(k)),length(ρ'_{I_i}(k))}

B_jk,ρ'_Ik分别为B_j(k)和ρ'_{I_i}(k)的第k个元素，则：

正常卫星的D_i,j应满足：

其中，λ为调节因子，经验值为1.7～2.5.不满足上式的通道作为欺 骗式信号剔除掉；由于转发式干扰会在码相位上有延时从而导致伪距解 算值和真实值差异过大，这一步可以将其剔除掉；

步骤BC⑤剔除伪距率不合理的卫星信号，利用类似上一步的方法 可以剔除伪距率不合理的卫星；

步骤BC⑥改变i的值，从步骤BC②开始重复以上步骤，直到通道 内没有重复的卫星号为至。

步骤D.将INS导航系统所获目标对象位置速度信息、惯性测量模 块误差、GPS导航系统钟差、钟漂、以及北斗导航系统与GPS导航系统 之间的钟差之差、钟漂之差，作为组合滤波的状态量，构建组合滤波器 状态方程。

上述步骤D具体包括如下：

差分GNSS/INS紧耦合系统采用直接法组合，模型系统方程是惯导 力学编排方程和IMU器件误差及GNSS钟差、钟漂变量方程的综合， 直接法直接估计系统的位置、速度、姿态，描述导航参数的动态过程, 能较准确地反映真实状态的演变情况,又可以使惯导系统避免力学编排 方程的许多重复计算，状态方程如下所示：

其中，X_I为惯性误差状态量，由于低成本IMU的误差较大，因此 状态量X_I的系统变量直接选取系统的位置p(lat(L)、lon(λ)、h)、速度 v(v_n、v_e、v_d)和姿态pitch(θ)、heading(Ψ))，由于低成本IMU具有相对 较大的零偏和比例因子误差，惯性测量模块误差模型为陀螺仪和加速度 计零偏(b_g、b_a)，以及陀螺仪和加速度计比例因子误差(s_g、s_a)；

W_I为惯性系统有关噪声，包含位置噪声(w_p)、速度噪声(w_v)、姿态噪 声(w_ε)、器件噪声(w_bg、w_ba、w_sg、w_sa)；

X_G为RNSS接收机误差状态量，由于本系统采用GPS导航系统/北 斗导航系统双系统同时滤波，因此X_G包含GPS和北斗的钟差钟漂相关 误差，又因为一般同一台GNSS接收机的GPS钟和BD钟采用同一个晶 振和数学矫正方法，因此GPS与北斗导航系统(BD)的钟差和钟漂之 间均有一个固定的误差，且该误差在组合滤波一段时间内即可估计准 确，因此为了提高组合滤波系统稳定定性，X_G选用GPS系统与时钟误 差等效的距离误差δl_u，以及时钟频率误差等效的距离率误差δl_ru，以及 BD系统与GPS系统钟差和钟漂之差Δl_u、Δl_tu；W_G为RNSS接收机有关 噪声，F_I为惯性系统状态转移矩阵，F_G为RNSS接收机状态转移矩阵， 具体的方程实现步骤如下：

步骤D①建立器件误差状态方程，由于我们选用的是低成本IMU， 其输出为角速率和比力f^b，换算成高级别IMU输出的角增量和速 度增量

且低成本IMU有大的零偏和比例因子误差，器件误差特性呈现非 零非稳定特性，因此INS器件误差不能采用传统的一阶马尔科夫模型， 而是采用随机游走模拟，且惯性测量模块误差可以在GPS输出时，保 持当前值；同时，增加了随机游走噪声项可以保障滤波器长时间运行后 协方差的正定性；

b_a,k＝b_g,k-1+w_bg,k (10)

b_a,k＝b_a,k-1+w_ba,k (11)

s_g,k＝s_g,k-1+w_sg,k (12)

s_a,k＝s_a,k-1+w_sa,k (13)

当公式(10)-(13)中的噪声增加的时候需要保证在滤波器进行较长时 间的滤波时仍然能够保证噪声协方差矩阵的正定县性，此时进行传感器 误差补偿为：

其中，(k-1/2)表示为k-1与k时刻的中间时刻；

步骤D②建立速度状态方程，根据捷联解算方程，速度更新如下：

其中，

其中，是重力场和地球自转引起来的速度增量，是n系到i系 的旋转矢量，是b系到n系的转换矩阵DCM，Δt_k＝t_k-t_k-1，(·×)是叉 乘反对称阵，和分别是地球角速率和位移角速率；

步骤D③建立位置状态方程为了更新位置，需要更新

其中，L为纬度，λ为经度；

其中，是从四元数中提取的，

高度更新如下：

h_k＝h_k-1-v_D,k-1/2Δt_k (24)

步骤D④建立姿态更新方程，因为低成本惯性测量模块IMU的误 差较大，本发明选用四元数法更新姿态，避免了传统的三角函数法中小 角度近似带来的误差，为了更新姿态，需要更新

假设，

步骤D⑤建立GNSS变量更新方程，F_G和W_G的取值由GNSS系统误 差方程决定，考虑其为一阶马尔科夫过程，有其中，-1/τ_f为反相关时间；W_G选取均值为零的白噪声。

步骤E.将各个可用卫星分别对应的伪距和伪距率信息，作为组合滤 波的观测量，构建组合滤波器观测方程，具体包括如下：

为了提高观测量精度，本发明选用差分GNSS处理后的测量值来进 行量测更新，由于一般情况下，差分GNSS在卫星可见数大于5颗时进 入RTK模式，小于5颗为RTD模式，因此，组合滤波器的观测方程建 立也要考虑两种模式：

RTD模式下组合滤波中，直接选用差分GNSS矫正误差侯的伪距 ρ_u，根据式(4)可以获得INS计算的伪距ρ_INS，将其做差可以得到 GNSS/INS组合滤波的观测量：

z＝ρ_INS-ρ_u (34)

RTK模式下组合滤波中，可根据RTK定位结果以及卫星位置做差 求得伪距观测量ρ_u，如下式所示：

将式(35)带入式(34)可以获得RTK模式下的组合滤波方程；

根据式(34)可看出，组合滤波的观测量仅由惯导位置误差和差分 GNSS测量误差组成，组合滤波运行一段时间后，IMU测量误差估计准 确后在一个组合滤波周期内可以忽略不计，此时组合滤波精度主要取决 于差分GNSS的精度。

步骤F.基于组合滤波器状态方程、组合滤波器观测方程，按如下过 程，进行自适应抗差滤波处理，获得惯性测量模块误差、以及钟差和钟 漂，并采用所获惯性测量模块误差针对INS导航系统进行反馈矫正，以 及将所获钟差和钟漂作为GNSS卫星导航系统的矫正参考值，进一步计 算获得目标对象的位置、速度和姿态，实现目标对象的导航。

UKF直接利用非线性模型，避免引入线性化误差，从而提高了滤波 精度，而且不必计算Jacobian矩阵，容易实现，此外，UKF在计算量上 没有太大增加，而是和EKF相当，UKF的过程就是UT转换和Kalman 滤波过程。

在Kalman滤波中，当观测向量存在粗差时，状态向量的估计将受 到粗差的影响，根据稳健M估计等价权原理，通过分析增益矩阵，可 以选取适当的权函数代替观测噪声协方差矩阵，以减小或消除粗差对估 计结果的影响。

根据稳健估计的准则求取权因子：

则观测噪声协方差阵R为一个加权后的误差值，实现卡尔曼滤波观 测噪声阵的自适应估计；

R＝{r_ka_k}

在卡尔曼滤波中，当观测向量Z存在粗差时，和都将受到粗 差的影响；根据稳健M估计等价权原理，通过分析增益矩阵K_k，选取 适当的权函数代替观测噪声协方差阵，以减少或消除粗差对估计结果的 影响。等价权(或等价协方差阵)选定之后，重新利用广义最小二乘原 理，可导出卡尔曼滤波的稳健估计的递推方程：

状态一步预测

一步预测误差方差阵

滤波增益矩阵

或

状态估计

估计误差方差阵

式(3.9e)可以写成

或

其中， 是观测权矩阵。

卡尔曼滤波抗差估计的递推方程与原递推方程具有相似的形式，但 此时P_k由于受观测噪声等价协方差阵的影响而与原方程不同。由于等价 权要由观测残差确定，因此可进行迭代计算，将参数估计与上一步参数 估值进行比较，若之间的差值小于指定的限差，则停止计算。

上述技术方案即本发明所设计的基于惯性测量的差分GNSS与INS 自适应紧耦合导航方法，将北斗导航系统(BD)和GPS导航系统的观 测量数据进行选择和组合，共同作为量测量信息，进行组合滤波，观测 卫星数量增多，改善了卫星的空间分布，且增加了数据的冗余度，有效 提高定位的精度；增加了在“城市峡谷”等复杂环境导航系统可观测卫星 的数量，有效提高了导航系统的有效性；BD/GPS信息融合技术降低了 对单一系统的依赖性，摆脱了GPS选择失效情况的影响；组合滤波的状 态方程设计中设计的系统方程更新方式采用四元数法，期间误差模型选 用了器件偏差和安装误差因子，更适用于低成本IMU器件误差大的特 点，使得系统状态模型描述更符合低成本IMU误差较大的实际工作特 性，通过提高组合滤波的建模准确性提高了组合滤波器的精度。其中的 观测量选用差分GNSS测量信息，适用于RTD和RTK两种模式下的组 合导航，使得系统精度提高至差分GNSS精度的同时，保障了系统高精 度定位的连续性；并且组合滤波器采用抗差自适应UKF，能够根据观测 量的实时状况，只要有一颗卫星可用信息便可进行量测更新，提高了系 统的适用性；同时采用抗差法对量测信息的有效性进行权值设置，最大 限度地利用了已有的观测量信息，同等观测条件下提高了组合导航系统 的精度。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不 限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可 以在不脱离本发明宗旨的前提下做出各种变动。

Claims (5)

1.一种基于惯性测量的差分GNSS与INS自适应紧耦合导航方法，其特征在于，包括如下步骤：

步骤A.INS导航系统通过惯性测量模块捕获目标对象位置速度信息；

步骤B.INS导航系统根据所获得的目标对象位置速度信息、以及卫星星历信息，获得所对应的伪距和伪距率信息，作为参考伪距和伪距率信息；

步骤C.获得差分GNSS卫星导航系统中各个卫星的伪距和伪距率信息，分别与参考伪距和伪距率信息之间的差异，并选择小于预设阈值的各个差异分别对应的卫星，作为各个可用卫星；

步骤D.将INS导航系统所获目标对象位置速度信息、惯性测量模块误差、GPS导航系统钟差、钟漂、以及北斗导航系统与GPS导航系统之间的钟差之差、钟漂之差，作为组合滤波的状态量，构建组合滤波器状态方程；

步骤E.将各个可用卫星分别对应的伪距和伪距率信息，作为组合滤波的观测量，构建组合滤波器观测方程；

步骤F.基于组合滤波器状态方程、组合滤波器观测方程，进行自适应抗差滤波处理，获得惯性测量模块误差、以及钟差和钟漂，并采用所获惯性测量模块误差针对INS导航系统进行反馈矫正，以及将所获钟差和钟漂作为GNSS卫星导航系统的矫正参考值，进一步计算获得目标对象的位置、速度和姿态，实现目标对象的导航。

2.根据权利要求1所述的基于惯性测量的差分GNSS与INS自适应紧耦合导航方法，其特征在于，所述步骤B至步骤C包括如下过程：根据INS导航系统所获得的目标对象位置速度信息、以及卫星星历信息，按如下公式：

其中,

并且，

r_i＝ρ_{I_i}

解算出各个卫星通道一段时间内的伪距ρ_{I_i}和伪距率其中i代表卫星号为自然数；INS导航系统提供接收机的位置(x,y,z)和位置(v_x,v_y,v_z)，卫星的位置(x_si,y_si,z_si)和速度(v_xsi,v_ysi,v_zsi)可通过存储的星历和INS提供的时间解算；

以ρ_{I_i}和为基准，判断接收机各个卫星通道中接收到的第i颗卫星的伪距ρ_i和伪距率是否合理，若伪距不合理，则可以剔除转发式干扰信号，若伪距率不合理，则可以提出生成的伪卫星信号，具体的判断步骤如下步骤，获得各个符合条件的卫星，作为各个可用卫星；

步骤BC①利用式(4)和(5)分别计算出第i颗卫星的伪距和伪距率的时间序列ρ_{I_i}(k)和其中，k＝1，2…N,k代表脉冲计时时刻，N为当前脉冲计时时刻；

步骤BC②找出卫星号均为i的一对卫星通道，利用接收机测得的伪距伪距率组成两组时间序列ρ_i1(k)，和ρ_i2(k)，其中，k＝1，2…N；

步骤BC③将上述的各个序列进行野值剔除处理，其中，设C(k)为一个长度为N的序列，则上述伪距、伪距率时间序列均可以用C(k)表示，C_k为C(k)中的第k个元素，利用宽度为M的窗口函数和剔除因子为η的方差法对C(k)进行野值剔除，η一般取2.5～5，序列中需要剔除的元素满足下列条件：

其中，

步骤BC④剔除伪距不合理的卫星信号，将野值剔除处理后的时间序列ρ′_i1(k)、ρ′_i2(k)和ρ′_{I_i}(k)分别按照时间对准以后求两个序列的方差D_i,j，j＝1,2，i为卫星号；B(k)＝{B₁(k),B₂(k)}＝{ρ′_i1(k),ρ′_i2(k)}，其中，k＝1,2....P_ij，P_ij＝min{length(ρ′_ij(k)),length(ρ′_{I_i}(k))}

B_jk,ρ′_Ik分别为B_j(k)和ρ′_{I_i}(k)的第k个元素，则：

正常卫星的D_i,j应满足：

其中，λ为调节因子，经验值为1.7～2.5.不满足上式的通道作为欺骗式信号剔除掉；由于转发式干扰会在码相位上有延时从而导致伪距解算值和真实值差异过大，这一步可以将其剔除掉；

步骤BC⑤剔除伪距率不合理的卫星信号，利用类似上一步的方法可以剔除伪距率不合理的卫星；

步骤BC⑥改变i的值，从步骤BC②开始重复以上步骤，直到通道内没有重复的卫星号为至。

3.根据权利要求2所述的基于惯性测量的差分GNSS与INS自适应紧耦合导航方法，其特征在于，所述步骤D包括如下：

针对差分GNSS/INS紧耦合系统采用直接法组合，构建状态方程如下所示：

其中，X_I为惯性误差状态量，针对状态量X_I的系统变量，选取系统的位置p(lat(L)、lon(λ)、h)、速度v(v_n、v_e、v_d)和姿态pitch(θ)、heading(Ψ))，惯性测量模块误差模型为陀螺仪和加速度计零偏(b_g、b_a)，以及陀螺仪和加速度计比例因子误差(s_g、s_a)；W_I为惯性系统有关噪声，包含位置噪声(w_p)、速度噪声(w_v)、姿态噪声(w_ε)、器件噪声(w_bg、w_ba、w_sg、w_sa)；

X_G为RNSS接收机误差状态量，基于所采用GPS导航系统/北斗导航系统双系统同时滤波，因此，X_G包含GPS和北斗的钟差钟漂相关误差，以及GPS与北斗导航系统的钟差和钟漂之间均有一个固定的误差X_G选用GPS系统与时钟误差等效的距离误差δl_u，以及时钟频率误差等效的距离率误差δl_ru，以及BD系统与GPS系统钟差和钟漂之差Δl_u、Δl_tu；W_G为RNSS接收机有关噪声，F_I为惯性系统状态转移矩阵，F_G为RNSS接收机状态转移矩阵，具体的方程实现步骤如下：

步骤D①建立器件误差状态方程，由于我们选用的是低成本IMU，其输出为角速率和比力f^b，换算成高级别惯性测量模块IMU输出的角增量和速度增量

针对惯性测量模块IMU误差，采用随机游走模拟，如下：

b_a,k＝b_g,k-1+w_bg,k (10)

b_a,k＝b_a,k-1+w_ba,k (11)

s_g,k＝s_g,k-1+w_sg,k (12)

s_a,k＝s_a,k-1+w_sa,k (13)

当公式(10)-(13)中的噪声增加的时候，需要保证在滤波器进行较长时间的滤波时仍然能够保证噪声协方差矩阵的正定县性，此时进行传感器误差补偿为：

其中，(k-1/2)表示为k-1与k时刻的中间时刻；

步骤D②建立速度状态方程，根据捷联解算方程，速度更新如下：

其中，

其中，是重力场和地球自转引起来的速度增量，是n系到i系的旋转矢量，是b系到n系的转换矩阵DCM，Δt_k＝t_k-t_k-1，(·×)是叉乘反对称阵，和分别是地球角速率和位移角速率；

步骤D③建立位置状态方程为了更新位置，需要更新

其中，L为纬度，λ为经度；

其中，λ是从四元数中提取的，

高度更新如下：

h_k＝h_k-1-v_D,k-1/2Δt_k (24)

步骤D④建立姿态更新方程，采用四元数法更新姿态针对姿态进行更新，其中，为了更新姿态，需要更新如下：

假设，

步骤D⑤建立GNSS变量更新方程，F_G和W_G的取值由GNSS系统误差方程决定，考虑其为一阶马尔科夫过程，有其中，-1/τ_f为反相关时间；W_G选取均值为零的白噪声。

4.根据权利要求3所述的基于惯性测量的差分GNSS与INS自适应紧耦合导航方法，其特征在于，所述步骤E包括如下：

RTD模式下组合滤波中，直接选用差分GNSS矫正误差侯的伪距ρ_u，根据式(4)可以获得INS计算的伪距ρ_INS，将其做差可以得到GNSS/INS组合滤波的观测量：

z＝ρ_INS-ρ_u (34)

RTK模式下组合滤波中，可根据RTK定位结果以及卫星位置做差求得伪距观测量ρ_u，如下式所示：

将式(35)带入式(34)可以获得RTK模式下的组合滤波器观测方程。

5.根据权利要求4所述的基于惯性测量的差分GNSS与INS自适应紧耦合导航方法，其特征在于，所述步骤F中，基于组合滤波器状态方程、组合滤波器观测方程，按如下过程，进行自适应抗差滤波处理，获得惯性测量模块误差、以及钟差和钟漂，并采用所获惯性测量模块误差针对INS导航系统进行反馈矫正，以及将所获钟差和钟漂作为GNSS卫星导航系统的矫正参考值，进一步计算获得目标对象的位置、速度和姿态，实现目标对象的导航；

根据稳健估计的准则求取权因子：

则观测噪声协方差阵R为一个加权后的误差值，实现卡尔曼滤波观测噪声阵的自适应估计；

R＝{r_ka_k}

在卡尔曼滤波中，根据稳健M估计等价权原理，通过分析增益矩阵K_k，选取适当的权函数代替观测噪声协方差阵，等价权选定之后，重新利用广义最小二乘原理，可导出卡尔曼滤波的稳健估计的递推方程：状态一步预测

一步预测误差方差阵

滤波增益矩阵

或

状态估计

估计误差方差阵

式(3.9e)可以写成

或

其中， 是观测权矩阵；

基于等价权要由观测残差确定，因此进行迭代计算，将参数估计与上一步参数估值进行比较，若之间的差值小于指定的限差，则停止计算。