CN108225312B - 一种gnss/ins松组合中杆臂估计以及补偿方法 - Google Patents

Abstract

本发明公开了一种GNSS/INS松组合中杆臂估计以及补偿方法，本发明所要解决的问题为：针对GNSS/INS松组合导航系统中杆臂效应量测不便，而且在导航过程中杆臂效应不断变化从而影响组合导航系统导航精度的问题，提出一种将杆臂作为卡尔曼滤波状态量进行估计的方法，该方法无需事先确定惯导与卫导天线之间的杆臂，从而避免了在导航前对杆臂的量测过程，除此之外能够有效补偿导航过程中，由于载体震动而导致的杆臂误差实时变化的问题。

Description

一种GNSS/INS松组合中杆臂估计以及补偿方法

技术领域

本发明涉及GNSS/INS组合导航领域，尤其涉及GNSS/INS组合导航系统中通过卡尔曼滤波算法解决GNSS天线和INS之间由于安装位置不重合造成的杆臂效应的估计以及补偿的方法。

背景技术

当观测条件良好时，GNSS设备单独工作就能得到可靠的导航结果。然而在高楼林立的城市、森林、峡谷等恶劣的观测条件下，当可见卫星数量小于四颗时，GNSS无法进行导航解算。INS具有较高的信息更新频率，在初始化完成后，依靠自身测得的角速度以及比力信息能够计算出载体的位置、速度、姿态等信息，然而由于受误差积累的影响，导致导航定位解算精度随时间发散。GNSS和INS具有良好的互补性，这两者的组合能够提供相较于单一系统更精确、可靠的导航定位结果。在GNSS/INS组合导航中，GNSS提供惯导需要的更新信息，从而抑制惯导信息的发散，而惯导又能输出高采样率的导航定位信息，当GNSS因为信号受到遮挡或者干扰而中断时，惯导仍能继续工作从而增加系统的可靠性和健壮性。

在实际的导航应用中，为了使惯导能够准确地反映载体的姿态，惯导通常会被安装固定在载体内部，而为了得到良好的观测信号，接收机天线通常被安装在载体顶部。由于惯导和卫导天线安装位置不一致从而产生了杆臂效应。为了得到高精度的组合导航结果，必须进行杆臂效应的补偿。传统的杆臂补偿方法是在导航进行前测量出惯导与接收机天线之间的杆臂，在实时导航的过程中进行补偿，然而在实际应用中惯导设备以及接收机天线安装复杂从而给测量带来不便，甚至导致测量得到的杆臂不准确，从而在卡尔曼滤波估计中难以准确地补偿杆臂误差，除此之外对于一些大型载体，在应用中由于震动的存在，杆臂并非常值而是处于实时变化的状态，因此将杆臂误差认定为一个固定的常值会给导航估计引入误差。因此对杆臂效应进行有效补偿是提高导航精度的关键步骤。

发明内容

本发明所要解决的问题为：针对GNSS/INS松组合导航系统中杆臂效应量测不便，而且在导航过程中杆臂效应不断变化从而影响组合导航系统导航精度的问题，提出一种将杆臂作为卡尔曼滤波状态量进行估计的方法，该方法无需事先确定惯导与卫导天线之间的杆臂，从而避免了在导航前对杆臂的量测过程，除此之外能够有效补偿导航过程中由于载体震动而导致的杆臂误差实时变化的问题。

本发明的内容如下：

一种GNSS/INS松组合中杆臂估计以及补偿方法，包括以下步骤：

步骤一：建立包含杆臂效应的卡尔曼滤波模型；

步骤二：设定包含杆臂效应的卡尔曼滤波的噪声矩阵，所述的卡尔曼滤波的噪声矩阵包括反映惯导噪声水平的状态量噪声矩阵Q阵，状态向量方差矩阵P阵，以及量测噪声矩阵R阵；

步骤三：利用卡尔曼滤波模型和包含杆臂效应的卡尔曼滤波的噪声矩阵进行包含杆臂效应的卡尔曼滤波预测以及更新，得到包含杆臂效应误差的状态向量；

步骤四：利用包含杆臂效应误差的状态向量对杆臂效应进行补偿。

其中，步骤一具体包括以下步骤：

1.1根据导航系统以及导航场景选取包含杆臂效应的卡尔曼滤波的状态向量，所述的杆臂效应L＝[l_x l_y l_z]，包含杆臂效应的卡尔曼滤波的状态向量为：

其中L为杆臂效应向量，x₀为包含位置、速度、姿态以及惯导误差的状态向量，x为包含杆臂效应的状态向量，l_x、l_y、l_z为杆臂在载体坐标系下x,y,z方向的残余误差；

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

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为状态的噪声向量，Φ_k,k-1表示为：

上式中，F₀为不包含杆臂效应的卡尔曼滤波方程对应的状态转移矩阵，I矩阵为单位矩阵；

1.3建立包含有杆臂效应的卡尔曼滤波的量测方程

Z＝Hx+V

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

上式中，H₀为不包含杆臂效应的卡尔曼滤波对应的量测方程系数矩阵，式中

其中M代表子午圈半径，N代表卯酉圈半径，h代表载体的大地高，

为从机体坐标系到导航坐标系的方向余弦矩阵，l为机体坐标系下的杆臂效应，m、n分别为不包含杆臂效应的卡尔曼滤波模型的矩阵行数和列数。

其中，步骤二中的状态向量初始不确定性矩阵P阵和反映惯导噪声水平的状态量噪声矩阵Q阵的设定方法如下：

卡尔曼滤波的状态量噪声矩阵Q阵设定如下：

其中Q₀为不包含杆臂效应的卡尔曼滤波对应的噪声阵，Q_l表示为：

其中σ_lx、σ_ly、σ_lz为杆臂效应的误差值，取值0.05；

卡尔曼滤波的状态向量初始不确定性矩阵P阵设定如下：

其中，P₀为不包含杆臂效应的卡尔曼滤波对应的状态向量初始值不确定性，P_l表示为：

其中，σ_lx0、σ_ly0、σ_lz0代表杆臂效应初始值的不确定性，当杆臂效应初始值设定为零时，该值根据应用场景进行选定。

其中，步骤三具体为：

卡尔曼滤波的预测过程：

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时刻的的量测信息矩阵；

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

其中，步骤四具体为：

利用包含杆臂效应误差的状态向量对杆臂效应进行补偿，得到杆臂效应：

lx_k＝lx_k-1+l_x

ly_k＝ly_k-1+l_y

lz_k＝lz_k-1+l_z

式中，lx_k、ly_k、lz_k依次为k时刻卡尔曼滤波估计得到的杆臂效应在载体坐标系x、y、z轴方向的分量，lx_k-1、ly_k-1、lz_k-1依次为k-1时刻卡尔曼滤波估计得到的杆臂效应在载体坐标系x、y、z轴方向的分量，l_x,l_y,l_z依次为卡尔曼滤波估计的杆臂效应x,y,z方向上的残余误差；如果在卡尔曼滤波过程中对杆臂效应进行了补偿则将杆臂效应对应的卡尔曼滤波状态向量相应项置零，否则不置零。

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

本发明的优点为：

本发明专利将杆臂效应值作为卡尔曼滤波的状态向量进行估计，从而避免了在实时导航前对杆臂值得量测，有效提高了导航效率，同时能够避免载体运动过程中由于震动的存在而导致杆臂值不断变化引入的导航误差。

附图说明

图1为卡尔曼滤波结构框图。

图2杆臂效应示意图。

具体实施方式

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

如附图1所示，本发明专利主要包括四个步骤，具体如下：

步骤一：建立包含杆臂效应的卡尔曼滤波模型。子步骤如下所示：

1.1根据导航应用的场景选取卡尔曼滤波的状态向量，一般而言选取的状态量越多越能准确地反映运动模型，越能够准确地对状态量进行估计，除此之外状态量的选取也取决于所使用的惯导等级，例如对于低等级的MEMS惯导，其比例因子和交轴耦合误差较大，在选取状态量时需要综合考虑，然而随着状态量的增多计算的复杂度也会随之增大，因此在选取状态量时需要进行综合考虑，在本实施例中选取位置误差、速度误差、姿态误差、陀螺零偏误差、加速度计零偏误差以及杆臂误差一共18个误差作为状态量。状态量如下：

其中δr_N δr_E δr_D代表北东地方向的位置误差，δv_N δv_E δv_D代表北东地方向的速度误差，ψ_N ψ_E ψ_D代表横滚、俯仰、航向角误差，b_gx b_gy b_gz代表陀螺x，y，z轴的零偏误差，b_axb_ay b_az代表加速度计x，y，z轴的零偏误差，Δlx Δly Δlz代表杆臂效应在载体坐标系中x,y,z方向上的误差。

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

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为状态的噪声向量，Φ_k,k-1表示为：

本实施例中选取了位置误差、速度误差、姿态误差、陀螺零偏误差、加速度计零偏误差以及杆臂误差一共18个状态量其对应的状态转移矩阵为：

其中：

其中，v_E、v_N、v_D依次代表载体东向速度、北向速度以及天向速度，ω_e代表地球的自传角速度，M，N，h依次代表地球卯酉圈半径、子午圈半径以及载体的大地高，

代表载体纬度，γ代表当地正常重力值，Δt,T_ab代表采样间隔以及惯导噪声的相关时间。f^b,fⁿ代表传感器输出在载体坐标系以及导航坐标系的投影。

1.3建立包含有杆臂效应的卡尔曼滤波的量测方程

Z_k＝H_kx_k+V_k

上式中Z矩阵代表卡尔曼滤波的量测信息矩阵，V矩阵代表量测噪声项，H矩阵代表量测方程系数矩阵，其中H矩阵形式如下：

其中l代表杆臂值，

代表从载体坐标系到导航坐标系的方向余弦矩阵，

为载体相对于惯性系运动在载体坐标系的投影，

为导航坐标系相对于地球坐标系运动在导航坐标系的投影，

为地球坐标系相对于惯性坐标系在导航坐标系的投影。

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

其中

为GNSS观测信息东向位置方差值、北向位置方差值、天向位置方差值、东向速度方差值、北向速度方差值以及天向速度方差值。

步骤二：设定卡尔曼滤波的噪声矩阵。卡尔曼滤波噪声矩阵包括更新方程噪声Q阵，初始状态量不确定性矩阵P阵，以及量测噪声矩阵R阵。

2.1卡尔曼滤波的Q阵设定如下：

Q_k＝diag{0 0 0 vrw² vrw² vrw² arw² arw² arw² Q_gb Q_gb Q_gb Q_ab Q_ab Q_ab σ_lx σ_lyσ_lz}

vrw代表惯性传感器的速度随机游走、arw代表惯性传感器的角度随机游走，Q_gb，Q_ab依次代表惯性传感器的输出噪声。

2.2卡尔曼滤波的量测噪声矩阵如下所示：

2.3卡尔曼滤波的P阵初值反映了状态向量的初始不确定性，P矩阵的初始值P₀设定如下：

步骤三：卡尔曼滤波预测以及更新过程。

卡尔曼滤波的预测过程：

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代表k时刻的量测噪声矩阵，I代表单位矩阵。

步骤四：补偿反馈。

卡尔曼滤波更新后便估计得到了包含杆臂效应误差的状态向量，此时便可以得到杆臂效应：

lx_k＝lx_k-1+l_x

ly_k＝ly_k-1+l_y

lz_k＝lz_k-1+l_z

在上式中lx_k、ly_k、lz_k依次为k历元卡尔曼滤波估计得到的杆臂效应在载体坐标系x、y、z轴方向的分量，lx_k-1、ly_k-1、lz_k-1依次为k-1历元卡尔曼滤波估计得到的杆臂效应在载体坐标系x、y、z轴方向的分量。如果在卡尔曼滤波过程中对杆臂效应进行了补偿则将杆臂效应对应的卡尔曼滤波状态向量相应项置零，否则不置零。

在完成步骤四后便可进入下一历元的预测过程，以上步骤便完成了一次卡尔曼滤波的更新过程，在整个导航过程中，不断地进行卡尔曼滤波的循环计算，从而在整个导航过程中提供连续可靠地导航定姿信息。

Claims (4)

1.一种GNSS/INS松组合中杆臂估计以及补偿方法，其特征在于包括以下步骤：

步骤一：建立包含杆臂效应的卡尔曼滤波模型；

步骤二：设定包含杆臂效应的卡尔曼滤波的噪声矩阵，所述的卡尔曼滤波的噪声矩阵包括反映惯导噪声水平的状态量噪声矩阵Q阵，状态向量方差矩阵P阵，以及量测噪声方差矩阵R阵；

步骤三：利用卡尔曼滤波模型和包含杆臂效应的卡尔曼滤波的噪声矩阵进行包含杆臂效应的卡尔曼滤波预测以及更新，得到包含杆臂效应误差的状态向量；

步骤四：利用包含杆臂效应误差的状态向量对杆臂效应进行补偿；

所述步骤一具体包括以下步骤：

1.1根据导航系统以及导航场景选取包含杆臂效应的卡尔曼滤波的状态向量，所述的杆臂效应L＝[l_x l_y l_z]，包含杆臂效应的卡尔曼滤波的状态向量为：

具体为：

其中，L为杆臂效应向量，x₀为包含位置、速度、姿态以及惯导误差的状态向量，x为包含杆臂效应的状态向量，l_x、l_y、l_z为杆臂在载体坐标系下x,y,z方向的残余误差；δr_Nδr_Eδr_D代表北东地方向的位置误差，δv_Nδv_Eδv_D代表北东地方向的速度误差，ψ_Nψ_Eψ_D代表横滚、俯仰、航向角误差，

代表陀螺x，y，z轴的零偏误差，

代表加速度计x，y，z轴的零偏误差，ΔlxΔlyΔlz代表杆臂效应在载体坐标系中x,y,z方向上的误差；

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

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为状态的噪声向量，Φ_k,k-1表示为：

状态转移矩阵具体为：

其中，

式中，F₀为不包含杆臂效应的卡尔曼滤波方程对应的状态转移矩阵，I矩阵为单位矩阵，v_E、v_N、v_D依次代表载体东向速度、北向速度以及天向速度，ω_e代表地球的自传角速度，M，N，h依次代表地球卯酉圈半径、子午圈半径以及载体的大地高，

代表载体纬度，γ代表当地正常重力值，Δt,T_ab代表采样间隔以及惯导噪声的相关时间，f^b,fⁿ代表传感器输出在载体坐标系以及导航坐标系的投影；

1.3建立包含有杆臂效应的卡尔曼滤波的量测方程

Z＝Hx+V

式中，Z矩阵为卡尔曼滤波的量测信息矩阵，V是量测噪声矩阵，H矩阵为量测方程系数矩阵，其中H矩阵形式如下：

上式中，H₀为不包含杆臂效应的卡尔曼滤波对应的量测方程系数矩阵，式中

其中M代表子午圈半径，N代表卯酉圈半径，h代表载体的大地高，

为从机体坐标系到导航坐标系的方向余弦矩阵，l为机体坐标系下的杆臂效应，m、n分别为不包含杆臂效应的卡尔曼滤波模型的矩阵行数和列数。

2.根据权利要求1所述的一种GNSS/INS松组合中杆臂估计以及补偿方法，其特征在于步骤二中的状态向量方差矩阵P阵和反映惯导噪声水平的状态量噪声矩阵Q阵的设定方法如下：

卡尔曼滤波的状态量噪声矩阵Q阵设定如下：

其中Q₀为不包含杆臂效应的卡尔曼滤波对应的噪声阵，n为不包含杆臂效应的卡尔曼滤波对应的噪声矩阵的维数，Q_l表示为：

其中σ_lx、σ_ly、σ_lz为杆臂效应的误差值，取值0.05；

卡尔曼滤波的状态向量方差矩阵P阵设定如下：

其中，P₀为不包含杆臂效应的卡尔曼滤波对应的噪声阵，P_l表示为：

其中，σ_lx0、σ_ly0、σ_lz0代表杆臂效应初始值的不确定性，当杆臂效应初始值设定为零时，该值根据应用场景进行选定。

3.根据权利要求1所述的一种GNSS/INS松组合中杆臂估计以及补偿方法，其特征在于步骤三具体为：

卡尔曼滤波的预测过程：

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代表k时刻的量测噪声方差矩阵，I代表单位矩阵，Z为k时刻的的量测信息矩阵；

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

4.根据权利要求1所述的一种GNSS/INS松组合中杆臂估计以及补偿方法，其特征在于步骤四具体为：

利用包含杆臂效应误差的状态向量对杆臂效应进行补偿，得到杆臂效应：

lx_k＝lx_k-1+l_x

ly_k＝ly_k-1+l_y

lz_k＝lz_k-1+l_z

式中，lx_k、ly_k、lz_k依次为k时刻卡尔曼滤波估计得到的杆臂效应在载体坐标系x、y、z轴方向的分量，lx_k-1、ly_k-1、lz_k-1依次为k-1时刻卡尔曼滤波估计得到的杆臂效应在载体坐标系x、y、z轴方向的分量，l_x,l_y,l_z依次为卡尔曼滤波估计的杆臂效应x,y,z方向上的残余误差；如果在卡尔曼滤波过程中对杆臂效应进行了补偿则将杆臂效应对应的卡尔曼滤波状态向量相应项置零，否则不置零。

Images