一种基于优化卡尔曼滤波的非视距室内定位方法与流程

本发明涉及室内定位中非视距情况领域，具体是一种基于优化卡尔曼滤波的非视距室内定位方法。

背景技术：

随着物联网技术的广泛应用，人们对于基于位置的服务(locationbasedservice,lbs)，尤其是室内位置服务(indoorlbs,ilbs)出现了爆发式的需求。虽然卫星定位系统已经十分完善，但卫星信号在室内急剧衰减，同时受到室内复杂环境的影响，对室内定位准确性要求更高，因此现有的卫星导航系统完全不能满足室内定位的需求。

近年来，室内定位逐渐引起学界和业界的关注，人们日益增长的定位服务需求也促使该领域的兴起，超市自动导购、停车场自动寻路、机器人自动分拣快递、工厂监狱等特殊场所的人员管理等等领域都离不开高精度的室内定位服务。室内定位技术基于定位原理大致分为三类，分别是基于测距、基于信号指纹和基于地图构建，目前市面上的室内定位有些是单一技术，有些是混合了几种定位技术，形成多模态定位。超声波室内定位是基于到达时间差的测距技术，利用超声波到达接收器的时间推算出发射点的坐标。然而，超声波信号在室内有一定局限性，具体表现在容易受墙面的吸收衰减和漫反射造成的多径效应，定位锚点时间不同步，以及非视距情况下的接收信号不稳定造成定位结果漂移的情况。tdoa算法结合卡尔曼滤波器可以有效的减少以上不利因素造成的衰减，通过两次加权最小二乘迭代的tdoa算法可以消除时间同步问题和强弱混合的多径效应问题，卡尔曼滤波器则可以平滑滤掉定位误差漂移过大的点。

目前应用在室内定位的卡尔曼滤波器，在非视距情况下虽然初始误差可以有效消除，但仍避免不了多次非视距测量的累计误差，此累计误差过大，也会造成定位不准，偏离正常物体运动轨迹，定位的有效性大大降低。

因此，在非视距室内定位的研究工作中，如何优化卡尔曼滤波器来减少累计误差，提高连续定位的精确度，成为本发明要解决的问题。

技术实现要素：

针对上述现有技术的不足，本发明提供了一种基于优化卡尔曼滤波的非视距室内定位方法，该方法用于解决减少多次非视距情况的累计误差，进一步提高复杂室内情况下的定位精度。

实现本发明目的的技术方案是：选取超声波信号作为媒介，采取被动式定位，降低超声波波束角过小的影响；采用tdoa算法处理接收信号，避免了各个锚点时间不同步的问题，采用优化卡尔曼滤波器平滑滤波，在消除过大信号漂移的同时，也有效的减少了累计误差，实现非视距室内复杂环境下的连续精准定位。其方法包括如下步骤：

(1)配置超声波定位系统：建议室内定位模型，采取超声波作为载体，被动式定位方式，在定位锚点bsi安放超声波发射装置，待定位节点p安放超声波接收装置；

(2)采用tdoa算法处理接收信号：采取tdoa算法δτ＝τi-τ1，其中，bs1为tdoa基准锚点，假设bs1无故障，利用两次wls迭代计算出tdoa值坐标；

(3)采用优化卡尔曼滤波器减小连续定位误差：根据步骤(2)所得定位数据和系统参数，建立卡尔曼滤波模型，利用时间更新过程和状态更新过程得到tdoa优化值，根据增益矩阵kt对更新值正反馈，kt的求解公式为

(4)上传连续定位数据：将经过处理后的精确定位结果呈现出来。

步骤(1)所述的超声波室内定位系统，是选取20khz左右的超声波，既考虑到室内日常声信号的干扰，又顾及了超高频超声波成本过高的问题。采用被动定位方式，是由于超声波有着波束角小方向性强的特点，若采取主动定位时，容易造成多个固定锚点无法有效接收初次超声波信号，从而受多径效应影响的问题。

步骤(2)所应用的tdoa算法，其实质上是一种双曲线定位方法，算法原理为：若伪超声波在待定位节点p和各锚点bsi之间按直线传播，并且已经测得伪超声波从p到bsi的传播距离与p到基准锚点(设定为bs1)的传播距离差，则p应位于分别以bsi和bs1作为焦点的一组双曲线交点上。tdoa定位算法至少需要3个以上的定位锚点参与，并且要保障基准锚点无故障。

对于tdoa测量，本发明采用chan算法进行求解。chan算法是一种具有解析表达式解的非递归双曲线方程组解法，其优点是计算量小，在噪声服从高斯分布的环境下定位精度高。该方法通过两次加权最小二乘迭代对定位预测估值达到最优。算法表达式如下：

其中，待定位节点坐标p(x0，y0，z0),基准锚点bs1(x1，y1，z1),其余各锚点bsi(xi，yi)(i≠1)。该表达式的最大似然估计为其中，

ψ为误差矢量。此结果计算出未知节点的最大似然估计解，但其中ψ仍是未知量。再次对此式进行wls计算，得到下式：

根据上式得到，待定位节点坐标p(x0，y0，z0)为

此算法能够利用超声波传感器系统所提供的所有tdoa测量值，因此能降低个别较大的随机误差的影响，取得相对不错的定位结果。在对最大似然估计上，进行了两次加权最小二乘估计，得到了明确的表达式解，在tdoa测量高斯白噪声是基本能够到达克拉美罗下界。然而该算法的求解过程中，假定了高斯白随机噪声，实际环境的非均匀噪声甚至非视距情况下，单单使用此算法，室内定位准确性会显著下降。

步骤(3)中所应用优化卡尔曼滤波器，是将步骤(2)所得结果结果进行有效滤波，当待定位节点和测量锚点处于非视距情况时，tdoa算法无法进行有效定位计算，而卡尔曼滤波器，将同频带的信号和噪声，在线性系统的状态空间基础上，利用输出和输入数据进行系统状态的最优估计，在室内定位的非视距条件下，标准卡尔曼滤波器可以通过tdoa计算值、系统噪声和预测值，来平滑滤除偏差过大的漂移坐标。卡尔曼滤波器的标准方程如下：

zt＝azt-1+bωt-1+εt-1

上式为状态方程，zt为t时刻的状态值，ωt为l维tdoa系统控制向量，εt为系统过程激励噪声。a为n×n阶的增益方阵，将t-1时刻的测量值和t时刻的测量值联系一起；b为n×1阶的控制增益矩阵，无特殊情况下一般为零矩阵。

下式为观测方程，为系统的观测噪声，一般为期望为零的白噪声向量。h为状态变量zt对观测变量μt的增益：

本发明提出的优化卡尔曼滤波器，在传统方法基础上，增加了经验阈值ρ的判定，通过计算残差矩阵的奇异值和经验阈值比较来判定是否累积了过大的漂移误差。该优化过程可以看做惯性优化，即非视距累计误差随着惯性逐渐增大时，经卡尔曼滤波器滤波的定位值也会逐渐偏离实际值，此时便产生了较大的残余奇异值。当残余奇异值最小值超过了经验阈值时，本发明认定此时定位结果非常不准确，应予以校准，使计算值回归正常范围内。

本发明优化了卡尔曼滤波器在非视距情况下的累计误差，卡尔曼滤波器的后验估计值公式如下：

其中，卡尔曼增益矩阵为kt＝p′th^t(hp′th^t+r)^-1。r为观察噪声的协方差矩阵，一般为常数。为残差，即真实值和估计值的差值。当残差值为零时，后验结果和先验结果相同，此系统无误差。本发明在此计算每一个时刻的残差奇异值，将奇异值最大值与经验阈值相比较，判定是否累计误差过大。经验阈值ρ表达式如下：

其中，λ为测距误差阈值，δ为超声波定位系统误差阈值，此二值在配置室内定位系统时应提前测量。当残差奇异值的最大值大于或等于经验阈值ρ，代表累计误差已经影响了测量值的准确性。此时将增益矩阵kt置零，其余操作不变，进行第t+1次自循环滤波。

本发明的有益效果是：

1.本发明首次提出了适用于超声波室内定位技术的优化卡尔曼滤波器，针对室内环境噪声参数和超声波系统参数，对算法进行有效优化。

2.本发明首次引入了一种计算残差奇异值最大值来判断多次非视距定位偏移过大的方法，使得复杂室内环境的连续定位精确度得到保障。

附图说明

图1为本发明的超声波室内定位数据处理流程图。

具体实施方式

下面结合附图对本发明进一步地详细说明。

如图1所示，本发明一种基于优化卡尔曼滤波的非视距室内定位解决方法，该方法包括如下步骤:

步骤1：配置超声波定位系统，建立室内定位模型，采取超声波作为载体，被动式定位方式，在定位锚点bsi安放超声波发射装置，待定位节点p安放超声波接收装置。

选取20khz左右的超声波，既考虑到室内日常声信号的干扰，又顾及了超高频超声波成本过高的问题。采用被动定位方式，是由于超声波有着波束角小方向性强的特点，若采取主动定位时，容易造成多个固定锚点无法有效接收初次超声波信号，从而受多径效应影响的问题；

步骤2：采用tdoa算法处理接收信号，采取tdoa算法δτ＝τi-τ1,其中，bs1为tdoa基准锚点，假设bs1无故障，利用两次wls迭代计算出tdoa值坐标；

所应用的tdoa算法，其实质上是一种双曲线定位方法，算法原理为：若伪超声波在待定位节点p和各锚点bsi之间按直线传播，并且已经测得伪超声波从p到bsi的传播距离与p到基准锚点(设定为bs1)的传播距离差，则p应位于分别以bsi和bs1作为焦点的一组双曲线交点上。tdoa定位算法至少需要3个以上的定位锚点参与，并且要保障基准锚点无故障。

对于tdoa测量，本发明采用chan算法进行求解。chan算法是一种具有解析表达式解的非递归双曲线方程组解法，其优点是计算量小，在噪声服从高斯分布的环境下定位精度高。该方法通过两次加权最小二乘迭代对定位预测估值达到最优。算法表达式如下：

其中，待定位节点坐标p(x0，y0，z0),基准锚点bs1(x1，y1，z1),其余各锚点bsi(xi，yi)(i≠1)。该表达式的最大似然估计为其中，

ψ为误差矢量。此结果计算出未知节点的最大似然估计解，但其中ψ仍是未知量。再次对此式进行wls计算，得到下式：

根据上式得到，待定位节点坐标p(x0，y0，z0)为

此算法能够利用超声波传感器系统所提供的所有tdoa测量值，因此能降低个别较大的随机误差的影响，取得相对不错的定位结果。在对最大似然估计上，进行了两次加权最小二乘估计，得到了明确的表达式解，在tdoa测量高斯白噪声是基本能够到达克拉美罗下界。然而该算法的求解过程中，假定了高斯白随机噪声，实际环境的非均匀噪声甚至非视距情况下，单单使用此算法，室内定位准确性会显著下降。

步骤3：采用优化卡尔曼滤波器减小连续定位误差，根据步骤2所得定位数据和系统参数，建立卡尔曼滤波模型，利用时间更新过程和状态更新过程来得到tdoa优化值，根据增益矩阵kt对更新值正反馈，kt的求解公式为

kt＝p′th^t(hp′th^t+r)^-1

由于在非视距情况信号的不稳定造成过大漂移情况，传统卡尔曼滤波器的平滑滤波过程也会逐渐积累较大误差，本发明在该情形下求解增益矩阵kt之前，先进行经验阈值ρ的判定。当残差奇异值最大值大于经验阈值，则将增益矩阵kt置零并进入第t+1次自循环中，否则不进行额外操作。

当待定位节点和测量锚点处于非视距情况时，tdoa算法无法进行有效定位计算。而卡尔曼滤波器，将同频带的信号和噪声，在线性系统的状态空间基础上，利用输出和输入数据进行系统状态的最优估计。在室内定位的非视距条件下，标准卡尔曼滤波器可以通过tdoa计算值、系统噪声和预测值，来平滑滤除偏差过大的漂移坐标。卡尔曼滤波器的标准方程如下：

zt＝azt-1+bωt-1+εt-1

上式为状态方程，zt为t时刻的状态值，ωt为l维tdoa系统控制向量，εt为系统过程激励噪声。a为n×n阶的增益方阵，将t-1时刻的测量值和t时刻的测量值联系一起；b为n×1阶的控制增益矩阵，无特殊情况下一般为零矩阵。

下式为观测方程，为系统的观测噪声，一般为期望为零的白噪声向量。h为状态变量zt对观测变量μt的增益：

本发明提出的优化卡尔曼滤波器，在传统方法基础上，增加了经验阈值ρ的判定，通过计算残差矩阵的奇异值和经验阈值比较来判定是否累积了过大的漂移误差。该优化过程可以看做惯性优化，即非视距累计误差随着惯性逐渐增大时，经卡尔曼滤波器滤波的定位值也会逐渐偏离实际值，此时便产生了较大的残余奇异值。当残余奇异值最小值超过了经验阈值时，本发明认定此时定位结果非常不准确，应予以校准，使计算值回归正常范围内。

本发明优化了卡尔曼滤波器在非视距情况下的累计误差，卡尔曼滤波器的后验估计值公式如下：

其中，卡尔曼增益矩阵为kt＝p′th^t(hp′th^t+r)^-1。r为观察噪声的协方差矩阵，一般为常数。为残差，即真实值和估计值的差值。当残差值为零时，后验结果和先验结果相同，此系统无误差。本发明在此计算每一个时刻的残差奇异值，将奇异值最大值与经验阈值相比较，判定是否累计误差过大。经验阈值ρ表达式如下：

其中，λ为测距误差阈值，δ为超声波定位系统误差阈值，此二值在配置室内定位系统时应提前测量。当残差奇异值的最大值大于或等于经验阈值ρ，代表累计误差已经影响了测量值的准确性。此时将增益矩阵kt置零，其余操作不变，进行第t+1次自循环滤波。

步骤4：上传连续定位数据

将经过处理后的精确定位结果呈现出来。

本发明不限于上述实施例，一切采用等同替换或等效替换形成的技术方案均属于本发明要求保护的范围。