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

本发明专利技术公开了一种基于MEMS传感器和VLC定位融合的单卡尔曼滤波导航装置和方法，包括MEMS传感器、INS模块、VLC定位模块、PDR定位模块和测姿定位单卡尔曼滤波器模块；本发明专利技术基于INS惯导机械编排的误差方程作为融合滤波器的系统方程，观测方程包括VLC定位信息更新、PDR定位信息更新和磁力计观测量更新。融合滤波器输出VLC接收器的姿态给VLC定位模块，输出PDR设备的姿态给PDR定位模块以校正姿态的影响。首次在VLC定位领域使用融合测姿准确估计VLC接收器的姿态信息，主要解决了VLC定位容易受设备姿态影响以及在光信号被遮挡情况下定位不连续的问题。

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

The invention discloses a single Calman filtering navigation device and method of MEMS sensor and VLC positioning based on fusion, including MEMS sensor, INS module, VLC positioning module, PDR positioning module and attitude measuring and positioning of single Calman filter module; the invention INS SINS error equation mechanization as the system equation of fusion filter based on observation the equation including VLC positioning information update and PDR positioning information update and magnetometer measurements update. The attitude of the output VLC receiver is fused to the VLC positioning module, and the attitude of the output PDR device is given to the PDR positioning module to correct the influence of the attitude. It is the first time that the attitude of VLC receiver is accurately estimated by using fusion attitude measurement in the field of VLC positioning, which mainly solves the problem that the VLC positioning is easily affected by the attitude of the equipment and the location is not continuous under the condition that the optical signal is occluded.

【技术实现步骤摘要】
基于MEMS传感器和VLC定位融合的单卡尔曼滤波导航装置和方法
本专利技术涉及智能定位装置及方法，特别是涉及基于MEMS传感器和VLC定位融合的单卡尔曼滤波导航装置和方法。
技术介绍
随着室内定位技术的发展和应用，基于可见光通信的室内定位技术也在迅速兴起并得到广泛关注。在充足室内光源的环境下，通过光传感器等设备检测得到经过复用协议调制的光信号，通过信号解调技术可以将不同光源信号数据分离，从而结合环境参数可以计算出定位目标相对每个光源的距离或角度信息，最后通过定位算法如三边定位可以完成目标定位。然而，由于目标接收设备的姿态会随着目标移动产生抖动，将对VLC定位结果带来较大的影响。另一方面，实际场景中光信号容易被遮挡，将会导致定位不连续。针对前一个问题，目前的方案主要是通过多传感器组合共同定位。针对后一个问题，主流的解决方案是通过卡尔曼滤波或者粒子滤波来估计目标位置。但这些方案存在一些问题：1)相比于单传感器定位，多传感器组合定位算法复杂且成本较高；2)目前定位方案中提出的滤波器融合是以探测器姿态平稳为前提，在实际场景中稳定性较差；3)在信号被遮挡的情况频繁发生的场景中，仅通过VLC数据加滤波器的定位系统推测的目标位置仍然与实际位置有较大偏差，且VLC定位结果不连续、不平滑。
技术实现思路
专利技术目的：为解决现有技术的不足，提供一种可消除姿态对VLC定位的影响，可弥补VLC定位结果不连续、不平滑的缺点的基于MEMS传感器和VLC定位融合的测姿定位单卡尔曼滤波器导航装置和方法。技术方案：基于MEMS传感器和VLC定位融合的测姿定位单卡尔曼滤波器导航装置，包括MEMS传感器、惯性导航系统(InertialNavigationSystem，INS)模块、可见光通信(VisibleLightCommunication，VLC)定位模块、行人航位推算(PedestrianDeadReckoning，PDR)定位模块和测姿定位单卡尔曼滤波器模块；所述MEMS传感器包括加速度计、陀螺仪以及磁力计；所述测姿定位单卡尔曼滤波器模块的输入包括：加速度计测得的接收器在XYZ方向的加速度信息和陀螺仪测得的接收器在XYZ方向上的角速度信息传递给INS机械编排模块，由INS模块对其进行编排得到接收器的INS位置、速度和姿态信息；磁力计测得的接收器相对东、南、西、北方向的角度信息；加速度计测得的接收器在XYZ方向的加速度信息给PDR定位模块进行位置估计得到的PDR定位信息和VLC定位模块输出的VLC定位信息通过加权平均后的位置信息；所述测姿定位单卡尔曼滤波器模块输出当前时刻接收器的位置信息、速度信息和姿态信息给INS机械编排模块，输出当前时刻接收器的姿态信息给PDR定位模块或VLC定位模块，输出噪声补偿信号反馈给加速度计和陀螺仪，同时输出当前时刻接收器的定位信息。其中，所述PDR定位信息和VLC定位信息的权重由VLC定位模块中光信号是否被遮挡来设定，若光信号被遮挡时系统将VLC位置信息的权重设为0，只采用PDR位置信息。一种基于所述导航装置的导航方法，包括以下步骤：(1)建立S-EKF的状态向量；(2)建立S-EKF的系统模型；(3)建立S-EKF的观测方程；(4)S-EKF滤波输出定位信息。进一步的，所述步骤(1)中S-EKF的状态向量为：x＝[δrnδvnψbgba]T其中，δrn、δvn、ψ、bg和ba分别是接收器的位置误差矢量、速度误差矢量、姿态误差矢量、陀螺仪偏差矢量和加速度计偏差矢量。进一步的，所述步骤(2)包括：(21)将加速度计和陀螺仪采集到的接收器数据信息输入到INS模块进行机械编排算法处理得到接收器当前位置信息、速度信息和姿态信息并输入到S-EKF中；对姿态矩阵进行坐标变换，其坐标转移方程为：其中，为rn的一阶导数，是导航坐标系中的位置向量，表示纬度，λ表示经度和h表示高度；是vn的一阶导数，vn是三维速度向量；gn是导航坐标系中的重力向量；表示位置增量；表示速度增量；fb是载体坐标系中的特定力矢量；D-1是一个关于和h的3×3矩阵；是的一阶导数，是由INS机械编排预测的从载体坐标系到导航坐标系的方向余弦矩阵；和分别为角速度矢量和的斜对称矩阵；而和表示地球自传的角速度和导航坐标系相对于地心地固坐标系的转动角速度；和表示载体坐标系相对于惯性坐标的转动角速度和导航坐标系相对于惯性坐标的转动角速度；(22)A-EKF的系统模型为具体展开公式如下：其中，δ符号表示误差，即真实值和系统标称值之差；和分别表示δrn、δvn、ψ、bg和ba的一阶导数，fn是投影到导航坐标系的特定力矢量；wg和wa是传感器噪声；τbg和τba代表惯导噪声的相关时间；wbg和wba是驱动噪声，符号“×”表示叉乘。进一步的，所述步骤(3)包括：(31)PDR或VLC输出的位置观测方程当发生光信号遮挡VLC无法输出有效定位信息的情况时，PDR位置信息作为测姿定位单卡尔曼滤波器模块的输入量；根据PDR位置信息建立的观测方程为：其中和为INS机械编排算出的纬度和经度；和分别为来自PDR的纬度和经度；和nλ是测量噪声；否则，测姿定位单卡尔曼滤波器模块接收VLC位置信息，而忽略PDR位置信息；根据VLC输出信息建立的观测方程为：其中，和分别是来自于INS机械编排和VLC的位置向量；δrn是位置误差矢量；n1是量测噪声；(32)磁力计观测方程滤波器直接通过磁力计读数进行姿态更新，磁力计观测方程如下：其中，是磁力计读数矢量，mn是校准的LMF矢量，n3是噪声。进一步的，所述步骤(4)包括：(41)将系统模型根据卡尔曼滤波的模型形式进行变换成如下形式：其中，(42)观测模型Z＝HX+V对应以下方程：其中，H＝[I3×303×303×303×303×3]，V＝[n1]；当VLC输出无效位置信息时，采用PDR输入的位置信息，观测方程为：当采用VLC位置信息时，观测方程为：当采用磁力计观测方程滤波时，将磁力计观测方程依Z＝HX+V形式变换后参与滤波，其中，H＝[03×303×3[mn×]03×303×3]，(43)扩展卡尔曼滤波递推步骤如下：(a)设置初值令协方差矩阵P的初始值为：P0＝diag([var(r0)var(v0)var(ψ0)var(bg0)var(ba0)])其中，r0、v0、ψ0、bg0和ba0分别表示向量δrn、δvn、ψ、bg和ba的初始值，P0表示各个向量初始向量的协方差构成的对角矩阵；设状态量X初始值为0向量；(b)利用k-1时刻的对角矩阵对k时刻的对角矩阵进行预测，预测公式为：Pk′＝FPk-1FT+Q其中，Pk-1表示k-1时刻的对角矩阵，Pk′表示k时刻的对角矩阵的预测值，Q＝E[(GW)(GW)T]；(c)利用k-1时刻的状态量对k时刻的状态量进行预测，预测公式为：其中，表示k-1时刻的状态量，表示k时刻状态量的预测值；(d)计算k时刻的卡尔曼增益K′，计算公式如下：Kk′＝Pk′HT(HPk′HT+R)-1其中，Kk′表示k时刻的卡尔曼增益，R＝E[VVT]；(e)利用k时刻的卡尔曼增益估计k时刻的状态量的最优值，令该最优值作为k时刻的状态量则：(f)利用k时刻的卡尔曼增益估计k时刻的对角矩阵的最优值，令该最优值作为k时刻的对角矩阵本文档来自技高网...

【技术保护点】
基于MEMS传感器和VLC定位融合的单卡尔曼滤波导航装置，其特征在于：包括MEMS传感器、INS模块、VLC定位模块、PDR定位模块和测姿定位单卡尔曼滤波器模块；所述MEMS传感器包括加速度计、陀螺仪以及磁力计；所述测姿定位单卡尔曼滤波器模块的输入包括：加速度计测得的接收器在XYZ方向的加速度信息和陀螺仪测得的接收器在XYZ方向上的角速度信息传递给INS模块，由INS模块对其进行编排得到接收器的INS位置、速度和姿态信息；磁力计测得的接收器相对东、南、西、北方向的角度信息；加速度计测得的接收器在XYZ方向的加速度信息给PDR定位模块进行位置估计得到的PDR定位信息和VLC定位模块输出的VLC定位信息通过加权平均后的位置信息；所述测姿定位单卡尔曼滤波器模块输出当前时刻接收器的位置信息、速度信息和姿态信息给INS模块，输出当前时刻接收器的姿态信息给PDR定位模块或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＝[δrnδvnψbgba]T其中，δrn、δvn、ψ、bg和ba分别是接收器的位置误差矢量、速度误差矢量、姿态误差矢量、陀螺仪偏差矢量和加速度计偏差矢量。5.根据权利要求4所述的一种导航方法，其特征在于，所述步骤(2)包括：(21)将加速度计和陀螺仪采集到的接收器数据信息输入到INS模块进行机械编排算法处理得到接收器当前位置信息、速度信息和姿态信息并输入到S-EKF中；对姿态矩阵进行坐标变换，其坐标转移方程为：其中，为rn的一阶导数，是导航坐标系中的位置向量，表示纬度，λ表示经度和h表示高度；是vn的一阶导数，vn是三维速度向量；gn是导航坐标系中的重力向量；表示位置增量；表示速度增量；fb是载体坐标系中的特定力矢量；D-1是一个关于和h的3×3矩阵；是的一阶导数，是由INS机械编排预测的从载体坐标系到导航坐标系的方向余弦矩阵；和分别为角速度矢量和的斜对称矩阵；而和表示地球自传的角速度和导航坐标系相对于地心地固坐标系的转动角速度；和表示载体坐标系相对于惯性坐标的转动角速度和导航坐标系相对于惯性坐标的转动角速度；(22)A-EKF的系统模型为具体展开公式如下：其中，δ符号表示误差，即真实值和系统标称值之差；和分别表示δrn、δvn、ψ、bg和ba的一阶导数，fn是投影到导航坐标系的特定力矢量；wg和wa是传感器噪声；τbg和τba代表惯导噪声的相关时间；wbg和wba是驱动噪声，符号“×”表示叉乘。6.根据权利要求5所述的一种导航方法，其特征在于，所述步骤(3)包括：(31)PDR或VLC输出的位置观测方程当发生光信号遮挡VLC无法输出有效定位信息的情况时，PDR位置信息作为测姿定位单卡尔曼滤波器模块的输入量；根据PDR位置信息建立的观测方程为：其中和为INS机械编排算出的纬度和经度；和分别为来自PDR的纬度和经度；和nλ是测量噪声；否则，测姿定位单卡尔曼滤波器模块接收VLC位置信息，而忽略PDR位置信息；根据VLC输出信息建立的观测方程为：其中，和分别是来自于INS机械编排和VLC的位置向量；δrn是位置误差矢量；n1是量测噪声；(32)磁力计观测方程滤波器直接通过磁力计读数进行姿态更新，磁力计观测方程如下：其中，是磁力计读数矢量，mn是校准的LMF矢量，n3是噪声。7.根据权...

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