CN109459044A - 一种gnss双天线辅助的车载mems惯导组合导航方法 - Google Patents

Abstract

本发明涉及一种GNSS双天线辅助的车载MEMS惯导组合导航方法，其中，包括：MEMS惯导的位置，则可由GNSS提供，MEMS惯导的速度MEMS惯导进行导航解算；建立捷联惯导误差模型；建立观测误差模型；采用卡尔曼滤波对MEMS惯导误差进行估计；进行MEMS惯性导航误差校正；重复上述步骤，完成组合导航循环递推解算。本发明一种GNSS双天线辅助的车载MEMS惯导组合导航方法，在信息融合过程中，有GNSS观测值时采用反馈校正，无观测值时采用输出校正的方法，使组合导航系统能持续工作，不发散。

Description

一种GNSS双天线辅助的车载MEMS惯导组合导航方法

技术领域

本发明涉及导航技术，特别涉及一种GNSS双天线辅助的车载MEMS惯导组合导航方法。

背景技术

车载组合导航系统一般采用惯导系统+卫星导航(GNSS)的组合导航方式，可以为载车提供连续的位置、速度和姿态信息。其中，惯导可以连续输出载体的位置、速度和姿态，但是长时间会发散，而GNSS可提供不随时间发散的位置、速度信息，但是更新速率慢，且容易受到树木、高楼、桥梁等遮挡失去信号。而将惯导系统和GNSS组合起来，则可以互相弥补二者的缺点，实现高速率、高精度的导航。

传统的车载组合导航系统为了得到高精度的姿态信息，需要惯导系统的精度较高，一般惯导系统要求陀螺仪零偏稳定性在1°/h以内，加速度计偏值稳定性在100ug以内。然而由于高精度惯性器件较为昂贵，导致组合导航系统的成本高昂，进而限制了应用。基于该缺点，也有基于低成本MEMS惯导的组合导航系统，配备地磁传感器，为导航系统提供航向角信息，避免航向角的发散。由于MEMS和地磁传感器价格较低，因此整个组合导航系统的成本会有大幅下降。

传统的车载组合导航系统对惯导系统的器件精度要求高，而高精度惯性器件的成本高昂，限制了车载组合导航系统的应用。而基于MEMS惯导的组合导航系统，虽然有地磁传感器辅助，但是由于地磁传感器的精度较差，且在不同地区还需对地磁传感器输出的航向角进行校正，严重限制了组合导航系统的精度和使用的便利性。

发明内容

本发明的目的在于提供一种基于GNSS双天线辅助定向的车载MEMS组合导航方法，用于解决目前高精度车载组合导航系统价格昂贵，基于MEMS惯导的组合导航系统精度较差的缺点。

本发明一种GNSS双天线辅助的车载MEMS惯导组合导航方法，其中，包括：MEMS惯导的位置，则可由GNSS提供，MEMS惯导的速度，由于载体处于静止状态，则速度为0，MEMS惯导能敏感到载体的角运动和线运动，角速度为比力为f^b，输出俯仰角为θ_GNSS，记输出的航向角为输出的位置为p_GNSS＝[L_GNSS λ_GNSS h_GNSS]，L_GNSS为GNSS输出的纬度，λ_GNSS为GNSS输出的经度，h_GNSS为GNSS输出的高度，记输出的速度为v_GNSS＝[v_GNSS,E v_GNSS,N v_GNSS,U]^T，v_GNSS,E为GNSS输出的东向速度，v_GNSS,N为GNSS输出的北向速度，v_GNSS,U为GNSS输出的天向速度；

MEMS惯导进行导航解算包括：

进行MEMS惯导位置速度姿态解算，已知k-1时刻MEMS惯导解算出的位置p_k-1＝[L_k-1λ_k-1 h_k-1]^T，其中L_k-1为k-1时刻的纬度、λ_k-1为k-1时刻的经度、h_k-1为k-1时刻的高度，速度其中，为k-1时刻的东向、为k-1时刻的北向以及为k-1时刻的天向速度，姿态矩阵MEMS惯导输出的角速度比力则k时刻速度更新公式为：

k时刻位置更新为:

其中，T为采样周期，当a＝[a_x a_y a_z]^T时，符号(a×)的意义为:

R_Mh,k-1＝R_M,k-1+h_k-1，R_Nh,k-1＝R_N,k-1+h_k-1；

g_k-1＝g₀(1+β₁sin²L_k-1+β₂sin⁴L_k-1)-β₃h_k-1；

g₀＝9.7803267714，R_e＝6378137；

f＝1/298.257，ω_ie＝7.2921151467E-5；

β₁＝5.27094×10^-3,β₂＝2.32718×10^-5,β₃＝3.086×10^-6；

k时刻的姿态四元数更新公式为:

由k时刻姿态四元数求得k时刻姿态矩阵

建立捷联惯导误差模型包括：

以捷联惯导位置误差、速度误差、姿态误差及陀螺常值漂移和加速度零偏为状态量，建立捷联惯导误差模型：

其中，

φ＝[φ_x φ_y φ_z]^T，φ_x为俯仰角误差，φ_y为横滚角误差，φ_z为航向角误差； 为东向速度误差，为北向速度误差，为天向速度误差；δp＝[δL δλ δh]^T，δL为纬度误差，δλ为经度误差，δh为高度误差；ε＝[ε_x ε_y ε_z]^T，ε_x为X陀螺常值漂移，ε_y为Y陀螺常值漂移，ε_z为Z陀螺常值漂移； 为X轴加速度计零偏，为Y轴加速度计零偏，为Z轴加速度计零偏；w_g＝[w_gx w_gy w_gz]^T，w_gx为X陀螺随机噪声，w_gy为Y陀螺随机噪声，w_gz为Z陀螺随机噪声；w_a＝[w_ax w_ay w_az]^T，w_ax为X加速度计随机噪声，w_ay为Y加速度计随机噪声，w_az为Z加速度计随机噪声，设w的方差为Q：

M₂＝M′+M″，M₄＝(vⁿ×)(2M′+M″)；

0_3×3表示一个3×3维的全零矩阵，对误差方程进行离散化处理，则k时刻离散化方程为：

X_k＝F_k-1X_k-1+G_k-1w_k-1；

其中，

G_k-1＝G(t_k-1)；

T为离散时间间隔，I_m表示m×m维的单位矩阵，A(t_k-1)和G(t_k-1)通过将k-1时刻MEMS惯导输出的角速度比力以及解算的位置p_k-1、速度以及姿态矩阵代入到A和G计算得到，w_k-1为k-1时刻离散化后的系统噪声，其方差为Q_k-1＝Q·T；

建立观测误差模型，包括：

系统的观测方程选择位置误差、速度误差、俯仰角误差和航向角误差，则有Z_k＝H_kX_k+υ_k；

其中，

θ_k为k时刻MEMS惯导解算的俯仰角，为k时刻MEMS惯导解算的航向角，从k时刻解算出的姿态矩阵中提取出来，θ_GNSS,k为k时刻GNSS双天线系统输出的俯仰角，为k时刻GNSS双天线系统输出的航向角，υ_k为观测噪声，方差为R_k；

采用卡尔曼滤波对MEMS惯导误差进行估计；

进行MEMS惯性导航误差校正；

重复上述步骤，完成组合导航循环递推解算。

本发明设计了一种GNSS双天线辅助的车载MEMS惯导组合导航方法，创新性的采用GNSS双天线系统，克服了以往单天线GNSS只能提供位置、速度的缺点，还可以提供俯仰角和航向角，再采用卡尔曼滤波进行MEMSMEMS惯导和GNSS双天线系统的信息融合，在信息融合过程中，有GNSS观测值时采用反馈校正，无观测值时采用输出校正的方法，使组合导航系统能持续工作，不发散。

附图说明

无

具体实施方式

为使本发明的目的、内容、和优点更加清楚，下面结合实施例，对本发明的具体实施方式作进一步详细描述。

本发明一种GNSS双天线辅助的车载MEMS惯导组合导航方法，包括：

第一步MEMS惯导与GNSS天线的安装，包括：

首先定义载体坐标系b系：b系的X轴指向车体右向，Y轴指向车头，Z轴遵循右手螺旋法则。则MEMS惯导在安装时，MEMS的X轴与b系的X轴平行，Y轴与b系的Y轴平行，Z轴与b系的Z轴平行。一般来说，GNSS双天线具有主、副两个天线，需将主天线安装在车前方，副天线安装在车后方，且两个天线中心的连线与b系的Y轴平行，两个天线的距离要尽量长，距离越长，则GNSS输出的俯仰角和航向角精度越高。

MEMS惯导能敏感到载体的角运动和线运动，以100Hz以上的频率输出载体的角速度和比力，记角速度为比力为f^b。GNSS双天线能测量载体的俯仰角和航向角信息，以1-10Hz的频率输出，记输出俯仰角为θ_GNSS，记输出的航向角为且还能测量载体的位置和速度信息，记输出的位置为p_GNSS＝[L_GNSS λ_GNSS h_GNSS]，L_GNSS为GNSS输出的纬度，λ_GNSS为GNSS输出的经度，h_GNSS为GNSS输出的高度，记输出的速度为v_GNSS＝[v_GNSS,E v_GNSS,N v_GNSS,U]^T，v_GNSS,E为GNSS输出的东向速度，v_GNSS,N为GNSS输出的北向速度，v_GNSS,U为GNSS输出的天向速度。

第二步MEMS惯导的初始对准及位置速度初始化

在进行组合导航前，MEMS惯导需要进行初始对准，以确定初始姿态信息。该过程需在载车静止件下进行。

定义导航坐标系n为当地地理坐标系，即n系的X轴指向东，Y轴指向北，Z轴指向天。

首先，采集一段时间内(一般为10-60s，可根据MEMS器件精度及实际使用需求进行设置)MEMS惯导输出的比力f^b，并进行平均，记平均出的结果为 为三维向量，记为则可求出此时载体的俯仰角θ和横滚角γ

其中，

载体的航向角可由GNSS提供，即由于载体坐标系和MEMS惯导坐标系重合，则此时MEMS惯导的姿态矩阵可由下式求得：

此时，即完成了MEMS惯导的初始对准。

MEMS惯导的位置，则可由GNSS提供，MEMS惯导的速度，由于载体处于静止状态，则速度为0。

第三步MEMS惯导进行导航解算

MEMS惯导初始对准完成后，即可进入组合导航模式。在组合导航过程中，首先进行MEMS惯导位置速度姿态解算，采用的是递推的方法，即已知k-1时刻MEMS惯导解算出的位置p_k-1＝[L_k-1 λ_k-1 h_k-1]^T，其中L_k-1、λ_k-1、h_k-1分别为k-1时刻的纬度，经度和高度，速度其中，分别为k-1时刻的东向、北向、天向速度，姿态矩阵以及MEMS惯导输出的角速度比力则

则k时刻速度更新公式为

k时刻位置更新为

其中，T为采样周期，当a＝[a_x a_y a_z]^T时，符号(a×)的意义为

R_Mh,k-1＝R_M,k-1+h_k-1，R_Nh,k-1＝R_N,k-1+h_k-1

g_k-1＝g₀(1+β₁sin²L_k-1+β₂sin⁴L_k-1)-β₃h_k-1

g₀＝9.7803267714，R_e＝6378137

f＝1/298.257，ω_ie＝7.2921151467E-5

β₁＝5.27094×10^-3,β₂＝2.32718×10^-5,β₃＝3.086×10^-6

姿态矩阵的更新一般采用四元数法，具体为先将转化成四元数，具体公式为

其中，sign(x)为取符号的意思，即当x＞0，则sign(x)＝1，当x＜0，则sign(x)＝-1，符号M(m,n)表示矩阵M的第m行、第n列元素。

则k时刻的姿态四元数更新公式为

再由k时刻姿态四元数求得k时刻姿态矩阵

第四步建立捷联惯导误差模型

以捷联惯导位置误差、速度误差、姿态误差及陀螺常值漂移和加速度零偏为状态量，建立捷联惯导误差模型：

其中，

φ＝[φ_x φ_y φ_z]^T，φ_x为俯仰角误差，φ_y为横滚角误差，φ_z为航向角误差； 为东向速度误差，为北向速度误差，为天向速度误差；δp＝[δL δλ δh]^T，δL为纬度误差，δλ为经度误差，δh为高度误差；ε＝[ε_x ε_y ε_z]^T，ε_x为X陀螺常值漂移，ε_y为Y陀螺常值漂移，ε_z为Z陀螺常值漂移； 为X轴加速度计零偏，为Y轴加速度计零偏，为Z轴加速度计零偏；w_g＝[w_gx w_gy w_gz]^T，w_gx为X陀螺随机噪声，w_gy为Y陀螺随机噪声，w_gz为Z陀螺随机噪声；w_a＝[w_ax w_ay w_az]^T，w_ax为X加速度计随机噪声，w_ay为Y加速度计随机噪声，w_az为Z加速度计随机噪声，设w的方差为Q

M₂＝M′+M″，M₄＝(vⁿ×)(2M′+M″)

0_3×3表示一个3×3维的全零矩阵，

对误差方程进行离散化处理，则k时刻离散化方程为X_k＝F_k-1X_k-1+G_k-1w_k-1

其中，

G_k-1＝G(t_k-1)

T为离散时间间隔，I_m表示m×m维的单位矩阵，A(t_k-1)，G(t_k-1)只需将k-1时刻MEMS惯导输出的角速度比力以及解算的位置p_k-1，速度姿态矩阵代入到A和G即可计算得到，w_k-1为k-1时刻离散化后的系统噪声，其方差为Q_k-1＝Q·T。

第五步建立观测误差模型

系统的观测方程选择位置误差、速度误差、俯仰角误差和航向角误差，则有

Z_k＝H_kX_k+υ_k

其中，

θ_k、为k时刻MEMS惯导解算的俯仰角和航向角，可从k时刻解算出的姿态矩阵中提取出来，θ_GNSS,k、为k时刻GNSS双天线系统输出的俯仰角和航向角。υ_k为观测噪声，一般为高斯白噪声，设其方差为R_k

第六步采用卡尔曼滤波对MEMS惯导误差进行估计

由于MEMS惯导解算频率要高于GNSS输出的频率，因此，在进行卡尔曼滤波时，会出现某些时刻无观测值的情况，当无观测值时，卡尔曼滤波的递推公式为

当有观测值时，卡尔曼滤波递推公式为

P_k＝(I₁₅-K_kH_k)P_k/k-1

第七步MEMS惯性导航误差校正

假设k时刻，MEMS惯导解算的位置为p_k，速度为姿态矩阵为k时刻对导航误差的估计为其中，为估计的位置误差，为估计速度误差，为估计的姿态误差，为估计的加速度零偏，为估计的陀螺漂移。MEMS导航误差校正过程中，在不同时刻，校正方法也不同。当无观测值时，采用的输出校正，当有观测值时，采用反馈校正。

输出校正方法为

其中，是k时刻计算出的校正后位置，是k时刻计算出的校正后速度，k时刻校正后的姿态角可从中提取。

而反馈校正方法，在进行上述输出校正计算的同时，还需要将MEMS惯导的位置和速度姿态矩阵调整为输出校正的结果，且卡尔曼滤波的的前9项元素要修改成0。具体为

在不同时刻采用不同校正方法是为了使整个组合导航过程中卡尔曼滤波不发散，能时刻提供准确的导航信息。

第八步组合导航循环递推解算

第三至第七步完成了组合导航系统的一次解算，当MEMS惯导输出k+1时刻的角速度和比力，GNSS也在k+1时刻输出相应的位置、速度、及俯仰角和航向角时(由于MEMS惯导输出频率高，GNSS输出频率低，因此GNSS在k+1时刻也可能不输出，但即使GNSS不输出信息，也可进行组合导航解算)，则重复第三至第七步，完成k+1时刻的组合导航解算。依次循环递推下去。

本发明设计了一种GNSS双天线辅助的车载MEMS惯导组合导航方法，该方法具有以下优点：(1)GNSS双天线系统不但能够提供载体的位置和速度信息，还能提供俯仰角和航向角，使得GNSS与MEMS惯导进行组合导航时，能提供部分姿态信息；(2)GNSS双天线系统能提供部分姿态信息，使航向角不再发散，且组合导航系统对惯导的精度要求降低，因此可采用MEMS惯导来实现高精度的组合导航，提供高精度的位置、速度、姿态信息；(3)组合导航系统采用卡尔曼滤波的信息融合方法，且采用混合校正的导航误差修正方法，在无GNSS观测值时采用输出校正，有GNSS观测值时采用反馈校正，使组合导航系统能持续工作，不发散。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (8)

1.一种GNSS双天线辅助的车载MEMS惯导组合导航方法，其特征在于，包括：

MEMS惯导的位置，则可由GNSS提供，MEMS惯导的速度，由于载体处于静止状态，则速度为0，MEMS惯导能敏感到载体的角运动和线运动，角速度为比力为f^b，输出俯仰角为θ_GNSS，记输出的航向角为输出的位置为p_GNSS＝[L_GNSS λ_GNSS h_GNSS]，L_GNSS为GNSS输出的纬度，λ_GNSS为GNSS输出的经度，h_GNSS为GNSS输出的高度，记输出的速度为v_GNSS＝[v_GNSS,E v_GNSS,Nv_GNSS,U]^T，v_GNSS,E为GNSS输出的东向速度，v_GNSS,N为GNSS输出的北向速度，v_GNSS,U为GNSS输出的天向速度；

MEMS惯导进行导航解算包括：

进行MEMS惯导位置速度姿态解算，已知k-1时刻MEMS惯导解算出的位置p_k-1＝[L_k-1 λ_k-1h_k-1]^T，其中L_k-1为k-1时刻的纬度、λ_k-1为k-1时刻的经度、h_k-1为k-1时刻的高度，速度其中，为k-1时刻的东向、为k-1时刻的北向以及为k-1时刻的天向速度，姿态矩阵MEMS惯导输出的角速度比力则k时刻速度更新公式为：

k时刻位置更新为:

其中，T为采样周期，当a＝[a_x a_y a_z]^T时，符号(a×)的意义为:

R_Mh,k-1＝R_M,k-1+h_k-1，R_Nh,k-1＝R_N,k-1+h_k-1；

g_k-1＝g₀(1+β₁sin²L_k-1+β₂sin⁴L_k-1)-β₃h_k-1；

g₀＝9.7803267714，R_e＝6378137；

f＝1/298.257，ω_ie＝7.2921151467E-5；

β₁＝5.27094×10^-3,β₂＝2.32718×10^-5,β₃＝3.086×10^-6；

k时刻的姿态四元数更新公式为:

由k时刻姿态四元数求得k时刻姿态矩阵

建立捷联惯导误差模型包括：

以捷联惯导位置误差、速度误差、姿态误差及陀螺常值漂移和加速度零偏为状态量，建立捷联惯导误差模型：

其中，

φ＝[φ_x φ_y φ_z]^T，φ_x为俯仰角误差，φ_y为横滚角误差，φ_z为航向角误差； 为东向速度误差，为北向速度误差，为天向速度误差；δp＝[δL δλ δh]^T，δL为纬度误差，δλ为经度误差，δh为高度误差；ε＝[ε_x ε_y ε_z]^T，ε_x为X陀螺常值漂移，ε_y为Y陀螺常值漂移，ε_z为Z陀螺常值漂移； 为X轴加速度计零偏，为Y轴加速度计零偏，为Z轴加速度计零偏；w_g＝[w_gx w_gy w_gz]^T，w_gx为X陀螺随机噪声，w_gy为Y陀螺随机噪声，w_gz为Z陀螺随机噪声；w_a＝[w_ax w_ay w_az]^T，w_ax为X加速度计随机噪声，w_ay为Y加速度计随机噪声，w_az为Z加速度计随机噪声，设w的方差为Q：

M₂＝M′+M″，M₄＝(vⁿ×)(2M′+M″)；

0_3×3表示一个3×3维的全零矩阵，对误差方程进行离散化处理，则k时刻离散化方程为：

X_k＝F_k-1X_k-1+G_k-1w_k-1；

其中，

T为离散时间间隔，I_m表示m×m维的单位矩阵，A(t_k-1)和G(t_k-1)通过将k-1时刻MEMS惯导输出的角速度比力以及解算的位置p_k-1、速度以及姿态矩阵代入到A和G计算得到，w_k-1为k-1时刻离散化后的系统噪声，其方差为Q_k-1＝Q·T；

建立观测误差模型，包括：

系统的观测方程选择位置误差、速度误差、俯仰角误差和航向角误差，则有Z_k＝H_kX_k+υ_k；

其中，

θ_k为k时刻MEMS惯导解算的俯仰角，为k时刻MEMS惯导解算的航向角，从k时刻解算出的姿态矩阵中提取出来，θ_GNSS,k为k时刻GNSS双天线系统输出的俯仰角，为k时刻GNSS双天线系统输出的航向角，υ_k为观测噪声，方差为R_k；

采用卡尔曼滤波对MEMS惯导误差进行估计；

进行MEMS惯性导航误差校正；

重复上述步骤，完成组合导航循环递推解算。

2.如权利要求1所述的GNSS双天线辅助的车载MEMS惯导组合导航方法，还包括：MEMS惯导与GNSS天线的安装，包括：

首先定义载体坐标系b系：b系的X轴指向车体右向，Y轴指向车头，Z轴遵循右手螺旋法则，MEMS惯导在安装时，MEMS的X轴与b 系的X轴平行，Y轴与b系的Y轴平行，Z轴与b系的Z轴平行。

3.如权利要求1所述的GNSS双天线辅助的车载MEMS惯导组合导航方法，。GNSS双天线的主天线安装在车前方，副天线安装在车后方，且两个天线中心的连线与b系的Y轴平行。

4.如权利要求1所述的GNSS双天线辅助的车载MEMS惯导组合导航方法，还包括：进行MEMS惯导的初始对准及位置速度初始化，包括：

定义导航坐标系n为当地地理坐标系，n系的X轴指向东，Y轴指向北，Z轴指向天；

首先，采集一段时间内MEMS惯导输出的比力f^b，并进行平均，记平均出的结果为 为三维向量，记为则可求出此时载体的俯仰角θ和横滚角γ；

其中，

载体的航向角由于载体坐标系和MEMS惯导坐标系重合，则此时MEMS惯导的姿态矩阵可由下式求得：

完成MEMS惯导的初始对准。

5.如权利要求1所述的GNSS双天线辅助的车载MEMS惯导组合导航方法，姿态矩阵的更新采用四元数法，先将转化成四元数，包括：

其中，sign(x)为取符号，当x＞0，则sign(x)＝1，当x＜0，则sign(x)＝-1，符号M(m,n)表示矩阵M的第m行、第n列元素。

6.如权利要求1所述的GNSS双天线辅助的车载MEMS惯导组合导航方法，采用卡尔曼滤波对MEMS惯导误差进行估计包括：

当无观测值时，卡尔曼滤波的递推公式为：

当有观测值时，卡尔曼滤波递推公式为：

P_k＝(I₁₅-K_kH_k)P_k/k-1。

7.如权利要求1所述的GNSS双天线辅助的车载MEMS惯导组合导航方法，其特征在于，MEMS惯性导航误差校正包括：

假设k时刻，MEMS惯导解算的位置为p_k，速度为姿态矩阵为k时刻对导航误差的估计为其中，为估计的位置误差，为估计速度误差，为估计的姿态误差，为估计的加速度零偏，为估计的陀螺漂移，MEMS导航误差校正过程中，当无观测值时，采用的输出校正，当有观测值时，采用反馈校正；

输出校正方法为：

其中，是k时刻计算出的校正后位置，是k时刻计算出的校正后速度，k时刻校正后的姿态角可从中提取。

8.如权利要求7所述的GNSS双天线辅助的车载MEMS惯导组合导航方法，其特征在于，反馈校正方法，在进行上述输出校正计算的同时，将MEMS惯导的位置和速度姿态矩阵调整为输出校正的结果，且卡尔曼滤波的的前9项元素要修改成0，包括：