CN106949889A - 针对行人导航的低成本mems/gps组合导航系统及方法 - Google Patents

Abstract

本发明公开一种针对行人导航的低成本MEMS/GPS组合导航系统，包括STM32F4处理器、蓝牙模块、低成本加速度计和陀螺仪、低成本GPS模块和平板电脑，STM32F4处理器与平板电脑通过蓝牙模块进行数据交互；低成本加速度计、陀螺仪、低成本GPS模块的输出端均连接STM32F4处理器。本发明还公开一种针对行人导航的低成本MEMS/GPS组合导航方法，包括如下步骤：组合导航系统惯导误差状态量方程建模；建立SINS/GPS松组合导航系统状态方程；组合导航系统量测方程建模；按状态方程和量测方程进行卡尔曼滤波。此种技术方案可以高效实现捷联惯性导航和卡尔曼滤波解算，获取高精度、高可靠性的导航结果。同时为行人导航的MEMS/GPS组合导航的工程化实现和应用提供了有效的支撑，具有突出的实用价值。

Description

针对行人导航的低成本MEMS/GPS组合导航系统及方法

技术领域

本发明属于定位导航与控制技术领域，特别涉及一种针对行人导航的低成本MEMS/GPS组合导航系统及方法。

背景技术

目前GPS定位与导航技术广泛应用于生活和工作中，GPS定位精度高，成本低，智能手机中都已经植入了GPS模块，给人们的工作和生活提供了极大的便利。但是GPS信号受建筑物遮挡、多路径效应等影响，大大降低了GPS导航定位模式的稳定性和精确性。低成本MEMS短时导航精度高、自主无源，但位置和速度信息随着时间发散。MEMS和GPS组合导航系统可以克服两者自身的缺陷，提供更加可靠的导航定位服务。随着MEMS和GPS的发展，成本越来越低，精度越来越高，因此将低成本MEMS和GPS的组合导航系统用于行人导航中是一个很大的趋势。

发明内容

本发明的目的，在于提供一种针对行人导航的低成本MEMS/GPS组合导航系统及方法，可以高效实现捷联惯性导航和卡尔曼滤波解算，获取高精度、高可靠性的导航结果。同时为行人导航的MEMS/GPS组合导航的工程化实现和应用提供了有效的支撑，具有突出的实用价值。

为了达成上述目的，本发明的解决方案是：

一种针对行人导航的低成本MEMS/GPS组合导航系统，包括STM32F4处理器、蓝牙模块、低成本加速度计和陀螺仪、低成本GPS模块和平板电脑，STM32F4处理器与平板电脑通过蓝牙模块进行数据交互；低成本加速度计、陀螺仪、低成本GPS模块的输出端均连接STM32F4处理器。

一种针对行人导航的低成本MEMS/GPS组合导航方法，包括如下步骤：

(1)组合导航系统惯导误差状态量方程建模；

(2)在上述基础上，建立SINS/GPS松组合导航系统状态方程；

(3)组合导航系统量测方程建模；

(4)按状态方程和量测方程进行卡尔曼滤波。

上述步骤(1)的详细内容是：对于行人导航的低成本MEMS/GPS组合导航系统，速度计算公式为：

式中，为载体相当于地理系的加速度在载体上的投影，为载体相当于地理系的速度在载体上的投影，为机体系到导航坐标系的姿态转移矩阵，为载体相当于地理坐标系的速度在导航坐标系上的投影，其中的地理坐标系即为导航坐标系；

位置计算公式为：

用四元数描述载体的姿态运动，计算公式为：

用矩阵表示为：

式中，q为姿态四元数，为q对时间的导数，矢量为四元数形式，表示载体坐标系相对导航坐标系在载体坐标系上的投影。

上述步骤(2)中，SINS/GPS松组合导航系统状态方程为：

其中，状态变量为：

表示捷联惯性导航在地里坐标系下东、北、天方向平台误差角，δv_E,δv_N,δv_U表示惯性导航系统在地理坐标系下东、北、天方向的速度误差；δL,δλ,δh表示惯性导航系统在地里坐标系下纬度、经度、高度的误差；ε_bx,ε_by,ε_bz表示陀螺随机常数误差；ε_rx,ε_ry,ε_rz表示陀螺一阶马尔可夫过程随机误差；表示加速度计的一阶马尔可夫过程随机误差；A(t)_18×18为系统的状态转移矩阵；G(t)_18×9为噪声系数矩阵；W(t)_9×1为系统的白噪声矢量。

上述步骤(3)中，SINS/GPS系统的量测方程分为位置测部分和速度量测部分，其中位置量测为利用惯导提供的位置信息与GPS接收机提供的位置信息之差，速度量测为利用惯导提供的速度信息与GPS接收机提供的速度信息之差。

上述位置量测的位置观测量，即惯性导航系统给出的经度、纬度和高度信息和GPS接收机给出的相应信息的差值，定义位置量测方程为：

其中，L_I，λ_I，h_I分别表示捷联惯导的纬度、经度和高度，L_GPS，λ_GPS，h_GPS分别表示GPS接收机的纬度、经度和高度，V_P(t)＝[N_N N_E N_U]，表示GPS接收机沿东北天方向的位置误差。

上述步骤(4)中，MEMS/GPS组合导航系统的卡尔曼滤波模型如下：

上式中，表示t_k-1时刻的状态对t_k时刻的状态的最优估计值，Φ_k/k-1表示t_k-1时刻至t_k时刻系统的状态转移矩阵，表示t_k-1时刻的系统状态估计值，K_k表示增益矩阵，H_k表示滤波模块t_k时刻的观测系数矩阵，P_k/k-1表示最优预测估计误差协方差阵，P_k/k表示滤波模块t_k时刻的系统误差协方差阵，Q_k-1表示t_k-1时刻的噪声方差矩阵，Γ_k/k-1表示t_k-1时刻的噪声矢量对t_k时刻状态矢量影响的噪声系数矩阵，R_k表示t_k时刻的量测方差矩阵，I为单位矩阵。

采用上述方案后，本发明的有益效果如下：

(1)在MEMS/GPS导航系统中，捷联惯导与GPS采用卡尔曼滤波的形式进行数据融合，MEMS和GPS优势互补，提高了导航定位的精度和可靠性。

(2)硬件层面上，该导航装置由STM32F4处理器，MEMS惯性元件、GPS接收机、蓝牙通信模块、平板电脑等组成。GPS与处理器之间通过UART接口进行通信，上位机与处理器之间通过蓝牙通信进行数据交互。

(3)软件层面上，整个导航系统基于UCOS系统，采用多线程模式，处理器CPU在不同任务间进行高速运行与轮转，任务并发进行保证了系统的实时性，利于系统在任务上的扩展与裁剪。

附图说明

图1是本发明的组合导航方法原理图；

图2是本发明的导航系统硬件模块化构成图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

本发明提供一种针对行人导航的低成本MEMS/GPS组合导航系统及方法，采用多线程并发的组合导航解算方案。考虑到整个程序的实时性和精确性，导航程序基于UCOSIII系统编写，分别包括捷联惯导解算线程、卡尔曼滤波线程、上位机数据交互线程、定时器等模块；导航硬件装置由AHRS BOX板载的STM32F4处理器、蓝牙模块、低成本加速度计和陀螺仪、低成本GPS模块、平板电脑等组成。其中平板电脑用于监控显示导航解算数据，与处理器通过蓝牙通讯的方式进行数据交互。

如图1所示，本发明导航方法的原理是：先在地理坐标系下进行捷联惯导算法编排；然后再建立MEMS/GPS组合模型，利用GPS的位置和速度信息对捷联惯导进行实时修正。具体实施方法如下：

(1)组合导航系统惯导误差状态量方程建模

对于行人导航的低成本MEMS/GPS组合导航系统，其捷联惯导解算过程如下，速度计算公式为：

式中，为载体相当于地理系的加速度在载体上的投影，为载体相当于地理系的速度在载体上的投影，为机体系到导航坐标系的姿态转移矩阵，为载体相当于地理坐标系的速度在导航坐标系上的投影，本模型的地理坐标系即为导航坐标系。

位置计算公式为：

用四元数描述载体的姿态运动，计算公式为：

用矩阵表示为：

式中，q为姿态四元数，为q对时间的导数，矢量为四元数形式，表示载体坐标系相对导航坐标系在载体坐标系上的投影。

(2)在上述基础上，建立SINS/GPS松组合导航系统状态方程为：

其中，状态变量为：

表示捷联惯性导航在地里坐标系下东、北、天方向平台误差角，δv_E,δv_N,δv_U表示惯性导航系统在地理坐标系下东、北、天方向的速度误差；δL,δλ,δh表示惯性导航系统在地里坐标系下纬度、经度、高度的误差；ε_bx,ε_by,ε_bz表示陀螺随机常数误差；ε_rx,ε_ry,ε_rz表示陀螺一阶马尔可夫过程随机误差；表示加速度计的一阶马尔可夫过程随机误差。A(t)_18×18为系统的状态转移矩阵；G(t)_18×9为噪声系数矩阵；W(t)_9×1为系统的白噪声矢量。

(3)组合导航系统量测方程建模

在此所叙述的低成本SINS/GPS组合导航系统，是利用GPS接收机提供的位置和速度信息，对惯导的位置合速度信息进行滤波估计。SINS/GPS系统的量测方程分为位置测部分和速度量测部分，其中位置量测为利用惯导提供的位置信息与GPS接收机提供的位置信息之差，速度量测为利用惯导提供的速度信息与GPS接收机提供的速度信息之差。

在该系统中，位置观测量，即惯性导航系统给出的经度、纬度和高度信息和GPS接收机给出的相应信息的差值。定义位置量测方程为：

其中，L_I，λ_I，h_I分别表示捷联惯导的纬度、经度和高度，L_GPS，λ_GPS，h_GPS分别表示GPS接收机的纬度、经度和高度。V_P(t)＝[N_N N_E N_U]，表示GPS接收机沿东北天方向的位置误差。

速度信息的量测方程与位置信息的量测方程基本一致。

(4)MEMS/GPS卡尔曼滤波模型分析

按步骤(3)所述的状态方程和量测方程，进行卡尔曼滤波，以提高整个导航系统的稳定性和精确性。

MEMS/GPS组合导航系统的卡尔曼滤波模型如下：

上式中，表示t_k-1时刻的状态对t_k时刻的状态的最优估计值，Φ_k/k-1表示t_k-1时刻至t_k时刻系统的状态转移矩阵，表示t_k-1时刻的系统状态估计值，K_k表示增益矩阵，H_k表示滤波模块t_k时刻的观测系数矩阵，P_k/k-1表示最优预测估计误差协方差阵，P_k/k表示滤波模块t_k时刻的系统误差协方差阵，Q_k-1表示t_k-1时刻的噪声方差矩阵，Γ_k/k-1表示t_k-1时刻的噪声矢量对t_k时刻状态矢量影响的噪声系数矩阵，R_k表示t_k时刻的量测方差矩阵，I为单位矩阵。

综合上述，本发明一种针对行人导航的低成本MEMS/GPS组合导航系统及方法，考虑到组合导航解算的实时性和精确性，该组合导航程序基于UCOSIII系统编写，采用多线程的模式进行导航解算，以解决处理器在卡尔曼滤波计算中用时过多问题。导航程序主要由捷联惯导解算线程、卡尔曼滤波线程、上位机数据交互线程、定时器模块等组成。导航硬件装置由AHRS BOX平台板载的高性能处理器STM32F4、低成本加速度计和陀螺仪、低成本GPS模块、平板电脑等组成，其中平板电脑用于导航数据显示模块配置，与嵌入式平台采用蓝牙通讯的方式进行数据交互。导航方法包括以下步骤：首先在地理坐标系下进行捷联惯导导航算法的编排，然后建立惯性/GPS组合模型，利用GPS模块的量测信息与捷联惯导进行卡尔曼滤波。GPS信号易受遮挡和多路径效应的影响而导致定位精度降低或者不稳定，惯性导航短时精度高，长时间呈发散趋势，本装置可以很好地解决这个问题，为行人组合导航系统的工程实现和应用提供了有效的支撑。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (7)

1.一种针对行人导航的低成本MEMS/GPS组合导航系统，其特征在于：包括STM32F4处理器、蓝牙模块、低成本加速度计和陀螺仪、低成本GPS模块和平板电脑，STM32F4处理器与平板电脑通过蓝牙模块进行数据交互；低成本加速度计、陀螺仪、低成本GPS模块的输出端均连接STM32F4处理器。

2.一种针对行人导航的低成本MEMS/GPS组合导航方法，其特征在于包括如下步骤：

(1)组合导航系统惯导误差状态量方程建模；

(2)在上述基础上，建立SINS/GPS松组合导航系统状态方程；

(3)组合导航系统量测方程建模；

(4)按状态方程和量测方程进行卡尔曼滤波。

3.如权利要求2所述的针对行人导航的低成本MEMS/GPS组合导航方法，其特征在于：所述步骤(1)的详细内容是：对于行人导航的低成本MEMS/GPS组合导航系统，速度计算公式为：

$v_{en}^b=v_{en0}^b+{\∫}_0^t{\stackrel{\·}{v}}_{en}^{b}dt$

$v_{en}^n=C_b^n{v}_{en}^b$

式中，为载体相当于地理系的加速度在载体上的投影，为载体相当于地理系的速度在载体上的投影，为机体系到导航坐标系的姿态转移矩阵，为载体相当于地理坐标系的速度在导航坐标系上的投影，其中的地理坐标系即为导航坐标系；

位置计算公式为：

$L=L_0+{\∫}_0^t\frac{v_y^n}{R_m}dt$

$\mathrm{\λ}={\mathrm{\λ}}_0+{\∫}_0^t\frac{v_x^n}{R_n}\sec Ldt$

$h=h_0+{\∫}_0^t{v}_z^{n}dt$

用四元数描述载体的姿态运动，计算公式为：

用矩阵表示为：

$\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=0.5\×\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx}^b& -{\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b\\ {\mathrm{\ω}}_{nbx}^b& 0& {\mathrm{\ω}}_{nbz}^b& -{\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nby}^b& -{\mathrm{\ω}}_{nbz}^b& 0& {\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nbz}^b& {\mathrm{\ω}}_{nby}^b& {\mathrm{\ω}}_{nbz}^b& 0\end{array}\right]$

式中，q为姿态四元数，为q对时间的导数，矢量为四元数形式，表示载体坐标系相对导航坐标系在载体坐标系上的投影。

4.如权利要求2所述的针对行人导航的低成本MEMS/GPS组合导航方法，其特征在于：所述步骤(2)中，SINS/GPS松组合导航系统状态方程为：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_I\left(t\right)\\ {\stackrel{\·}{X}}_G\left(t\right)\end{array}\right]=\left[\begin{array}{cc}{F}_I\left(t\right)& 0\\ 0& F_G\left(t\right)\end{array}\right]\left[\begin{array}{c}{X}_I\left(t\right)\\ X_G\left(t\right)\end{array}\right]+\left[\begin{array}{cc}{G}_I\left(t\right)& 0\\ 0& G_G\left(t\right)\end{array}\right]\left[\begin{array}{c}{W}_I\left(t\right)\\ W_G\left(t\right)\end{array}\right]$

$\stackrel{\·}{X}{\left(t\right)}_{18\×1}=A{\left(t\right)}_{18\×18}X\left(t\right)+G{\left(t\right)}_{18\×9}W{\left(t\right)}_{9\×1}$

其中，状态变量为：

表示捷联惯性导航在地里坐标系下东、北、天方向平台误差角，δv_E,δv_N,δv_U表示惯性导航系统在地理坐标系下东、北、天方向的速度误差；δL,δλ,δh表示惯性导航系统在地里坐标系下纬度、经度、高度的误差；ε_bx,ε_by,ε_bz表示陀螺随机常数误差；ε_rx,ε_ry,ε_rz表示陀螺一阶马尔可夫过程随机误差；▽_x,▽_y,▽_z表示加速度计的一阶马尔可夫过程随机误差；A(t)_18×18为系统的状态转移矩阵；G(t)_18×9为噪声系数矩阵；W(t)_9×1为系统的白噪声矢量。

5.如权利要求2所述的针对行人导航的低成本MEMS/GPS组合导航方法，其特征在于：所述步骤(3)中，SINS/GPS系统的量测方程分为位置测部分和速度量测部分，其中位置量测为利用惯导提供的位置信息与GPS接收机提供的位置信息之差，速度量测为利用惯导提供的速度信息与GPS接收机提供的速度信息之差。

6.如权利要求5所述的针对行人导航的低成本MEMS/GPS组合导航方法，其特征在于：所述位置量测的位置观测量，即惯性导航系统给出的经度、纬度和高度信息和GPS接收机给出的相应信息的差值，定义位置量测方程为：

$Z_P\left(t\right)=\left[\begin{array}{c}(L_I-L_{GPS})R_M\\ ({\mathrm{\λ}}_I-{\mathrm{\λ}}_{GPS})R_N\cos L\\ h_I-h_{GPS}\end{array}\right]=H_P\left(t\right)X\left(t\right)+V_P\left(t\right)$

其中，L_I，λ_I，h_I分别表示捷联惯导的纬度、经度和高度，L_GPS，λ_GPS，h_GPS分别表示GPS接收机的纬度、经度和高度，V_P(t)＝[N_N N_E N_U]，表示GPS接收机沿东北天方向的位置误差。

7.如权利要求2所述的针对行人导航的低成本MEMS/GPS组合导航方法，其特征在于：所述步骤(4)中，MEMS/GPS组合导航系统的卡尔曼滤波模型如下：

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{X}}_{k-1/k-1}$

$P_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{P}_{k-1/k-1}{\mathrm{\Φ}}_{k/k-1}^T+{\mathrm{\Γ}}_{k/k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k/k-1}^T$

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$

${\hat{X}}_{k/k}={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$

$P_{k/k}=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

上式中，表示t_k-1时刻的状态对t_k时刻的状态的最优估计值，Φ_k/k-1表示t_k-1时刻至t_k时刻系统的状态转移矩阵，表示t_k-1时刻的系统状态估计值，K_k表示增益矩阵，H_k表示滤波模块t_k时刻的观测系数矩阵，P_k/k-1表示最优预测估计误差协方差阵，P_k/k表示滤波模块t_k时刻的系统误差协方差阵，Q_k-1表示t_k-1时刻的噪声方差矩阵，Γ_k/k-1表示t_k-1时刻的噪声矢量对t_k时刻状态矢量影响的噪声系数矩阵，R_k表示t_k时刻的量测方差矩阵，I为单位矩阵。