基于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 項優勢：①實用性：通過試驗驗證，方法性能穩定、運行時效性高，可滿足驗收指標要求；②低成本：整套技術方案無需額外加裝硬件裝置，完全借助軟件實現，具有低成本的優勢。