一种基于粒子滤波和卡尔曼滤波的蓝牙定位方法与流程

本申请属于无线电定位领域，具体涉及一种基于粒子滤波和卡尔曼滤波的蓝牙定位方法。

背景技术：

物联网(iot)技术的发展，使得室内定位技术获得广泛关注，而在室内环境中，全球导航卫星系统(gnss)的定位信号微弱，不能用于室内定位，由此衍生出gnss之外的室内定位技术。

目前蓝牙室内定位现有的方法有很多种，例如三边定位法和滤波结合的方式。三边定位算法就是通过标签与三个信标的距离，运用三边几何原理确定目标的位置。滤波算法常规有均值滤波、高斯滤波、粒子滤波、卡尔曼滤波等，通过滤波算法对目标的位置进行修正，以达到提高定位精度的效果。目前市面上蓝牙定位精度的效果是3～5米。

在实际应用中，三边定位算法结合滤波算法的方式存在缺陷，即三边定位法定位误差大，即使后期采用滤波算法进行修正，最终得出的定位效果也不理想。这是由于三边定位算法依赖于蓝牙rssi数值得出标签与信标的距离，而rssi非常容易受到环境因素和非环境因素的影响。事实上，即使固定标签于同一位置，其接收的rssi也会出现非常大的浮动。因此，采用三边定位算法很容易出现定位误差超出5米的结果，也会出现多个解的情况，即以信标为原点，距离为半径的三个圆会出现多个交点，甚至还会出现无解的情况，即三个圆没有任何交点。

滤波算法修正的基础是三边定位法的定位结果较准确或偏差小，若三边定位结果偏差大，滤波算法也无法将该结果修正到定位精度范围内。

技术实现要素：

本申请的目的在于提供一种基于粒子滤波和卡尔曼滤波的蓝牙定位方法，规避信号不稳定造成的测距不准，显著提高蓝牙定位的精度。

为实现上述目的，本申请所采取的技术方案为：

一种基于粒子滤波和卡尔曼滤波的蓝牙定位方法，用于根据预设在定位空间范围内的多个信标判断标签当前的位置信息，所述基于粒子滤波和卡尔曼滤波的蓝牙定位方法，包括：

接收预设时间内采集的信标的信息数据，所述信息数据中包含多条信标信息，各所述信标信息包括信号强度rssi值、信标属性、信标编号和信标的坐标；

查找所接收的信息数据中rssi值最强的信标信息，并将该信标信息对应的信标记为信标m；

根据所述信标属性判断信标m是否处于楼层切换位置，若是则输出信标m的坐标作为标签当前的位置信息，并结束本次执行；否则继续执行；

筛选信息数据中rssi值大于信号强度阈值的信标信息形成定位数据集；

判断所述定位数据集是否为空，若为空则输出上一次的标签位置信息作为标签当前的位置信息，并结束本次执行；否则继续执行；

根据所述定位数据集采用粒子滤波算法预估标签的位置，并采用卡尔曼滤波算法修正粒子滤波算法预估的位置，再利用限幅滤波算法修正卡尔曼滤波算法修正后的标签的位置，输出限幅滤波算法修正后的位置作为标签当前的位置信息。

作为优选，所述基于粒子滤波和卡尔曼滤波的蓝牙定位方法还包括：

在查找到信息数据中的信标m后，判断本次接收的信息数据是否为第一条信息数据，若是则先执行参数初始化后再继续执行；否则直接继续执行；

所述参数初始化包括：

初始化粒子滤波算法中粒子的位置：对信标m的坐标添加随机高斯噪声，将增加噪声后的坐标作为各粒子的初始位置partical0；

初始化卡尔曼滤波算法中的参数x0：所述参数x0包括标签的初始位置和初始速度，将信标m的坐标的横纵坐标分别赋值给标签初始位置的x0和y0，并取初始速度和为服从高斯分布的介于0～1之间的随机数。

作为优选，所述根据定位数据集采用粒子滤波算法预估标签的位置，包括：

以各粒子的位置为状态值，以所述定位数据集中的rssi值为观测值，预估标签的位置的具体过程如下：

1)预测粒子当前的位置：根据标签移动规则以及粒子前一位置，预测粒子当前的位置：

particalk＝particalk-1+vk-1×t

其中，particalk为粒子当前的预测位置，particalk-1为粒子前一位置，即第k-1条信息数据对应的粒子位置，vk-1为第k-1条信息数据对应的标签速度，也为卡尔曼滤波算法的状态值xk-1中的和的矢量和的大小，t为时间，也是位置更新时间；

2)计算粒子对应的rssi值：根据定位数据集中各信标的坐标以及各粒子当前的预测位置particalk，计算各粒子与信标的距离d，并根据距离d计算各粒子在当前的预测位置与信标对应的rssi值；

3)评估粒子赋权重：计算各粒子当前的预测位置particalk所对应的rssi值与当前实际的观测值中rssi值的欧氏距离，若该欧氏距离越小，则粒子赋予的权重w越大；

4)对粒子重采样，更新粒子位置：根据粒子权重w对粒子进行重要性采样，保留权值大的粒子，删除权值小的粒子，权值大的粒子会被多次复制以保证粒子个数的不变，将重采样后的粒子位置作为新的粒子位置

5)均值滤波更新粒子位置得标签预估位置：取更新后的粒子位置的均值作为标签当前预估的位置

作为优选，所述根据距离d计算各粒子在当前的预测位置与信标对应的rssi值，包括：

根据无线信号传播衰减模型进行计算：

其中，rssi(d)为距离d对应的rssi值，d0为初始距离，rssi(d0)为初始距离d0对应的rssi值，α为信号传播过程中的损耗因子，ζσ为服从均值为0、标准差为σ的正态分布的随机量。

作为优选，所述采用卡尔曼滤波算法修正粒子滤波预估的位置，包括：

以标签的位置和速度为状态值，以粒子滤波算法输出的标签当前预估的位置为观测值，建立卡尔曼滤波算法的迭代方程为：

xk＝fk·xk-1

其中，为第k条信息数据的状态值，即表示第k条信息数据标签的预测位置和预测速度，xk-1表示第k-1条信息数据标签的位置和速度；表示系统参数，fk中的t表示时间间隔，即位置更新时间；表示第k条信息数据的状态量xk的误差协方差阵，其初始值为表示过程噪声；表示测量参数；表示测量噪声；表示卡尔曼增益；表示第k条信息数据的观测值，即粒子滤波算法输出的标签当前预估的位置表示卡尔曼滤波算法本次修正后标签的位置和速度；表示对应的协方差阵。

作为优选，所述利用限幅滤波算法修正卡尔曼滤波算法修正后的标签位置，包括：

根据卡尔曼滤波算法中参数xk中的vx和vy以及时间t，计算出时间t内标签实际移动距离作为理论距离，即vx和vy的矢量和大小与时间t的乘积；

计算当前卡尔曼滤波算法输出的修正后标签的位置和标签上一次的标签位置的距离作为实际距离；

若所述实际距离超过理论距离，则取当前卡尔曼滤波算法输出的修正后的位置和上一次的标签位置的均值作为标签本次定位最终的位置信息；否则保持卡尔曼滤波算法输出的修正后的位置的值不变。

本申请提供的基于粒子滤波和卡尔曼滤波的蓝牙定位方法，将楼层切换处的位置情况作为特殊情况，排除楼层切换处干扰多的影响，并且可直接根据楼层切换处信标的坐标得到准确位置信息；其次对所采集的信标信息进行筛选，去除信号强度较弱的以提高数据的可靠性；并且本申请直接利用粒子滤波算法预估标签的位置，并且利用卡尔曼滤波算法修正粒子滤波算法预估的位置，并且利用限幅滤波算法修正卡尔曼滤波算法修正后的位置，以防止前后两次计算得到的标签位置波动过大，从而降低定位对rssi值的依赖，规避信号不稳定造成的测距不准，并且参考历史位置，避免出现预估轨迹突变的情况，显著提高定位的精度。

附图说明

图1为本申请的基于粒子滤波和卡尔曼滤波的蓝牙定位方法的流程图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本申请。

如图1所示，其中一个实施例中，提供了一种基于粒子滤波和卡尔曼滤波的蓝牙定位方法，用于根据预设在定位空间范围内的多个信标判断标签当前的位置信息，可将定位精度由现有的3～5米缩短至1～3米。

具体的，所述基于粒子滤波和卡尔曼滤波的蓝牙定位方法，包括：

步骤s1、接收预设时间内采集的信标的信息数据，所述信息数据中包含多条信标信息，各所述信标信息包括信号强度rssi值、信标属性、信标编号和信标的坐标。

其中，信标的特征为在物理位置上固定，并定时广播数据包；标签的特征为定时扫描广播的数据包，得出对应信标的rssi值；rssi值表示接受信号强度值。

为了保证数据的完整性，一般设置预设时间大于标签定时扫描广播的时间。例如标签2s扫描一次，而标签6s上传一次数据，即本次所接收的信息数据中包含标签3次扫描得到的数据，并且标签扫描一次可得到1个或多个响应信标的广播，一次扫描得到的每一个信标的广播均形成一条独立的信标信息。

步骤s2、查找所接收的信息数据中rssi值最强的信标信息，并将该信标信息对应的信标记为信标m。

步骤s3、在查找到信息数据中的信标m后，判断本次接收的信息数据是否为第一条信息数据，若是则先执行参数初始化后再继续执行；否则直接继续执行。

参数初始化主要针对粒子滤波算法和卡尔曼滤波算法进行，具体如下：

初始化粒子滤波算法中粒子的位置：对信标m的坐标添加随机高斯噪声，将增加噪声后的坐标作为各粒子的初始位置partical0。

初始化卡尔曼滤波算法中的参数x0：所述参数x0包括标签的初始位置和初始速度，将信标m的坐标的横纵坐标分别赋值给标签初始位置的x0和y0，并取初始速度和为服从高斯分布的介于0～1之间的随机数。

步骤s4、根据所述信标属性判断信标m是否处于楼层切换位置，若是则输出信标m的坐标作为标签当前的位置信息，并结束本次执行；否则继续执行。

信标信息中的信标属性包含信标的位置属性，例如“楼层”、“一楼客厅”、“二楼卧室”等等，并且该属性为布置信标时预配置。

若判断到信标m的信标属性为“楼层”，则表示信标m处于楼层切换位置，否则不处于楼层切换位置。

步骤s5、筛选信息数据中rssi值大于信号强度阈值的信标信息形成定位数据集。由于接收的信号强度越弱，信号受干扰的可能性越大，故本实施例中根据rssi值对信标信息进行筛选，以保证数据的可靠性。

步骤s6、判断所述定位数据集是否为空，若为空则输出上一次的标签位置信息作为标签当前的位置信息，并结束本次执行；否则继续执行。

步骤s7、根据所述定位数据集采用粒子滤波算法预估标签的位置，并采用卡尔曼滤波算法修正粒子滤波算法预估的位置，再利用限幅滤波算法修正卡尔曼滤波算法修正后的标签的位置，输出限幅滤波算法修正后的位置作为标签当前的位置信息。

本实施例直接采用粒子滤波算法计算标签的预估位置，可简化计算过程并且提高预估位置的准确性。在一实施例中，根据定位数据集采用粒子滤波算法预估标签的位置，包括：

以各粒子的位置为状态值，以所述定位数据集中的rssi值为观测值，预估标签的位置的具体过程如下：

1)预测粒子当前的位置：根据标签移动规则以及粒子前一位置，预测粒子当前的位置：

particalk＝particalk-1+vk-1×t

其中，particalk为粒子当前的预测位置，particalk-1为粒子前一位置，即第k-1条信息数据对应的粒子位置，vk-1为第k-1条信息数据对应的标签速度，也为卡尔曼滤波算法的状态值xk-1中的和的矢量和的大小，t为时间，也是位置更新时间，即为两条信息数据之间的时间间隔。若当前为第一条信息数据的处理，即k＝1时，则采用初始化的值进行计算。

2)计算粒子对应的rssi值：根据定位数据集中各信标的坐标以及各粒子当前的预测位置particalk，计算各粒子与信标的距离d，并根据距离d计算各粒子在当前的预测位置与信标对应的rssi值。需要说明的是，每个粒子都需要计算与当前数据中响应信标的对应的rssi，都对应着多个rssi数据。

3)评估粒子赋权重：计算各粒子当前的预测位置particalk所对应的rssi值与当前实际的观测值中rssi值的欧氏距离，若该欧氏距离越小，则粒子赋予的权重w越大。上一步计算的多个rssi数据需要与当前实际观测数据中的多个rssi进行欧式距离的计算，欧氏距离越小，权值越大。

4)对粒子重采样，更新粒子位置：根据粒子权重w对粒子进行重要性采样，保留权值大的粒子，删除权值小的粒子，权值大的粒子会被多次复制以保证粒子个数的不变，将重采样后的粒子位置作为新的粒子位置

5)均值滤波更新粒子位置得标签预估位置：取更新后的粒子位置的均值作为标签当前预估的位置

为了避免基于rssi值的测距不准确而导致定位误差大的问题，本实施例在粒子滤波算法中对所采集的rssi值进行筛选，去除波动大的数据，提高定位精度。并且根据距离计算粒子位置对应的rssi值，进一步对实际观测的rssi数据进行修正。

在一实施例中，根据距离d计算各粒子在当前的预测位置与信标对应的rssi值，包括：

根据无线信号传播衰减模型进行计算：

其中，rssi(d)为距离d对应的rssi值，d0为初始距离，rssi(d0)为初始距离d0对应的rssi值，α为信号传播过程中的损耗因子，ζσ为服从均值为0、标准差为σ的正态分布的随机量。

在确定rssi值与距离的关系时，分别采集了测试人员在面对和背对信标的两种情况下接收到的rssi值。由于rssi值浮动较大，在每种情况下的每个位置均采集了2分钟(甚至更久时间)数据。对于采集的2分钟数据，采用高斯滤波处理方式得到当前位置所对应的rssi值，最后将面对和背对情况下处理后的rssi值取均值作为确定关系的源数据，以提高模型的可靠性。

通过粒子滤波算法得到的标签的预估位置可能存在微小误差，为了进一步降低误差，采用卡尔曼滤波进行修正，在一个实施例中，所述采用卡尔曼滤波算法修正粒子滤波预估的位置，包括：

以标签的位置和速度为状态值，以粒子滤波算法输出的标签当前预估的位置为观测值，建立卡尔曼滤波算法的迭代方程为：

xk＝fk·xk-1

其中，为第k条信息数据的状态值，即表示第k条信息数据标签的预测位置和预测速度，xk-1表示第k-1条信息数据标签的位置和速度，若当前为第一条信息数据的处理，即k＝1时，则采用初始化的值进行计算；表示系统参数，fk中的t表示时间间隔，即位置更新时间；表示第k条信息数据的状态量xk的误差协方差阵，其初始值为表示过程噪声；表示测量参数；表示测量噪声；表示卡尔曼增益；表示第k条信息数据的观测值，即粒子滤波算法输出的标签当前预估的位置表示卡尔曼滤波算法本次修正后标签的位置和速度；表示对应的协方差阵。

需要说明的是，针对粒子滤波算法和卡尔曼滤波算法的参数初始化可以在步骤s3中进行，以保证每次执行算法仅需在步骤s3中判断一次是否需要初始化即可；参数初始化也可以在粒子滤波算法和卡尔曼滤波算法内部完成，即在执行前判断本次接收的信息数据是否为第一条信息数据，若是即各自执行参数初始化，即省略步骤s3。

对于粒子滤波算法和卡尔曼滤波算法而言，一条信息数据迭代一次，只有对于整个定位过程来说，两个滤波算法才会迭代多次，定位引擎关闭才是两个滤波算法结束停止迭代的条件。

并且为了避免预估的轨迹数据产生突变，本申请建立相邻两次算法执行的关联性，即在步骤s1中还接收针对前一条信息数据进行位置估计后的滤波算法参数(除第一条信息数据执行以外)，例如粒子滤波算法中粒子最终的位置、卡尔曼滤波算法中的参数x和参数p，并且在本次输出标签的位置信息的同时输出滤波算法的参数，以供下一条信息数据使用。

粒子滤波算法和卡尔曼滤波算法的结合可得到标签较为精确的位置信息，在一实施例中，为了进一步降低误差，防止标签前后两次预估的位置跳变过大，在卡尔曼滤波算法修正标签的预估位置后，利用限幅滤波算法修正卡尔曼滤波算法修正后的标签位置，包括：

根据卡尔曼滤波算法中参数xk中的vx和vy以及时间t，计算出时间t内标签实际移动距离作为理论距离，即vx和vy的矢量和大小与时间t的乘积；

计算当前卡尔曼滤波算法输出的修正后标签的位置和标签上一次的标签位置的距离作为实际距离；

若所述实际距离超过理论距离，则取当前卡尔曼滤波算法输出的修正后的位置和上一次的标签位置的均值作为标签本次定位最终的位置信息；否则保持卡尔曼滤波算法输出的修正后的位置的值不变。

本实施例的蓝牙定位方法避免基于rssi值的测距不准确而导致定位误差大的问题，对距离准确度的依赖性不强。规避信号不稳造成的测距不准，导致位置解算出现无解或者解误差大或者解相对历史位置突变性大等一系列问题；参考了历史位置信息预估当前位置，使整个运动轨迹突变性不会太强。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。