[发明专利]基于BD、GPS及MEMS的组合导航系统及导航方法

权利要求书

1.采用基于BD、GPS及MEMS的组合导航系统获取导航坐标的方法，所述基于BD、GPS及MEMS的组合导航系统，包括MEMS-IMU模块(100)、GNSS模块(200)、磁强计模块(300)和组合导航计算机(400)；MEMS-IMU模块(100)、GNSS模块(200)和磁强计模块(300)分别与组合导航计算机(400)连接；

所述的MEMS-IMU模块(100)包括三轴MEMS陀螺仪(101)和三轴MEMS加速度计(102)，其中，三轴MEMS陀螺仪(101)为单芯片数字化的角速度检测器，负责向组合导航计算机(400)输出角速度信号；三轴MEMS加速度计(102)为单芯片数字化的比力检测器，负责向组合导航计算机(400)输出比力信号；

所述的GNSS模块(200)包含一片集成BD2和GPS的导航芯片(205)，通过集成BD2和GPS的导航芯片(205)向组合导航计算机(400)输出导航信号；

所述的磁强计模块(300)负责向组合导航计算机(400)输出三维磁场强度信号；

所述组合导航计算机(400)将三轴MEMS陀螺仪(101)的输出信号、三轴MEMS加速度计(102)的输出信号与GNSS模块(200)的输出信号进行滤波运算，获得本组合导航系统的三维位置值、三维速度值和加速度计漂移估计值；

组合导航计算机(400)将三轴MEMS陀螺仪(101)的输出信号、三轴MEMS加速度计(102)的输出信号、磁强计模块(300)的输出信号、以及运算获得的加速度计漂移估计值进行融合运算，获得本组合导航系统的姿态角估计值；

上述的三维位置值、三维速度值和姿态角估计值做为本组合导航系统的导航数据输出值对外输出；

所述的GNSS模块(200)还包括天线(201)；所述的集成BD2和GPS导航芯片(205)由BD2信号前端处理单元(202)、GPS信号前端处理单元(203)和基带处理SOC(204)组成；其中，天线(201)分别与BD2信号前端处理单元(202)的信号输入端、GPS信号前端处理单元(203)的信号输入端相连接，BD2信号前端处理单元(202)的信号输出端与GPS信号前端处理单元(203)的信号输出端分别与基带处理SOC(204)的信号输入端相连接；

所述的磁强计模块(300)包括x轴磁阻传感器(301)、y轴磁阻传感器(302)、z轴磁阻传感器(303)和磁阻信号处理电路(304)；其中，x轴磁阻传感器(301)的信号输出端、y轴磁阻传感器(302)的信号输出端、z轴磁阻传感器(303)的信号输出端分别与磁阻信号处理电路(304)的信号输入端相连接，由磁阻信号处理电路(304)对接收到的信号进行模数转换后，输出数字化的三维磁场强度信号；

所述组合导航计算机(400)包括姿态解算滤波器(401)和组合导航扩展卡尔曼滤波器(402)；其中，姿态解算滤波器(401)获取三轴MEMS陀螺仪(101)的信号、三轴MEMS加速度计(102)的信号、以及磁强计模块(300)的信号；组合导航扩展卡尔曼滤波器(402)获取三轴MEMS陀螺仪(101)的信号、三轴MEMS加速度计(102)的信号、以及GNSS模块(200)的信号；组合导航扩展卡尔曼滤波器(402)的输出信号反馈至姿态解算滤波器(401)；

所述组合导航扩展卡尓曼滤波器(402)对接收到的GNSS模块(200)的输出信号、三轴MEMS陀螺仪(101)的输出信号和三轴MEMS加速度计(102)的输出信号进行滤波运算，获得本组合导航系统的三维位置值、三维速度值和加速度计漂移估计值；

所述姿态解算滤波器(401)对接收到的三轴MEMS陀螺仪(101)的输出信号、三轴MEMS加速度计(102)的输出信号、磁强计模块(300)的输出信号以及由组合导航扩展卡尓曼滤波器(402)通过滤波运算获得的加速度计漂移估计值进行融合运算，获得本组合导航系统的姿态角估计值；

本组合导航系统的姿态角估计值、三维位置值和三维速度值作为导航的结果向外输出；

其特征在于，基于BD、GPS、及MEMS的组合导航系统获取导航坐标的步骤如下：

步骤1：由组合导航计算机(400)读取三轴MEMS陀螺仪(101)输出的角速度信号、三轴MEMS加速度计(102)输出的比力信号和GNSS模块(200)输出的导航坐标信号，并通过组合导航扩展卡尔曼滤波器(402)转换得到本组合导航系统的三维位置值、三维速度值和加速度计漂移数值；

步骤2：由组合导航计算机(400)读取三轴MEMS陀螺仪(101)输出的角速度信号、三轴MEMS加速度计(102)输出的比力信号、磁强计模块(300)输出的三维磁场强度信号，并通过姿态解算滤波器(401)转换得到本组合导航系统的绝对值的姿态角数值；

步骤3：由组合导航计算机(400)将步骤1得到加速度计漂移数值转化成陀螺漂移数值和磁强计失真误差值；

步骤4：将步骤3得到陀螺漂移数值和磁强计失真误差值输入姿态解算滤波器(401)，对由步骤2获得的绝对值的姿态角数值进行校正，获得本组合导航系统的姿态角估计值；

步骤5：将本组合导航系统的三维位置值、三维速度值和姿态角估计值作为导航运算的结果对外输出；

本组合导航系统的姿态角估计值按如下步骤获得：

步骤a：由组合导航计算机(400)获取上一时刻的姿态角估计值磁场强度信号^bm、

比力信号^ba以及地球磁倾角ⁿb；

步骤b：由组合导航计算机(400)采用步骤a所获得的参数建立动力学模型；

所述动力学模型包含目标函数和雅克比矩阵

步骤c：由组合导航计算机(400)将步骤b获得的目标函数和雅克比矩阵代入如下公式，计算获得姿态角速率归一化估计误差

q^·nbError=Jq^nb,bnT·fq^nb,ab,bn,mb||Jq^nb,bnT·fq^nb,ab,bn,mb||;]]>

步骤d：由组合导航计算机(400)将步骤c获得的姿态角速率归一化估计误差代入如下公式，计算获得陀螺仪的漂移估计值^bω_Bias；

ωbError=q^nb*⊗q^·nbError;]]>

^bω_Bias＝αΣ^bω_ErrorΔt；

式中，系数α为校准零位的陀螺输出误差；

步骤e：由组合导航计算机(400)将步骤d获得的陀螺仪的漂移估计值^bω_Bias代入如下公式，计算获得校正后的陀螺输出角速度^bω_C；

^bω_C＝^bω-^bω_Bias；

式中，^bω为初始的陀螺仪归一化输出的角速度数据；

步骤f：由组合导航计算机(400)将步骤d获得的校正后的陀螺输出角速度^bω_C代入如下公式，求解实时姿态四元数

q^nb=0.5q^nb⊗ωbC-βq^·nbError;]]>

qnb=q^nb+q·nb·Δt;]]>

式中，滤波器系数β为校准非零位的陀螺输出误差；

该实时姿态四元数即为本组合导航系统的姿态角估计值。

2.如权利要求1所述的采用基于BD、GPS及MEMS的组合导航系统获取导航坐标的方法，其特征在于，由组合导航计算机(400)读取三轴MEMS加速度计(102)的比力信号和磁强计模块(300)的磁场强度信号，并按下式求出本组合导航系统的绝对的三维姿态角角度值[phi theta psi]：供组合导航计算机(400)未读取到上一时刻的姿态角估计值时，用本组合导航系统的初始状态的四元数值代替上一时刻的姿态角估计值使用；

phi=arctan(ayaz);]]>

theta=arcsin(axax2+ay2+az2);]]>

psi=-arctan(cos(phi)·my-sin(phi)·mzcos(theta)·mx+sin(phi)·sin(theta)·my+cos(phi)·sin(theta)·mz);]]>

其中：为三轴MEMS加速度计(102)的三维输出值，为磁强计模块(300)的三维输出值；

由组合导航计算机(400)将本组合导航系统的绝对的三维姿态角角度值[phi theta psi]按下式转换得到获得本组合导航系统的初始状态的四元数值：

q1=cos(phi2)·cos(theta2)·cos(psi2)+sin(phi2)·sin(theta2)·sin(psi2);]]>

q2=sin(phi2)·cos(theta2)·cos(psi2)-cos(phi2)·sin(theta2)·sin(psi2);]]>

q3=cos(phi2)·sin(theta2)·cos(psi2)+sin(phi2)·cos(theta2)·sin(psi2);]]>

q4=cos(phi2)·cos(theta2)·sin(psi2)-sin(phi2)·sin(theta2)·cos(psi2).]]> 3