基于MEMS传感器和VLC定位融合的单粒子滤波导航装置和方法与流程

本发明涉及智能定位装置及方法，特别是涉及基于mems传感器和vlc定位融合的单粒子滤波导航装置和方法。

背景技术：

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

然而，由于目标接收设备的姿态会随着目标移动产生抖动，将对vlc定位结果带来较大的影响。另一方面，实际场景中光信号容易被遮挡，将会导致定位不连续。针对前一个问题，目前的方案主要是通过多传感器组合共同定位。针对后一个问题，主流的解决方案是通过卡尔曼滤波或者粒子滤波来估计目标位置。但这些方案存在一些问题：1)相比于单传感器定位，多传感器组合定位算法复杂且成本较高；2)目前定位方案中提出的滤波器融合是以探测器姿态平稳为前提，在实际场景中稳定性较差。3)在信号被遮挡的情况频繁发生的场景中，仅通过vlc数据加滤波器的定位系统推测的目标位置仍然与实际位置有较大偏差。

另外，在惯导定位中一般采用卡尔曼滤波进行噪声处理和定位，卡尔曼滤波对于线性系统效果较好，而在实际环境中，很多系统是非线性的。扩展卡尔曼滤波是卡尔曼滤波的一种新的形式，它通过泰勒展开后忽略二阶以上的分量从而将非线性系统转化为线性系统。粒子滤波算法则能够处理完全非线性的系统，效果比卡尔曼滤波更好。

技术实现要素：

发明目的：为解决现有技术的不足，提供一种可消除姿态对vlc定位的影响，可弥补vlc定位结果不连续、不平滑的确定的基于mems传感器和vlc定位融合的测姿定位单粒子滤波导航装置和方法。

技术方案：基于mems传感器和vlc定位融合的单粒子滤波导航装置，包括mems传感器、ins模块、vlc定位模块、pdr定位模块和测姿定位单粒子滤波器模块；所述mems传感器包括加速度计、陀螺仪以及磁力计；

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

所述测姿定位单粒子滤波器模块输出当前时刻接收器的位置信息、速度信息和姿态信息给ins机械编排模块，输出当前时刻接收器的姿态信息给pdr定位模块或vlc定位模块，输出噪声补偿信号反馈给加速度计和陀螺仪，同时输出当前时刻接收器的定位信息。

进一步的，所述pdr定位信息和vlc定位信息的权重由vlc定位模块中光信号是否被遮挡来设定，若光信号被遮挡时系统将vlc位置信息的权重设为0，只采用pdr位置信息。

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

(1)建立s-pf的状态向量；

(2)建立s-pf的系统模型；

(3)建立s-pf的观测方程；

(4)s-pf滤波输出定位信息。

进一步的，所述步骤(1)中s-pf的状态向量为：

x＝[δrⁿδvⁿψbgba]^t

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

进一步的，所述步骤(2)包括：

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

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

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

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

进一步的，所述步骤(3)包括：

(31)pdr或vlc输出的位置观测方程

当发生光信号遮挡vlc无法输出有效定位信息的情况时，pdr位置信息作为测姿定位单粒子滤波器模块的输入量；根据pdr位置信息建立的观测方程为：

其中和为ins模块机械编排算出的纬度和经度；和分别为来自pdr的纬度和经度；和nλ是测量噪声；

否则，测姿定位单粒子滤波器模块接收vlc位置信息，而忽略pdr位置信息；根据vlc输出信息建立的观测方程为：

其中，和分别为ins模块机械编排位置向量和vlc定位模块得到的位置向量；δrⁿ是位置误差矢量；n1是量测噪声；

(32)磁力计观测方程

滤波器直接通过磁力计读数进行姿态更新，磁力计观测方程如下：

其中，是磁力计读数矢量，mⁿ是校准的lmf矢量，n3是噪声。

进一步的，所述步骤(4)包括：

(41)将输入量和zk输入s-pf

其中，为k-1时刻的粒子，表示k-1时刻的第i个粒子的状态；表示k-1时刻第i个粒子的权重，zk为k时刻的观测值；粒子状态量x＝[δrⁿδvⁿψbgba]^t；

(42)滤波过程

(a)对于每一个k-1时刻第i个粒子的状态通过系统方程模型(2)生成k时刻第i个粒子的状态

(b)对于每个新粒子利用pdr或vlc观测方程和磁力计观测方程来计算每个粒子的权重

其中，表示k时刻观测量zk对于粒子的条件概率；

(c)归一化权重

其中，ns为粒子个数。

(d)重采样过程

(43)滤波输出

输出k时刻的粒子并将k时刻的粒子状态信息中位置信息、速度信息和姿态信息反馈给ins模块，输出vlc接收器的姿态信息给vlc定位模块，输出pdr接收器的姿态信息给pdr定位模块，以矫正姿态的影响，输出陀螺偏差矢量反馈给陀螺仪用以噪声补偿，输出接收器定位信息。

有益效果：与现有技术相比，本发明具有以下优点：1)使用测姿定位单粒子滤波器首次在vlc定位领域使用融合测姿准确估计vlc接收器的姿态信息，并消除姿态对vlc定位的影响；2)该设计可弥补vlc定位结果不连续、不平滑的缺点；3)该设计可在vlc信号被遮挡的情况下使用mems传感器信息提供定位结果。

附图说明

图1是测姿定位单粒子滤波器导航装置结构示意图；

图2是俯仰角0°的仿真定位结果cdf曲线；

图3是俯仰角5°的仿真定位结果cdf曲线；

图4是俯仰角8°的仿真定位结果cdf曲线；

图5是接收设备平放、倾斜5°、倾斜8°时的定位误差分析结果。

具体实施方式

下面结合附图对本发明的技术方案进行详细的说明。

如图1所示，基于mems(micro-electromechanicalsystems，微型机电系统)传感器和vlc定位融合的测姿定位单粒子滤波器导航装置包括：mems传感器、惯性导航系统(inertialnavigationsystem，ins)模块、可见光通信(visiblelightcommunication，vlc)定位模块、行人航位推算(pedestriandeadreckoning，pdr)定位模块和测姿定位单粒子滤波器(singleparticlefilter，s-pf)模块，其中mems传感器包括加速度计、陀螺仪和磁力计。

加速度计测量接收器在xyz方向的加速度信号一路传递给pdr定位模块进行位置估计，另一路传递给ins机械编排模块，陀螺仪测量的接收器在xyz方向的角速度信号传递给ins机械编排模块。ins机械编排模块通过机械编排算法对加速度信号和角速度信号进行机械编排，得到ins位置信息、速度信息和姿态信息。pdr定位模块对加速度信号进行位置估计，得到pdr位置信息。磁力计测量接收器相对东、南、西、北方向的角度信息，vlc定位模块测到接收器的位置信息。

另外，vlc定位模块和pdr定位模块虽然同时进行定位，但各自的定位结果并不是等价输入到测姿定位单粒子滤波器模块中。一般情况下，测姿定位单粒子滤波器模块主要接受vlc定位模块的位置信息，而忽略pdr定位模块的位置信息。只有当发生光信号遮挡等vlc定位模块无法输出有效定位信息的情况时，pdr模块的位置信息才会作为主要的输入量。

即，将pdr定位模块的位置信息和vlc定位模块的位置信息通过加权平均后的位置信息作为唯一的位置信息输入到测姿定位单粒子滤波器模块，当vlc定位模块中光信号被遮挡时系统将vlc定位模块的位置信息的权重设为0，只采用pdr模块的位置信息。

pdr位置信息、vlc位置信息、磁力计测得的角度信息以及ins位置信息、速度信息和姿态信息一起作为输入量输入到测姿定位单粒子滤波器模块，经过滤波器处理输出当前时刻接收器的位置信息、速度信息和姿态信息反馈给ins机械编排模块，输出当前时刻接收器的姿态信息反馈给pdr模块或vlc定位模块，输出陀螺仪偏差矢量反馈给陀螺仪，输出加速度计偏差矢量反馈给加速度计，并输出当前时刻接收器的定位信息，同时更新下一时刻的状态量。

测姿定位测姿定位单粒子滤波器(singleparticlefilter，s-pf)的重点在于状态向量、系统方程、状态方程的建模。s-pf主要用来估计接收器的姿态信息(即：横滚角、俯仰角和方位角)和位置信息(即：经度、纬度和高度)。

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

(1)建立s-pf的状态向量

s-pf的状态向量定义为：

x＝[δrⁿδvⁿψbgba]^t(1)

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

(2)建立s-pf的系统模型

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

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

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

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

(3)建立s-pf的观测方程

s-pf的观测方程由两部分组成：1)pdr或vlc输出的位置观测方程2)磁力计观测方程。需要注意的是，应当根据采用pdr位置信息作为输入或vlc作为输入而采用不同的观测方程。

(31)pdr或vlc输出的位置观测方程

当发生光信号遮挡vlc无法输出有效定位信息的情况时，pdr的位置信息作为测姿定位单粒子滤波器模块主要的输入量；根据pdr位置建立的观测方程为：

其中和为ins机械编排算出的纬度和经度；和来自pdr的纬度和经度；和nλ是测量噪声。

否则，测姿定位单粒子滤波器模块主要接收vlc的位置信息，而忽略pdr的定位信息；根据vlc输出信息建立的观测方程为：

其中，和分别是来自于ins机械编排和vlc的位置向量；δrⁿ是位置误差矢量；n1是量测噪声。

所述pdr定位信息和vlc定位信息的权重由vlc定位模块中光信号是否被遮挡来设定，若光信号被遮挡时系统将vlc位置信息的权重设为0，只采用pdr位置信息。

(32)磁力计观测方程

滤波器直接通过磁力计读数进行姿态更新，磁力计观测方程如下：

其中，是磁力计读数矢量，mⁿ是校准的lmf矢量，n3是噪声。

(4)s-pf滤波实现流程

(41)将输入量和zk输入s-pf

其中，为k-1时刻的粒子，表示k-1时刻的第i个粒子的状态；表示k-1时刻第i个粒子的权重，zk为k时刻的观测值；粒子状态量x＝[δrⁿδvⁿψbgba]^t；

s-pf的初始输入为权重ω相同的n个粒子x，粒子数量n的值可自由设置。

(42)滤波过程

(a)对于每一个k-1时刻第i个粒子的状态通过系统方程模型(2)生成k时刻第i个粒子的状态

(b)对于每个新粒子利用观测方程(4-6)来计算每个粒子的权重

其中，表示k时刻观测量zk对于粒子的条件概率；

(c)归一化权重

(d)重采样过程

由于在滤波过程中，部分状态量相近的粒子的权重会逐渐增大并趋于相同，其余大多数粒子权重会逐渐减小，导致粒子多样性缺失，会使状态估计产生较大偏差，这就是粒子退化问题。为了解决该问题，一般在粒子滤波中引入重采样过程。重采样过程有重要性重采样、残差重采样、分层重采样和优化组合重采样等，任选其一即可。如重要性重采样为例，该过程将权重大的粒子按其权值复制出相应数量的副本，而淘汰权重小的粒子，但总体数量保持不变。

(43)滤波输出

输出k时刻的粒子并将k时刻的粒子状态信息中位置信息、速度信息和姿态信息反馈给ins模块，输出vlc接收器的姿态信息给vlc定位模块，输出pdr接收器的姿态信息给pdr定位模块，以矫正姿态的影响。输出陀螺偏差矢量反馈给陀螺仪用以噪声补偿。接收器定位信息(位置信息和速度信息)会向外输出。

该融合滤波器首次在vlc定位领域使用融合测姿准确估计vlc接收器的姿态信息，并消除姿态对vlc定位的影响。基于ins惯导机械编排的误差方程作为融合滤波器的系统方程，用于对粒子群的粒子进行更新。而观测方程则包括vlc定位信息更新、pdr定位信息更新和磁力计观测量更新，用于计算各个粒子的权重。融合滤波器输出vlc接收设备的姿态给vlc定位模块，输出pdr设备的姿态给pdr定位模块以校正姿态的影响。

技术原理：

在vlc定位模块中，定位算法一般采用三边定位算法。其原理是利用室内顶端的多盏灯作为发射源，一般至少有三盏led灯，通过接收器实时测得的光信号强度代入光信号传播模型估计出目标点到每盏灯的距离，最后通过联立方程估算出定位位置。然而，当光信号被遮挡时，探测器无法获取全部信号，求解出的位置会严重超出误差范围，这种情况会通过阈值进行排除，所以系统在该情况下不输出位置信息，导致定位结果不连续、不平滑。另外，考虑到vlc系统往往存在散粒噪声和热噪声等随机噪声，距离的估计往往存在一定误差，因此估得的位置形成的运动轨迹会存在陡峭变化等特征，这往往不符合实际目标的运动特征。由于在目标跟踪时，先验知识应当表示定位轨迹平滑，目标当前时刻的状态与上一时刻的状态相关，而滤波方法可以将先验值考虑进来，从而使得定位轨迹更加平滑。

目前一般的vlc定位往往假设设备是平放姿态，从而简化了定位算法。然而在实际情况中设备会产生抖动、倾斜。设备的随机倾斜会改变光信号的入射角，从而导致测得的光信号强度发生变化。如果无法准确估计设备的姿态，这部分的光信号强度变化将成为系统误差的一部分，导致定位误差变大。为了说明姿态误差对于vlc定位精度的影响程度，我们通过对比仿真了不同俯仰角的情况下vlc定位的精度。该仿真实验对接收器发生倾斜，但定位算法不校正的情况进行了模拟，对所有接收器仿真了两种不同的倾斜情况，定位结果cdf曲线分别如图3和图4所示，图2表示设备未发生倾斜的定位结果。实验结果显示接收器俯仰角为5°和8°时最大定位误差分别约为0.13m和0.21m。对比接收器倾斜和平放时的定位结果，如图5所示，可以看出当接收器发生了倾斜，如果不对接收器姿态进行修正，定位误差会显著的增大。该仿真结果证明准确估计接收器的姿态对提高可见光定位系统的性能具有重要意义。因此，引入mems姿态估计模块，将准确的姿态信息传递给vlc定位模块进行姿态校准，能够显著的减小姿态对vlc定位结果的影响。

由于光信号在被遮挡的情况下vlc定位无法进行，因此引入pdr定位模块进行定位。pdr定位可以完全利用mems传感器信号来计算步长和方向，利用行人上一时刻的位置推测出行人当前的位置。通过反复迭代，即可得到行人的运动轨迹。因此，当vlc定位发生信号遮挡时，无法输出位置结果，融合系统会采用pdr定位模块的定位结果进行弥补。