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

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

背景技术：

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

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

另外，在惯导定位中一般采用卡尔曼滤波进行噪声处理和定位，卡尔曼滤波对于线性系统效果较好，而在实际环境中，很多系统是非线性的。扩展卡尔曼滤波是卡尔曼滤波的一种新的形式，它通过泰勒展开后忽略二阶以上的分量从而将非线性系统转化为线性系统。粒子滤波算法则能够处理完全非线性的系统，效果比卡尔曼滤波更好。此外，在惯导定位技术中，多采用单个滤波器来进行噪声处理和定位，然而这样通过单一滤波器同时定位和测姿会造成姿态和位置相互影响。

技术实现要素：

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

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

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

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

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

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

(11)建立a-pf的状态向量；

(12)建立a-pf的系统模型；

(13)建立a-pf的观测模型；

(14)a-pf滤波输出姿态信息；

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

(21)建立l-pf的状态向量；

(22)建立l-pf的系统模型；

(23)建立l-pf的观测模型；

(24)l-pf滤波输出定位信息。

进一步的，所述步骤(11)中a-pf的状态向量定义为：

x＝[ψbg]^t

其中，ψ为姿态误差向量；bg为陀螺偏差向量。

进一步的，所述步骤(12)中先对姿态矩阵进行坐标系变换，其坐标转移方程为：

其中，是从载体坐标系到导航坐标系的方向余弦矩阵，即姿态矩阵；和分别是角速度矢量和的斜对称矩阵；而和表示载体坐标系相对于惯性坐标的转动角速度和导航坐标系相对于惯性坐标的转动角速度；

a-pf的系统模型为具体展开公式如下：

其中，和分别表示地球自传的角速度和导航坐标系相对于地心地固坐标系的转动角速度；wg是传感器噪声；τbg代表惯导噪声的相关时间；wbg是驱动噪声；符号“×”表示叉乘。

进一步的，所述步骤(13)中a-pf的观测模型包括加速度计观测模型和磁力计观测模型，观测模型的表达式为z＝hx+v其中，

加速度计观测模型具体为：

其中，是加速度计读数矢量，fⁿ＝-gⁿ＝[00-g]^t是导航坐标系中的特定力矢量，f^b是载体坐标系中的特定力矢量，g是局部重力值，ψ是姿态误差,i是单位矩阵，n2是噪声，[ψ×]表示ψ矢量的斜对称矩阵，[gⁿ×]表示gⁿ矢量的斜对称矩阵，此时，h＝-[gⁿ×]，

磁力计观测模型具体为：

其中，是磁力计读数矢量，mⁿ是校准的lmf矢量，n3是噪声，h＝[mⁿ×]，

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

(a)将输入量和zk输入a-pf

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

(b)滤波过程

①对于每一个k-1时刻第i个粒子的状态通过s-pf系统方程模型生成k时刻第i个粒子的状态

②对于每个新粒子利用s-pf观测模型来计算每个粒子的权重

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

③归一化权重

其中，ns为粒子个数；

④重采样过程

(c)滤波输出

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

进一步的，所述步骤(21)中l-pf的状态向量定义为：

其中，和λ分别为接收器纬度信息和经度信息。

进一步的，所述步骤(22)中使用pdr定位模块处理加速度计读数获得步数和步长信息，并从a-pf获得方位角信息，然后pdr通过上一时刻的位置接收器方位角和接收器步长来计算当前时刻新的位置其系统模型为：

其中和s是方位角和步长，下标k-1和k代表步数，rm和rn是地球的主垂直线中的子午线和曲率的曲率半径，h是高度。

进一步的，所述步骤(23)中l-pf的观测方程直接使用vlc输出的位置信息建立，观测方程如下：

其中，和分别为pdr预测的纬度和经度，和分别为vlc输出的纬度和经度，和nλ是测量噪声。

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

(a)将输入量和zk输入l-pf

其中，为k-1时刻的粒子，表示k-1时刻的第i个粒子的状态；表示k-1时刻第i个粒子的权重，zk为k时刻的观测值，即vlc定位系统得到位置信息；其中粒子状态向量

(b)滤波过程

①对于每一个k-1时刻的粒子通过将pdr位置信息作为s-pf系统方程模型生成新粒子

②对于每个新粒子利用s-pf观测模型来计算每个粒子的权重

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

③归一化权重

其中，ns为粒子个数；

④重采样过程

(c)滤波输出

输出k时刻的粒子输出接收器k时刻的定位信息。

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

附图说明

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

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

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

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

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

具体实施方式

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

如图1所示，基于mems传感器和vlc定位融合的双粒子滤波器导航装置，其特征在于：包括mems传感器、惯性导航系统(inertialnavigationsystem，ins)模块、行人航位推算(pedestriandeadreckoning，pdr)定位模块、可见光通信(visiblelightcommunication，vlc)定位模块、测姿扩展卡尔曼滤波器(attitudeextendedkalmanfilter，a-ekf)和定位扩展卡尔曼滤波器(locationextendedkalmanfilter，l-ekf)；所述mems传感器包括加速度计、陀螺仪以及磁力计。

陀螺仪测得的接收器在xyz方向的角速度信息传递给ins模块，通过陀螺机械编排算法处理数据得到ins姿态信息。加速度计测得的接收器在xyz方向的加速度信息传递给pdr定位模块进行位置估计。另一方面，加速度计测得的接收器在xyz方向的加速度信息、磁力计测得的接收器相对东南西北方向的角度信息与ins姿态信息三者作为测姿扩展卡尔曼滤波器的输入量。测姿扩展卡尔曼滤波器输出包含姿态误差向量和陀螺偏差向量。陀螺偏差向量反馈给陀螺仪进行噪声补偿，而姿态误差向量传递给vlc定位模块和pdr定位模块帮助定位。vlc定位模块和pdr定位模块通过各自的定位算法分别输出接收器位置信息给定位扩展卡尔曼滤波器。定位扩展卡尔曼滤波器会持续更新状态量并输出接收器的定位信息(位置、速度)。

系统中有两点需要注意：对于a-pf，磁力计和加速计的读数会按时间轴顺序参与到测姿扩展卡尔曼滤波器的观测方程中，并非同时参与滤波；对于l-pf，从pdr定位模块获得的位置信息作为定位扩展卡尔曼滤波器预测和更新的状态量，而从vlc定位模块获得的位置信息作为观测量参与到定位扩展卡尔曼滤波器中。

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

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

本发明基于ins惯导机械编排的误差方程作为融合滤波器的系统方程，观测方程包括vlc定位信息更新、pdr定位信息更新和磁力计观测量更新。融合滤波器输出vlc接收器的姿态信息给vlc定位模块，输出pdr接收器的姿态信息给pdr定位模块以校正姿态的影响。

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

步骤1：估计接收器姿态信息:

a-pf主要用来估计接收器的姿态信息(即：横滚角、俯仰角和方位角)。

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

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

x＝[ψbg]^t(1)

其中：ψ为姿态误差向量；bg为陀螺偏差向量。

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

先对姿态矩阵进行坐标系变换，其坐标转移方程为：

其中，是从载体坐标系(b-frame)到导航坐标系(n-frame)的方向余弦矩阵；和分别是角速度矢量和的斜对称矩阵；而和表示载体坐标系(b-frame)相对于惯性坐标(i-frame)的转动角速度和导航坐标系(n-frame)相对于惯性坐标(i-frame)的转动角速度。

a-pf的系统模型为具体展开公式如下：

其中，和表示地球自传的角速度和导航坐标系(n-frame)相对于地心地固坐标系(ecef坐标系，e-frame)的转动角速度；wg是传感器噪声；τbg代表惯导噪声的相关时间；wbg是驱动噪声；符号“×”表示叉乘。

(3)建立a-pf的观测模型

a-pf的观测方程直接使用加速度计和磁力计读数建立。本发明通过使用加速度计读数来构建观测模型，而不是应用加速度计导出的横滚角和俯仰角。这个选择对于具有任意接收器姿态的行人应用是重要的，因为它避免了当俯仰角达到±90°时的奇点问题。

a-pf的观测模型包括加速度计观测模型和磁力计观测模型，观测模型的表达式为z＝hx+v其中：

加速度计观测模型具体为：

其中，是加速度计读数矢量，fⁿ＝-gⁿ＝[00-g]^t是导航坐标系中的特定力矢量，f^b是载体坐标系中的特定力矢量，g是局部重力值，ψ是姿态误差,i是单位矩阵，n2是噪声，[ψ×]表示ψ矢量的斜对称矩阵，[gⁿ×]表示gⁿ矢量的斜对称矩阵，此时h＝-[gⁿ×]，

a-pf使用磁力计读数建立观测模型的主要挑战在于存在频繁的磁扰动。典型类型的磁扰动是局部磁场(localmagneticfield,lmf)的方向和强度都改变，但是该改变在有限的空间(或时间段)内是稳定的。lmf稳定的周期可以称为准静态磁场(quasi-staticmagneticfield,qsmf)周期，并且可以通过使用磁力计读数的大小来检测。本发明所使用的算法将使用磁力计在qsmf期间的观测值，假设完全不知道lmf参数，并通过算法在每个qsmf周期的开始阶段校准lmf。第k个qsmf周期期间的lmf向量下列公式计算：

其中，是在第k个qsmf周期的第一个时期的开始处的磁力计读数，然后将计算的用作第k个qsmf周期期间的参考。本发明直接使用磁力计读数建立观测模型，该策略避免了调平步骤(即：使用加速度计读数来计算横滚角和俯仰角)。因此，下面列出的磁力计测量模型与加速度计测量值无关。

磁力计观测模型具体为：

其中，是磁力计读数矢量，mⁿ是校准的lmf矢量，n3是噪声，此时，h＝[mⁿ×]，

其中，由于加速度计和磁力计从测量到返回读数不一定同步，且和二者的读数频率有关，所以二者作为输入量在时间上是错开的。同时a-ekf滤波器在一个时刻只能接收一个观测方程，即两个观测方程根据时间轴顺序参与到算法中。

(4)a-pf滤波输出姿态信息；

(a)将输入量和zk输入a-pf

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

(b)滤波过程

①对于每一个k-1时刻第i个粒子的状态通过s-pf系统方程模型生成k时刻第i个粒子的状态

②对于每个新粒子利用s-pf观测模型来计算每个粒子的权重

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

③归一化权重

其中，ns为粒子个数；

④重采样过程

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

(c)滤波输出

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

步骤2：估计接收器位置信息

l-pf主要用来估计接收器的二维位置信息(即：纬度和经度)。图1显示l-pf的输入为1)pdr得出的位置信息和2)vlc得出的位置信息；输出为接收器的位置信息。

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

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

其中，和λ分别为接收器纬度信息和经度信息。

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

使用行人航位推算(pdr)定位模块处理加速度计读数获得步数和步长信息，并从l-pf获得方位角信息，然后pdr通过上一时刻的位置接收器方位角和接收器步长来计算当前时刻新的位置其系统模型为：

其中和是k-1时刻的方位角，sk是k时刻的步长，下标k-1和k代表步数，rm和rn分别是是地球的主垂直线中的子午线和曲率的曲率半径，h是高度。

(3)建立l-pf的观测模型

l-pf的观测方程直接使用vlc输出的位置信息建立，观测方程如下：

其中，和分别为pdr预测的纬度和经度，和分别为vlc输出的纬度和经度，和nλ是测量噪声。

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

(a)将输入量和zk输入l-pf

其中，为k-1时刻的粒子，表示k-1时刻的第i个粒子的状态；表示k-1时刻第i个粒子的权重，zk为k时刻的观测值，即vlc定位系统得到位置信息；其中粒子状态向量

(b)滤波过程

①对于每一个k-1时刻的粒子通过将pdr位置信息作为s-pf系统方程模型生成新粒子

②对于每个新粒子利用s-pf观测模型来计算每个粒子的权重

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

③归一化权重

其中，ns为粒子个数；

④重采样过程

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

(c)滤波输出

输出k时刻的粒子输出接收器k时刻的定位信息(位置信息和速度信息)。

技术原理：

在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模块的定位结果进行弥补。

单滤波器结构姿态和位置误差间相互影响：由于滤波器中的系统模型利用了惯导定位的位置预测公式。而在惯导中，速度误差矢量的状态转移包含姿态变量，位置误差矢量的状态转移又和速度相关。因此，当采用单滤波器的架构时，姿态和位置信息同时预测估计，必然会造成姿态的预测估计误差影响位置信息的预测。所以本发明通过采用双滤波器架构将姿态和位置的预测估计过程解耦，避免两者之间相互影响。