煤礦井下連續采煤機定位方法研究

賀建偉

(1.中國煤炭科工集團太原研究院有限公司， 山西 太原 030006；2.山西天地煤機裝備有限公司， 山西 太原 030006)

0 引言

連續采煤機適用于“三下”壓煤、不規則塊段及殘采區的采煤作業，也可用于巷道掘進。連續采煤機在掘進或采煤過程中大多使用傳統的點激光光源對其方向進行指示，采掘司機通過目視斷面上的激光光斑控制截割[1]。該方法由于掘進工作面粉塵較大、作業現場光線差、操作人員視線受裝備機身遮擋等，難以準確判斷截割部位置。同時隨著掘進裝備的掘進深入，點激光光源需要多次移站跟進，指示精度降低，且測量工作量大，無法滿足工作面無人化遠程操控的要求，因此，如何實現連續采煤機井下精準定位定向成為目前亟需解決的關鍵問題。

煤礦井下由于環境特殊且條件惡劣，GPS等地面應用較多的導航定位系統無法在井下完全發揮作用[2]。慣性導航系統作為一種自主式定位定向系統，適用范圍廣，不依賴外界信息便可獲得載體的姿態、速度及位置信息，對煤礦井下采掘裝備進行定位定向指示具有較好的效果。

當前較為主流的井下采掘裝備定位方法大多依附于慣導系統進行[2]，但慣導系統需通過對測量得到的加速度信息進行二次積分才可獲得載體的位置信息，存在隨時間累積會產生較大位置漂移誤差的缺陷[3]。目前多使用基于慣導的多傳感器組合定位方法解決單獨使用慣導系統進行定位存在的位置漂移誤差問題。文獻[4]將慣導和里程計相結合進行井下采掘裝備的定位研究，利用卡爾曼濾波結合里程計測得的里程信息對慣導相應測量數據進行校正，但采掘設備在井下易受浮煤影響造成履帶打滑及滑移現象，此時里程計的測量誤差會遠大于慣導自身的漂移誤差，僅依靠里程計與慣導進行組合會降低整個定位方法的測量精度。文獻[5]采用聯邦濾波將慣性測量單元(IMU)分別與立體相機、里程計構成子濾波器，避免了因里程計打滑或滑移失效產生的較大測量誤差污染整個定位系統[6]，但井下視覺定位方法目前還處于理論研究階段，未出現成功應用的工業性案例，故其可實施性與可靠性均屬未知。文獻[7]采用集中式濾波處理慣性導航系統、里程計及測距雷達所獲取的各類傳感器的測量信息，在進行卡爾曼濾波狀態方程建模時維數較高，整個系統計算量較大且容錯性差。

綜上可知，目前多采用里程計、視覺測量、測距雷達等作為輔助測量方式，與慣導組合測量降低慣導隨時間累積所產生的位置漂移誤差，其中輔助測量方式要求可靠性與測量精度均較高。對不同測量方式得到的數據目前多使用集中式卡爾曼濾波進行濾波解算，但集中式卡爾曼濾波的計算量較大且容錯性較差。針對上述問題，本文提出了一種由捷聯式慣導、測距雷達及里程計組成的連續采煤機定位方法，使用測距雷達與里程計測量連續采煤機的位移量，通過航位推算法推算出機身的二維位置信息，利用二維位置信息對慣導位置解算信息進行反饋修正，減小慣導隨時間累積所產生的位置漂移誤差[8]。采用聯邦式卡爾曼濾波對各傳感器采集的位姿信息進行濾波與融合[9-11]，通過降低方程維數減少計算量,在保證精度的同時還具備一定的容錯性。該定位方法可為連續采煤機在巷道約束環境下的位姿精確測量、巷道自動成形控制和遠程控制奠定基礎。

1 連續采煤機定位方法

連續采煤機定位方法結構組成如圖1所示。使用測距雷達與里程計同時輔助修正慣導系統，采用聯邦式卡爾曼濾波將慣導分別與測距雷達、里程計組成子濾波器。在里程計因履帶打滑或滑移產生較大的測量誤差時，提高測距雷達與慣導所組成的子濾波器1的置信度；在測距雷達因人員移動或其他障礙物遮擋而導致測量誤差較大時，里程計及慣導所組成的子濾波器2采取較高的置信度，防止因個別輔助傳感器測量失效產生較大誤差而降低定位方法的精度。

1-連續采煤機本體； 2-慣導組件； 3-電控箱； 4-里程計；

1.1 慣導系統

慣導系統根據安裝方式可分為平臺式與捷聯式，根據所采用的陀螺儀種類不同又可分為光纖級、激光級等[12]。本文采用某型號光纖級捷聯式慣導，慣導本體及交換機直接固連于連續采煤機機身中部右側位置，緊鄰電控箱，可實時測量機身的姿態、速度及位置信息。該定位方法取測距雷達、里程計與慣導系統所推算得到的機身位置信息差值作為濾波輸入來進行濾波處理[13]，各傳感器所采集到的位姿信息可通過交換機傳送至巷道后方安全位置或地面控制室內的上位機進行顯示與控制。

1.2 測距雷達

測距雷達安裝于連續采煤機機身尾部右后下方，將規格為1 m×1 m的標靶放置于與測距雷達處于同一水平線的正后方。連續采煤機行進過程中通過測距雷達不斷測量與后方標靶的距離，所得的當前測量值與前一次采樣頻率下的測量值作差便可得到連續采煤機的相對位置增量。利用航位推算法對相對位置增量進行解算便可得到機體坐標系下的位置信息，借助慣導解算中的姿態矩陣實時地將機體坐標系下的位置信息轉換為導航坐標系下的信息后再進行濾波解算[14]。

1.3 里程計

里程計安裝于履帶驅動齒輪處，輸出量為連續采煤機短時間內路程增量，通過航位推算法得出所需要的位置信息[15]。里程計由于固連于車體履帶轉軸處，所測得的信息量依舊是在機體坐標系下的，同樣需借助慣導解算中的姿態矩陣實時地將其轉換為導航坐標系下的信息量后再進行濾波解算。

2 航位推算法

航位推算法是一種使用航向和里程信息來計算運載體相對于起點的相對位置的定位方法，原理如圖2所示。已知起始位置坐標(x0，y0)及起始方位角θ0，通過測量行駛距離Si及方位角變化值Δθi，結合二者值便可推算出下一時刻位置坐標(xk，yk)和方位角θk。

(1)

(2)

θk=θk-1+Δθk-1k=1,2,…

(3)

式中：θk-1為上一時刻方位角，k為不同時刻；Δθk-1為上一時刻至當前時刻方位角變化。

圖2 航位推算法原理Fig.2 Principle of dead reckoning algorithm

3 濾波器模型構建

3.1 聯邦式卡爾曼濾波原理

(4)

圖3 聯邦式卡爾曼濾波器原理Fig.3 Federated Kalman filter principle

為了簡化計算，在信息分配上，使用平均分配方式分配子濾波器與主濾波器間的方差矩陣值，即

βj=βm=1/3

(5)

3.2 誤差分析

定位方法中的誤差主要來源于捷聯慣導自身器件誤差及算法更新誤差、里程計及測距雷達測量誤差，通過誤差分析可為濾波器建模奠定基礎。

3.2.1 慣性導航誤差

其中關于捷聯式慣導更新算法中的姿態、速度及位置誤差推導見文獻[16]。

3.2.2 測距雷達測量誤差

測距雷達測量誤差主要為自身固有誤差δρ及初始輸入誤差δη。經分析這2類誤差特性，同時為簡化模型及降低計算量，將其設置為固定常值對誤差進行補償，以消除其對實驗結果的影響。故在后續建模過程中，將測距雷達這2類誤差均設為固定常值。

3.2.3 里程計測量誤差

里程計的測量誤差主要包括標度因數誤差δKD和安裝誤差角δα，設δα=[δαθδαγδαψ]，δαθ,δαγ和δαψ分別為安裝角誤差中的俯仰角、橫滾角和航向角誤差。其中里程計的橫滾角安裝誤差一般不會對測距雷達的位置輸出量造成干擾，故這里不考慮橫滾角安裝誤差。俯仰角安裝誤差及航向角安裝誤差對里程計測量誤差有影響，將δKD、δαθ、δαψ三者均設為隨機常值。

3.3 濾波器模型構建

聯邦式卡爾曼濾波作為卡爾曼濾波的變式，其與集中式卡爾曼濾波的區別在于聯邦式卡爾曼濾波子濾波器先進行一級濾波，而后進入聯邦式卡爾曼濾波主濾波器進行二級濾波，同時將全局濾波最優估計及誤差方差矩陣放大后反饋給子濾波器進行下一次濾波[18-19]。

2個子濾波器均采用間接法進行卡爾曼濾波器數學建模，即狀態變量由導航參數的誤差構成，同時對該連續變量信息進行離散化，以便于卡爾曼濾波進行分步迭代更新[20]，本文為描述簡便直接給出離散化后的數學模型。

3.3.1 子濾波器1數學模型

狀態方程為

Xk1=Φk1/k1-1Xk1-1+Γk1-1Wk1-1

(6)

式中：Φk1/k1-1為子濾波器1的十五維狀態轉移矩陣；Γk1-1為子濾波器1的噪聲驅動矩陣；Wk1-1為子濾波器1的陀螺與加速度計的白噪聲矩陣。

狀態向量為

(7)

量測方程為

Zk1=Hk1Xk1+Vk1

(8)

式中：Zk1為子濾波器1以慣導與測距雷達所輸出的位置量作差得到的觀測量；Hk1為子濾波器1的量測矩陣；Vk1為子濾波器1的測距雷達的位置量測白噪聲矩陣。

3.3.2 子濾波器2數學模型

狀態方程為

Xk2=Φk2/k2-1Xk2-1+Γk2-1Wk2-1

(9)

式中：Φk2/k2-1為子濾波器2的二十一維狀態轉移矩陣；Γk2-1為子濾波器2的噪聲驅動矩陣；Wk2-1為子濾波器2的陀螺與加速度計的白噪聲矩陣[20]。

狀態向量為

(10)

量測方程為

Zk2=Hk2Xk+Vk2

(11)

式中：Zk2為觀測量，子濾波器2以慣導與里程計根據航位推算法推算出的位置量作差作為觀測量；Hk2為子濾波器2的量測矩陣；Vk2為子濾波器2的里程計的位置量測白噪聲矩陣。

4 仿真

為檢驗連續采煤機定位方法的定位精度及2類卡爾曼濾波的容錯性，本文進行了Matlab 仿真實驗。仿真時長取20 min即1 200 s，模擬連續采煤機實際掘進過程中不停機生產時長。在仿真時間為5 min即300 s時加入里程計測量誤差擾動，持續時間為30 s，模擬連續采煤機打滑情況。慣導關鍵精度參數設置：陀螺儀的常值漂移εb設置為0.001 °/h, 加速度計零偏設置為1×10-4g(g為重力加速度)。

4.1 姿態誤差

集中式與聯邦式卡爾曼濾波航向角誤差對比如圖4所示。從圖4可看出，在前200 s 時，2類濾波波動均較大，其中集中式卡爾曼濾波最大波動可達10′。在200 s后，2類濾波的濾波值逐漸趨于平穩，在0～-2.5′內小幅波動。在300 s時加入里程計測量誤差擾動后，與聯邦式卡爾曼濾波相比，集中式卡爾曼濾波出現的突變值較大，最大可達-8′，且在30 s內加入誤差的過程中持續出現突變值，而聯邦式卡爾曼濾波出現突變后，短時間內便恢復至正常濾波值范圍。

圖4 航向角誤差對比Fig.4 Comparison of heading angle error

集中式與聯邦式卡爾曼濾波橫滾角誤差對比如圖5所示。從圖5可看出，在前80 s時，2類濾波的濾波值波動均較大，集中式卡爾曼濾波波動范圍更大，最高可達14′；在80 s后，2類濾波的濾波值恢復至-1～1′內波動。在300 s時加入里程計測量誤差擾動后，2類濾波均出現了瞬時突變，但聯邦式卡爾曼濾波突變值較小且持續時間較短，且在較短時間內恢復到了正常值。

圖5 橫滾角誤差對比 Fig.5 Comparison of roll angle error

集中式與聯邦式卡爾曼濾波俯仰角誤差對比如圖6所示。從圖6可看出，在濾波初期240 s內，2類濾波的濾波值波動均較大，且與航向角、橫滾角相比，波動峰值更大，最大處可達270′。在240 s后，濾波波動值穩定于15′附近。在300 s時加入里程計測量誤差擾動后，與前2個姿態角波動相類似，集中式卡爾曼濾波產生的瞬時突變值較大且持續時間長，而聯邦式卡爾曼濾波則可以在短時間內恢復至正常波動范圍內。

由3個姿態角誤差可得出，誤差角在濾波穩定后可達到角分級，天向通道中的俯仰角誤差在前期濾波過程中波動較大，最大波動值可達270′，穩定后在15′內波動；東向通道中的航向角和北向通道中的橫滾角仿真誤差值較小，穩定后波動值為2.5′左右。注入里程計測量誤差擾動后，與集中式卡爾曼濾波相對比，聯邦式卡爾曼濾波出現的瞬時波動值較小且持續時間短，容錯能力更強。

圖6 俯仰角誤差對比 Fig.6 Comparison of pitch angle error

4.2 位置誤差

集中式與聯邦式卡爾曼濾波經度誤差對比如圖7所示。從圖7可看出，在300 s時加入里程計測量誤差擾動后，集中式卡爾曼濾波較聯邦式卡爾曼濾波出現的瞬時波動值更大且持續時間更長，可達30 s，與注入里程計擾動時長相符。待穩定后，2類濾波的波動值為-1～1 cm。

圖7 經度誤差對比 Fig.7 Comparison of longitude error

集中式與聯邦式卡爾曼濾波緯度誤差對比如圖8所示。從圖8可看出，在300 s時加入里程計測量誤差擾動后，2類濾波均出現了相應波動，其中集中式卡爾曼濾波的波動值較大且持續時間較長，濾波穩定后，2類濾波的波動值為-0.5～0.5 cm。

圖8 緯度誤差對比Fig.8 Comparison of latitude error

集中式與聯邦式卡爾曼濾波高度誤差對比如圖9所示。從圖9可看出，在300 s時加入里程計測量誤差擾動后，集中式卡爾曼濾波出現了較大瞬時波動值，最高達-4.5 cm，且波動持續時間較長，濾波穩定后，2類濾波的波動值為-2～2 cm。

圖9 高度誤差對比Fig.9 Comparison of height error

由上述分析可得出，位置誤差在濾波穩定后可達厘米級精度，天向通道的高度誤差相較于經緯度誤差較大，達到10 cm，穩定后在-2～2 cm內波動。經度誤差穩定后為-1～1 cm，緯度誤差穩定后為-0.5～0.5 cm。與姿態誤差最終結果表現相同，聯邦式卡爾曼濾波在加入誤差擾動后出現的瞬時波動值較小且持續時間短，容錯能力強。

5 結論

(1) 使用捷聯式慣導、里程計及測距雷達組合測量，可實現煤礦井下連續采煤機較高精度的位姿信息獲取。

(2) 通過測距雷達與里程計共同進行位置推算輔助修正捷聯慣導系統，可有效抑制慣導隨時間積累所產生的位置漂移誤差。

(3) 聯邦式卡爾曼濾波與集中式卡爾曼濾波相比，2類濾波在精度基本一致的前提下，聯邦式卡爾曼濾波在受到較大測量誤差干擾后出現的瞬時波動值小且波動時間短，具備一定的容錯性。

(4) 采用聯邦式卡爾曼濾波的定位方法的定位精度可達±2 cm，定向精度可達15′，可靠性較高，抗干擾能力較強，可為下一步井下現場試驗提供理論與實驗依據。

DOI：10.3390/s19173812.

DOI：10.3390/s17020239.