[发明专利]针对行人导航的低成本MEMS/GPS组合导航系统及方法

权利要求书

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对时间的导数，矢量为四元数形式，表示载体坐标系相对导航坐标系在载体坐标系上的投影。