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

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

Low cost MEMS/GPS integrated navigation system and method for pedestrian navigation

The invention discloses a method for pedestrian navigation of the low-cost MEMS/GPS integrated navigation system, including STM32F4 processor, Bluetooth module, low cost accelerometers and gyroscopes, low-cost GPS module and tablet computer, data processor and STM32F4 tablet computer via Bluetooth module; the output of low cost accelerometers, gyroscopes, low cost GPS module end are connected to the STM32F4 processor. The invention also discloses a pedestrian navigation for the low-cost MEMS/GPS integrated navigation method, which comprises the following steps: Integrated Navigation System INS error state equation modeling; establish loosely coupled SINS/GPS integrated navigation system integrated navigation system state equation; measurement equation modeling; Calman filter according to the state equation and measurement equation. The proposed scheme can efficiently realize strapdown inertial navigation and Calman filter calculation, and obtain high precision and high reliability navigation results. At the same time, it provides effective support for engineering realization and application of MEMS/GPS integrated navigation for pedestrian navigation, and has prominent practical value.

【技术实现步骤摘要】
针对行人导航的低成本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松组合导航系统状态方程为：其中，状态变量为：表示捷联惯性导航在地里坐标系下东、北、天方向平台误差角，δvE,δvN,δvU表示惯性导航系统在地理坐标系下东、北、天方向的速度误差；δ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接收机给出的相应信息的差值，定义位置量测方程为：其中，LI，λI，hI分别表示捷联惯导的纬度、经度和高度，LGPS，λGPS，hGPS分别表示GPS接收机的纬度、经度和高度，VP(t)＝[NNNENU]，表示GPS接收机沿东北天方向的位置误差。上述步骤(4)中，MEMS/GPS组合导航系统的卡尔曼滤波模型如下：上式中，表示tk-1时刻的状态对tk时刻的状态的最优估计值，Φk/k-1表示tk-1时刻至tk时刻系统的状态转移矩阵，表示tk-1时刻的系统状态估计值，Kk表示增益矩阵，Hk表示滤波模块tk时刻的观测系数矩阵，Pk/k-1表示最优预测估计误差协方差阵，Pk/k表示滤波模块tk时刻的系统误差协方差阵，Qk-1表示tk-1时刻的噪声方差矩阵，Γk/k-1表示tk-1时刻的噪声矢量对tk时刻状态矢量影响的噪声系数矩阵，Rk表示tk时刻的量测方差矩阵，I为单位矩阵。采用上述方案后，本专利技术的有益效果如下：(1)在MEMS/GPS导航系统中，捷联惯导与GPS采用卡尔曼滤波的形式进行数据融合，MEMS和GPS优势互补，提高了导航定位的精度和可靠性。(2)硬件层面上，该导航装置由STM32F4处理器，MEMS惯性元件、GPS接收机、蓝牙通信模块、平板电脑等组成。GPS与处理器之间通过UART接口进行通信，上位机与处理器之间通过蓝牙通信进行数据交互。(3)软件层面上，整个导航系统基于UCOS系统，采用多线程模式，处理器CPU在不同任务间进行高速运行与轮转，任务并发进行保证了系统的实时性，利于系统在任务上的扩展与裁剪。附图说明图1是本专利技术的组合导航方法原理图；图2是本专利技术的导航系统硬件模块化构成图。具体实施方式以下将结合附图，对本专利技术的技术方案进行详细说明。本专利技术提供一种针对行人导航的低成本MEMS/GPS组合导航系统及方法，采用多线程并发的组合导航解算方案。考虑到整个程序的实时性和精确性，导航程序基于UCOSIII系统编写，分别包括捷联惯导解算线程、卡尔曼滤波线程、上位机数据交互线程、定时器等模块；导航硬件装置由AHRSBOX板载的STM32F4处理器、蓝牙模块、低成本加速度计和陀螺仪、低成本GPS模块、平板电脑等组成。其中平板电脑用于监控显示导航解算数据，与处理器通过蓝牙通讯的方式进行数据交互。如图1所示，本专利技术导航方法的原理是：先在地理坐标系下进行捷联惯导算法编排；然后再建立MEMS/GPS组合模型，利用GPS的位置和速度信息对捷联惯导进行实时修正。具体实施方法如下：(1)组合导航系统惯导误差状态量方程建模对于行人导航的低成本MEMS/GPS组合导航系统，其捷联惯导解算过程如下，速度计算公式为：式中，为载体相当于地理系的加速度在载体上的投影，为载体相当于地理系的速度在载体上的投影，为机体系到导航坐标系的姿态转移矩阵，为载体相当于地理坐标系的速度在导航坐标系上的投影，本模型的地理坐标系即为导航坐标系。位置计算公式为：用四元数描述载体的姿态运动，计算公式为：用矩阵表示为：式中，q为姿态四元数，为q对时间的导数，矢量为四元数形式，表示载体坐标系相对导航坐标系在载体坐标系上的投影。(2)在上述基础上，建立SINS/GPS松组合导航系统状态方程为：其中，状态变量为：表示捷联惯性导航在地里坐标系下东、北、天方向平台误差角，δvE,δvN,δvU表示惯性导航系统在地理坐标系下东、北、天方向的速度误差；δL,δλ,δh表示惯性导航系统在地里坐标系下纬度、经度、高度的误差；εbx,εby,εbz表示陀螺随机常数误差；εrx,εry,εrz表示陀螺一阶马尔可夫过程随机误差；表示加速度计的一阶马尔可夫过程随机误差。A(t)18×18为系统的状态转移矩阵；G(t)18×9为噪声系数矩阵；W(t)9×1为系统的白噪声矢量。(3)组合导航系统量测方程建模在此所叙述的低成本S本文档来自技高网...

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

【技术特征摘要】
1.一种针对行人导航的低成本MEMS/GPS组合导航系统，其特征在于：包括STM32F4处理器、蓝牙模块、低成本加速度计和陀螺仪、低成本GPS模块和平板电脑，STM32F4处理器与平板电脑通过蓝牙模块进行数据交互；低成本加速度计、陀螺仪、低成本GPS模块的输出端均连接STM32F4处理器。2.一种针对行人导航的低成本MEMS/GPS组合导航方法，其特征在于包括如下步骤：(1)组合导航系统惯导误差状态量方程建模；(2)在上述基础上，建立SINS/GPS松组合导航系统状态方程；(3)组合导航系统量测方程建模；(4)按状态方程和量测方程进行卡尔曼滤波。3.如权利要求2所述的针对行人导航的低成本MEMS/GPS组合导航方法，其特征在于：所述步骤(1)的详细内容是：对于行人导航的低成本MEMS/GPS组合导航系统，速度计算公式为：式中，为载体相当于地理系的加速度在载体上的投影，为载体相当于地理系的速度在载体上的投影，为机体系到导航坐标系的姿态转移矩阵，为载体相当于地理坐标系的速度在导航坐标系上的投影，其中的地理坐标系即为导航坐标系；位置计算公式为：用四元数描述载体的姿态运动，计算公式为：用矩阵表示为：式中，q为姿态四元数，为q对时间的导数，矢量为四元数形式，表示载体坐标系相对导航坐标系在载体坐标系上的投影。4.如权利要求2所述的针对行人导航的低成本MEMS/GPS组合导航方法，其特征在于：所述步骤(2)中，SINS/GPS松组合导航系统状态方程为：其中，状...

【专利技术属性】
技术研发人员：殷德全，熊智，闵艳玲，杨菁华，许建新，景羿铭，施丽娟，孔雪博，唐攀飞，戴怡洁，赵宣懿，黄欣，万众，徐丽敏，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏,32