CN103235328A - 一种gnss与mems组合导航的方法 - Google Patents

Abstract

本发明公开了一种GNSS与MEMS组合导航的方法，该方法包括：根据全球卫星导航***GNSS接收机测量到的载体速度v_GNSS与微机械技术MEMS传感器测量到的载体加速度与角速度，获得载体的姿态角观测量；将所述载体的姿态角观测量与GNSS接收机测量到的载体的速度v_GNSS与位置r_GNSS作为INS卡尔曼滤波器的直接观测量，构成全维状态观测量；所述INS卡尔曼滤波器基于全维状态观测量估算INS计算单元计算到的载体姿态角、速度与位置的误差，并反馈至所述INS计算单元进行导航修正。通过采用本发明公开的导航方法提高了导航的精度。

Description

一种GNSS与MEMS组合导航的方法

技术领域

本发明涉及导航技术领域，尤其涉及一种GNSS与MEMS组合导航的方法。

背景技术

美国的全球卫星导航***（GPS）的成功，极大推动了卫星导航技术向各个行业快速的扩张。全球卫星导航***（GNSS）除美国的GPS和俄罗斯的GLONASS外，欧盟和中国也正在大力发展各自的卫星导航***（Galileo和北斗），随着我国今年第16颗北斗卫星成功发射，北斗区域导航能力正式建成。

由于技术自身的限制，通过无线电传输的卫星导航信号会被建筑和树木遮挡导致定位精度下降或***失效，不能确保定位导航服务的连续性。要克服卫星导航的这一先天缺陷，与其它传感器组合是必然选择。惯性导航技术（INS）具有短期的高精度，可以弥补卫星导航失效时留下的空白，两者组合可为用户提供更全面、更可靠的导航定位解决方案。但是，采用高精度的INS***，使组合***成本高、体积大、功耗高，应用领域有限。

近年来，随着微电子微机械技术（MEMS）的长足进步，MEMS惯性传感器也取得突破性进展，芯片级的MEMS惯性传感器已应用在智能手机、机器人和汽车上，但过快的误差积累成为其导航应用的瓶颈。目前各国都在大力研制GPS和MEMS惯性传感器的组合导航***，市场上也出现了一些GPS结合MEMS惯性传感器的产品，由于对克服MEMS惯性传感器误差积累仍没有好的对策，限制了其向导航应用市场的推广。

现有技术基于GPS/INS组合技术设计的MEMS惯性卫星组合***中，MEMS惯性卫星组合导航***通常由MEM加速度计、陀螺仪、和GPS接收机单元组成。其原理框图如图1所示，加速度计和陀螺仪分别敏感载体的加速度和角速度，GPS单元测量载体的速度和位置。利用陀螺仪输出的角速度通过姿态运动方程获得当前时刻的姿态角，利用加速度计输出的加速度通过一次积分得到当前时刻的速度，再进一步积分得到当前位置。INS的位置和速度与GPS输出的位置和速度比较，得到INS的位置和速度误差，它们构成组合卡尔曼滤波器的观测量，INS的误差方程用作卡尔曼滤波器的状态方程，通过卡尔曼滤波，估计出INS的位置、速度和姿态误差，以及加速度计和陀螺仪的误差，这些误差估计被反馈给捷联惯性计算单元，修正INS的导航解，和传感器误差，从而抑制INS误差随时间的增长，获得稳定的导航解。

卡尔曼滤波是上世纪六十年代提出的基于时域的最优状态估计理论，自其提出之后，得到迅速而广泛的应用，也是GPS/INS组合导航技术的核心和基石。给定***的状态方程和观测方程分别为：x_k+1=Φ_k+1|kx_k+G_k+1|kw_k；z_k=H_kx_k+v_k；

其中，x是***的状态向量，Φ是***的状态转移矩阵，w是***噪声（零均值，方差为Q），G是***噪声增益矩阵；z是***的观测向量，H是***的观测矩阵，v是测量噪声（零均值，方差为R）。离散形式的卡尔曼滤波由如下五个方程组成：

${\hat{x}}_{k+1|k}={\mathrm{\Φ}}_{k+1|k}{\hat{x}}_{k|k};$

$P_{k+1|k}={\mathrm{\Φ}}_{k+1|k}^T{P}_{k|k}{\mathrm{\Φ}}_{k+1|k}+G_{k+1|k}^T{Q}_k{G}_{k+1|k};$

${\hat{x}}_{k+1|k+1}={\hat{x}}_{k+1|k}+K_{k+1}(z_{k+1}-H_{k+1}{\hat{x}}_{k+1|k});$

$K_{k+1}=P_{k+1|k}{H}_{k+1}^T{(H_{k+1}{P}_{k+1|k}{H}_{k+1}^T+R_{k+1})}^{-1};$

$P_{k+1|k+1}=(I-K_{k+1}{H}_{k+1})P_{k+1|k}{(I-K_{k+1}{H}_{k+1})}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T.$

其中，

是状态预测或估计，P是其对应的估计误差的方差矩阵；K是卡尔曼滤波器的增益矩阵。

以上五个方程形成一个完整的递推循环，即给定初始时刻（k=0）的状态

和其方差P_0|0，通过以上五个方程，可以得到下一时刻（k=1）的状态估计

和其方差P_1|1，依次类推，得到k=2，3，…，N时刻的状态估计

和其方差。

GPS/INS组合卡尔曼滤波器的状态中包含位置、速度和姿态角分量，所以当状态估计通过卡尔曼滤波获得后，也相应地获得位置、速度、和姿态角的估计。

现有技术中基于GPS/INS组合技术设计的MEMS惯性卫星组合***，其缺点主要体现在：

（1）以GPS的位置和速度为观测量，由于姿态角不是直接观测量，姿态角的可观测性弱，姿态估计精度低。

（2）因为MEMS陀螺仪误差大，不能实现INS静基座对准，而姿态角可观测性差，动态对准需要载体做特定机动，而且航向角对准和收敛时间很长。

（3）双天线GPS与MEMS组合***可以提供直接的真航向角辅助，但双天线GPS定姿需要具有精确载波相位输出功能的接收机，将带来组合***硬件复杂度、成本、功耗都将大幅提升；另一方面确定载波相位的整周模糊度，算法的复杂度将大幅提升；GPS定姿受环境影响大，特别是多路径效应，很容易造成定姿失败，从而影响整个组合导航的精度。

发明内容

本发明的目的是提供一种GNSS与MEMS组合导航的方法，提高了导航的精度。

本发明的目的是通过以下技术方案实现的：

一种GNSS与MEMS组合导航的方法，该方法包括：

根据全球卫星导航***GNSS接收机测量到的载体速度v_GNSS与微机械技术MEMS传感器测量到的载体加速度与角速度，获得载体的姿态角观测量；

将所述载体的姿态角观测量与GNSS接收机测量到的载体的速度v_GNSS与位置r_GNSS作为INS卡尔曼滤波器的直接观测量，构成全维状态观测量；

所述INS卡尔曼滤波器基于全维状态观测量估算INS计算单元计算到的载体姿态角、速度与位置的误差，并反馈至所述INS计算单元进行导航修正。

由上述本发明提供的技术方案可以看出，在不增加硬件的基础上，结合GNSS速度与MEMS的加速度计和陀螺仪数据，获得载体姿态角，并将其与GNSS的位置与速度一起作为INS卡尔曼滤波器的直接观测量，构成全维状态观测量，估计惯性导航***的误差，提高了姿态的可观测性和估计精度，进而提高了组合导航的精度。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他附图。

图1为本背景技术提供的基于GPS/INS组合技术设计的MEMS惯性卫星组合***的示意图；

图2为本发明实施例一提供的一种GNSS与MEMS组合导航的方法的流程图；

图3为本发明实施例二提供的一种GNSS与MEMS组合导航的方法的示意图；

图4为本发明实施例二提供的***初始化工作流程的示意图；

图5为本发明实施例二提供的陀螺仪单元完成陀螺仪温度误差模型的检测和修正的流程图；

图6为本发明实施例二提供的加速度计完成加速度计温度误差模型的检测和修正的流程图；

图7为本发明实施例二提供的GNSS接收机完成初始位置确定的流程图。

具体实施方式

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

实施例一

图2为本发明实施例一提供的一种GNSS与MEMS组合导航的方法的流程图。如图2所示，主要包括如下步骤：

步骤21、根据全球卫星导航***GNSS接收机测量到的载体速度v_GNSS与微机械技术MEMS传感器测量到的载体加速度与角速度，获得载体的姿态角观测量。

本步骤所述的GNSS接收机可以是GPS、GLONASS、Galileo和北斗中的任一种或其多个相结合的混合导航***接收机。

MEMS传感器包括：加速度计与陀螺仪；其中加速度计用于测量载体的加速度，陀螺仪用于测量载体的角速度。

步骤22、将所述载体的姿态角观测量与GNSS接收机测量到的载体的速度v_GNSS与位置r_GNSS作为INS卡尔曼滤波器的直接观测量，构成全维状态观测量。

MEMS惯性传感器误差大，利用GNSS位置和速度虽然可以通过调节滤波参数获得稳定的导航解，但由于姿态分量可观测性差，估计精度有限。在对姿态精度要求较高的应用中（例如，自动化精确农业应用MEMS惯性卫星组合导航***提供一定精度的姿态信息，以满足自动控制车辆和耕种的要求），需要提高姿态角度的可观测性；因此，将载体的姿态角管测量与GNSS的位置和速度一起作为INS卡尔曼滤波器的直接观测量，以提高姿态估计精度。

步骤23、所述INS卡尔曼滤波器基于全维状态观测量估算INS计算单元计算到的载体姿态角、速度与位置的误差，并反馈至所述INS计算单元进行导航修正。

基于全维状态观测量估算INS计算单元计算到的载体姿态角、速度与位置的误差，提高了姿态的可观测性和估计精度。在自动化精确农业应用测试结果显示，本发明实施例的技术相比与背景技术的方案可以把姿态估计精度提高2～3倍，完全满足自动化精确农业应用对姿态精度的要求。

本发明实施例通过结合GNSS速度与MEMS的加速度计和陀螺仪数据，获得载体姿态角，并将其与GNSS的位置与速度一起作为INS卡尔曼滤波器的直接观测量，构成全维状态观测量，估计惯性导航***的误差，提高了姿态的可观测性和估计精度，进而提高了组合导航的精度。

实施例二

为了便于理解本发明，现结合附图3对发明做进一步说明。

如图3所示，为本发明提供的一种GNSS与MEMS组合导航的方法的示意图。如图3所示，本发明实施例包括9个单元，由这9个单元协同工作实现精确导航。

本发明实施例采用的MEMS传感器包括：三轴陀螺仪与加速度计，分别敏感载体的三轴角速度与线加速度。GNSS接收机单元（可以是GPS、GLONASS、Galileo和北斗中的任一种或其多个相结合的混合导航***接收机。）提供时间、载体的位置与速度。在现场可编程门阵列（FPGA）上实现MEMS数据与GNSS数据的时间同步，两者的融合在FPGA的软核处理器（CPU）上实现。

***运行可以分为初始化和正常更新两种工作模式，当***加电开始运行后，可以静止放置一段时间（如，1分钟）等待***完成初始化。初始化完成后，***转入正常更新模式。

初始化模式下，***要完成初始姿态和初始位置确定，以及温度补偿模型的检测和修正。其中，MEMS中的陀螺仪完成温度补偿模型的检测和修正，加速度计完成温度补偿模型的检测和修正与初始姿态角的确定，GNSS接收机完成初始位置确定；工作流程由加速度计、陀螺仪、和GNSS三个单元独立并行完成。

如图4所示，图4中的陀螺仪单元完成陀螺仪温度误差模型的检测和修正，具体的：如图5所示，陀螺仪输出平均值后与陀螺仪温度误差模型的估计值比较，如果它们的差值（σ_g）超过预设的门限值（μ_g），则更新陀螺仪温度误差模型。

其中，陀螺仪的温度误差模型可以采用二阶多项式拟合，陀螺仪温度误差模型与陀螺仪输出值的关系式为：

所述ε为陀螺仪输出的平均值，T_ε为陀螺仪温度，a_g、b_g与c_g为温差误差模型参数；通过三个以上温度段的采样，得到三个以上的上述方程，通过最小二乘法，得到a_g、b_g与c_g的估值，完成对陀螺仪温度误差模型的更新。

图4中的加速度计完成加速度计温度误差模型的检测和修正的工作原理与陀螺仪的类似。具体的：如图6所示，加速度计输出平均值后与加速度计温度误差模型的估计值比较，如果它们的差值（σ_a）超过预设的门限值（μ_a），则更新加速度计的温度误差模型。

其中，加速度计的温度误差模型可以采用二阶多项式拟合，加速度计温度误差模型与加速度计输出值的关系式为：

所述

为加速度计输出的平均值，

为加速度计温度，a_a、b_a与c_a为温差误差模型参数；通过三个以上温度段的采样，得到三个以上的上述方程，通过最小二乘法，得到a_a、b_a与c_a的估值，完成对加速度计温度误差模型的更新。

再通过加速度计完成初始姿态角的确定。初始姿态角可利用加速度计输出的平均值计算，其公式为： ${\mathrm{roll}}_1=a\tan \frac{f_y}{f_z};{\mathrm{pitch}}_1=a\tan \left(\frac{f_x}{\sqrt{f_y^2+f_z^2}}\right);$

其中，f_x、f_y与f_z为加速度计输出的三轴平均值；roll₁为滚转角，pitch₁是俯仰角；由于MEMS陀螺仪精度低，不能用来确定初始方位，所以初始航向角（yaw₁）可以设为零或用上次运行存储的最后时刻的航向角作为当前的初始值。

图4中的GNSS接收机完成初始位置确定。其工作流程如图7所示，GNSS输出位置平均值作为***的初始位置。

当初始化完成之后，***转入正常更新模式。此时，由温度补偿单元进行温度补偿。温度补偿可根据初始化步骤确定的温度误差模型来实现：即，根据当前的温度及对应的温度补偿模型估计加速度计与陀螺仪误差并进行补偿，将补偿后的加速度计与陀螺仪的输出值作为所述MEMS传感器测量到的载体加速度与角速度输出至INS计算单元。

所述INS计算单元包括：姿态角更新单元、速度更新单元与位置更新单元。INS计算单元接受到MEMS传感器输出的数据后进行姿态角、速度、和位置更新；

姿态矩阵更新公式为：

其中

是陀螺仪的输出矢量

的叉乘矩阵，利用更新后的姿态矩阵更新姿态角；

速度更新公式为： ${\stackrel{\·}{v}}^n=-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×v^n+f^n+g;$ 其中

是地球自转角速度，

是导航坐标系相对于地球的转动角速度，fⁿ是载体在导航坐标系中的运动加速度，g是地球重力加速度，vⁿ是载体在导航坐标系中的运动速度。

位置更新公式为：

其中rⁿ是载体的在导航坐标系中的位置。

另一方面，MEMS传感器测量到的载体加速度与角速度还用于和GNSS接收机测量到的数据相结合，计算载体的姿态角观测量。

通常情况下，我们将GNSS接收机计算到的载体速度与位置输入至GNSS卡尔曼滤波器中进行速度、加速度与位置的估计，将获得的估计值作为GNSS接收机测量到的载体初始速度、加速度与位置。

具体的：将GNSS接收机计算的载体速度v与位置r作为GNSS卡尔曼滤波器的观测量z_GNSS，其中，Z_GNSS=[r v]。

所述GNSS卡尔曼滤波器的观测方程可以写为：z_GNSS=H_GNSSx_GNSS+v_GNSS；观测方程的含义表示观测量z_GNSS中初始速度v与位置r与待估计参数x_GNSS间的函数关系。

其中，x_GNSS为GNSS卡尔曼滤波器的状态向量，x_GNSS=[r v a]，a为加速度；H_GNSS为观测矩阵： $H_{\mathrm{GNSS}}=\left[\begin{array}{cc}{I}_{3\×3}& O_{3\×3}\\ O_{3\×3}& I_{3\×3}\end{array}\right];$ I_3×3是3×3的单位矩阵，0_3×3是3×3的零矩阵；观测噪声v_GNSS为零均值，方差为R_GNSS的白噪声；

采用常值加速度模型作为GNSS卡尔曼滤波器的状态方程：x_GNSS(k+1)=Φ_GNSSx_GNSS+G_GNSSw_GNSS；

其中状态转移矩阵为： ${\mathrm{\Φ}}_{\mathrm{GNSS}}=\left[\begin{array}{ccc}{I}_{3\×3}& \mathrm{\Δt}\·I_{3\×3}& O_{3\×3}\\ O_{3\×3}& I_{3\×3}& \mathrm{\Δt}\·I_{3\×3}\\ O_{3\×3}& O_{3\×3}& I_{3\×3}\end{array}\right];$ △t为GNSS信号的采样时间间隔；加速度误差w_GNSS为零均值，方差为Q_GNSS的白噪声；噪声增益矩阵 $G_{\mathrm{GNSS}}=\left[\begin{array}{c}{O}_{3\×3}\\ O_{3\×3}\\ I_{3\×3}\end{array}\right];$

可以利用卡尔曼滤波方程求解上述GNSS卡尔曼滤波器的观测方程与状态方程，获得状态向量x_GNSS=[r v a]中的位置、速度与加速度的估计值r_GNSS、v_GNSS与a_GNSS，并将该估计值作为所述GNSS接收机的测量值。

之后，利用姿态角估计单元完成姿态角的估算，并将该结果作为载体的姿态角观测量，所述姿态角观测量中包括滚转角、俯仰角与航向角。

主要有以下两种方式：

1）利用GNSS接收机测量到的载体速度与MEMS传感器测量到的载体加速度与角速度相结合，计算载体的姿态角观测量，公式为：

${\mathrm{yaw}}_2=a\tan \frac{v_e}{v_n};$

其中，roll₂为滚转角，pitch₂为俯仰角，yaw₂为航向角；f^by与f^bz为加速度计测量到的y轴与z轴加速度；

与为陀螺仪测量到的y轴与z轴的角速度；v_e与v_n为GNSS接收机测量到的水平面速度，v_GNSS为GNSS接收机测量到的速度，g为重力加速度。

2）利用GNSS接收机测量到的载体速度和加速度，与MEMS传感器测量到的载体加速度与角速度相结合，计算载体的姿态角观测量，滚轴角与航向角的计算公式与1）中的类似，仅俯仰角的计算方法存在区别：

其中，a_GNSS为GNSS卡尔曼滤波器估计的加速度。

再将所述载体的姿态角观测量与GNSS接收机测量到的载体的速度v_GNSS与位置r_GNSS作为INS卡尔曼滤波器的直接观测量，构成全维状态观测量；并计算INS计算单元的载体速度v_ins、位置r_ins与姿态角的误差。

其中，δr=r_ins-r_GNSS为三维位置误差，δv=v_ins-v_GNSS为三维速度误差，ψ为三维姿态角误差；所述三维姿态角误差ψ包括：滚转角误差（δr_roll=r_ins-roll₂）、俯仰角误差（δp_pitch=p_ins-pitch₂）及航向角误差（δy_yaw=y_ins-yaw₂）；且其转换关系为：

$\mathrm{\ψ}=\left[\begin{array}{ccc}-\cos p\cos y& \sin y& 0\\ -\cos p\sin y& -\cos y& 0\\ \sin p& 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\δr}}_{\mathrm{roll}}\\ {\mathrm{\δp}}_{\mathrm{pitch}}\\ {\mathrm{\δy}}_{\mathrm{yaw}}\end{array}\right];$

其中，sinp与cosp为俯仰角的正弦和余弦，siny与cosy为航向角的正弦和余弦。

将上述误差δr、δv与ψ作为INS卡尔曼滤波器的观测量z_ins；其观测方程由全维状态观测方程单元提供，可写为：z_ins=H_insx_ins+v_ins；

优选的，还可基于INS卡尔曼滤波器的观测方程，估计MEMS中加速度计与陀螺仪的误差；即，x_ins为INS卡尔曼滤波器的状态向量， $x_{\mathrm{ins}}=\left[\begin{array}{ccccc}\mathrm{\δr}& \mathrm{\δv}& \mathrm{\ψ}& {\▿}^{*}& {\mathrm{\ϵ}}^{*}\end{array}\right],$

与ε^*分别为加速度计与陀螺仪的误差（初值在温度补偿时获得）；H_ins为观测矩阵： $H_{\mathrm{ins}}=\left[\begin{array}{ccc}{I}_{3\×3}& O_{3\×3}& O_{3\×3}\\ O_{3\×3}& I_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& I_{3\×3}\end{array}\right];$ 观测噪声v_ins是零均值、方差为R_ins的白噪声。

对应的状态方程由误差方程单元提供，可写为：

$\mathrm{\δ}\stackrel{\·}{r}=-{\mathrm{\ω}}_{\mathrm{en}}\×\mathrm{\δr}+\mathrm{\δv};$

$\mathrm{\δ}\stackrel{\·}{v}=-({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{in}})\×\mathrm{\δv}+f\×\mathrm{\ψ}+{\▿}^{*}+\mathrm{\Δg};$

$\stackrel{\·}{\mathrm{\ψ}}=-{\mathrm{\ω}}_{\mathrm{in}}\×\mathrm{\ψ}+{\mathrm{\ϵ}}^{*};$

${\stackrel{\·}{\▿}}^{*}=-\frac{1}{T_{{\▿}^{*}}}{\▿}^{*}+w_{{\▿}^{*}};$

${\stackrel{\·}{\mathrm{\ϵ}}}^{*}=-\frac{1}{T_{{\mathrm{\ϵ}}^{*}}}{\mathrm{\ϵ}}^{*}+w_{{\mathrm{\ϵ}}^{*}}.$

其中，

是地球自转角速度，

是导航坐标系相对于惯性空间的转动角速度，f是载体的运动加速度，△g是地球重力加速度异常，

是加速度计误差的量测噪声，

是陀螺仪误差的量测噪声。

为在计算设备上（例如数字计算机）实现INS卡尔曼滤波器，需要对所述INS卡尔曼滤波器的状态方程离散化。可以使用下述离散化的方法，把上述方程写为矩阵形式：

${\stackrel{\·}{X}}_{\mathrm{ins}}={\mathrm{FX}}_{\mathrm{ins}}+{\mathrm{GW}}_{\mathrm{ins}}$

其中，F为连续时间形式的状态转移矩阵，G是噪声增益矩阵，W_ins是零均值方差为Q的***噪声。其离散化形式如下：

X_ins(k)=Φ_k,k-1X_ins(k-1)+Γ_k,k-1W_ins(k-1)；

其中，Γ为离散形式的***噪声增益矩阵，离散形式的状态转移矩阵Φ和***噪声方差矩阵如下计算：

${\mathrm{\Φ}}_{k,k-1}=I+{\mathrm{F\Δt}}_{\mathrm{ins}}+\frac{1}{2}{\left({\mathrm{F\Δt}}_{\mathrm{ins}}\right)}^2$

${\mathrm{\Γ}}_k{Q}_k{\mathrm{\Γ}}_k^T=\stackrel{\‾}{Q}\mathrm{\Δt}+[F\stackrel{\‾}{Q}+{\left(F\stackrel{\‾}{Q}\right)}^T]\frac{{\mathrm{\Δt}}^2}{2};$

其中△t_ins为采样周期，

根据上述计算获得对应的离散形式，再利用卡尔曼滤波方程求解INS卡尔曼滤波器的观测方程与离散形式的状态方程，获得状态向量 $x_{\mathrm{ins}}={\left[\begin{array}{ccccc}\mathrm{\δr}& \mathrm{\δv}& \mathrm{\ψ}& {\▿}^{*}& {\mathrm{\ϵ}}^{*}\end{array}\right]}^{T_{\mathrm{ins}}}$ 的估计值，从而获得INS计算单元的速度、位置与姿态角误差的估计值以及加速度计与陀螺仪误差的估计值；并将获得的误差估计值反馈INS计算单元及MEMS中；同时对对位置、速度、姿态、加速度计与陀螺仪进行导航修正。

本发明实施例在不增加硬件的基础上，结合GNSS速度与MEMS的加速度计和陀螺仪数据，获得载体姿态角，并将其与GNSS的位置与速度一起作为INS卡尔曼滤波器的直接观测量，构成全维状态观测量，估计惯性导航***的误差，提高了姿态的可观测性和估计精度，进而提高了组合导航的精度。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例可以通过软件实现，也可以借助软件加必要的通用硬件平台的方式来实现。基于这样的理解，上述实施例的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质（可以是CD-ROM，U盘，移动硬盘等）中，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行本发明各个实施例所述的方法。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (8)

1.一种GNSS与MEMS组合导航的方法，其特征在于，该方法包括：

根据全球卫星导航***GNSS接收机测量到的载体速度v_GNSS与微机械技术MEMS传感器测量到的载体加速度与角速度，获得载体的姿态角观测量；

将所述载体的姿态角观测量与GNSS接收机测量到的载体的速度v_GNSS与位置r_GNSS作为INS卡尔曼滤波器的直接观测量，构成全维状态观测量；

所述INS卡尔曼滤波器基于全维状态观测量估算INS计算单元计算到的载体姿态角、速度与位置的误差，并反馈至所述INS计算单元进行导航修正。

2.根据权利要求1所述的方法，其特征在于，所述获得载体的姿态角观测量之前还包括，对MEMS传感器进行温度补偿并将补偿后的值作为测量值的步骤，且该步骤包括：

确定MEMS传感器中加速度计与陀螺仪的温度补偿模型，所述加速度计与陀螺仪的温度误差模型均采用二阶多项式拟合；

其中，加速度计温度误差模型与加速度计输出值的关系式为：

所述

为加速度计输出的平均值，

为加速度计温度，a_a、b_a与c_a为温差误差模型参数；

陀螺仪温度误差模型与陀螺仪输出值的关系式为：

所述ε为陀螺仪输出的平均值，T_ε为陀螺仪温度，a_g、b_g与c_g为温差误差模型参数；

根据当前的温度及对应的温度补偿模型估计加速度计与陀螺仪误差并进行补偿，将补偿后的加速度计与陀螺仪的输出值作为所述MEMS传感器测量到的载体加速度与角速度。

3.根据权利要求2所述的方法，其特征在于，该方法还包括：根据补偿后的加速度计与陀螺仪的输出值对INS计算单元计算到的载体的姿态角、速度与位置进行更新；

姿态矩阵更新公式为：

其中是陀螺仪的输出矢量的叉乘矩阵，利用更新后的姿态矩阵更新姿态角；

速度更新公式为： ${\stackrel{\·}{v}}^n=-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×v^n+f^n+g$ 其中

是地球自转角速度，

是导航坐标系相对于地球的转动角速度，fⁿ是载体在导航坐标系中的运动加速度，g是地球重力加速度，vⁿ是载体在导航坐标系中的运动速度；

位置更新公式为：

其中rⁿ是载体的在导航坐标系中的位置。

4.根据权利要求1所述的方法，其特征在于，所述根据GNSS接收机测量到的载体速度与MEMS传感器测量到的载体加速度与角速度，获得载体的姿态角观测量的公式包括：所述姿态角观测量中包括滚转角、俯仰角与航向角，其计算公式为：

${\mathrm{yaw}}_2=a\tan \frac{v_e}{v_n};$

其中，roll₂为滚转角，pitch₂为俯仰角，yaw₂为航向角；f^by与f^bz为加速度计测量到的y轴与z轴加速度；

与为陀螺仪测量到的y轴与z轴的角速度；v_e与v_n为GNSS接收机测量到的水平面速度，g为重力加速度。

5.根据权利要求1-4任一项所述的方法，其特征在于，所述GNSS接收机测量的载体速度、位置包括：

通过GNSS卡尔曼滤波器对GNSS接收机计算到的载体的速度与位置进行估算，获得速度、位置及加速度的估计值，并估计值作为GNSS接收机的测量值；

具体的：所述GNSS接收机将计算的载体速度r与位置v作为GNSS卡尔曼滤波器的观测量z_GNSS，其中，Z_GNSS=[r v]；

所述GNSS卡尔曼滤波器的观测方程为：z_GNSS=H_GNSSx_GNSS+v_GNSS；

其中，x_GNSS为GNSS卡尔曼滤波器的状态向量，x_GNSS=[r v a]，a为加速度；H_GNSS为观测矩阵： $H_{\mathrm{GNSS}}=\left[\begin{array}{cc}{I}_{3\×3}& O_{3\×3}\\ O_{3\×3}& I_{3\×3}\end{array}\right];$ I_3×3是3×3的单位矩阵，0_3×3是3×3的零矩阵；观测噪声v_GNSS为零均值，方差为R_GNSS的白噪声；

采用常值加速度模型作为GNSS卡尔曼滤波器的状态方程：x_GNSS(k+1)=Φ_GNSSx_GNSS+G_GNSSw_GNSS；

其中，状态转移矩阵为： ${\mathrm{\Φ}}_{\mathrm{GNSS}}=\left[\begin{array}{ccc}{I}_{3\×3}& \mathrm{\Δt}\·I_{3\×3}& O_{3\×3}\\ O_{3\×3}& I_{3\×3}& \mathrm{\Δt}\·I_{3\×3}\\ O_{3\×3}& O_{3\×3}& I_{3\×3}\end{array}\right];$ △t为GNSS信号的采样时间间隔；加速度误差w_GNSS为零均值，方差为Q_GNSS的白噪声；噪声增益矩阵 $G_{\mathrm{GNSS}}=\left[\begin{array}{c}{O}_{3\×3}\\ O_{3\×3}\\ I_{3\×3}\end{array}\right];$

利用卡尔曼滤波方程求解GNSS卡尔曼滤波器的观测方程与状态方程，获得状态向量x_GNSS=[r v a]中的位置、速度与加速度的估计值r_GNSS、v_GNSS与a_GNSS，并将该估计值作为所述GNSS接收机的测量值。

6.根据权利要求5所述的方法，其特征在于，该方法还包括：根据GNSS接收机测量到的载体速度及加速度，与MEMS传感器输出的载体加速度与角速度，获得载体的姿态角观测量；所述姿态角观测量中包括滚转角、俯仰角与航向角，其计算公式为：

${\mathrm{yaw}}_2=a\tan \frac{v_e}{v_n};$

其中，roll₂为滚转角，

为俯仰角，yaw₂为航向角；f^by与f^bz为加速度计测量到的y轴与z轴加速度；

与

为陀螺仪测量到的y轴与z轴的角速度；v_e与v_n为GNSS接收机测量到的水平面速度。

7.根据权利要求1所述的方法，其特征在于，所述INS卡尔曼滤波器基于全维状态观测量估算INS计算单元计算到的载体姿态角、速度与位置的误差的步骤包括：

利用全维状态观测量中载体速度v_GNSS、位置r_GNSS与姿态角计算INS计算单元的载体速度v_ins、位置r_ins与姿态角的误差，并将该误差作为INS卡尔曼滤波器的观测量z_ins，其中，观测向量Z_ins=[δr δv ψ]；包括δr=r_ins-r_GNSS为三维位置误差，δv=v_ins-v_GNSS为三维速度误差，ψ为三维姿态角误差；

所述INS卡尔曼滤波器的观测方程为：z_ins=H_insx_ins+v_ins；

其中，x_ins为INS卡尔曼滤波器的状态向量， $X_{\mathrm{ins}}=\left[\begin{array}{ccccc}\mathrm{\δr}& \mathrm{\δv}& \mathrm{\ψ}& {\▿}^{*}& {\mathrm{\ϵ}}^{*}\end{array}\right],$

为加速度计的误差，ε^*为陀螺仪的误差；H_ins为观测矩阵： $H_{\mathrm{ins}}=\left[\begin{array}{ccc}{I}_{3\×3}& O_{3\×3}& O_{3\×3}\\ O_{3\×3}& I_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& I_{3\×3}\end{array}\right];$ 观测噪声v_ins是零均值、方差为R_ins的白噪声；

所述INS卡尔曼滤波器的状态方程为：

$\mathrm{\δ}\stackrel{\·}{r}=-{\mathrm{\ω}}_{\mathrm{en}}\×\mathrm{\δr}+\mathrm{\δv};$

$\mathrm{\δ}\stackrel{\·}{v}=-({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{in}})\×\mathrm{\δv}+f\×\mathrm{\ψ}+{\▿}^{*}+\mathrm{\Δg};$

$\stackrel{\·}{\mathrm{\ψ}}=-{\mathrm{\ω}}_{\mathrm{in}}\×\mathrm{\ψ}+{\mathrm{\ϵ}}^{*};$

${\stackrel{\·}{\▿}}^{*}=-\frac{1}{T_{{\▿}^{*}}}{\▿}^{*}+w_{{\▿}^{*}};$

${\stackrel{\·}{\mathrm{\ϵ}}}^{*}=-\frac{1}{T_{{\mathrm{\ϵ}}^{*}}}{\mathrm{\ϵ}}^{*}+w_{{\mathrm{\ϵ}}^{*}};$

其中，

是地球自转角速度，

是导航坐标系相对于惯性空间的转动角速度，f是载体的运动加速度，△g是地球重力加速度异常，

是加速度计误差的量测噪声，是陀螺仪误差的量测噪声；

对所述INS卡尔曼滤波器的状态方程离散化获得到对应的离散形式，利用卡尔曼滤波方程求解GNSS卡尔曼滤波器的观测方程与离散形式的状态方程，获得状态向量 $X_{\mathrm{ins}}=\left[\begin{array}{ccccc}\mathrm{\δr}& \mathrm{\δv}& \mathrm{\ψ}& {\▿}^{*}& {\mathrm{\ϵ}}^{*}\end{array}\right]$ 的估计值，从而获得INS计算单元中的速度、位置与姿态角误差的估计值以及加速度计与陀螺仪误差的估计值；并将获得的误差估计值反馈至INS计算单元及MEMS中，对位置、速度、姿态、加速度计与陀螺仪进行导航修正。

8.根据权利要求7所述的方法，其特征在于，所述三维姿态角误差ψ包括：滚转角误差δr_roll、俯仰角误差δp_pitch及航向角误差δy_yaw；且其转换关系为：

$\mathrm{\ψ}=\left[\begin{array}{ccc}-\cos p\cos y& \sin y& 0\\ -\cos p\sin y& -\cos y& 0\\ \sin p& 0& -1\end{array}\right]\left[\begin{array}{c}{\mathrm{\δr}}_{\mathrm{roll}}\\ {\mathrm{\δp}}_{\mathrm{pitch}}\\ {\mathrm{\δy}}_{\mathrm{yaw}}\end{array}\right];$

其中，sinp与cosp为俯仰角的正弦和余弦，siny与cosy为航向角的正弦和余弦。

Images