基于UWB 定位技术的无轨胶轮车路径偏离报警

韩燕南

（1.中煤科工集团常州研究院有限公司，江苏 常州 213015；2.天地（常州）自动化股份有限公司，江苏 常州 213015）

自2020 年2 月8 部委联合发布《关于加快煤矿智能化发展的指导意见》以来，国内掀起了煤矿智能化建设的浪潮[1-3]。辅助运输作为煤矿生产的准备环节，担负着全矿井生产所需人员、物资及设备的中转和运输，其智能化对提供矿井生产效率和确保矿井安全有着重要意义[4]。2021 年12 月，国家能源局发布了《智能化示范煤矿验收管理办法（试行）》，明确了智能化示范建设煤矿的验收流程、技术要求及评分办法，为验收评审提供了依据[5]。在办法中规定I 类煤矿无轨胶轮车运输须具有车辆运行状态参数监测（超速、路径偏离报警、运动轨迹等）功能。由此可见，采用无轨胶轮车运输时，路径偏离报警作为车辆运行状态参数监测指标之一，是智能化煤矿建设验收的硬性指标。

路径偏离报警数据常被应用于车辆智能调度系统运输效能分析模块，用以提升车辆运输效率。许多学者对此开展了研究。常凯等[6]提出根据井下所有机车和车辆运行信息及作业需求，快速自动规划车辆运行路线，并自动调度车辆出发时间、会车地点，此方法充分体现了智能化，但是对车辆运行状态参数监测有所缺失，无法对分析模块提供数据支撑；刘宏杰等[7]提出采用多系统融合技术将煤矿井下无轨胶轮车运行相关有效数据充分筛选，实现车辆运行流程全数字化管控，实现了运力大数据分析、司机工时分析、交通效率分析等各式各样数据报表，此系统虽然提供了多维度的报表信息，但未考虑验收标准中的路径偏离元素；刘文涛[8]提出通过无线通信链路将采集数据传送至监控中心并进行显示，同时监控中心对数据进行分析，此方法仅上传了动态位置、运行时间、运行参数和故障参数等，检测的手段已无法满足验收标准的需要。

针对上述问题，设计了一种基于UWB 技术[9]的无轨胶轮车路径偏离报警方法。首先，该方法基于图论理论，利用巷道中线点之间的邻接关系，对井下路网信息结构化；其次，车辆行进过程中定位数据应呈现线性关系的物理特征，对UWB 定位基站输出的车辆定位数据执行滑动窗口滤波，以屏蔽定位数据中的噪声点，提高方法整体的鲁棒性；再次，利用已构建的井下路网结构化信息，根据运输任务中设定的运输出发地和目的地，按照路线优、节能佳、安全性高的原则，采用A*搜索算法[10]对运输路径进行规划，并在搜索算法的估价函数中新增权重函数和判断因子，以提高规划的智能性；最后，对车辆当前位置的偏离情况进行实时判断，当出现偏离时，将结果实时反馈至车载交互终端中。采用此方法的应用系统在新疆伊犁一矿有限公司进行验证，证明所提方法的可行性和有效性。

1 系统架构

系统由硬件装置和软件模块2 个部分组成。硬件装置主要负责采集无轨胶轮车实时位置；软件模块根据车辆实时位置与规划好的运输路径做比对，得出是否偏离结果，并将结果推送至车载终端中，给驾驶员提示。

1.1 硬件装置

硬件装置分为井上和井下2 个部分，硬件装置示意图如图1。

图1 硬件装置示意图Fig.1 Hardware device schematic diagram

井上部分为辅助运输系统服务器，主要是承载软件模块。井下部分主要由UWB 定位基站和无轨胶轮车组成：UWB 定位基站一般安装在巷道壁一侧，2 根天线分别朝向2 个不同方向固定在巷道上；车辆标识卡固定在无轨胶轮车的顶部，用以减少防爆结构对标识卡的信号干扰；车载终端固定在驾驶员侧，用于人机交互。UWB 定位基站通过光缆接入井下环网，车载终端通过4G/WIFI 接入环网。

当车辆经过基站时，UWB 定位基站的天线会侦测到车辆标识卡信号并实时计算出位置信息，同时传输至位于井上的辅助运输系统服务器内，交由软件模块做逻辑判断。

1.2 软件模块

软件部分主要由构建井下路网、车辆位置滤波、运输路径规划和偏离报警判断组成。软件模块处理流程如图2。

图2 软件模块处理流程Fig.2 Software module processing flow

1）构建井下路网。巷道中线是由具有三维地理空间坐标的中线点组成的，基于图论理论，构建中线点数据结构，形成具有邻接关系的邻接矩阵，从而构建井下路网地图。

2）车辆位置滤波。利用车辆行进过程中定位数据应呈现线性关系的物理特征，对定位数据中的噪声点进行识别并执行滑动窗口滤波，以获得较为平滑且符合运动规律的定位数据，避免因干扰因素造成的误判，提高方法整体的鲁棒性。

3）运输路径规划。利用已构建的井下路网结构化信息，根据运输任务中设定的运输出发地和目的地，按照路线优、节能佳、安全性高的原则，采用基于图的A*搜索方法对运输路径进行规划，快速合理地规划出线路。

4）偏离报警判断。将车辆当前位置与运输路径的关系抽象为点与线段的位置关系判断，当点远离线段时，代表偏离。反之则代表不偏离。当出现偏离时，将结果实时反馈至车载交互终端中。

2 功能实现

2.1 构建井下路网

井下路网数据模型示意图如图3，

图3 井下路网数据模型示意图Fig.3 Underground road network data model diagram

井下巷道是由离散中线点和中线点间的连通关系所组成，进而形成路网结构。该结构采用图G=（V,E）描述，式中：V为顶点集合；E为边集合。E=（Vi,Vj）表示顶点i和顶点j之间的连线。中线点形成的顶点集合为{1，2，3，···，7}，中线点间连通关系采用边来描述，因连通关系具有方向性，故此图为有向图。顶点间的距离作为边的权重。

采用邻接矩阵表达上述数据模型，其构建流程如下：

1）存储中线点数据，并进行顶点唯一性编号；同时，构建顶点数组队列。中线点数据结构如图4，顶点数组如图5。

图4 中线点数据结构Fig.4 Midline point data structure

图5 顶点数组Fig.5 Array of vertices

2）G-（V,E）的邻接矩阵由1 个|V|×|V|的矩阵A=aij予以表示，该矩阵满足下述条件：

式中：aij为矩阵存储值；dist 为顶点i到顶点j的欧几里得距离，距离不可达到时，为∞。

通过上述步骤循环遍历操作井下巷道各中线点数据，最终得到井下路网邻接矩阵，形成井下路网结构。井下路网邻接矩阵如图6。

图6 井下路网邻接矩阵Fig.6 Adjacency matrix of underground road network

2.2 车辆位置滤波

根据井下车辆单向行驶特性，从UWB 定位数据角度来看，理论上定位数据应该呈现V 字形分布特征，定位数据理论与实际情况如图7。

图7 定位数据理论与实际情况Fig.7 Positioning data theory and actual situation

车辆第1 次进入基站信号覆盖范围时，此时距离值为最远。随着行进时间的推移，当接近基站时，距离值趋于0。当驶离基站时，距离值再次变大，直至信号消失，进入下1 个基站的信号覆盖范围。但是现实情况是，受巷道环境恶劣、空间狭小；存在信号干扰、遮挡、反射；以及车辆行进中的抖动等多重因素影响，定位数据存在一定的漂移，也就是噪声点。这些噪声点会被上位机处理单元识别为车辆在频繁调头，但是井下物理环境已经确定车辆无法如此操作。为了避免此种噪声点对路径偏离报警方法的准确性产生影响，因此采用局部滑动窗口滤波思路对定位数据执行滤波处理。滑动窗口处理原理及数据结构如图8。

图8 滑动窗口处理原理及数据结构Fig.8 Sliding window processing principle and data structure

设定滑动窗口为5 s，当识别出噪声点时，基于滑动窗口内的距离值和数据时间得出预估车速，再根据预估车速和当前的数据时间，利用式（2）得出估算距离。

式中：s为估算距离；v为预估车速；Δt为当前数据时间与上一次数据时间差。

车辆位置滤波工作流程如图9。

图9 车辆位置滤波工作流程Fig.9 Vehicle position filtering workflow

第1 步：滑动窗口队列初始化，获取当前位置和数据时间依据数据结构入队列。

第2 步：判断当前位置数据所处定位基站位置，存在2 种情况：①进入/驶离阶段：将当前位置数据与上2 次位置数据做比对，判断是否具有线性关系，若具有，代表不是噪声点，启动正常处理子流程，反之，代表属于噪声点，进入滤波子流程；②接近基站阶段：默认为正常数据，按正常流程处理。

滤波子流程：首先，根据当前位置的数据时间对滑动窗口队列过滤，排除非法值；其次，根据滑动窗口内的距离值和数据时间，计算出估算车速；最后，估算车速×时间得出估算距离。

正常处理子流程：判断数据时间是否在滑动窗口期内，若是，就加入队列；反之，丢弃。

按照上述步骤，完成车辆位置滤波处理。

2.3 运输路径规划

基于已构建的井下路网结构，根据运输任务运输起始点和目的地，按照路线优、节能佳、安全性高的原则，采用全局路径规划算法，快速合理地规划出线路。由于业务中的起始点和目的地大概率不是巷道中线点（即路网结构中的节点），故起始点和目的地间的路径关系分为可以直连和无法直连2 种情况，无法直连是指2 点之间需要借助其他节点才能够到达。2 种情况的处理方式存在差异，因此以下分情况进行讨论。

2.3.1 可以直连路径规划

直接连通场景如图10。图中：S为起始点；E为目的地；a、b为中线点。

图10 直接连通场景Fig.10 Directly connected scene

起始点和目的地同处于同一段巷道内，根据观察可得：2 点可直接通行，此时可直接返回规划路线集合为：{S，E}。

2.3.2 无法直连路径规划

无法直连场景如图11。图中：S为起始点；E为目的地；a、b、c、d为中线点；a→c、b→c、a→d、b→d之间存在{0，1，2，···，n}条路径可能。

图11 无法直连场景Fig.11 Unable to connect to the scene directly

起始点和目的地处于不同巷道内，需要经过中间点才能够到达，根据观察可得：S点行至E点存在4 条路径，分别为：

分别计算这4 条路径距离，取其中代价最少的路径作为最终规划路径。2 坐标之间的距离d12计算公式为：

式中：（x1,y1,z1）d为1#坐标；（x2,y2,z2）d为2#坐标。

由于a→···→c、a→···→d、b→···→c、b→···→d的路径存在0～n种可能性，因此采用A*搜索算法做全局路径规划。A*算法是经典的启发式搜索算法，它是在经典单源路径算法Dijkstra 算法的基础上改进而来。其最显著的特点就是，通过在搜索过程中给定启发函数来减少搜索节点，从而提高路径搜索效率。A*算法的估价函数f（n）表示为：

式中：f（n）为起始点至目标点的预估消耗；g（n）为起始点至当前节点的实际消耗；h（n）为当前节点至目标点的估计消耗，也称为启发函数。

A*算法搜索路径过程是：先对每一个将要访问的节点使用估价函数f（n）进行评估计算，然后选择具有最小代价估算值的节点作为下一步要到达的路径节点，再不断迭代更新搜索，直至寻找到最优路线，达到目标节点。A*算法的搜索方向是根据估价函数f（n）来确定，在搜索过程中，不用遍历地图中的所有节点。A*搜索算法工作流程如图12。

图12 A＊搜索算法工作流程Fig.12 A ＊ search algorithm workflow

整个工作流程如下：①第1 步：起点入OPEN 列表；②第2 步：检索相邻可达到且不在CLOSE 列表中的节点，分别加入CLOSE 列表并计算总代价；③第3 步：代价是否最小，若最小，证明下1 个节点寻找成功，将它从OPEN 列表移动至CLOSE 列表中，若不是最小，则直接将该点存入CLOSE 列表中；④第4 步：比较上一步选出的节点是否为目标点，若是就结束搜索，若不是则跳至第2 步进行循环检索。

其中，OPEN 列表存储未被访问过的节点；CLOSE 列表存储已被访问过的节点。

A*算法的核心是估价函数f（n）的设计。在实际情况下，车辆路径规划须满足以下2 个应用条件：①规划路径满足车辆行车条件：井下存在部分无法行车的巷道，如部分的运输巷、回风巷、水仓等地方，须在路径搜索算法中给予排除；②同一时段、同一路段，限制车辆进入数量：应避免规划原因导致道路拥堵，进而引发其他安全风险，实现车辆智能规划调度。

因此，常规的最短路径并不一定是运输路径最优解。对式（4）的估价函数进行改进，在其中增加权重函数和行车判断因子，得到优化后的估价函数：

式中：α为权重函数；β为道路是否满足行车条件的判断因子。

式中：vechile_num 为当前依据运输任务已规划出的该路段在5 min 内将经过的车辆数。

表达式vechile_num≤2 时，代表该路段为空闲状态，可以规划行车，此时权重值为1；反之，代表该路段为繁忙状态，应避免规划。

满足行车条件的判断因子条件为：

h（n）采用曼哈顿距离进行计算，计算公式为：

2.4 偏离报警判断

车辆实时位置与运输路径比较，最终得出车辆是否偏离预定路线的结果。比较过程可以抽象为点、线的关系，从分析来看，存在3 种情况。点、线关系如图13。图13（a）～图13（c）中：A、B为规划路径的2 个端点；C为车辆实时位置；D点为C点在AB线段的投影；CD线段的模为C点到AB线段的距离值。

图13 点、线关系Fig.13 Point-line relationship

根据图13 观察可得：可通过计算在的占比，从而判断出C点在线段AB的位置。计算公式为：

式中：r为占比；为线段AC向量；为线段AB向量；为线段AB的模长。

结果值存在5 种情况，分别为：

r=0 为C在AB左边的特殊情况；r=1 为C在AB右边的特殊情况；当r=0 或者r=1 时，处于路径内；当r＜0 或者r＞1，不在线段AB内，继续判断其他线段；0＜r＜1，C 点在线段AB中，需要通过公式计算出D点坐标，计算公式为：

式中：（xd，yd，zd）为D点坐标；（xa，ya，za）为A点坐标；（xb，yb，zb）为B点坐标。

最终，通过计算点C和点D2 点间的距离，获得点C距离线段AB的距离。根据这个距离值进行最终判断。

3 试验分析

新疆伊犁能源伊犁一矿使用无轨胶轮车执行井下运输任务，井下路网是由109 条巷道，总计452 个中线点组成；UWB 定位基站65 个，已基本实现重点路段UWB 信号全覆盖；在册车辆数为35 辆。

3.1 车辆位置滤波试验

一般情况，弯道处的UWB 定位基站容易受煤璧反射信号的影响，而出现定位数据偏差的情况。为了验证滤波效果，故选取车流量大，且位于弯道处的上、下井均需要经过的“26-缓坡斜口600 m”UWB 定位基站作为试验对象。车辆位置滤波试验环境如图14。

图14 车辆位置滤波试验环境Fig.14 Vehicle position filtering experimental environment

以车牌号为新KY0060 为例，持续观察车辆经过该UWB 定位基站时定位数据的变化情况。通过观察车辆位置原始值与滤波值可得：滤波函数可识别出噪声点数据，并对其进行滤波，滤波后的轨迹数据更符合现实运动规律。

为了验证滤波函数是否对正常数据产生影响，在图14 中选择“22-缓坡井口200 m”UWB 定位基站作为试验对象，该定位基站位于巷道中部，天线正负方向与巷道方向水平，信号干扰少。再次以新YK0060 为例，观察车辆经过定位基站时定位数据的变化情况。观察可得：滤波值与原始值曲线重合，证明滤波函数不会对正常数据产生影响。

3.2 运输路径规划试验

在车辆调度模块中，选取“59-1506W 下巷口”作为用车申请的出发地，“99-1503 联络巷上巷交叉口”作为目的地。路径规划实验环境如图15。

图15 路径规划实验环境Fig.15 Path planning experimental environment

出发地至目的地存在2 条路径可选，路线Ⅰ为：出发地→1 503 进风巷→1506W 回风巷→辅运大巷→1503E 至辅运联络巷→目的地路径距离为639.68 m；路线Ⅱ为：出发地→1506W 回风巷→1506W 回风巷外段→1503E 联络巷→目的地，路径距离为659.15 m。在用车申请中选择不同的用车时间，观察路径规划逻辑的正确性。试验方法如下：

1）用车时间选择为当前时刻，此时系统中仅存在1 条该时刻的用车申请，由于路径Ⅰ的代价为639.68 m，小于路径Ⅱ的代价（659.15 m），根据代价最小的策略，系统规划出的路径为路径1。

2）再次提交相同时刻的用车申请，此时系统中存在2 条该时刻的相同出发地和目的地的用车申请，由于路径Ⅰ在相同时段经过的车辆数为2，经过式（6）计算，路径Ⅰ的权重为1，因此路径Ⅰ的总代价仍为639.68 m，小于路径Ⅱ的代价，因此系统规划出的路径为路径Ⅰ。

3）参照方法2，提交相同时刻的用车申请，此时系统中存在3 条该时刻相同出发地和目的地的用车申请，在此时，由于路径Ⅰ在相同时段经过的车辆数为3，经过式（6）计算，路径Ⅰ的权重值为1.5，因此路径Ⅰ的总代价为639.68×1.5×1=959.52 m，远超过路径Ⅱ的代价，因此系统规划出的路径为路径Ⅱ。

4）最后，提交不同时刻的用车申请，此时系统中仅存在1 条该时刻的用车申请，因此路径Ⅰ的权重值为1，其总代价小于路径Ⅱ，因此系统规划出的路径为路径Ⅰ。

经过上述4 种方法试验结果表明：预期理论值与程序逻辑一致，路径规划准确性得到验证。

3.3 偏离报警判断试验

以试验现场为例，在判断函数前后增加性能监听函数，经过一段时间的运行，累计记录1 243次性能记录，其中运行最大耗时为50 ms，最小耗时为13 ms，平均耗时约为25 ms。

试验结果表明：此判断计算方法可满足于现场应用要求。

4 结 语

为满足《智能化示范煤矿验收管理办法（试行）》中针对I 类煤矿无轨胶轮车运输的验收指标要求，设计了一种基于UWB 定位技术的无轨胶轮车路径偏离报警方法。该方法通过井下路网构建、车辆位置滤波、运输路径规划和偏离报警4 个步骤实现。方法在新疆伊犁一矿现场试验结果表明：①滤波函数可有效过滤定位数据中的噪声点，提高系统的鲁棒性；②运输路径规划中在估价函数中增加权重函数和判断因子参数，使得路径规划可结合巷道内行车拥堵情况，同时可避开某些无法行车的道路，实现了运输路径的智能规划和车辆调度；③偏离报警判断平均逻辑响应时间约为25 ms，可满足现场应用要求。法具有2 项优势：①实用性：通过试验验证，方法性能稳定、运行时效性高，可满足验收指标要求；②低成本：整套技术方案无需额外加装硬件装置，完全借助软件实现，具有低成本的优势。