CN112051598B - 一种基于双重校正的车载gnss/ins组合导航方法 - Google Patents

Abstract

本发明属于卫星导航定位技术领域，具体提供了一种基于双重校正的车载GNSS/INS组合导航方法，首先，对GNSS系统进行初始化，获得固定解后，根据GNSS的载波以及伪距观测值，利用扩展卡尔曼滤波估计车的GNSS位置；然后将卡尔曼滤波校正后的GNSS位置与INS输出的位置求差，将结果作为观测值再次进行卡尔曼滤波估计，根据结果对INS输出的位置进行输出校正，最后获得的INS位置为系统位置的最终结果。该方案首先对GNSS输出的位置进行卡尔曼滤波校正，再利用校正后的GNSS位置对INS位置进行卡尔曼滤波校正，提高了组合导航定位系统位置的精度及可靠性。克服了在卫星信号不佳的情况下GNSS/INS组合导航定位系统输出位置精度较差的问题。

Description

一种基于双重校正的车载GNSS/INS组合导航方法

技术领域

本发明属于卫星导航定位技术领域，具体涉及一种基于双重校正的车载GNSS/INS组合导航方法。

背景技术

铁路作为我国国民经济的大动脉、国家重要基础设施、大众化交通工具，具有运能大、效率高、运距长、能耗低等诸多优点，非常适合我国人口众多、幅员辽阔的基本国情，在我国经济社会发展中发挥着重要的作用。列车运行控制系统作为铁路运输的“神经中枢”，是保障铁路行车安全、提高运输效率的核心，需要对列车的速度、位置等信息进行实时、准确的掌握。提高列车运行安全水平、降低运营和维护成本、满足线路运量现在已经成为了列车控制系统的一个研究热点。

随着GPS、GLONASS、Galileo和我国自主研发的北斗卫星导航系统的逐步发展和完善，卫星导航定位技术的应用领域也在不断扩展，利用卫星定位系统技术快速、准确地提供列车位置信息，不仅可以减少轨旁设备，降低建设和运行成本，减少铁路维修周期，还能提高列车定位精度，缩短列车行驶间隔时间，提高线路运输效率。对于基于差分定位技术的卫星定位系统，其输出位置信息的频率不高于CORS站播发差分信息的频率(如1HZ/秒)，即卫星定位系统输出位置的频率受CORS站播发信息的频率限制。然而，对于时速较高的列车(如达到600km/小时的磁悬浮列车)，其定位系统须具有更高的位置输出频率(至少100HZ)，才能满足列车实时定位的需求。此外，因为列车在实际运行环境中不仅会遇到隧道、桥梁、山坡、城市建筑物等遮挡卫星信号等问题，还会受到电磁波干扰、阻塞，出现卫星信号接受困难或丢失、错误等情况，降低列车定位的可靠性和精确性。因此，仅仅依靠GNSS(GlobalNavigation Satellite System，全球卫星导航系统)定位技术难以实现列车的连续定位。一些研究提出增加INS(Inertial Navigation System)传感器进行多源信息融合处理，弥补GNSS定位的局限性，扬长避短，形成有效列车组合定位功能整体。

GNSS/INS组合导航技术与卡尔曼滤波器构成了列车组合导航定位系统。INS系统保证了组合导航定位系统位置的高频输出，同时利用GNSS输出的位置信息来修正INS系统的误差。在实时定位的模式下，GNSS输出的位置精度不如静态模式。并且，在卫星信号不佳的情况下，GNSS输出的位置为浮点解，精度较差。此时，INS无法得到有效的修正，影响组合导航定位系统整体的定位效果，可靠性较差。

发明内容

本发明的目的是克服现有技术中在卫星信号不佳的情况下组合导航定位系统整体的定位精度低可靠性差的问题。

为此，本发明提供了一种基于双重校正的车载GNSS/INS组合导航方法，包括以下步骤：

S1：对GNSS系统进行初始化，获得固定解后，根据GNSS的载波以及伪距观测值，利用扩展卡尔曼滤波估计车的GNSS位置；

S2：将卡尔曼滤波校正后的GNSS位置与INS输出的位置求差，将结果作为观测值再次进行卡尔曼滤波估计，根据结果对INS输出的位置进行输出校正，最后获得的INS位置为系统位置的最终结果。

优选地，所述S1具体包括：

所述GNSS系统采用载波实时动态差分技术进行定位，且采用扩展卡尔曼滤波进行解算。

优选地，对于三频GNSS接收机，估计参数即状态向量为接受机的位置、速度以及P1、P2、P5频率上单差载波相位整周模糊度，具体形式如下：

x＝(r_r ^T,v_r ^T,B₁ ^T,B₂ ^T,B₅ ^T)^T (1)

其中，r_r＝(r_x,r,r_y,r,r_z,r)^T表示GNSS接收机输出的x、y、z方向的位置，v_r＝(v_x,r,v_y,r,v_z,r)^T表示GNSS接收机输出的在x、y、z方向的速度，下标r表示流动站，是L_i载波的SD模糊度向量，/>表示流动站与基准站在观测卫星j下的单差载波相位整周模糊度，下标b表示基准站。

优选地，滤波系统的观测量为相位距离以及伪距观测值的双差，具体形式如下：

y＝(Φ_1, ^T,Φ_2, ^T,Φ_5, ^T,P₁ ^T,P₂ ^T,P₅ ^T)^T (2)

其中，为频率i的双差相位距离向量，表示流动站与基准站、观测卫星j1与j2在i频率载波距离的双差，m为卫星个数，为频率i的双差伪距向量，/>为流动站与基准站、观测卫星j1与j2在i频率伪距的双差。

优选地，观测量模型y＝h(x)，具体形式如下：

h(x)＝(h_Φ,1 ^T,h_Φ,2 ^T,h_Φ,5 ^T,h_P,1 ^T,h_P,2 ^T,h_P,5 ^T)^T (3)

其中，式中λ_i为载波L_i的波长，/>为伪距的双差，对h(x)求其估计参数的一阶偏导数，结果如下：

其中， 为接收机至卫星视线的单位向量，/>

优选地，利用扩展卡尔曼滤波估计车的GNSS位置的具体公式包括：

P_k/k-1＝F_kP_k-1/k-1F_k ^T+Q_k

K_k＝P_k/k-1H_k ^T[R_k+H_kP_k/k-1H_k ^T]^-1

P_k/k＝[I-K_kH_k]P_k/k-1

其中为从历元t_k-1至t_k的转换矩阵，为系统噪声的方差矩阵，τ_r＝t_k-t_k-1，Q_v为三方向速度的协方差。

优选地，所述S2具体包括：利用经过经典卡尔曼滤波校正后的GNSS位置校正INS输出的位置。

优选地，具体包括：

先对GNSS接收机进行初始化，获得固定解后组合导航定位系统及车子开始运行，此时GNSS输出的位置信息作为初始值，并向滤波系统输入初始的位置信息

根据前一历元扩展卡尔曼滤波后计算后的GNSS位置等估计参数构建本历元GNSS扩展卡尔曼滤波估计所需的矩阵，根据本历元GNSS接收机接收的载波信号以及伪距计算载波距离以及伪距的双差值，并将结果构成扩展卡尔曼滤波的观测量y＝(Φ_1, ^T,Φ_2, ^T,Φ_5, ^T,P₁ ^T,P₂ ^T,P₅ ^T)^T，根据公式计算车的GNSS位置/>

根据本历元INS系统输出的位置以及卡尔曼滤波校正后的GNSS位置计算GNSS/INS组合导航系统卡尔曼滤波的观测量/>同时，利用上一历元卡尔曼校正后的INS输出的位置/>计算经典卡尔曼滤波中所须的矩阵，包括A_k/k-1，B_k/k-1，P_k/k-1，K_k；

根据公式计算系统的状态估值，利用估计结果对INS输出的位置进行校正：/>最终得到t_k时刻卡尔曼滤波估计校正后的系统位置。

本发明的有益效果：本发明提供的这种基于双重校正的车载GNSS/INS组合导航方法，首先，对GNSS系统进行初始化，获得固定解后，根据GNSS的载波以及伪距观测值，利用扩展卡尔曼滤波估计车的GNSS位置；然后将卡尔曼滤波校正后的GNSS位置与INS输出的位置求差，将结果作为观测值再次进行卡尔曼滤波估计，根据结果对INS输出的位置进行输出校正，最后获得的INS位置为系统位置的最终结果。该方案首先对GNSS输出的位置进行卡尔曼滤波校正，再利用校正后的GNSS位置对INS位置进行卡尔曼滤波校正，提高了组合导航定位系统位置的精度及可靠性。克服了在卫星信号不佳的情况下GNSS/INS组合导航定位系统输出位置精度较差的问题。

以下将结合附图对本发明做进一步详细说明。

附图说明

图1是本发明基于双重校正的车载GNSS/INS组合导航方法的原理示意图；

图2是本发明基于双重校正的车载GNSS/INS组合导航方法的流程图；

图3是本发明基于双重校正的车载GNSS/INS组合导航方法的组合导航运动轨迹示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

在本发明的描述中，需要理解的是，术语“中心”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征；在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

本发明实施例提供了一种基于双重校正的车载GNSS/INS组合导航方法，包括以下步骤：

S1：对GNSS系统进行初始化，获得固定解后，根据GNSS的载波以及伪距观测值，利用扩展卡尔曼滤波估计车的GNSS位置；

S2：将卡尔曼滤波校正后的GNSS位置与INS输出的位置求差，将结果作为观测值再次进行卡尔曼滤波估计，根据结果对INS输出的位置进行输出校正，最后获得的INS位置为系统位置的最终结果。

如图1所示，为GNSS/INS组合导航系统混合校正原理。该图简要内容介绍如下：首先，对GNSS系统进行初始化，获得固定解后，根据GNSS的载波以及伪距观测值，利用扩展卡尔曼滤波估计车的GNSS位置；其次，将卡尔曼滤波校正后的GNSS位置与INS输出的位置求差，将结果作为观测值再次进行卡尔曼滤波估计，根据结果对INS输出的位置进行输出校正，最后获得的INS位置为系统位置的最终结果。

本发明研究了在车载GNSS/INS组合导航定位系统中，利用卡尔曼滤波估计对列车的位置进行双重校正的方法。在GNSS系统与INS系统同时工作时，首先利用GNSS接收的载波相位以及伪距进行扩展卡尔曼滤波估计，获得GNSS的位置信息。在此基础上利用校正过后的列车GNSS位置、以及INS系统的其他导航参数再次进行卡尔曼滤波估计，根据滤波结果对INS提供的列车位置进行输出校正，最后输出列车位置。

与以往的列车GNSS/INS组合定位方法相比，该方法对组合导航系统的位置进行了两次校正。本发明首先利用扩展卡尔曼滤波对GNSS输出的位置进行了校正，在此基础上利用经典卡尔曼滤波对组合导航定位系统进行输出校正，提高了组合导航系统的位置精度以及可靠性。

该方案原理如图1和图2所示，具体包括：

(1)GNSS实时定位卡尔曼滤波校正

GNSS系统采用载波实时动态差分技术进行定位，且采用扩展卡尔曼滤波进行解算。假设对于三频GNSS接收机，估计参数即状态向量为接受机的位置、速度以及P1、P2、P5频率上单差(single-differencing,SD)载波相位整周模糊度。具体形式如下：

x＝(r_r ^T,v_r ^T,B₁ ^T,B₂ ^T,B₅ ^T)^T (1)

其中，r_r＝(r_x,r,r_y,r,r_z,r)^T表示GNSS接收机输出的x、y、z方向的位置，v_r＝(v_x,r,v_y,r,v_z,r)^T表示GNSS接收机输出的在x、y、z方向的速度，下标r表示流动站，是Li载波的SD模糊度向量，/>表示流动站与基准站在观测卫星j下的单差载波相位整周模糊度，下标b表示基准站。

滤波系统的观测量为相位距离以及伪距观测值的双差，具体形式如下：

y＝(Φ_1, ^T,Φ_2, ^T,Φ_5, ^T,P₁ ^T,P₂ ^T,P₅ ^T)^T (2)

其中，为频率i的双差相位距离向量，表示流动站与基准站、观测卫星j1与j2在i频率载波距离的双差，m为卫星个数，为频率i的双差伪距向量，/>为流动站与基准站、观测卫星j1与j2在i频率伪距的双差。

观测量模型y＝h(x)，具体形式如下：

h(x)＝(h_Φ,1 ^T,h_Φ,2 ^T,h_Φ,5 ^T,h_P,1 ^T,h_P,2 ^T,h_P,5 ^T)^T (3)

其中，式中λ_i为载波L_i的波长，/>为伪距的双差。对h(x)求其估计参数的一阶偏导数，结果如下：

其中， 为接收机至卫星视线的单位向量，/> 观测值的协方差阵如下：

其中和/>为载波和伪距的GNSS观测值误差，/>为Li载波相位距离的标准差，/>为L_i载波伪距测量的标准差。

利用扩展卡尔曼滤波，可得较为准确的位置信息，具体公式如下：

P_k/k-1＝F_kP_k-1/k-1F_k ^T+Q_k

K_k＝P_k/k-1H_k ^T[R_k+H_kP_k/k-1H_k ^T]^-1

P_k/k＝[I-K_kH_k]P_k/k-1

其中为从历元t_k-1至t_k的转换矩阵，为系统噪声的方差矩阵，τ_r＝t_k-t_k-1，Q_v为三方向速度的协方差。

(2)GNSS/INS组合导航卡尔曼滤波校正。

利用经过卡尔曼滤波校正后的GNSS位置校正INS输出的位置。本次采用经典卡尔曼滤波进行输出校正。

惯导系统的坐标系为东北天坐标系，系统状态变量为：其中φ＝[φ_x φ_y φ_z]为平台失准角，δv＝[δv_x δv_y δv_z]为速度误差，δp＝[δL δλ δh]为位置误差；ε_b＝[ε_bx ε_by ε_bz]为陀螺漂移随机常数，ε_r＝[ε_rxε_ry ε_rz]为陀螺漂移一阶马尔柯夫过程，/>为加速度计零位误差，此时下标x,y,z分别表示东、北、天三个轴向，L、λ和h分别表示载体的纬度、经度以及高度。系统误差状态方程为：

上式中X(t)的初始值为0，F(t)为状态转移矩阵，W(t)为系统噪声矩阵，G(t)为噪声驱动矩阵。系统采用松组合的方式，利用GNSS与INS提供的位置之差作为卡尔曼滤波的量测信息。由于GNSS系统输出的是WGS84坐标系下的位置，须先将其转化为大地坐标系下的坐标再与INS提供的位置求差。

量测方程如下：

Z(t)＝H(t)X(t)+V(t) (7)

上式中，量测量分别为纬度、经度、高度之差，下标I和G分别表示INS以及GNSS系统；/>V(t)为量测信号的噪声。

根据t_k-1历元经卡尔曼滤波输出校正后的系统位置[L_k-1 λ_k-1 h_k-1]^T以及姿态四元数，更新状态方程(6)中的矩阵F_k,k-1与G_k,k-1。将连续状态方程公式(1)转化为离散状态方程：X_k＝A_k,k-1X_k-1+B_k,k-1W_k-1，其中，A_k,k-1＝I+F_k,k-1T_s，B_k,k-1＝[I+F_k,k-1T_s/2]×G_k,k-1×T_s，T_s为滤波的周期。

构建卡尔曼滤波过程必须的矩阵，包括：

一步预测方差阵

估计协方差阵P_k＝[I-K_kH_k]×P_k,k-1

滤波增益矩阵

其中Q_k-1为INS系统噪声的方差矩阵，R_k为量测值的方差阵，值得说明的是，该卡尔曼滤波过程中的预测方差阵、估计协方差阵以及滤波增益矩阵，与GNSS扩展卡尔曼滤波中的相关矩阵并不相同。根据t_k时刻INS与GNSS输出的系统位置之差获得滤波估计结果/>其中/>为前一历元INS卡尔曼滤波输出校正的估计结果。其中上标0表示由卡尔曼滤波校正之前INS输出的状态(包括位置与速度)。根据滤波估计结果，对INS输出的位置进行校正，得到最终的组合导航系统卡尔曼滤波结果/>其中/>分别为/>的第7、8以及9个元素，即INS系统位置误差的估计结果。以上过程的具体原理如图1所示。

在一个具体的实施场景中，以车载GNSS/INS系统在武汉大学友谊广场采集的数据为例，采用卡尔曼滤波双重校正实时获得车在运行中的位置。图2为该方法的流程图，具体步骤如下：

(1)先对GNSS接收机进行初始化，获得固定解后组合导航定位系统及车子开始运行，此时GNSS输出的位置信息作为初始值，并向滤波系统输入初始的位置信息分别属于GNSS系统以及INS系统的估计协方差阵P₀、系统噪声方差阵Q₀，量测信息的方差阵R₀，以及INS系统的导航参数(包括速度，姿态角，姿态四元数，列车地理坐标系下的比力)。

(2)根据前一历元扩展卡尔曼滤波后计算后的GNSS位置等估计参数构建本历元GNSS扩展卡尔曼滤波估计所需的矩阵，如F_k，P_k/k-1，P_k/k，K_k。根据本历元GNSS接收机接收的载波信号以及伪距计算载波距离以及伪距的双差值，并将结果构成扩展卡尔曼滤波的观测量y＝(Φ_1, ^T,Φ_2, ^T,Φ_5, ^T,P₁ ^T,P₂ ^T,P₅ ^T)^T。根据公式计算车的GNSS位置/>

(3)根据本历元INS系统输出的位置以及卡尔曼滤波校正后的GNSS位置计算GNSS/INS组合导航系统卡尔曼滤波的观测量/>同时，利用上一历元卡尔曼校正后的INS输出的位置/>计算经典卡尔曼滤波中所须的矩阵，包括A_k/k-1，B_k/k-1，P_k/k-1，K_k。

根据公式计算系统的状态估值，利用估计结果对INS输出的位置进行校正：/>最终得到t_k时刻卡尔曼滤波估计校正后的系统位置。由于没有绝对坐标，图3中只展示出行进路径而没有参照，从输出的协方差阵可以看到精度约为3cm，证明了本发明提升GNSS/INS组合导航定位精度的有效性。

本发明的有益效果：本发明提供的这种基于双重校正的车载GNSS/INS组合导航方法，首先，对GNSS系统进行初始化，获得固定解后，根据GNSS的载波以及伪距观测值，利用扩展卡尔曼滤波估计车的GNSS位置；然后将卡尔曼滤波校正后的GNSS位置与INS输出的位置求差，将结果作为观测值再次进行卡尔曼滤波估计，根据结果对INS输出的位置进行输出校正，最后获得的INS位置为系统位置的最终结果。该方案首先对GNSS输出的位置进行卡尔曼滤波校正，再利用校正后的GNSS位置对INS位置进行卡尔曼滤波校正，提高了组合导航定位系统位置的精度及可靠性。克服了在卫星信号不佳的情况下GNSS/INS组合导航定位系统输出位置精度较差的问题。

以上例举仅仅是对本发明的举例说明，并不构成对本发明的保护范围的限制，凡是与本发明相同或相似的设计均属于本发明的保护范围之内。

Claims (4)

1.一种基于双重校正的车载GNSS/INS组合导航方法，其特征在于，包括以下步骤：

S1：对GNSS系统进行初始化，获得固定解后，根据GNSS的载波以及伪距观测值，利用扩展卡尔曼滤波估计车的GNSS位置；

S2：将卡尔曼滤波校正后的GNSS位置与INS输出的位置求差，将结果作为观测值再次进行卡尔曼滤波估计，根据结果对INS输出的位置进行输出校正，最后获得的INS位置为系统位置的最终结果；所述S1具体包括：

所述GNSS系统采用载波实时动态差分技术进行定位，且采用扩展卡尔曼滤波进行解算；

滤波系统的观测量为相位距离以及伪距观测值的双差，具体形式如下：

y＝(Φ_1, ^T,Φ_2, ^T,Φ_5, ^T,P₁ ^T,P₂ ^T,P₅ ^T)^T (2)

其中，为频率i的双差相位距离向量，/>表示流动站与基准站、观测卫星j1与j2在i频率载波距离的双差，m为卫星个数，为频率i的双差伪距向量，/>为流动站与基准站、观测卫星j1与j2在i频率伪距的双差；

观测量模型y＝h(x)，具体形式如下：

h(x)＝(h_Φ,1 ^T,h_Φ,2 ^T,h_Φ,5 ^T,h_P,1 ^T,h_P,2 ^T,h_P,5 ^T)^T (3)

其中，式中λ_i为载波L_i的波长，为伪距的双差，对h(x)求其估计参数的一阶偏导数，结果如下：

其中， 为接收机至卫星视线的单位向量，/>

其中，对于三频GNSS接收机，估计参数即状态向量为接受机的位置、速度以及P1、P2、P5频率上单差载波相位整周模糊度，具体形式如下：

x＝(r_r ^T,v_r ^T,B₁ ^T,B₂ ^T,B₅ ^T)^T (1)

其中，r_r＝(r_x,r,r_y,r,r_z,r)^T表示GNSS接收机输出的x、y、z方向的位置，v_r＝(v_x,r,v_y,r,v_z,r)^T表示GNSS接收机输出的在x、y、z方向的速度，下标r表示流动站，是L_i载波的SD模糊度向量，/>表示流动站与基准站在观测卫星j下的单差载波相位整周模糊度，下标b表示基准站。

2.根据权利要求1所述的基于双重校正的车载GNSS/INS组合导航方法，其特征在于，利用扩展卡尔曼滤波估计车的GNSS位置的具体公式包括：

P_k/k-1＝F_kP_k-1/k-1F_k ^T+Q_k

K_k＝P_k/k-1H_k ^T[R_k+H_kP_k/k-1H_k ^T]^-1

P_k/k＝[I-K_kH_k]P_k/k-1

其中为从历元t_k-1至t_k的状态转移矩阵，为系统噪声的方差矩阵，τ_r＝t_k-t_k-1，Q_v为三方向速度的协方差，P_k/k表示状态向量的协方差矩阵，H_k为转换矩阵，R_k为观测值的协方差阵，K_k表示状态增益矩阵。

3.根据权利要求1所述的基于双重校正的车载GNSS/INS组合导航方法，其特征在于，所述S2具体包括：利用经过经典卡尔曼滤波校正后的GNSS位置校正INS输出的位置。

4.根据权利要求1所述的基于双重校正的车载GNSS/INS组合导航方法，其特征在于，具体包括：

先对GNSS接收机进行初始化，获得固定解后组合导航定位系统及车子开始运行，此时GNSS输出的位置信息作为初始值，并向滤波系统输入初始的位置信息

根据前一历元扩展卡尔曼滤波后计算后的GNSS位置估计参数构建本历元GNSS扩展卡尔曼滤波估计所需的矩阵，根据本历元GNSS接收机接收的载波信号以及伪距计算载波距离以及伪距的双差值，并将结果构成扩展卡尔曼滤波的观测量y＝(Φ_1， ^T，Φ_2, ^T，Φ_5, ^T，P₁ ^T，P₂ ^T，P₅ ^T)^T，根据公式计算车的GNSS位置/>

根据本历元INS系统输出的位置以及卡尔曼滤波校正后的GNSS位置计算GNSS/INS组合导航系统卡尔曼滤波的观测量/>同时，利用上一历元卡尔曼校正后的INS输出的位置/>计算经典卡尔曼滤波中所须的矩阵，包括A_k/k-1，B_k/k-1，P_k/k-1，K_k；

根据公式计算系统的状态估值，利用估计结果对INS输出的位置进行校正：/>最终得到t_k时刻卡尔曼滤波估计校正后的系统位置。