CN108226980B - 基于惯性测量单元的差分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(ρ′_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②开始重复以上步骤，直到通道内没有重复的卫星号为至。

作为本发明的一种优选技术方案，所述步骤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_e)、器件噪声(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(ρ′_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②开始重复以上步骤，直到通道内没有重复的卫星号为至。

步骤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导航***所获得的目标对象位置速度信息、以及卫星星历信息，按如下公式：

其中，

解算出各个卫星通道一段时间内的伪距ρ_{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)，

步骤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为卫星号；

ρ’_Ik为ρ'_{I_i}(k)中的第k个元素，则：

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

其中，j＝1、2，正常卫星的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_e)、器件高斯噪声(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①：建立器件误差状态方程，其输出为角速率

和比力f^b，换算成高级别惯性测量模块IMU输出的角增量

和速度增量

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

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

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

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

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

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

其中，△Θ_k和

分别是角增量

和速度增量

经过误差修正后的值；(k-1/2)表示为k-1与k时刻的中间时刻；

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

其中，

其中，

是重力场和地球自转引起来的速度增量，

是b系到n系的转换矩阵DCM，

和

分别是地球角速率和位移角速率；

步骤D③：建立状态方程为了更新位置，需要更新位置四元数；

其中，L为纬度，λ为经度，位置四元数的更新四元数由载体的位移角速度

乘时间间隔组成：

位置四元数更新以后，新的经纬度坐标

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

高度需要单独更新，更新如下：

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

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

如下：

姿态四元数更新公式如式(25)：

其中，

为载体姿态更新四元数，

为导航坐标系更新四元数；

和

可相互先换，根据：

步骤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 (35)

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

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

5.根据权利要求4所述的基于惯性测量的差分GNSS与INS自适应紧耦合导航方法，其特征在于，

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

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

上式中，C₀，C₁为滤波器的两个阈值，

为式(34)中的观测向量的第k项，a_k为对应的权因子；

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

R＝{r_ka_k}

其中，r_k为式(34)中的观测向量的第k项的先验误差值；

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

一步预测误差方差阵

滤波增益矩阵

状态估计

估计误差方差阵

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

Images