[发明专利]针对行人导航的低成本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接收机给出的相应信息的差值，定义位置量测方程为：