基于改进卡尔曼滤波和状态观测器的井下信号灯闭锁控制

关键词：非煤矿山；井下斜坡道运输；信号灯闭锁控制；UWB；车辆定位；信号灯逻辑判定；强跟踪卡尔曼滤波；状态观测器

中图分类号：TD525 文献标志码：A

0引言

随着斜坡道建设技术的完善及无轨车辆在非煤矿山的广泛运用，众多矿山已经建立了用于运输的斜坡道[1-3]。然而，由于斜坡道施工难度和成本限制，国内井下斜坡道大多为单行道。除常规岔路口外，为便于井下无轨胶轮车错车，斜坡道每隔一定距离便设有避让路口，加之斜坡道路线的复杂性，导致井下斜坡道的辅助运输和监管变得复杂[4-5]。此外，井下还存在运输管理不严和司机安全意识不足等问题。若没有系统的有效监管和引导，时常发生超速、闯红灯、交通堵塞等现象，甚至导致碰撞等事故。因此，斜坡道辅助运输车辆管控系统成为矿山安全运营中不可或缺的重要组成部分[6-8]。

信号灯闭锁控制系统作为斜坡道辅助运输系统的重要组成部分，对于确保无轨胶轮车安全行驶起到至关重要的作用[9-11]。信号灯闭锁控制系统的基础是车辆定位。最简单、最早使用的定位方式是地感线圈，通过地感线圈检测固定位置是否有车辆经过，再通过PLC 统计闭锁区间内的车辆数量，结合线圈感应顺序实现信号灯判定[12]。但地感线圈只能实现定点检测，无法进行车辆识别。李朝金[13]采用RFID 车辆识别技术，通过车卡与基站的数据通信来区分车辆。佘九华等[14]提出采用位置传感器判断车辆位置的方法，较地感线圈定位范围大，可实现井下全范围车辆定位。随着无线通信技术的发展，ZigBee、WiFi 和UWB 等被应用到煤矿井下定位中[15-17]。其中，UWB 技术具有较高的静态定位精度，但其动态定位精度会随着速度提高而降低。

信号灯闭锁控制系统的另一个关键是控制逻辑。米彦军[18]提出的闭锁控制逻辑，可有效解决车辆“顶牛”问题，但逻辑仅限于闭锁区间，缺少提前警示功能，易造成后发司机看到红灯后倒车的问题。包翔宇等[19]提出的车辆放行逻辑适用于视线不好的复杂路口，实际车辆放行效率较低，易造成车辆等待时间过长的问题。

针对上述问题，本文提出一种基于改进卡尔曼滤波和状态观测器的井下信号灯闭锁控制算法。结合非煤矿山井下斜坡道运输的实际情况，提出了井下信号灯控制逻辑规则，可避免井下常见的错车问题，提高斜坡道运输效率；通过强跟踪卡尔曼滤波进行UWB 定位数据处理，以提高动态定位精度；通过预测到达门限时间，保证逻辑判定与信号灯响应的及时性；最后采用基于结果分析的远程状态观测器评估算法性能。

1基于UWB的车辆定位原理

采用UWB 技术进行车辆定位，测距采用双向飞行时间法，静态定位误差小于0.3 m，可视覆盖范围达400 m。位于直线上的车辆仅需2 个定位接收器即可完成定位，因此，将2 个UWB 定位模块合并放入定位分站中，分站左右两端各连接1 个5 m 长的定位天线（图1）用于接收和发送数据。图1 中，红色为远路口天线，蓝色为近路口天线。

2井下信号灯逻辑判定方法

2.1逻辑判定基本参数

进行信号灯逻辑判定时，首先要确定车辆行驶状态。车辆行驶状态可分为静止状态、驶入路口状态、驶离路口状态3 种。定位分站采集到连续离散定位数据并进行分析，推断出车辆行驶状态。车辆行驶状态判定流程如图5 所示。当分站检测到车辆连续2 组定位数据后，通过计算车速判定车辆是否静止，若为非静止状态，再基于前后位置变化判定是驶入还是驶离路口。

除车辆行驶状态外，系统还需要根据车辆不同位置做出不同逻辑判定。基于判定逻辑，车辆位置可划分为入列、关联闭锁及区间闭锁3 个区域，如图6 所示。当车辆驶入入列门限时，车辆进入入列状态。此状态下，该车辆的控制权归本地路口所有，本地分站同时向关联路口分享该车辆位置数据。当车辆继续驶过关联闭锁门限时，车辆进入关联闭锁状态，此时关联路口需要响应以提前作出警示。当车辆驶出区间闭锁门限时，车辆进入区间闭锁状态，驶向的关联路口需要响应以继续作出警示，其余关联路口恢复通行状态。

2.2逻辑判定规则

逻辑判定的目是引导车辆在斜坡道安全行驶，以避免产生交通问题。本文结合斜坡道运输的实际情况对逻辑判定规则进行了归纳，主要分为8 种情况：① 未入列状态；② 入列限行；③ 关联闭锁；④ 区间闭锁；⑤车辆饱和限行； ⑥ 特殊车辆跟车禁止；⑦ 辅助信号灯判定；⑧ 优先放行。

1） 未入列状态如图7 所示。当车辆驶向路口A 且未经过入列门限时，若驶离的地方为车场、工作面或与路口A 不关联的路口，则认为车辆为未入列状态，路口A 的灯1 为绿色状态。

2） 当车辆驶过入列门限后，进入路口A 的控制区域。此时车辆位置状态切换为入列限行状态（图8），路口A 的灯1 应变为红色。

3） 当车辆驶过路口A 的关联闭锁门限后，车辆切换为关联闭锁状态（图9） ，本地路口A 的灯1 释放，恢复成绿灯状态。路口B、C 与路口A 关联，当路口B、C 的分站从路口A 的分站处读到有非关联车辆（即不在关联闭锁区内的车辆）驶入关联闭锁门限时，路口B、C 的关联灯（灯1）变为红色响应状态，以警示路口内司机：关联路口可能有车驶向本路口。

4） 当车辆驶出路口A 并经过区间闭锁门限后，车辆切换为区间闭锁状态（图10）。路口B 的分站未检测到路口A 有非关联的关联闭锁车及关联的区间闭锁车，因此路口B 的灯1 恢复为绿色；而路口C 的分站读到与路口A 的闭锁区有区间闭锁车辆，路口C 的灯1 保持红色。当车辆行驶到路口C 的入列门限后，车辆位置状态重新进入入列限行状态，开始新的循环。

5） 车辆饱和限行包括闭锁区车辆超限限行和路口饱和限行。闭锁区车辆超限限行如图11（a）所示，当同时有3 辆车（闭锁区限行3 辆）驶入路口A 与路口B 之间的闭锁区时，路口A 的灯1 会变红，限制车辆继续进入闭锁区，以免造成拥堵。路口饱和限行如图11（b）所示，当路口A 出现≤1 个岔口能放行车辆的情况时，除最先有车的关联路口的关联灯（路口B 的灯1）为绿灯外，其余关联路口的关联灯（路口C、D 的灯1）均为红灯，以免造成拥堵。

6） 特殊车辆跟车限行是指炸药运输车、油罐车等驶入闭锁区间时，该闭锁区关联的信号灯会变红，禁止后续车辆跟车。

7） 当井下环境不好、斜坡道过窄或视线受阻时，可能存在司机开车过程中看不到对应闭锁区信号灯的问题，需要增加辅助信号灯。辅助信号灯按逻辑可分为2 种，一种是同步信号灯，一种是放行信号灯。同步信号灯与对应关联信号灯同步显示，放行信号灯需要根据本地路口其他信号灯情况判定，一般遵循“有红则红”的原则。

同步信号灯如图12（a）所示。由于视线受阻，区间②与区间③的司机互相看不到对方信号灯，此时可分别在区间②、③安装对方的同步信号灯，以提醒区间内的司机对向区间内有来车。放行信号灯如图12（b）所示。区间①、②为主巷道，区间②连接到停车场，可在停车场出口安装放行信号灯，用于管控停车场内车辆的驶离。当主巷道有来车时，放行信号灯变红，禁止车辆驶离，让主巷道车辆先行，以免发生“顶牛”。

8） 优先放行车辆包括优先放行闭锁区车辆和特殊车辆，二者逻辑相似。当优先放行闭锁区有车或一般闭锁区有特殊车辆时，除该闭锁区外的关联信号灯都变红，以确保其余闭锁区没有车辆进入，保证车辆享有优先通行权。

3改进卡尔曼滤波算法

3.1问题分析

为降低施工和维修难度，提高便携性，本文使用的车辆定位卡为无源定位卡，受低功耗设计限制，定位卡位置更新频率约为0.5 s/次，这就造成信号灯响应延时问题。举例，如图13 所示，T 时刻车辆位置临近门限位置，车辆位置状态未发生变化，此时系统不会进行逻辑切换，直到下一采样时刻T'，车辆已驶离门限位置较长一段距离，此时系统才会进行响应，这就造成信号灯响应延时，继而影响关联路口的及时响应。

此外，受本身动态精度降低及外在环境等因素的影响，UWB 定位数据存在动态误差大、数据失真和间断性数据丢失等问题，通过卡尔曼滤波算法可以有效改善这些问题[20-21]。但在实际运算过程中，在非线性条件下，如在临时下客、车辆启动或者路障减速等车辆加/减速的情况下，卡尔曼滤波后的数据与实际数值偏差较大。为提高信号灯闭锁控制的可靠性、及时性和UWB 动态定位精度，本文对卡尔曼滤波算法进行改进，提出一种强跟踪卡尔曼滤波算法。

4状态观测器

采用基于结果分析的状态观测器验证闭锁控制算法性能。状态观测器布置于上位机内，通过以太网TCP 协议与井下各定位分站通信，获取路口各信号灯状态、车辆实时行驶位置信息与算法过程数据等。状态观测器工作流程如下：

1） 车辆行驶到路口B 时，状态观测器采集到进入车辆的位置数据，并记录该车辆定位卡号。

2） 状态观测器采集关联路口信号灯状态信息，缓存采样周期内井下各岔路口带时间戳的信号灯状态信息。本文设置采样时间间隔为1 s，采样周期为15 s。

3） 车辆行驶到路口B 的相邻路口A 或C 时，状态观测器再次采集到该车辆信息。

4） 获取当前时间前关联路口信号灯状态信息。

5） 将通过车辆闭锁逻辑关系判断的信号灯状态与采集的信号灯状态进行对比，若一致则判定信号控制闭锁成功，否则为闭锁失败。

6） 读取并保存强跟踪卡尔曼滤波算法过程数据，用于算法滤波效果的追踪反馈和参数调整。

5仿真与工业现场验证

5.1仿真分析

根据工业现场数据进行仿真模拟，模拟车辆以6 m/s 的初速度进入路口，在第27 s 时以−0.6 m/s² 的加速度行驶，在路口中心停留40 s 后，以0.6 m/s² 的加速度驶离路口。测量噪声按速度大小线性增加且符合正态分布，噪声均值为0，车辆静止时噪声误差标准差为0.1 m，车辆速度为6 m/s 时噪声误差标准差为0.3 m。采用常见的加权递推平均滤波算法、卡尔曼滤波算法与强跟踪卡尔曼滤波算法进行对比分析。

5.1.1定位性能

UWB 定位模块测得的车辆距离滤波前后对比如图14 所示。可看出4 种情况下的距离几乎一致。将测得的距离减去模拟时的给定值，得到测距误差，如图15所示。

当车辆以6 m/s 速度稳定运行时，经过加权递推平均滤波后的误差（均方根平均值）不降反增，这是因为平均值计算导致数据滞后；而经过卡尔曼滤波后的误差由30.62 cm 降到20.18 cm， 精度提高34.09%； 经过强跟踪卡尔曼滤波后的误差降到22.76 cm，精度提高25.67%。当车辆静止时，经过加权递推平均滤波后的误差由10.04 cm 降到10.28 cm，精度提高7.57%； 经过卡尔曼滤波后的误差降到7.10 cm，精度提高29.28%；经过强跟踪卡尔曼滤波后的误差降到7.31 cm，精度提高27.19%。与加权递推平均滤波相比，卡尔曼滤波在动态滤波方面优势较大，虽然强跟踪卡尔曼滤波精度不如卡尔曼滤波，但当车辆减速或加速时，强跟踪卡尔曼滤波适应能力更强。

5.1.2速度跟踪性

能车辆速度滤波前后对比如图16 所示。当车辆以6 m/s 速度稳定运行时，经过加权递推平均滤波后的速度误差由16.81 cm/s 降到14.33 cm/s，精度提高14.74%；经卡尔曼滤波后速度误差降到11.64 cm/s，精度提高30.75%；经过强跟踪卡尔曼滤波后的速度误差降到12.56 cm/s，精度提高25.28%。当车辆静止时，经加权递推平均滤波后速度误差由10.94 cm/s 降到8.71 cm/s，精度提高20.38%；经过卡尔曼滤波后的速度误差降到6.65 cm/s，精度提高39.21%；经过强跟踪卡尔曼滤波后的速度误差降到7.14 cm/s，精度提高34.73%。由图16 中放大部分可知，当车速变动时，强跟踪卡尔曼滤波响应更快。

5.2工业现场验证

在西部矿业锡铁山分公司斜坡道进行了工业性试验。西部矿业锡铁山分公司运输斜坡道全长10 km，建有31 个错车路口，5 个三岔口。本次工业性试验共安装定位分站36 台，信号灯78 台，其余硬件主要包括上位机、地面核心交换机、环网接入器、不间断电源箱及车辆定位卡等，如图17 所示。

5.2.1位置及速度滤波数据分析

状态观测器记录的工业现场数据如图18 所示，可看出现场试验结果与仿真结果基本相符。

状态观测器记录的车辆经过门限的预测时间及前后采样时间对比见表1，采样时间间隔为500 ms。当前一时刻采样结束后，系统通过算法预测到车辆经过门限的时间，并在对应时间点切换逻辑做出响应，不用等到后一时刻采样的时间点，这极大提高了信号灯闭锁控制的准确性与及时性。其中，第4 组数据的前后时刻间隔为1 029 ms，表明系统采样时丢失中间1 帧数据，通过预测时间可有效解决采样失败后的控制延时问题。

5.2.2信号灯闭锁效果分析

为了进一步验证优化后的结果，通过远程状态观测器获取信号灯控制状态，统计井下30 d 内的信号灯闭锁次数及闭锁成功次数，结果见表2。

滤波前后信号灯闭锁成功率对比如图19 所示，可看出算法改进后的闭锁成功率达99.5% 以上，远高于卡尔曼滤波算法。同时，状态观察器的数据分析可为井下分站故障诊断、数据优化迭代提供数据支撑。

6结论

1） 分析了基于UWB 的井下车辆定位原理，提出了适合非煤矿山井下特点的信号灯逻辑判定方法。

2） 针对UWB 动态定位精度不足及车辆定位数据丢失等问题，提出一种强跟踪卡尔曼滤波算法，通过强跟踪自适应方式计算预测误差，提高定位精度，通过准确预测车辆到达门限的时间，使斜坡道系统信号灯门限逻辑判定响应更快，闭锁成功率更高。

3） 采用远程状态观测器评估信号灯闭锁控制效果，基于时域自动跟踪的统计，实现了闭锁可靠性的量化评估，为井下分站故障诊断、数据优化迭代提供数据支撑。

4） 仿真结果表明，改进卡尔曼滤波算法后，车辆动态与静态位置误差分别降低25.67% 和27.19%，动态与静态速度误差分别降低25.28% 和34.73%，信号灯门限逻辑响应更快。井下工业性试验和应用结果表明，采用强跟踪尔曼滤算法后，井下信号闭锁成功率达99.5% 以上，有效提高了井下斜坡道岔路口信号闭锁控制的可靠性，保障了井下车辆行驶安全。