基于卡尔曼滤波算法的UWB+IMU组合精确定位系统在选煤厂中的应用

白怡明，曾祥玉，李 杰，辛凤阳，郭晓松，朱金龙

(1.中煤科工集团沈阳设计研究院有限公司，辽宁 沈阳 110015；2.内蒙古锡林河煤化工有限责任公司，内蒙古 锡林郭勒 026324)

煤炭产业作为传统行业,多年以来,人员的安全问题一直被社会持续关注，而选煤作为提高煤炭高附加值产业，推进其智能化建设发展是科技进步时代发展的必然趋势[1-2]。目前,人员精确定位系统在选煤行业的应用尚处于初级阶段[3]。选煤厂人员定位系统最早源于主要机电设备离线巡检系统，即人员在固定的巡检路线中多个地点和设备位置巡查，人员定位系统则大多照搬煤矿井下的模式[4-5]，从最开始的Wi-Fi技术到ZigBee通信技术再到现在主流的超宽带定位技术(Ultra-Wide Band，UWB)，随着技术手段的提升[6-7]，定位精准度逐渐提高。

选煤厂工作环境较为复杂，与矿井井下的工作环境差异较大[8]，存在严重的遮挡、屏蔽、电磁干扰等现象，导致普通的定位技术在实际计算过程中出现人员定位不准确[9-10]，会间断地出现“超人”、画面人员漂移、穿越设备或墙体等现象，个别大型设备密集地方还会出现人员数量不准确等情况[11-13]。一般的无线定位技术难以达到较高的定位精度，而UWB定位技术具有短脉冲、高多径分辨率、高定位精度的特点，适合在室内定位中应用[14-16]。但是UWB定位需要3个以上的固定基站，因为在某个基站的信号完全被遮挡的非视距(NLOS)情况下[17]，会出现可见基站变少不能完成定位或者定位精度较差的情况[18-19]。IMU(Inertial Measurement Uunit)即惯性测量单元，是一种测量物体空间三轴姿态角及运动的传感装置[20]。UWB+IMU组合精确定位系统通过辅以惯性测量单元，将UWB的位置和定位标签的速度作为扩展卡尔曼滤波算法的观测数据，来更新IMU的推算值。

为提高定位系统数据的准确性，开发了基于卡尔曼滤波算法的选煤厂UWB+IMU组合精确定位系统，并在大柳塔选煤厂中测试应用。

1 选煤厂人员精确定位技术

当前我国智能化选煤厂的建设，对人员定位的安全和精准度要求越来越高[21]，大多数定位系统的功能已不能满足选煤厂智能化建设发展的需求[22-23]，因此需要加大对选煤厂室内定位功能的开发和设计，不断完善人员定位管理系统的监控功能，以及完善人员监管的有效性、准确性，从而降低事故发生率与人员伤亡率[24-25]。选煤厂现阶段应用最多的定位技术有蓝牙、Wi-Fi、精准ZigBee定位技术、RFID定位技术、UWB定位技术[26]等。定位技术对比见表1。

表1 定位技术对比Table 1 Comparison of positioning techniques currently available

2 UWB+IMU组合精确定位系统

UWB+IMU组合精确定位系统是一种针对室内定位NLOS现象，提出减弱其影响误差的一种算法模型，主要是利用UWB做定位测距定位技术，通过辅以惯性测量单元(Inertial Measurement Unit)做平滑处理，预测并修正NLOS误差概率曲线，最后利用卡尔曼滤波算法处理NLOS误差干扰对UWB系统精确定位计算的影响。

2.1 模型建立

卡尔曼滤波算法是一种典型滤波算法，是一种积累历史数据，更新数据预测值的迭代过程。滤波的作用就是减少噪声误差与干扰对数据测量的影响。

卡尔曼滤波模型是可用于时变线性系统的算法机理模型，这种算法模型将过去大量的实际数据和误差数据汇总，合并到现在的误差数据上，以预测未来的误差数据。目前基于卡尔曼滤波算法的UWB精确定位系统研究仍处于初级阶段。从影响准确度的参数入手，利用卡尔曼滤波算法建立过程模型，对其进行各种响应测试。该算法是在变化的初始数据中去除噪声后对未来进行数据预测的算法，是基于大数据统计原理的分析算法。任何一个卡尔曼滤波输入值都是可测量的，仅需要知道可测量的精度是多少，通过卡尔曼滤波算法模型不断进行数据积累，对系统工程的不断测量，然后不断估计，持续一段时间就能模拟出一个相对准确的预测值。通过卡尔曼滤波算法原理，可以在测量值可能不准确的前提下，通过不断估算，迭代更新，估计出一个相对准确的系统输出值。模型建立的基础是采用卡尔曼滤波算法通过simulink仿真平台，建立UWB+IMU组合精确定位系统。卡尔曼滤波算法模型原理如图1所示。

图1 卡尔曼滤波算法模型原理Fig. 1 Principle of the Kalman filtering algorithm-based positioning model

建立卡尔曼滤波算法模型分为两部分：第一部分就是卡尔曼滤波算法的状态方程和观测方程，它是一个预测数据实时更新的过程；第二部分是卡尔曼滤波算法分析值迭代过程，每个节点默认仅有自身实际的变量指标，没有其他控制变量存在，这个算法模型可以帮助UWB精确定位系统提高整个系统的响应时间。在该处的初始控制量是0，初始测量值1，将初始测量值分为实际值0.5和预测值0.5，也就是用卡尔曼滤波算法来预测UWB精确定位系统中的距离变化时，将默认为不变化，预测数据实时更新的变化值就是1，由于距离变化是可以直接测量的，因此测量矩阵变化值也是1。

卡尔曼滤波算法模型的基本公式参数较多，但在UWB精确定位系统中有大量参数都在自我不断地迭代更新，整个UWB精确定位系统真正受外界调整的参数只有三个，而系统开始循环的初始值，需要根据选煤厂的实际情况人为定义，其他影响UWB精确定位系统精准度的两个参数是过程误差和测量误差的协方差。在卡尔曼滤波算法模型测试中，给定一个真实的选煤厂UWB精确定位系统数据，通过对真实的系统数据进行迭代更新，再加上测量误差噪声，可同时检测系统实际值、测量值还有卡尔曼预测估值。卡尔曼滤波算法迭代模型如图2所示。

图2 卡尔曼滤波算法迭代模型Fig. 2 The Kalman filtering algorithm-based iterative model

在卡尔曼滤波算法模型的实际测试中，得到了大量过程误差和测量误差，这些误差的协方差是不可直接测量的，是存在于UWB精确定位系统本身的，但误差数据仍存在一定关联性，因此在实际模拟过程中发现，卡尔曼滤波算法模型中即便存在一个对误差的协方差有不准确的判断时，也能对系统的真实输出数据有一个较好的预测。

2.2 精确定位

定位标签行走路线为已固定坐标为圆心、半径为2.5 m的圆，在无遮挡的定位区域中心，UWB精确定位精度基本上比较可靠，而在非视距环境下的定位试验中，混凝土柱子、较大的金属物体对定位精度影响较大。此外，人体对UWB信号的阻挡也十分严重，单UWB精确定位系统位置误差较大并且部分数据丢失，而在无遮挡区域定位精度相对较高且比较稳定。UWB+IMU组合精确定位系统通过更新扩展卡尔曼滤波算法可更新NLOS误差值并获得更准确的估算位置，从而实现在NLOS影响下的高精度定位。在选煤厂复杂环境的定位系统中，普通UWB基站测出的距离数据，可能因遮挡、多径干扰导致定位误差较大，有的地方达到100 cm左右，而采用基于卡尔曼滤波算法进行数据预处理后，数据得到较大的优化，定位精度可以提升至30 cm左右。 UWB+IMU组合精确定位融合算法流程如图3所示。

图3 UWB+IMU组合精确定位融合算法流程Fig. 3 Flowchart of the fusion algorithm process of the UWB+IMU integrated accurate positioning system

2.3 系统组成

基于卡尔曼滤波算法的UWB+IMU组合精确定位系统由人员定位卡、UWB定位基站、定位引擎、系统软件等组成。人员定位卡与现场的UWB定位基站之间分别测距，基站将与定位卡之间的定位距离传输给定位引擎，定位引擎内运行定位算法，将计算出的位置数据输送给平台软件进行位置展示。卡尔曼滤波算法运行在定位引擎内部，定位基站传输的原始数据先经过滤波算法处理后，再进行定位算法计算，能显著提升定位系统精度。

3 UWB+IMU组合精确定位系统的应用

3.1 系统网络拓扑结构

大柳塔选煤厂UWB+IMU组合精确定位系统网络拓扑结构如图4所示。该系统由服务器、路由器、交换机、光纤环网、集中控制器、智能定位模块、定位标签卡组成。服务器配有定位精度解析软件及Web发布软件，交换机用于连接各个集中控制器，智能定位模块、定位标签卡均通过无线网络全覆盖来实现通信。

图4 UWB+IMU组合精确定位系统网络拓扑图Fig. 4 Network topology structure of the UWB+IMU integrated accurate personnel positioning system

根据UWB信号传输的特点，一般情况下，智能定位模块天线覆盖的定位范围在30 m左右，由于选煤厂复杂的现场作业环境，在大柳塔选煤厂内智能定位模块距离考虑在15～20 m左右(安装在智能照明灯具上)。与定位相关的主要决定因素是UWB定位卡与UWB定位基站之间距离的准确程度，以及定位数据的连续性。当人员携带定位标签卡出现在智能定位模块(智能灯具)周围时，多个智能定位模块可分别测算出该定位标签卡与自身的距离，并将原始数据进行噪声误差剔除、初步测距算法等处理，然后将人员定位标签卡信息及距离信息上传至服务器端；在服务器端，通过卡尔曼滤波算法模型过滤，并结合选煤厂各灯具的具体位置及高度，构建参与运算的坐标系统，结合灯具的空间坐标位置，即可测算出人员定位标签卡的位置范围。

3.2 应用效果

在大柳塔选煤厂某空间场景布置8台UWB基站，UWB定位采用3D定位方式，定位基站选用2主站和6从站，以主基站为坐标原点建立X，Y，Z坐标轴，当人员携带定位标签卡进入基站覆盖范围内，定位标签卡将发送数据通过基站传输至上位机解算定位标签卡实际位置。图5为大柳塔选煤厂基于卡尔曼滤波算法的UWB+IMU组合精确定位系统的精确度图谱。

由图5可以看出，采用UWB+IMU组合精确定位算法的定位效果比单独UWB最小二乘法解算要好。单独UWB解算无论怎么平滑，依然保持较严重的滞后性，出现更多的毛刺和偏离，无法与融合算法相比。基于卡尔曼滤波算法融合后将初始测量值分为实际值和预测值，最终绘制出的定位轨迹很好地避免了数据毛刺，定位轨迹与真实轨迹基本吻合，实现了更为精准的定位。

图5 UWB+IMU组合定位系统精确度图谱Fig. 5 Spectral analysis of the accuracy of the positioning system

3.3 系统优势

(1)定位准确。选煤厂内定位人员携带定位卡出现在灯具或者定位设备周围时，多个灯具及定位设备可分别测算出该定位标签卡与自身的距离，并将原始数据进行噪声剔除、卡尔曼滤波算法、初步测距算法等融合处理，将人员定位卡信息及距离信息上传至后台服务器端，甚至仅需要较少的基站(≥2)即可实现定位。

(2)高效响应。利用AI算法并结合选煤厂厂区内各灯具的具体位置及高度，构建三维可视化模型并生成坐标系统，同时考虑各种数据的滤波及筛选结果，以达到更高效的计算响应时间。

(3)可与其他设备实现联动。服务器后台通过空间位置的核心算法，结合灯具的空间坐标，测算出人员定位标签卡的位置范围，同时可以与照明灯具、各类环境监测传感器、主要机电设备保护装置互联互通，实现选煤厂内“人来灯亮，人走灯灭”。

4 结语

基于卡尔曼滤波算法的UWB+IMU组合精确定位系统，可以解决目前由于生产环境遮挡、屏蔽、电磁干扰等现象影响人员位置实时计算过程中位置不断变化、定位不准确的问题，基本不会出现较大数据误差，还原了人员精准定位的误差轨迹。同时由于智能定位模块的适当增加，基本解决了人员数量计算总数与选煤厂内实际人员数量不一致的现象，系统数据真实可靠。

现阶段我国智能选煤厂建设起步时间较短,智能选煤厂中人员精准定位系统技术仍有不足，由于主厂房、筛分破碎车间等建筑内的大型设备立体空间分布复杂，众多运行的变频电气设备存在大量谐波及电磁干扰，如何有效地提高选煤厂人员定位的定位精度和准确度的要求十分迫切。选煤厂人员定位系统是保证安全生产的重要环节，高效、精准的人员定位系统将会给生产管理工作带来极大的便利，进一步保障现场工作人员的生命安全。精准的选煤厂人员定位系统将作为智能选煤厂的一个重要子系统，在安全管理方面起到重要作用。