基于改進卡爾曼濾波和狀態(tài)觀測器的井下信號燈閉鎖控制

關鍵詞：非煤礦山；井下斜坡道運輸；信號燈閉鎖控制；UWB；車輛定位；信號燈邏輯判定；強跟蹤卡爾曼濾波；狀態(tài)觀測器

中圖分類號：TD525 文獻標志碼：A

0引言

隨著斜坡道建設技術的完善及無軌車輛在非煤礦山的廣泛運用，眾多礦山已經(jīng)建立了用于運輸?shù)男逼碌繹1-3]。然而，由于斜坡道施工難度和成本限制，國內井下斜坡道大多為單行道。除常規(guī)岔路口外，為便于井下無軌膠輪車錯車，斜坡道每隔一定距離便設有避讓路口，加之斜坡道路線的復雜性，導致井下斜坡道的輔助運輸和監(jiān)管變得復雜[4-5]。此外，井下還存在運輸管理不嚴和司機安全意識不足等問題。若沒有系統(tǒng)的有效監(jiān)管和引導，時常發(fā)生超速、闖紅燈、交通堵塞等現(xiàn)象，甚至導致碰撞等事故。因此，斜坡道輔助運輸車輛管控系統(tǒng)成為礦山安全運營中不可或缺的重要組成部分[6-8]。

信號燈閉鎖控制系統(tǒng)作為斜坡道輔助運輸系統(tǒng)的重要組成部分，對于確保無軌膠輪車安全行駛起到至關重要的作用[9-11]。信號燈閉鎖控制系統(tǒng)的基礎是車輛定位。最簡單、最早使用的定位方式是地感線圈，通過地感線圈檢測固定位置是否有車輛經(jīng)過，再通過PLC 統(tǒng)計閉鎖區(qū)間內的車輛數(shù)量，結合線圈感應順序實現(xiàn)信號燈判定[12]。但地感線圈只能實現(xiàn)定點檢測，無法進行車輛識別。李朝金[13]采用RFID 車輛識別技術，通過車卡與基站的數(shù)據(jù)通信來區(qū)分車輛。佘九華等[14]提出采用位置傳感器判斷車輛位置的方法，較地感線圈定位范圍大，可實現(xiàn)井下全范圍車輛定位。隨著無線通信技術的發(fā)展，ZigBee、WiFi 和UWB 等被應用到煤礦井下定位中[15-17]。其中，UWB 技術具有較高的靜態(tài)定位精度，但其動態(tài)定位精度會隨著速度提高而降低。

信號燈閉鎖控制系統(tǒng)的另一個關鍵是控制邏輯。米彥軍[18]提出的閉鎖控制邏輯，可有效解決車輛“頂?！眴栴}，但邏輯僅限于閉鎖區(qū)間，缺少提前警示功能，易造成后發(fā)司機看到紅燈后倒車的問題。包翔宇等[19]提出的車輛放行邏輯適用于視線不好的復雜路口，實際車輛放行效率較低，易造成車輛等待時間過長的問題。

針對上述問題，本文提出一種基于改進卡爾曼濾波和狀態(tài)觀測器的井下信號燈閉鎖控制算法。結合非煤礦山井下斜坡道運輸?shù)膶嶋H情況，提出了井下信號燈控制邏輯規(guī)則，可避免井下常見的錯車問題，提高斜坡道運輸效率；通過強跟蹤卡爾曼濾波進行UWB 定位數(shù)據(jù)處理，以提高動態(tài)定位精度；通過預測到達門限時間，保證邏輯判定與信號燈響應的及時性；最后采用基于結果分析的遠程狀態(tài)觀測器評估算法性能。

1基于UWB的車輛定位原理

采用UWB 技術進行車輛定位，測距采用雙向飛行時間法，靜態(tài)定位誤差小于0.3 m，可視覆蓋范圍達400 m。位于直線上的車輛僅需2 個定位接收器即可完成定位，因此，將2 個UWB 定位模塊合并放入定位分站中，分站左右兩端各連接1 個5 m 長的定位天線（圖1）用于接收和發(fā)送數(shù)據(jù)。圖1 中，紅色為遠路口天線，藍色為近路口天線。

2井下信號燈邏輯判定方法

2.1邏輯判定基本參數(shù)

進行信號燈邏輯判定時，首先要確定車輛行駛狀態(tài)。車輛行駛狀態(tài)可分為靜止狀態(tài)、駛入路口狀態(tài)、駛離路口狀態(tài)3 種。定位分站采集到連續(xù)離散定位數(shù)據(jù)并進行分析，推斷出車輛行駛狀態(tài)。車輛行駛狀態(tài)判定流程如圖5 所示。當分站檢測到車輛連續(xù)2 組定位數(shù)據(jù)后，通過計算車速判定車輛是否靜止，若為非靜止狀態(tài)，再基于前后位置變化判定是駛入還是駛離路口。

除車輛行駛狀態(tài)外，系統(tǒng)還需要根據(jù)車輛不同位置做出不同邏輯判定。基于判定邏輯，車輛位置可劃分為入列、關聯(lián)閉鎖及區(qū)間閉鎖3 個區(qū)域，如圖6 所示。當車輛駛入入列門限時，車輛進入入列狀態(tài)。此狀態(tài)下，該車輛的控制權歸本地路口所有，本地分站同時向關聯(lián)路口分享該車輛位置數(shù)據(jù)。當車輛繼續(xù)駛過關聯(lián)閉鎖門限時，車輛進入關聯(lián)閉鎖狀態(tài)，此時關聯(lián)路口需要響應以提前作出警示。當車輛駛出區(qū)間閉鎖門限時，車輛進入?yún)^(qū)間閉鎖狀態(tài)，駛向的關聯(lián)路口需要響應以繼續(xù)作出警示，其余關聯(lián)路口恢復通行狀態(tài)。

2.2邏輯判定規(guī)則

邏輯判定的目是引導車輛在斜坡道安全行駛，以避免產生交通問題。本文結合斜坡道運輸?shù)膶嶋H情況對邏輯判定規(guī)則進行了歸納，主要分為8 種情況：① 未入列狀態(tài)；② 入列限行；③ 關聯(lián)閉鎖；④ 區(qū)間閉鎖；⑤車輛飽和限行； ⑥ 特殊車輛跟車禁止；⑦ 輔助信號燈判定；⑧ 優(yōu)先放行。

1） 未入列狀態(tài)如圖7 所示。當車輛駛向路口A 且未經(jīng)過入列門限時，若駛離的地方為車場、工作面或與路口A 不關聯(lián)的路口，則認為車輛為未入列狀態(tài)，路口A 的燈1 為綠色狀態(tài)。

2） 當車輛駛過入列門限后，進入路口A 的控制區(qū)域。此時車輛位置狀態(tài)切換為入列限行狀態(tài)（圖8），路口A 的燈1 應變?yōu)榧t色。

3） 當車輛駛過路口A 的關聯(lián)閉鎖門限后，車輛切換為關聯(lián)閉鎖狀態(tài)（圖9） ，本地路口A 的燈1 釋放，恢復成綠燈狀態(tài)。路口B、C 與路口A 關聯(lián)，當路口B、C 的分站從路口A 的分站處讀到有非關聯(lián)車輛（即不在關聯(lián)閉鎖區(qū)內的車輛）駛入關聯(lián)閉鎖門限時，路口B、C 的關聯(lián)燈（燈1）變?yōu)榧t色響應狀態(tài)，以警示路口內司機：關聯(lián)路口可能有車駛向本路口。

4） 當車輛駛出路口A 并經(jīng)過區(qū)間閉鎖門限后，車輛切換為區(qū)間閉鎖狀態(tài)（圖10）。路口B 的分站未檢測到路口A 有非關聯(lián)的關聯(lián)閉鎖車及關聯(lián)的區(qū)間閉鎖車，因此路口B 的燈1 恢復為綠色；而路口C 的分站讀到與路口A 的閉鎖區(qū)有區(qū)間閉鎖車輛，路口C 的燈1 保持紅色。當車輛行駛到路口C 的入列門限后，車輛位置狀態(tài)重新進入入列限行狀態(tài)，開始新的循環(huán)。

5） 車輛飽和限行包括閉鎖區(qū)車輛超限限行和路口飽和限行。閉鎖區(qū)車輛超限限行如圖11（a）所示，當同時有3 輛車（閉鎖區(qū)限行3 輛）駛入路口A 與路口B 之間的閉鎖區(qū)時，路口A 的燈1 會變紅，限制車輛繼續(xù)進入閉鎖區(qū)，以免造成擁堵。路口飽和限行如圖11（b）所示，當路口A 出現(xiàn)≤1 個岔口能放行車輛的情況時，除最先有車的關聯(lián)路口的關聯(lián)燈（路口B 的燈1）為綠燈外，其余關聯(lián)路口的關聯(lián)燈（路口C、D 的燈1）均為紅燈，以免造成擁堵。

6） 特殊車輛跟車限行是指炸藥運輸車、油罐車等駛入閉鎖區(qū)間時，該閉鎖區(qū)關聯(lián)的信號燈會變紅，禁止后續(xù)車輛跟車。

7） 當井下環(huán)境不好、斜坡道過窄或視線受阻時，可能存在司機開車過程中看不到對應閉鎖區(qū)信號燈的問題，需要增加輔助信號燈。輔助信號燈按邏輯可分為2 種，一種是同步信號燈，一種是放行信號燈。同步信號燈與對應關聯(lián)信號燈同步顯示，放行信號燈需要根據(jù)本地路口其他信號燈情況判定，一般遵循“有紅則紅”的原則。

同步信號燈如圖12（a）所示。由于視線受阻，區(qū)間②與區(qū)間③的司機互相看不到對方信號燈，此時可分別在區(qū)間②、③安裝對方的同步信號燈，以提醒區(qū)間內的司機對向區(qū)間內有來車。放行信號燈如圖12（b）所示。區(qū)間①、②為主巷道，區(qū)間②連接到停車場，可在停車場出口安裝放行信號燈，用于管控停車場內車輛的駛離。當主巷道有來車時，放行信號燈變紅，禁止車輛駛離，讓主巷道車輛先行，以免發(fā)生“頂?！薄?/p>

8） 優(yōu)先放行車輛包括優(yōu)先放行閉鎖區(qū)車輛和特殊車輛，二者邏輯相似。當優(yōu)先放行閉鎖區(qū)有車或一般閉鎖區(qū)有特殊車輛時，除該閉鎖區(qū)外的關聯(lián)信號燈都變紅，以確保其余閉鎖區(qū)沒有車輛進入，保證車輛享有優(yōu)先通行權。

3改進卡爾曼濾波算法

3.1問題分析

為降低施工和維修難度，提高便攜性，本文使用的車輛定位卡為無源定位卡，受低功耗設計限制，定位卡位置更新頻率約為0.5 s/次，這就造成信號燈響應延時問題。舉例，如圖13 所示，T 時刻車輛位置臨近門限位置，車輛位置狀態(tài)未發(fā)生變化，此時系統(tǒng)不會進行邏輯切換，直到下一采樣時刻T'，車輛已駛離門限位置較長一段距離，此時系統(tǒng)才會進行響應，這就造成信號燈響應延時，繼而影響關聯(lián)路口的及時響應。

此外，受本身動態(tài)精度降低及外在環(huán)境等因素的影響，UWB 定位數(shù)據(jù)存在動態(tài)誤差大、數(shù)據(jù)失真和間斷性數(shù)據(jù)丟失等問題，通過卡爾曼濾波算法可以有效改善這些問題[20-21]。但在實際運算過程中，在非線性條件下，如在臨時下客、車輛啟動或者路障減速等車輛加/減速的情況下，卡爾曼濾波后的數(shù)據(jù)與實際數(shù)值偏差較大。為提高信號燈閉鎖控制的可靠性、及時性和UWB 動態(tài)定位精度，本文對卡爾曼濾波算法進行改進，提出一種強跟蹤卡爾曼濾波算法。

4狀態(tài)觀測器

采用基于結果分析的狀態(tài)觀測器驗證閉鎖控制算法性能。狀態(tài)觀測器布置于上位機內，通過以太網(wǎng)TCP 協(xié)議與井下各定位分站通信，獲取路口各信號燈狀態(tài)、車輛實時行駛位置信息與算法過程數(shù)據(jù)等。狀態(tài)觀測器工作流程如下：

1） 車輛行駛到路口B 時，狀態(tài)觀測器采集到進入車輛的位置數(shù)據(jù)，并記錄該車輛定位卡號。

2） 狀態(tài)觀測器采集關聯(lián)路口信號燈狀態(tài)信息，緩存采樣周期內井下各岔路口帶時間戳的信號燈狀態(tài)信息。本文設置采樣時間間隔為1 s，采樣周期為15 s。

3） 車輛行駛到路口B 的相鄰路口A 或C 時，狀態(tài)觀測器再次采集到該車輛信息。

4） 獲取當前時間前關聯(lián)路口信號燈狀態(tài)信息。

5） 將通過車輛閉鎖邏輯關系判斷的信號燈狀態(tài)與采集的信號燈狀態(tài)進行對比，若一致則判定信號控制閉鎖成功，否則為閉鎖失敗。

6） 讀取并保存強跟蹤卡爾曼濾波算法過程數(shù)據(jù)，用于算法濾波效果的追蹤反饋和參數(shù)調整。

5仿真與工業(yè)現(xiàn)場驗證

5.1仿真分析

根據(jù)工業(yè)現(xiàn)場數(shù)據(jù)進行仿真模擬，模擬車輛以6 m/s 的初速度進入路口，在第27 s 時以?0.6 m/s² 的加速度行駛，在路口中心停留40 s 后，以0.6 m/s² 的加速度駛離路口。測量噪聲按速度大小線性增加且符合正態(tài)分布，噪聲均值為0，車輛靜止時噪聲誤差標準差為0.1 m，車輛速度為6 m/s 時噪聲誤差標準差為0.3 m。采用常見的加權遞推平均濾波算法、卡爾曼濾波算法與強跟蹤卡爾曼濾波算法進行對比分析。

5.1.1定位性能

UWB 定位模塊測得的車輛距離濾波前后對比如圖14 所示?？煽闯? 種情況下的距離幾乎一致。將測得的距離減去模擬時的給定值，得到測距誤差，如圖15所示。

當車輛以6 m/s 速度穩(wěn)定運行時，經(jīng)過加權遞推平均濾波后的誤差（均方根平均值）不降反增，這是因為平均值計算導致數(shù)據(jù)滯后；而經(jīng)過卡爾曼濾波后的誤差由30.62 cm 降到20.18 cm， 精度提高34.09%； 經(jīng)過強跟蹤卡爾曼濾波后的誤差降到22.76 cm，精度提高25.67%。當車輛靜止時，經(jīng)過加權遞推平均濾波后的誤差由10.04 cm 降到10.28 cm，精度提高7.57%； 經(jīng)過卡爾曼濾波后的誤差降到7.10 cm，精度提高29.28%；經(jīng)過強跟蹤卡爾曼濾波后的誤差降到7.31 cm，精度提高27.19%。與加權遞推平均濾波相比，卡爾曼濾波在動態(tài)濾波方面優(yōu)勢較大，雖然強跟蹤卡爾曼濾波精度不如卡爾曼濾波，但當車輛減速或加速時，強跟蹤卡爾曼濾波適應能力更強。

5.1.2速度跟蹤性

能車輛速度濾波前后對比如圖16 所示。當車輛以6 m/s 速度穩(wěn)定運行時，經(jīng)過加權遞推平均濾波后的速度誤差由16.81 cm/s 降到14.33 cm/s，精度提高14.74%；經(jīng)卡爾曼濾波后速度誤差降到11.64 cm/s，精度提高30.75%；經(jīng)過強跟蹤卡爾曼濾波后的速度誤差降到12.56 cm/s，精度提高25.28%。當車輛靜止時，經(jīng)加權遞推平均濾波后速度誤差由10.94 cm/s 降到8.71 cm/s，精度提高20.38%；經(jīng)過卡爾曼濾波后的速度誤差降到6.65 cm/s，精度提高39.21%；經(jīng)過強跟蹤卡爾曼濾波后的速度誤差降到7.14 cm/s，精度提高34.73%。由圖16 中放大部分可知，當車速變動時，強跟蹤卡爾曼濾波響應更快。

5.2工業(yè)現(xiàn)場驗證

在西部礦業(yè)錫鐵山分公司斜坡道進行了工業(yè)性試驗。西部礦業(yè)錫鐵山分公司運輸斜坡道全長10 km，建有31 個錯車路口，5 個三岔口。本次工業(yè)性試驗共安裝定位分站36 臺，信號燈78 臺，其余硬件主要包括上位機、地面核心交換機、環(huán)網(wǎng)接入器、不間斷電源箱及車輛定位卡等，如圖17 所示。

5.2.1位置及速度濾波數(shù)據(jù)分析

狀態(tài)觀測器記錄的工業(yè)現(xiàn)場數(shù)據(jù)如圖18 所示，可看出現(xiàn)場試驗結果與仿真結果基本相符。

狀態(tài)觀測器記錄的車輛經(jīng)過門限的預測時間及前后采樣時間對比見表1，采樣時間間隔為500 ms。當前一時刻采樣結束后，系統(tǒng)通過算法預測到車輛經(jīng)過門限的時間，并在對應時間點切換邏輯做出響應，不用等到后一時刻采樣的時間點，這極大提高了信號燈閉鎖控制的準確性與及時性。其中，第4 組數(shù)據(jù)的前后時刻間隔為1 029 ms，表明系統(tǒng)采樣時丟失中間1 幀數(shù)據(jù)，通過預測時間可有效解決采樣失敗后的控制延時問題。

5.2.2信號燈閉鎖效果分析

為了進一步驗證優(yōu)化后的結果，通過遠程狀態(tài)觀測器獲取信號燈控制狀態(tài)，統(tǒng)計井下30 d 內的信號燈閉鎖次數(shù)及閉鎖成功次數(shù)，結果見表2。

濾波前后信號燈閉鎖成功率對比如圖19 所示，可看出算法改進后的閉鎖成功率達99.5% 以上，遠高于卡爾曼濾波算法。同時，狀態(tài)觀察器的數(shù)據(jù)分析可為井下分站故障診斷、數(shù)據(jù)優(yōu)化迭代提供數(shù)據(jù)支撐。

6結論

1） 分析了基于UWB 的井下車輛定位原理，提出了適合非煤礦山井下特點的信號燈邏輯判定方法。

2） 針對UWB 動態(tài)定位精度不足及車輛定位數(shù)據(jù)丟失等問題，提出一種強跟蹤卡爾曼濾波算法，通過強跟蹤自適應方式計算預測誤差，提高定位精度，通過準確預測車輛到達門限的時間，使斜坡道系統(tǒng)信號燈門限邏輯判定響應更快，閉鎖成功率更高。

3） 采用遠程狀態(tài)觀測器評估信號燈閉鎖控制效果，基于時域自動跟蹤的統(tǒng)計，實現(xiàn)了閉鎖可靠性的量化評估，為井下分站故障診斷、數(shù)據(jù)優(yōu)化迭代提供數(shù)據(jù)支撐。

4） 仿真結果表明，改進卡爾曼濾波算法后，車輛動態(tài)與靜態(tài)位置誤差分別降低25.67% 和27.19%，動態(tài)與靜態(tài)速度誤差分別降低25.28% 和34.73%，信號燈門限邏輯響應更快。井下工業(yè)性試驗和應用結果表明，采用強跟蹤爾曼濾算法后，井下信號閉鎖成功率達99.5% 以上，有效提高了井下斜坡道岔路口信號閉鎖控制的可靠性，保障了井下車輛行駛安全。