基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置和方法制造方法及图纸

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

Dual Calman filtering navigation device and method based on MEMS sensor and VLC positioning fusion

The invention discloses a double Calman filtering method and device for MEMS navigation sensors and VLC positioning based on fusion, including MEMS sensor, INS module, PDR positioning module, VLC positioning module and attitude measuring and positioning for double Calman filter; attitude measurement filter error equation of inertial navigation based on machine layout as the system equation, observation equation including the accelerometer and magnetometer measurements to update the concept of measurement update, affect the attitude information output to the VLC positioning module and PDR positioning module to correct posture. For positioning filter, two-dimensional position information as the state vector of the system, the error equation of pedestrian dead reckoning based on VLC as the system equation, and the positioning results as the observation equations. The technical scheme of the invention solves the problem that the positioning of the VLC is easily affected by the posture of the equipment and the position of the optical signal is not continuous under the condition of being blocked, so as to eliminate the influence of the posture on the VLC positioning.

【技术实现步骤摘要】
基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置和方法
本专利技术涉及导航定位装置和方法，特别是涉及一种基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置和方法。
技术介绍
随着室内定位技术的发展和应用，基于可见光通信的室内定位技术也在迅速兴起并得到广泛关注。在充足室内光源的环境下，通过光传感器等设备检测得到经过复用协议调制的光信号，通过信号解调技术可以将不同光源信号数据分离，从而结合环境参数可以计算出定位目标相对每个光源的距离或角度信息，最后通过定位算法如三边定位可以完成目标定位。然而，由于目标接收设备的姿态会随着目标移动产生抖动，将对VLC定位结果带来较大的影响。另一方面，实际场景中光信号容易被遮挡，将会导致定位不连续。针对前一个问题，目前的方案主要是通过多传感器组合共同定位。针对后一个问题，主流的解决方案是通过卡尔曼滤波或者粒子滤波来估计目标位置。但这些方案存在一些问题：1)相比于单传感器定位，多传感器组合定位算法复杂且成本较高；2)目前定位方案中提出的滤波器融合是以探测器姿态平稳为前提，在实际场景中稳定性较差。3)在信号被遮挡的情况频繁发生的场景中，仅通过VLC本文档来自技高网...

【技术保护点】
基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置，其特征在于：包括MEMS传感器、INS模块、PDR定位模块、VLC定位模块、测姿扩展卡尔曼滤波器和定位扩展卡尔曼滤波器；所述MEMS传感器包括加速度计、陀螺仪以及磁力计；测姿扩展卡尔曼滤波器的输入包括：陀螺仪采集接收器在XYZ方向的角速度信息经过INS模块的机械编排算法处理得到的INS姿态信息、加速度计采集的接收器在XYZ方向的加速度信息以及磁力计采集的接收器相对东、南、西、北方向的角度信息；其中，加速度信息和角速度信息按时间轴顺序输入至测姿扩展卡尔曼滤波器；输出包括传递给PRD定位模块和VLC定位模块的姿态误差向量和反馈给陀螺仪用以噪...

【技术特征摘要】
1.基于MEMS传感器和VLC定位融合的双卡尔曼滤波导航装置，其特征在于：包括MEMS传感器、INS模块、PDR定位模块、VLC定位模块、测姿扩展卡尔曼滤波器和定位扩展卡尔曼滤波器；所述MEMS传感器包括加速度计、陀螺仪以及磁力计；测姿扩展卡尔曼滤波器的输入包括：陀螺仪采集接收器在XYZ方向的角速度信息经过INS模块的机械编排算法处理得到的INS姿态信息、加速度计采集的接收器在XYZ方向的加速度信息以及磁力计采集的接收器相对东、南、西、北方向的角度信息；其中，加速度信息和角速度信息按时间轴顺序输入至测姿扩展卡尔曼滤波器；输出包括传递给PRD定位模块和VLC定位模块的姿态误差向量和反馈给陀螺仪用以噪声补偿的陀螺偏差向量；定位扩展卡尔曼滤波器的输入包括PDR定位模块输出的PDR位置信息和VLC定位模块输出的位置信息，PDR定位模块的输入包括采集的接收器在XYZ方向的加速度信息和所述测姿扩展卡尔曼滤波器输出的姿态信息；定位扩展卡尔曼滤波器输出接收器的定位信息。2.一种基于权利要求1所述导航装置的导航方法，其特征在于，包括以下步骤：(1)估计接收器姿态信息(11)建立A-EKF的状态向量；(12)建立A-EKF的系统模型；(13)建立A-EKF的观测模型；(14)S-EKF滤波输出姿态信息；(2)估计接收器位置信息(21)建立L-EKF的状态向量；(22)建立L-EKF的系统模型；(23)建立L-EKF的观测模型；(24)L-EKF滤波输出定位信息。3.根据权利要求2所述的导航方法，其特征在于，所述步骤(11)中A-EKF的状态向量定义为：x＝[ψbg]T其中：ψ为姿态误差向量；bg为陀螺偏差向量。4.根据权利要求3所述的导航方法，其特征在于，所述步骤(12)中先对姿态矩阵进行坐标系变换，其坐标转移方程为：其中，是从载体坐标系到导航坐标系的方向余弦矩阵，即姿态矩阵；和分别是角速度矢量和的斜对称矩阵；而和表示载体坐标系相对于惯性坐标的转动角速度和导航坐标系相对于惯性坐标的转动角速度；A-EKF的系统模型为具体展开公式如下：其中，和分别表示地球自传的角速度和导航坐标系相对于地心地固坐标系的转动角速度；wg是传感器噪声；τbg代表惯导噪声的相关时间；wbg是驱动噪声；符号“×”表示叉乘。5.根据权利要求4所述的导航方法，其特征在于，所述步骤(13)中A-EKF的观测模型包括加速度计观测模型和磁力计观测模型，观测模型的表达式为Z＝HX+V，其中，加速度计观测模型具体为：其中，是加速度计读数矢量,fn＝-gn＝[00-g]T是导航坐标系中的特定力矢量，fb是载体坐标系中的特定力矢量，g是局部重力值，ψ是姿态误差,I是单位矩阵，n2是噪声，[ψ×]表示ψ矢量的斜对称矩阵，[gn×]表示gn矢量的斜对称矩阵，此时，H＝-[gn×]，磁力计观测模型具体为：其中，是磁力计读数矢量，mn是校准的LMF矢量，n3是噪声，此时，H＝[mn×]，6.根据权利要求5所述的导航方法，其特征在于，所述步骤(14)包括：(a)将系统模型根据卡尔曼滤波的模型形式进行变换成如下形式：其中，(b)观测模型Z＝HX+V对应以下方程：加速计观测方程为：此时，H＝[-[gn×]03×3]，磁力计观测方程为：

【专利技术属性】
技术研发人员：华鲁驰，庄园，戚隆宁，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏,32