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

权利要求书

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

所述测姿定位单卡尔曼滤波器模块的输入包括：加速度计测得的接收器在XYZ方向的加速度信息和陀螺仪测得的接收器在XYZ方向上的角速度信息传递给INS模块，由INS模块对其进行编排得到接收器的INS位置、速度和姿态信息；磁力计测得的接收器相对东、南、西、北方向的角度信息；加速度计测得的接收器在XYZ方向的加速度信息给PDR定位模块进行位置估计得到的PDR定位信息和VLC定位模块输出的VLC定位信息通过加权平均后的位置信息；

所述测姿定位单卡尔曼滤波器模块输出当前时刻接收器的位置信息、速度信息和姿态信息给INS模块，输出当前时刻接收器的姿态信息给PDR定位模块或VLC定位模块，输出噪声补偿信号反馈给加速度计和陀螺仪，同时输出当前时刻接收器的定位信息。

2.根据权利要求1所述的导航装置，其特征在于：所述PDR定位信息和VLC定位信息的权重由VLC定位模块中光信号是否被遮挡来设定，若光信号被遮挡时系统将VLC位置信息的权重设为0，只采用PDR位置信息。

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

(1)建立测姿定位单卡尔曼滤波器模块S-EKF的状态向量；

(2)建立测姿定位单卡尔曼滤波器模块S-EKF的系统模型；

(3)建立测姿定位单卡尔曼滤波器模块S-EKF的观测方程；

(4)测姿定位单卡尔曼滤波器模块S-EKF滤波输出定位信息。

4.根据权利要求3所述的一种导航方法，其特征在于：所述步骤(1)中S-EKF的状态向量为：

x＝[δrⁿ δvⁿ ψ b_g b_a]^T

其中，δrⁿ、δvⁿ、ψ、b_g和b_a分别是接收器的位置误差矢量、速度误差矢量、姿态误差矢量、陀螺仪偏差矢量和加速度计偏差矢量。

5.根据权利要求4所述的一种导航方法，其特征在于，所述步骤(2)包括：

(21)将加速度计和陀螺仪采集到的接收器数据信息输入到INS模块进行机械编排算法处理得到接收器当前位置信息、速度信息和姿态信息并输入到S-EKF中；对姿态矩阵进行坐标变换，其坐标转移方程为：

其中，为rⁿ的一阶导数，是导航坐标系中的位置向量，表示纬度，λ表示经度和h表示高度；是vⁿ的一阶导数，vⁿ是三维速度向量；gⁿ是导航坐标系中的重力向量；表示位置增量；表示速度增量；f^b是载体坐标系中的特定力矢量；D^-1是一个关于和h的3×3矩阵；是的一阶导数，是由INS机械编排预测的从载体坐标系到导航坐标系的方向余弦矩阵；和分别为角速度矢量和的斜对称矩阵；而和表示地球自传的角速度和导航坐标系相对于地心地固坐标系的转动角速度；和表示载体坐标系相对于惯性坐标的转动角速度和导航坐标系相对于惯性坐标的转动角速度；

(22)S-EKF的系统模型为具体展开公式如下：

其中，δ符号表示误差，即真实值和系统标称值之差；和分别表示δrⁿ、δvⁿ、ψ、b_g和b_a的一阶导数，fⁿ是投影到导航坐标系的特定力矢量；w_g和w_a是传感器噪声；τ_bg和τ_ba代表惯导噪声的相关时间；w_bg和w_ba是驱动噪声，符号“×”表示叉乘。