CN114111792B - 一种车载gnss/ins/里程计组合导航方法 - Google Patents

Abstract

本发明公开了一种车载GNSS/INS/里程计组合导航方法，属于组合导航领域。本发明首先建立包含杆臂效应的GNSS/INS/里程计组合导航系统的卡尔曼滤波模型；然后利用GNSS和里程计的输出、车辆的非完整性约束为外部观测进行卡尔曼滤波预测以及更新，估计出杆臂效应误差的状态向量。该方法增强了组合导航系统GNSS观测条件不利时的精度和自主能力，无需事先确定INS与里程计之间的杆臂，从而避免了在导航前对杆臂的量测过程，同时能够有效补偿导航过程中由于载体震动而导致的杆臂误差实时变化的问题，对不同车辆具有普适性。

Description

一种车载GNSS/INS/里程计组合导航方法

技术领域

本发明涉及组合导航领域，尤其涉及一种车载GNSS/INS/里程计组合导航方法。

背景技术

当可见卫星数量不小于四颗且卫星观测条件良好时GNSS可以得到可靠的位置、速度、时间信息，且误差不随时间积累，缺点是无姿态信息、输出频率低、易受外界电磁环境干扰和建筑物等的遮挡，导致无法正常工作。INS有完全自主性，经过初始化后可以较高的频率输出位置、信息、姿态，不受外界干扰，缺点是误差随时间积累，无法独立的长时间工作。GNSS和INS具有良好的互补性，两者的组合能够提供相较于单一系统更精确、可靠的定位测姿结果，但也存在对GNSS的依赖性。车载应用中，通过引入里程计这一完全自主式、经济高效、部署方便的陆地车辆传感器，组成车载GNSS/INS/里程计组合导航系统，适用于GNSS受限且自主性要求高的组合导航领域。

工程实际中，通常将里程计安装在前轮/后轮，而将INS安装在车辆的其他任何位置，比如车的尾部。为消除由于里程计和INS安装位置不同造成车载速度测量信息不同的影响，需要将精确补偿两者之间的杆臂误差。为了使惯导能够准确地反映载体的姿态，惯导通常会被安装固定在载体内部，由于惯导和里程计安装位置不一致从而产生了杆臂效应。为了得到高精度的组合导航结果，必须进行杆臂效应的补偿。传统的杆臂补偿方法通过提前测量出杆臂长度，在实时导航的过程中进行补偿，然而在实际应用中安装位置的复杂性给测量带来不便，甚至导致测量得到的杆臂不准确，从而在卡尔曼滤波估计中难以准确地补偿杆臂误差。此外，同一车辆的杆臂长度随车辆机械结构的变化而变化，同时也为了实现对不同车辆的普适性，将杆臂长度视为常值进行一次性补偿是不适宜的。

发明内容

本发明所要解决的问题为：针对GNSS/INS松组合导航系统中对GNSS的依赖性提出一种车载GNSS/INS/里程计组合导航方法，为克服INS与里程计间杆臂不宜测量的问题，充分利用车辆运动的非完整性约束条件，将杆臂作为卡尔曼滤波状态量进行估计。该方法增强了组合导航系统GNSS观测条件不利时的精度和自主能力，无需事先确定INS与里程计之间的杆臂，从而避免了在导航前对杆臂的量测过程，同时能够有效补偿导航过程中由于载体震动而导致的杆臂误差实时变化的问题，对不同车辆具有普适性。

为了实现上述目的，本发明采用的技术方案如下：

一种车载GNSS/INS/里程计组合导航方法，包括以下步骤：

(1)建立包含杆臂效应的GNSS/INS/里程计组合导航系统的卡尔曼滤波模型；

(2)利用GNSS和里程计的输出、车辆的非完整性约束为外部观测进行卡尔曼滤波预测以及更新，估计出杆臂效应误差的状态向量。

进一步的，步骤(1)具体包括以下步骤：

(101)在GNSS/INS松组合组合导航系统的卡尔曼滤波15维状态向量y基础上，增加杆臂效应的卡尔曼滤波的状态向量L，得到包含杆臂效应的卡尔曼滤波的状态向量为：x＝[y,L]，其中y为包含位置、速度、姿态以及IMU误差的状态向量；

(102)建立包含有杆臂效应的卡尔曼滤波的状态转移方程：

x_k＝Φ_k,k-1x_k-1+Γ_k-1W_k-1

式中，x_k,x_k-1分别为k和k-1时刻的状态向量，Φ_k,k-1为离散后的状态转移矩阵，Γ_k-1为系统噪声驱动阵，W_k-1为状态的噪声向量，且

式中，F₀为不包含杆臂效应的GNSS/INS松组合卡尔曼滤波方程对应的状态转移矩阵，I和0矩阵分别为单位矩阵和零矩阵，t为更新周期；

(103)设定卡尔曼滤波P阵和Q阵如下：

式中，P₀和Q₀为GNSS/INS松组合卡尔曼滤波模型的状态向量方差矩阵和噪声方差矩阵，PL和QL为杆臂效应对应的状态方差矩阵和噪声方差矩阵，表示如下：

式中，σ_lx、σ_ly、σ_lz为代表杆臂效应初始值的不确定性，σ_qx、σ_qy、σ_qz为杆臂效应的误差值。

进一步的，步骤(2)具体包括以下步骤：

(201)在GNSS/INS松组合组合导航系统的卡尔曼滤波6维状态向量z₀基础上，增加由里程计量测的车辆运动非完整性约束z_OD的卡尔曼滤波的量测方程：

Z＝Hx+V

式中，Z为9维量测信息矩阵，分别为位置误差、速度误差和里程计速度约束误差，V是量测噪声项，其噪声方差矩阵用R阵表示，H矩阵为量测方程系数矩阵，形式如下：

式中，H₀为不包含里程计量测的GNSS/INS松组合卡尔曼滤波模型对应的量测方程系数矩阵；

式中，为姿态阵，vⁿ为地理系下的速度，/>为陀螺仪舒输出，×表示反对称阵；

(202)卡尔曼滤波的预测过程如下：

x_k+1＝Φ_kx_k

式中，Φ_k为状态转移矩阵，x_k、x_k+1依次代表k时刻以及k+1时刻的状态向量，Q_k代表k时刻的状态量噪声矩阵，P_k、P_k+1依次代表k时刻以及k+1时刻的状态向量方差矩阵；

卡尔曼滤波的更新过程如下：

x_k+1＝x_k+K_k(Z_k-H_kx_k)

P_k+1＝(I-K_kH_k)P_k

式中，K_k代表k时刻的增益矩阵，H_k代表k时刻的量测方程系数矩阵，R_k代表量测噪声矩阵，I代表单位矩阵，Z_k为k时刻的量测信息矩阵；

预测和更新完成后得到包含杆臂效应误差的状态向量；

在滤波过程中对状态向量中的杆臂长度持续估计，状态量不清零。

本发明的有益效果在于：

1、本发明在传统GNSS/INS松组合导航系统基础上引入了车辆上常见的里程计，降低了系统对GNSS的依赖性。

2、本发明将INS与里程计之间的杆臂长度扩充到卡尔曼滤波的状态向量并进行实时估计，克服了杆臂难以测量的问题，增加了对不同车辆的普适性。

3、本发明可用于车载GNSS/INS/里程计组合导航系统中，可通过卡尔曼滤波算法解决INS与里程计之间由于安装位置不重合造成的杆臂效应的估计以及补偿的问题。

附图说明

图1为车辆中INS与里程计之间的杆臂示意图；

图2为卡尔曼滤波流程框图。

具体实施方式

下面结合具体实施例和附图对本发明做进一步的描述。

一种GNSS/INS/里程计组合导航方法，包括以下步骤：

(1)建立包含杆臂效应的GNSS/INS/里程计组合导航系统的卡尔曼滤波模型；

(2)利用GNSS和里程计的输出、车辆的非完整性约束为外部观测进行卡尔曼滤波预测以及更新，估计出杆臂效应误差的状态向量。

步骤(1)的具体方式为：

(101)在GNSS/INS松组合组合导航系统的卡尔曼滤波15维状态向量y基础上，增加杆臂效应的卡尔曼滤波的状态向量L，得到包含杆臂效应的卡尔曼滤波的状态向量为：x＝[y,L]。其中y为包含位置、速度、姿态以及IMU误差的状态向量；

(102)建立包含有杆臂效应的卡尔曼滤波的状态转移方程：

x_k＝Φ_k,k-1x_k-1+Γ_k-1W_k-1

式中，x_k,x_k-1分别为k和k-1时刻的状态向量，Φ_k,k-1为离散后的状态转移矩阵，Γ_k-1为系统噪声驱动阵，W_k-1为状态的噪声向量，且

式中，F₀为不包含杆臂效应的GNSS/INS松组合卡尔曼滤波方程对应的状态转移矩阵，I和0矩阵分别为为单位矩阵和零矩阵，t为更新周期；

(103)设定卡尔曼滤波P阵和Q阵，方法如下：

式中，P₀和Q₀为GNSS/INS松组合卡尔曼滤波模型的状态向量方差矩阵和噪声方差矩阵，PL和QL为杆臂效应对应的状态方差矩阵和噪声方差矩阵，表示如下：

式中，σ_lx、σ_ly、σ_lz为代表杆臂效应初始值的不确定性，σ_qx、σ_qy、σ_qz为杆臂效应的误差值。

步骤(2)的具体方式为：

(201)在GNSS/INS松组合组合导航系统的卡尔曼滤波6维状态向量z₀基础上，增加由里程计量测的车辆运动非完整性约束z_OD的卡尔曼滤波的量测方程：

Z＝Hx+V

式中，Z为9维量测信息矩阵，分别为位置误差、速度误差和里程计速度约束误差，V是量测噪声项，其噪声方差矩阵用R阵表示，H矩阵为量测方程系数矩阵形式如下：

式中，H₀为不包含里程计量测的GNSS/INS松组合卡尔曼滤波模型对应的量测方程系数矩阵；

式中，为姿态阵，vⁿ为地理系下的速度，/>为陀螺仪舒输出。

(202)卡尔曼滤波的预测过程：

x_k+1＝Φ_kx_k

式中，Φ_k为状态转移矩阵，x_k、x_k+1依次代表k时刻以及k+1时刻的状态向量，Q_k代表k时刻的状态量噪声矩阵，P_k、P_k+1依次代表k时刻以及k+1时刻的状态向量方差矩阵；

卡尔曼滤波的更新过程：

x_k+1＝x_k+K_k(Z_k-H_kx_k)

P_k+1＝(I-K_kH_k)P_k

式中，K_k代表k时刻的增益矩阵，H_k代表k时刻的量测方程系数矩阵，R_k代表量测噪声矩阵，I代表单位矩阵，Z为k时刻的量测信息矩阵；预测和更新完成得到了包含杆臂效应误差的状态向量。在滤波过程中持续对状态向量中的杆臂长度持续估计，状态量不清零。

通过以上步骤便完成了一次卡尔曼滤波的时间更新和量测更新，在组合导航过程中，不断地进行卡尔曼滤波的循环计算，从而可以提供连续可靠地定位测姿信息。

以下为一个更具体的例子：

一种GNSS/INS/里程计组合导航方法，包括以下步骤：

(1)建立包含杆臂效应的GNSS/INS/里程计组合导航系统的卡尔曼滤波模型。具体步骤如下：

(101)基于车载组合导航系统中使用的低成本MEMS惯性器件和GNSS定位模块、软件算法的低计算强度等因素，在本实施例中选取位置误差、速度误差、姿态误差、陀螺零偏误差、里程计杆臂误差一共15个误差作为状态量。状态量如下：

式中，[δλ δL δh]为纬度、经度和高度误差，[δv_E δv_N δv_U]为东北天方向的速度误差，为失准角误差，[ε_x ε_y ε_z]为陀螺三个轴向的零偏，[l_x l_y l_z]为载体坐标系中三个方向上的杆臂长度，如图1所示。

(102)建立包含有杆臂效应的卡尔曼滤波的状态转移方程：

x_k＝Φ_k,k-1x_k-1+Γ_k-1W_k-1

式中，x_k,x_k-1分别为k和k-1时刻的状态向量，Φ_k,k-1为离散后的状态转移矩阵，Γ_k-1为系统噪声驱动阵，W_k-1为状态的噪声向量。根据本实施例中选取的15个状态变量，Φ_k,k-1表示为：

式中：

M_vp＝(vⁿ×)(2M₁+M₂)，/>

R_Mh、R_Nh分别为地球卯酉圈半径和子午圈半径，为地球自转角速度在地理系的投影，/>为地理系相对于地球的转动角速度在地理系的投影，fⁿ为加速度计输出在地理系的投影，/>为姿态矩阵，t为更新周期。

(103)设定卡尔曼滤波P阵和Q阵：

式中vrw、arw分别为代表惯性传感器的速度随机游走和角度随机游走。

(2)利用GNSS和里程计的输出、车辆的非完整性约束为外部观测进行卡尔曼滤波预测以及更新，估计出杆臂效应误差的状态向量。具体步骤如下：

(201)建立包含有杆臂效应、车辆运动非完整性约束的卡尔曼滤波的量测方程

Z＝Hx+V

式中，Z矩阵为卡尔曼滤波的(m+3)维量测信息矩阵，V是量测噪声项，其噪声方差矩阵用R阵表示，H矩阵为量测方程系数矩阵形式如下：

式中，H₀为常规GNSS/INS松组合卡尔曼滤波模型对应的量测方程系数矩阵，可表示为：

量测噪声项的方差矩阵R阵表示如下：

其中，分别为GNSS观测信息的纬度、经度和高度的方差，/> 分别为GNSS观测信息的东向速度、北向速度和天向速度的方差，/>分别为里程计观测信息的三轴速度方差，其中两轴为车辆的非完整约束。

(202)如图2所示，卡尔曼滤波的预测过程：

x_k+1＝Φ_kx_k

式中，Φ_k为状态转移矩阵，x_k、x_k+1依次代表k时刻以及k+1时刻的状态向量，Q_k代表k时刻的状态量噪声矩阵，P_k、P_k+1依次代表k时刻以及k+1时刻的状态向量方差矩阵；

卡尔曼滤波的更新过程：

x_k+1＝x_k+K_k(Z_k-H_kx_k)

P_k+1＝(I-K_kH_k)P_k

式中K_k代表k时刻的增益矩阵，H_k代表k时刻的量测方程系数矩阵，R_k代表量测噪声矩阵，I代表单位矩阵，Z为k时刻的量测信息矩阵；预测和更新完成得到了包含杆臂效应误差的状态向量。在滤波过程中持续对状态向量中的杆臂长度持续估计，状态量不清零。

通过以上步骤便完成了一次卡尔曼滤波的时间更新和量测更新，在组合导航过程中，不断地进行卡尔曼滤波的循环计算，从而可以提供连续可靠地定位测姿信息。

总之，本发明针对GNSS/INS松组合导航系统中对GNSS的依赖性，采用一种车载GNSS/INS/里程计组合导航方法，该方法充分利用车辆运动的非完整性约束条件，将杆臂作为卡尔曼滤波状态量进行估计，从而克服INS与里程计间杆臂不宜测量的问题。该方法增强了组合导航系统GNSS观测条件不利时的精度和自主能力，无需事先确定INS与里程计之间的杆臂，从而避免了在导航前对杆臂的量测过程，同时能够有效补偿导航过程中由于载体震动而导致的杆臂误差实时变化的问题，对不同车辆具有普适性。

Claims (1)

1.一种车载GNSS/INS/里程计组合导航方法，其特征在于，包括以下步骤：

(1)建立包含杆臂效应的GNSS/INS/里程计组合导航系统的卡尔曼滤波模型；包括以下步骤：

(101)在GNSS/INS松组合组合导航系统的卡尔曼滤波15维状态向量y基础上，增加杆臂效应的卡尔曼滤波的状态向量L，得到包含杆臂效应的卡尔曼滤波的状态向量为：x＝[y,L]，其中y为包含位置、速度、姿态以及IMU误差的状态向量；

(102)建立包含有杆臂效应的卡尔曼滤波的状态转移方程：

x_k＝Φ_k,k-1x_k-1+Γ_k-1W_k-1

式中，x_k,x_k-1分别为k和k-1时刻的状态向量，Φ_k,k-1为离散后的状态转移矩阵，Γ_k-1为系统噪声驱动阵，W_k-1为状态的噪声向量，且

式中，F₀为不包含杆臂效应的GNSS/INS松组合卡尔曼滤波方程对应的状态转移矩阵，I和0矩阵分别为单位矩阵和零矩阵，t为更新周期；

(103)设定卡尔曼滤波P阵和Q阵如下：

式中，P₀和Q₀为GNSS/INS松组合卡尔曼滤波模型的状态向量方差矩阵和噪声方差矩阵，PL和QL为杆臂效应对应的状态方差矩阵和噪声方差矩阵，表示如下：

式中，σ_lx、σ_ly、σ_lz为代表杆臂效应初始值的不确定性，σ_qx、σ_qy、σ_qz为杆臂效应的误差值；

(2)利用GNSS和里程计的输出、车辆的非完整性约束为外部观测进行卡尔曼滤波预测以及更新，估计出杆臂效应误差的状态向量；包括以下步骤：

(201)在GNSS/INS松组合组合导航系统的卡尔曼滤波6维状态向量z₀基础上，增加由里程计量测的车辆运动非完整性约束z_OD的卡尔曼滤波的量测方程

Z＝Hx+V

式中，Z为9维量测信息矩阵，分别为位置误差、速度误差和里程计速度约束误差，V是量测噪声项，其噪声方差矩阵用R阵表示，H矩阵为量测方程系数矩阵，形式如下：

式中，H₀为不包含里程计量测的GNSS/INS松组合卡尔曼滤波模型对应的量测方程系数矩阵；

式中，为姿态阵，vⁿ为地理系下的速度，/>为陀螺仪舒输出，×表示反对称阵；

(202)卡尔曼滤波的预测过程如下：

x_k+1＝Φ_kx_k

式中，Φ_k为状态转移矩阵，x_k、x_k+1依次代表k时刻以及k+1时刻的状态向量，Q_k代表k时刻的状态量噪声矩阵，P_k、P_k+1依次代表k时刻以及k+1时刻的状态向量方差矩阵；

卡尔曼滤波的更新过程如下：

x_k+1＝x_k+K_k(Z_k-H_kx_k)

P_k+1＝(I-K_kH_k)P_k

式中，K_k代表k时刻的增益矩阵，H_k代表k时刻的量测方程系数矩阵，R_k代表量测噪声矩阵，I代表单位矩阵，Z_k为k时刻的量测信息矩阵；

预测和更新完成后得到包含杆臂效应误差的状态向量；

在滤波过程中对状态向量中的杆臂长度持续估计，状态量不清零。