[发明专利]基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置和方法

权利要求书

1.基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置，其特征在于：包括MEMS传感器、INS模块、PDR定位模块、VLC定位模块、测姿扩展卡尔曼滤波器和定位扩展卡尔曼滤波器；所述MEMS传感器包括加速度计、陀螺仪以及磁力计；

测姿扩展卡尔曼滤波器的输入包括：陀螺仪采集接收器在XYZ方向的角速度信息经过INS模块的机械编排算法处理得到的INS姿态信息、加速度计采集的接收器在XYZ方向的加速度信息以及磁力计采集的接收器相对东、南、西、北方向的角度信息；其中，加速度信息和角速度信息按时间轴顺序输入至测姿扩展卡尔曼滤波器；输出包括传递给PDR 定位模块和VLC定位模块的姿态误差向量和反馈给陀螺仪用以噪声补偿的陀螺偏差向量；

定位扩展卡尔曼滤波器的输入包括PDR定位模块输出的PDR位置信息和VLC定位模块输出的位置信息，PDR定位模块的输入包括采集的接收器在XYZ方向的加速度信息和所述测姿扩展卡尔曼滤波器输出的姿态信息；定位扩展卡尔曼滤波器输出接收器的定位信息。

2.一种基于权利要求1所述导航装置的导航方法，其特征在于，包括以下步骤：

(1)估计接收器姿态信息

(11)建立测姿扩展卡尔曼滤波器A-EKF的状态向量；

(12)建立测姿扩展卡尔曼滤波器A-EKF的系统模型；

(13)建立测姿扩展卡尔曼滤波器A-EKF的观测模型；

(14)测姿扩展卡尔曼滤波器A-EKF滤波输出姿态信息；

(2)估计接收器位置信息

(21)建立定位扩展卡尔曼滤波器L-EKF的状态向量；

(22)建立定位扩展卡尔曼滤波器L-EKF的系统模型；

(23)建立定位扩展卡尔曼滤波器L-EKF的观测模型；

(24)定位扩展卡尔曼滤波器L-EKF滤波输出定位信息。

3.根据权利要求2所述的导航方法，其特征在于，所述步骤(11)中A-EKF的状态向量定义为：

x＝[ψ b_g]^T

其中：ψ为姿态误差向量；b_g为陀螺偏差向量。

4.根据权利要求3所述的导航方法，其特征在于，所述步骤(12)中先对姿态矩阵进行坐标系变换，其坐标转移方程为：

其中，是从载体坐标系到导航坐标系的方向余弦矩阵，即姿态矩阵；和分别是角速度矢量和的斜对称矩阵；而和表示载体坐标系相对于惯性坐标的转动角速度和导航坐标系相对于惯性坐标的转动角速度；

A-EKF的系统模型为具体展开公式如下：

其中，和分别表示地球自传的角速度和导航坐标系相对于地心地固坐标系的转动角速度；w_g是传感器噪声；τ_bg代表惯导噪声的相关时间；w_bg是驱动噪声；符号“×”表示叉乘。

5.根据权利要求4所述的导航方法，其特征在于，所述步骤(13)中A-EKF的观测模型包括加速度计观测模型和磁力计观测模型，观测模型的表达式为Z＝HX+V，其中，

加速度计观测模型具体为：

其中，是加速度计读数矢量,fⁿ＝-gⁿ＝[0 0 -g]^T是导航坐标系中的特定力矢量，f^b是载体坐标系中的特定力矢量，g是局部重力值，ψ是姿态误差,I是单位矩阵，n₂是噪声，[ψ×]表示ψ矢量的斜对称矩阵，[gⁿ×]表示gⁿ矢量的斜对称矩阵，此时，H＝-[gⁿ×]，

磁力计观测模型具体为：

其中，是磁力计读数矢量，mⁿ是校准的LMF矢量，n₃是噪声，此时，H＝[mⁿ×]，