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

说明书

本发明公开了一种基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置和方法，包括MEMS传感器、INS模块、PDR定位模块、VLC定位模块以及测姿定位双卡尔曼滤波器；对于测姿滤波器，基于惯导机械编排的误差方程作为系统方程，观测方程包括加速度计观测量更新和磁力计观测量更新，输出姿态信息给VLC定位模块和PDR定位模块以校正姿态的影响。对于定位滤波器，二维平面的位置信息作为系统状态向量，基于行人航位推算的误差方程作为系统方程，而VLC的定位结果为观测方程。本发明的技术方案解决VLC定位容易受设备姿态影响以及在光信号被遮挡情况下定位不连续的问题，消除姿态对VLC定位的影响。

技术领域

本发明涉及导航定位装置和方法，特别是涉及一种基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置和方法。

背景技术

随着室内定位技术的发展和应用，基于可见光通信的室内定位技术也在迅速兴起并得到广泛关注。在充足室内光源的环境下，通过光传感器等设备检测得到经过复用协议调制的光信号，通过信号解调技术可以将不同光源信号数据分离，从而结合环境参数可以计算出定位目标相对每个光源的距离或角度信息，最后通过定位算法如三边定位可以完成目标定位。

然而，由于目标接收设备的姿态会随着目标移动产生抖动，将对VLC定位结果带来较大的影响。另一方面，实际场景中光信号容易被遮挡，将会导致定位不连续。针对前一个问题，目前的方案主要是通过多传感器组合共同定位。针对后一个问题，主流的解决方案是通过卡尔曼滤波或者粒子滤波来估计目标位置。

但这些方案存在一些问题：1)相比于单传感器定位，多传感器组合定位算法复杂且成本较高；2)目前定位方案中提出的滤波器融合是以探测器姿态平稳为前提，在实际场景中稳定性较差。3)在信号被遮挡的情况频繁发生的场景中，仅通过VLC数据加滤波器的定位系统推测的目标位置仍然与实际位置有较大偏差，VLC定位结果不连续、不平滑；另外，在惯导定位技术中，多采用一个卡尔曼滤波来进行噪声处理和定位，然而这样通过单一滤波器同时定位和测姿会造成姿态和位置相互影响。

发明内容

发明目的：为解决现有技术的不足，提供一种基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置和方法。

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

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

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

一种基于所述导航装置的导航方法，包括以下步骤：

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

(11)建立A-EKF的状态向量；

(12)建立A-EKF的系统模型；

(13)建立A-EKF的观测模型；

(14)S-EKF滤波输出姿态信息；

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

(21)建立L-EKF的状态向量；