一种地图约束的粒子位置及权重强自适应滤波方法

技术领域

本发明属于室内定位领域，尤其涉及一种地图约束的粒子位置及权重强自适应滤波方法。

背景技术

化工业作为国家支柱性产业，是国家经济的重要组成部分。但由于化工厂区环境复杂且含有大量危险化学品，使得化工厂区工作具有一定的危险性，通过对人员位置的精准管控，可以有效预防事故发生的同时，在事发后对受灾人员进行及时精准救助，降低安全风险。但由于化工厂区环境复杂、空间大、含有危险化学品等诸多特点，使得化工厂内的精准定位尤为困难。因此，如何通过有效手段实现室内环境下人员精确定位已经成为亟需解决的问题。

飞行时间测距法(Time of Flight,TOF)通过测量信号从发送端到接收端的飞行时间来确定目标的位置。在TOF定位系统中，通常使用无线电波、红外线或激光等信号传输技术。通过测量信号从发送到接收的时间延迟，可以计算出信号传播的距离。结合多个发送和接收点，可以利用三角定位等方法计算目标的空间位置。但由于反射和多路径干扰、遮挡和阻挡物等诸多原因的影响，使得实际环境中，特别是在复杂环境的恶劣位置，定位精度较低。粒子滤波(Particle Filter，PF)，又称蒙特卡洛滤波(Monte Carlo Filter，MCF)，是一种用于估计状态变量的非线性和非高斯分布的滤波方法。粒子滤波是贝叶斯滤波的一种变种，它通过随机抽样的方式来表示潜在状态的分布，并使用这些随机粒子来估计状态变量的后验分布，但传统的粒子滤波收敛性较差，无法根据地图信息改善定位精度。

发明内容

本发明针对高复杂度场景室内定位精度低问题，提出了一种地图约束的粒子位置及权重强自适应滤波方法；首先，我们将提出的等概率采取粒子权重置零、粒子位置匹配和粒子重生操作称为粒子保护操作。粒子保护操作中，粒子权重置零操作是将粒子权重置为0，权重为0后可以认为将禁区内粒子清除，只保留禁区外粒子。粒子匹配操作为将禁区内部粒子匹配到粒子最近边界上的最近点处，其在不清除粒子的前提下将坐标移动到禁区外部。粒子重生操作为将禁区内粒子按照非禁区粒子的均值和方差的高斯分布重新生成新的粒子。仅使用粒子位置匹配操作时的定位精度要分别低于使用粒子权重置零、粒子位置匹配操作，但三种操作均可以将坐标接近真实地移动到禁区外部。当定位点误差极大时，即所有粒子均位于禁区内部时，禁区权重置零和粒子重生失效，粒子位置匹配发挥主要作用，于是我们对每一个粒子采取等概率的选取三种操作，在保证定位精度的同时，将坐标控制在禁区外部。粒子保护操作在滤波过程中主要体现在粒子保护初始化、粒子保护更新和粒子保护重采样三步中。具体步骤如下：

步骤1：搭建TOF定位环境采集TOF数据，形成轨迹数据集D；

步骤2：获取地图信息；将地图中人员正常移动时不应存在的区域设置为禁区F，将封闭环境下外围墙体或围栏等不可穿过的边界区域称为保护区边界P，该边界所围区域称为保护区；

步骤3：对初始坐标定位解算和滤波；使用d

步骤4：粒子初始化为

通过TOF测距样本d

其中，

对于初始化后的粒子

其中，SUM(i

其中，

然后执行粒子保护操作，即将禁区内粒子等概率随机采取粒子权重置零、粒子位置匹配和粒子重生操作，直至所有非0权重粒子均不在禁区内；

步骤5：执行粒子传播，根据状态方程计算转移后粒子状态；假设当前状态S

第n时刻第i个粒子状态向量为

步骤6：粒子保护更新；判断粒子传播过程中粒子是否与地图中禁区相交，判断粒子状态，对与禁区相交的粒子采用粒子保护操作；

步骤7：目标位置确定，根据粒子权重和粒子位置确定目标位置；根据式(7)得到目标在第n时刻的位置：

其中[x

步骤8：粒子保护重采样；粒子位置自适应修正，进行粒子保护操作并根据粒子权重对粒子进行重采样操作；

步骤9.循环步骤5-8，直到满足预设迭代次数，输出估计结果，此时禁区内不存在定位点，无超出边界点，且定位点接近真实值。

本发明提出了一种地图约束的粒子位置及权重强自适应滤波方法，克服了TOF在复杂小场景下由于非视距或多径等因素造成的正向误差，利用地图信息，将粒子滤波中违规粒子进行自适应调整，在粒子初始化、粒子更新和重采样时分别等概率随机采取粒子权重置零、粒子位置匹配和粒子重生操作，从而修正目标位置，减小了位置更新过程中的累积误差，尤其是在角落或盲区位置，有效的提升了定位精度。

附图表说明

图1为本发明的流程图。

图2为技术背景中采用的传统粒子滤波方法和本发明方法在办公室环境下的轨迹对比图。

图3为技术背景中采用的传统粒子滤波方法和本发明方法在办公室环境下的定位误差累积百分比。

具体实施方式

本实施例提供了一种地图约束的粒子位置及权重强自适应滤波方法，具体步骤如下：

步骤1：采集TOF数据，形成轨迹数据集；

首先进行实验场地布置，实验环境为成都市某办公楼内，18m×10m的区域，环境由工位办公桌、柱子、随机移动的办公人员和各种杂物组成，八个接入点(Access Point,AP)均匀的部署在整个环境中。

搭建好TOF定位环境后进行测距样本采集；手握定位标签在环境中的起始点，开启定位，按照预定轨迹步进，记录每时刻人在路途中的TOF数据，设第n时刻(1

步骤2：获取地图信息。将地图中人员正常移动时不应存在的区域设置为禁区，如在办公室场景下的办公桌和柱子，化工厂场景中的化工罐和管道等。室内定位时，人员在某一封闭环境下活动，四周存在墙体或者围栏，人员正常移动时轨迹不应直接穿越墙体或围栏，轨迹应始终位于墙体或围栏内部，我们将封闭环境下外围墙体或围栏等不可穿过的边界区域称为保护区边界，该边界所围区域称为保护区。我们使用一系列点组成向量描述禁区和保护区，则禁区表示为

步骤3：对初始坐标定位解算和滤波；

通过TOF定位解算出第n时刻坐标为

步骤4：对非初始时刻进行粒子保护初始化；

粒子初始化为

通过TOF测距样本d

其中，

对于初始化后的粒子

其中，SUM(i

其中，

然后执行粒子保护操作，即将禁区内粒子等概率随机采取粒子权重置零、粒子位置匹配和粒子重生操作，直至所有非0权重粒子均不在禁区内；其中，粒子权重置零为将粒子权重系数置为0；粒子位置匹配为将粒子匹配到上一时刻最近的禁区边界上到该粒子直线距离最短的点，即若上时刻人位置坐标为[x

步骤5：执行粒子传播，根据状态方程计算转移后粒子状态；在处理目标跟踪问题时，假设目标的状态转移过程服从一阶马尔科夫模型，即当前状态S

第n时刻第i个粒子状态向量为

步骤6：粒子保护更新；判断粒子传播过程中粒子是否与地图中禁区相交，判断粒子状态，对与禁区相交的粒子采用粒子保护操作。

步骤7：目标位置确定，根据粒子权重和粒子位置确定目标位置；根据式(7)得到目标在第n时刻的位置：

其中[x

步骤8：粒子保护重采样，粒子位置自适应修正，进行粒子保护操作并根据粒子权重对粒子进行重采样操作；其中粒子重采样采用轮盘赌法。

步骤9.循环步骤5-8，直到满足预设迭代次数，输出估计结果，此时禁区内不存在定位点，无超出边界点，且定位点接近真实值。

化工厂环境和办公室环境下人员定位均属于高复杂度室内场景定位问题，在不同地图约束下，滤波方法在两种场景中是通用的。本发明在实际办公室环境下设计实验来验证提出方法的优越性。实验是在成都市某办公室内，考虑工位办公桌、柱子、随机移动的办公人员和各种杂物影响下，人在办公室内行走，连续采集了1345个测试样本进行实测定位，图2为背景技术方法和本发明方法在办公室复杂环境下的轨迹对比图。图3为背景技术方法和本发明方法在办公室复杂环境下的定位误差性能比较图。可以看到，与背景方法轨迹大量进入禁区和超出边界相比，本发明方法轨迹始终处于禁区外部和边界内部，更贴合实际轨迹，定位误差明显低于背景技术。本发明提出的方法在越界和定位精度上明显好于传统的粒子滤波方法，这是由于传统滤波方法在反射和多路径干扰、遮挡和阻挡物等诸多原因的影响造成定位错误，而本发明提出的方法通过粒子保护操作进行粒子位置及权重自适应克服了粒子越界问题，并且利用地图信息进行精准的位置估计，实验结果证明本发明提出的方法是一种能够位置矫正的高精度实时定位滤波方法，所提方法能够有效解决高复杂度室内环境下的人员定位难题。