采煤機三維軌跡的慣性基厘米級相對測量方法

江一夫，李四海，謝 波，嚴恭敏，雷孟宇，

(1. 西北工業大學自動化學院，西安 710029；2. 西安航天精密機電研究所，西安710100；3. 西安科技大學 機械工程學院，西安710054)

采煤機精確自主定位導航技術是實現長壁綜采工作面自動化生產的關鍵技術之一。煤礦安全規程規定，綜合機械化采煤時，工作面煤壁、刮板輸送機和支架必須保持直線，底板和頂板必須保持平整，即“三直兩平兩暢通”中的“三直兩平”。當前即使國內自動化程度最高的綜采工作面，推進2～3 刀后也需要依賴人工調整支架，平直度問題嚴重制約著綜采自動化水平[1]。國外長壁自動化指導委員會（Longwall Automation Steering Committee, LASC）研制的綜采工作面自動化系統已經趨于成熟[2]，其對工作面平直度的檢測誤差小于10 cm，是目前行業內最為主流的采煤機位置測量解決方案。而我國在煤礦井下裝備自主導航方面的研究起步較晚，總體來說尚處于落后局面。

慣性導航系統由于其導航信息的連續性、自主性和高可靠性，是目前井下拒止環境中一種不可替代的位置測量方式。鑒于慣性導航定位誤差具有隨時間發散的特性，其與編碼器組合航位推算成為一種簡單易用的誤差抑制方式[3]。文獻[4]對航位推算算法進行了誤差分析，認為慣導三維定位精度與慣導初始對準誤差及其相對采煤機的安裝偏角誤差有關。為了消除這些誤差的影響，文獻[3]基于航位推算軌跡與實際軌跡的相似性，提出了兩點法偏差角校準算法。但這種方法需要井下具有全工作面的精確GPS-RTK 信息，且慣導每次斷電后初始對準都需要重新校準，不符合實際采煤工況。文獻[5]提出了卡爾曼濾波動態零速修正算法來解決該問題，但事實上航位推算本質上就利用了動態零速修正的非完整約束條件，且動態零速修正中航向誤差和航向安裝角誤差在采煤機近似直線運動中幾乎不可觀測，反而會由于卡爾曼濾波系統模型不準確、振動量測噪聲大等問題，難以實現采煤機軌跡的精確刻畫。

文獻[6]提出了全站儀和超寬帶（Ultra Wide-band, UWB）組合的方式，但全站儀和UWB 信號都易受粉塵和非視距傳播的影響而出現定位無效的情況，且基站需要跟隨工作面的推進而移動，影響采煤作業效率。文獻[7]提出了回環路徑識別算法，認為采煤機在回環點的跟機移架距離近似為1 m，以此作為量測對位置誤差進行修正。但由于機械滑動和實際運行調整，真實推進量會偏離1 m，文獻[8][9]因此采用激光雷達輔助來測量采煤機的移架距離，增加自動調直系統的長時間可靠性，但這無疑都增加了系統的硬件成本。

應該注意到的是，上述定位方法及其誤差分析都是相對地理坐標系而言的，但實際采煤作業關注的是長壁工作面軌跡的平直度，而非精確的地理坐標。基于此，從航位推算軌跡和真實軌跡的關系出發，提出一種相對測量方法，可以避開慣導系統初始對準誤差和安裝偏角的影響，工作面三維軌跡的測量精度僅僅取決于導航階段的航向和姿態保持精度，即將航姿測量誤差維持在初始對準誤差的能力，其理論定位精度可到厘米級。

1 航位推算誤差分析

定義b系(Oxbyb zb)為慣性測量坐標系，xb、yb和zb軸分別與慣性測量組合本體右前上重合；定義m系Oxm ym zm為采煤機坐標系，ym軸沿采煤機編碼器表征的虛擬縱軸指向前，xm軸位于采煤機橫截面垂直于ym軸指向右，zm軸與xm、ym軸構成右手正交坐標系；定義n系(Oxn yn zn)為導航坐標系，其與東北天地理坐標系重合。令表示采煤機坐標系到慣性測量坐標系之間的轉換矩陣，αθ為俯仰安裝偏角，αψ為航向安裝偏角，表示慣性測量坐標系到導航坐標系的轉換矩陣，ΔSi表示編碼器第i個采樣周期的里程增量，表示第i個采樣周期的地理系三維位移增量，則有：

從式(1)可以看出，航位推算實質上已經利用了采煤機的非完整約束，即側向和法向速度為零的量測信息。式(1)描述了測量采煤機三維地理軌跡的基本原理，慣性測量和編碼器輸出頻率越高，軌跡刻畫得越精細。

令φU表示慣性測量航向誤差，δαψ表示航向安裝偏角誤差，δαθ表示俯仰安裝偏角誤差，同時忽略編碼器刻度因子誤差，可以由式(1)計算得到每周期位置增量誤差模型。

認為慣性測量航向誤差φU，安裝偏角誤差δαψ和δαθ在一段時間內近似為常值，對式(2)積分可以得到位置誤差模型。

可見φU+δαψ僅產生水平位置誤差，且垂直于采煤機水平位移方向，由于工作面近似直線，因此φU+δαψ產生垂直于工作面的定位誤差，且誤差大小與位移的大小成正比。δαθ僅產生高度誤差，且誤差大小與行駛里程相關。具體可用圖1 表示。

圖中O為采煤機起始位置，航位推算在該點無誤差，當采煤機沿工作面行駛到A點時，航位推算顯示位置為A′。則圖中表示真實位移矢量表示航位推算位移矢量在水平面的投影，在水平面的投影，誤差角φU+δαψ產生的水平位置誤差與水平位移矢量垂直，為在鉛垂面的投影，表示在鉛垂面的投影，誤差角δαθ產生的高度誤差為

實際工作時，采煤機在刮板運輸機上往復運動，當采煤機由前進轉為后退時，其位移矢量ΔSn和里程ΔS也由正轉為負，當采煤機回到原點時產生的定位誤差也變為零，因此無法利用原點重合信息對初始對準誤差和安裝角誤差進行標定。文獻2 因此提出采用GNSS-RTK 對航向安裝角和俯仰安裝角進行精確標定的方法，但井下拒止環境沒有精確的GNSS-RTK 條件，何況每次慣導上電重新尋北，尋北誤差φU都是變化的，即便采用等效零偏漂移為0.003 °/h 的高精度光學陀螺，在北緯40 °地區的理論初始對準航向誤差最大可達0.05 °，長度為300 m 的工作面，其水平定位誤差可以達到δD=300 ×0.05×π180 =0.2618m ，難以達到厘米級定位精度。

2 采煤機三維軌跡的相對測量方法

2.1 相對定位的基本原理

從圖1 中可以看出，航位推算軌跡與真實軌跡具有軌跡相似性，通過一定角度的旋轉，兩者能夠重合。考慮到工作面的平直度測量并不需要地理系坐標，而只需要軌跡點偏離目標工作面的坐標，如圖[2]所示，定義df系(Oxdf ydf zdf)為目標工作面坐標系，xdf軸由起始目標工作面的機頭指向機尾，ydf軸垂直于xdf軸及其水平面投影構成的平面，指向煤層，zdf軸與xdf、ydf軸構成右手正交坐標系。若采煤機第一刀走過起始目標工作面的首尾兩個端點，可以獲得兩個端點的航位推算坐標。定義rf系(Oxrf yrf zrf)為相對工作面坐標系，xrf軸為兩個航位推算坐標的連線，yrf軸垂直于xrf軸及其在水平面投影構成的平面，zrf軸與xrf、yrf軸構成右手正交坐標系。圖中J為真實軌跡上的一點，J′為航位推算軌跡上的一點，K為J在Oxdf ydf平面的投影，K′為J′在Oxrf yrf平面的投影，L為J在Oydf zdf平面的投影，L′為J′在Oyrf zrf平面的投影。從圖中可以看出，相對工作面坐標系及其航位推算軌跡相當于目標工作面坐標系及其真實軌跡整體旋轉了一定角度，航位推算軌跡點在相對工作面坐標系的三維坐標與真實軌跡點在目標工作面坐標系的三維坐標是一致的，實際采煤機軌跡各位置點偏離目標工作面的偏移量能夠準確測量。

圖1 航位推算位置誤差示意圖Fig.1 Positioning error diagram of dead-reckoning

圖2 采煤工作面平直度的相對測量原理Fig.2 Relative measurement principle of coal mining face flatness and straightness

因此，雖然慣性導航初始對準航向誤差較大，其相對采煤機的安裝偏角無法井下標校，但是采煤機軌跡目標工作面的定位坐標能夠得到測量，測量精度取決于慣性導航系統的航姿保持精度。假設陀螺等效零偏漂移為0.003 °/h，采煤機1 h 走完300 m 工作面，考慮到慣性導航誤差的舒拉振蕩周期和地球振蕩周期，其航向保持誤差不大于0.003 °，產生的定位誤差δD=300 ×0.003×π180 =0.0157m 。

2.2 相對定位坐標計算方法

采煤機相對定位方法首先在于確定目標工作面的兩個端點，其可以根據支架到煤壁的距離測定，也可簡單認定采煤機在機頭和機尾的位置為目標工作面兩個端點。驅動采煤機經過這兩個端點，不考慮慣性測量與采煤機直接的安裝偏角和尋北誤差，此時航位推算得到的采煤機地理系坐標為：

由此測量得到的采煤機軌跡相對真實軌跡旋轉了一定角度。這里將航向安裝偏角和俯仰安裝偏角當作小量處理，因此要求安裝時慣性測量坐標系與采煤機坐標系近似一致。如圖3 所示，以采煤機經過兩個端點時測量坐標的連線建立相對工作面坐標系。

圖3 相對工作面坐標系與導航坐標系的關系Fig 3 Relationship between relative face frame and navigation frame

假設經過目標工作面首端點O時的航位推算n系三維坐標為經過尾端點A′時的航位推算n系三維坐標為則有：

由相對工作面的定義可知，xrf軸在n系的單位矢量為：

yrf軸垂直于OA′B′平面，因此yrf軸在n系的單位矢量為：

由此可以計算得到zrf軸在n系的單位矢量為：

導航坐標系到相對工作面坐標系的轉換矩陣為：

采煤機在相對工作面坐標系同時也是目標工作面坐標系的坐標為：

至此，可以繪制出反映工作面平直度的采煤機軌跡，計算出各支架偏離初始目標基準線的距離。

軌跡坐標轉換到相對工作面坐標系后，目標工作面兩個端點被設置在同一個大地高度上，只反映了高度軌跡的形狀，不能反映工作面的真實高程變化。為了實現智能自動調高，需要引入目標工作面兩端巷道的相對高度差信息hΔ ，該信息具體可由震波CT等地形探測手段獲取。此時將OA′改寫為：

按照式(6)～(10)重新計算相對工作面坐標系采煤機軌跡，既可以實現工作面平直度測量，又可以反映工作面真實高度變化。

2.3 斷電的處理方法

慣導系統由采煤機供電，在實際采煤時會遇到采煤機停機斷電的情況，此時慣導系統丟失可用的連續積分信息，一般而言，需要重新進行初始對準，才能繼續開展軌跡測量。但5 min 靜止對準不符合實際工況和高效開采原則，而運動中對準需要精確的慣性測量坐標系與采煤機坐標系的安裝偏角ψα和θα，且不管何種對準方法，都會引入了初始對準誤差，使斷電前后軌跡測量不連續，相對測量方法失效。

對于采煤機斷電的問題，目前LASC 系統額外給慣導系統配備不間斷電源UPS，但UPS 電量有限，長時間條件下慣導系統最終還是會陷入斷電狀態。考慮到采煤機和慣導系統固聯安裝且采煤機依靠電力驅動，采煤機斷電則慣導系統保持靜止，采煤機上電則慣導系統上電，因此可以在正常上電工作時實時存儲導航信息，在遭遇斷電時，由慣導系統內部閃存或者上位機存儲該信息，在重新上電后，慣導系統讀取該信息進行接續導航。

為了實現接續慣性導航和航位推算，需要實時保存當前時刻方向余弦矩陣和航位推算坐標，其中方向余弦矩陣可以等價轉換為航向角、俯仰角和橫滾角保存，降低保存數據量。為了實現相對測量轉換，需要保存導航坐標系到相對工作面坐標系的轉換矩陣，從圖3 可以看出導航坐標系經過兩次旋轉即可轉換到相對工作面坐標系，因此也可等價轉換為航向旋轉角和俯仰旋轉角保存。考慮到第一刀尚未走完，采煤機就可能斷電，因此還需保存是否經過起始端點標志、起始端點航位推算坐標是否經過末尾端點標志以及末尾端點航位推算坐標這樣的話無論采煤機何時斷電，都可以在重新上電時實現連續位置測量，而不需要重新對準。

2.4 相對定位方法的誤差分析

根據前文分析，采煤機三維軌跡的相對測量方法避開了初始對準誤差和安裝偏角誤差的影響，定位精度主要取決于航姿保持精度。

采煤機工作面近似為直線，航向基本保持不變，按照捷聯慣導誤差傳播方程進行誤差分析，慣導系統初始對準航向誤差導致的航向保持誤差和等效東向陀螺零偏導致的航向保持誤差相互對消，同時水平初始對準誤差與水平加計零偏造成的俯仰角保持誤差相互對消。導航階段的俯仰保持誤差Esφ和航向保持誤差φUs可以分別表示為：

其中Eε表示等效東向陀螺零偏，Nε表示等效北向陀螺零偏，Uε表示等效天向陀螺零偏，t表示導航時間，L表示當地地理緯度，ieω表示地球自轉角速度，約為 15.0411 °/h，sω為舒拉角頻率，約為255.9242 °/h，ωf為傅科角頻率，ωf=ωiesinL，在緯度40L= °地區，fω約為9.6682 °/h。

式(12)中，取正余弦項都為最大值 1，εE=εN=0.003 ° h ，可以計算得到俯仰角保持誤差最大為0.00134 °。式(13)中，sω遠大于ieω，因此忽略最后一項的影響，令 sin（ωiet）為最大值 1，εU=-0.003 ° h ，εN=0.003 ° h ，L= 40°，可以計算航向角的保持誤差最大為0.011 °。

注意到式(3)中，采煤機定位誤差與采煤機位移大小和里程有關，采煤機采用前進后退的方式，而非走完一個工作面掉頭的方式，無論是位移矢量還是里程都是一正一負的方式，因此慣性測量誤差導致的定位誤差在一正一負的過程中也是相互對消的。最大定位誤差取決于工作面長度與航姿保持誤差的乘積，假設工作面長度為300 m，則理論最大水平定位誤差為300m ×0.011 °×180π=0.058m ，最大高程定位誤差為300m ×0.00134 °× 1 80π=0.007 m 。

3 試驗驗證

采用自研激光陀螺慣導系統在山西雙柳煤礦進行了試驗驗證，由于編碼器信息采用無線傳輸給集控，再由集控下發給采煤機慣導，因此存在不定期延時，且分辨率相當粗糙，許多時候達到0.5 m。慣導系統激光陀螺精度約為 0.003 °/h，加速度計精度約為50 μg。實際采煤作業時，由于安全因素和工作條件等限制，無法下井準確測量實際支架的推移距離，而控制系統給定的推移距離可能偏移實際距離多達數十厘米，無法以之作為位置基準考核慣導系統位置測量精度，因此井下實驗采取了跑空刀的驗證方式。采煤機從中間進刀在固定工作面進行了幾次往復運動，期間由于工作面機電檢修緣故，在60架和116架附近進行了兩次長達半小時的斷電。理論上采煤機的定位軌跡是重合的，其不重合誤差即為慣導系統的定位誤差。整個試驗持續時間6 h，其中上電工作時間約為5 h，繪制出采煤機三維/定位軌跡如圖4 所示。

圖4 采煤機空刀試驗的三維軌跡Fig.4 Shearer 3D path in empty cutting test

為了更直觀地分析定位精度，將水平定位軌跡和高程定位軌跡分別畫出，如圖5 和圖6 所示。

圖5 采煤機空刀試驗的水平定位軌跡Fig.5 Shearer horizontal positioning path in empty cutting test

圖5 中最大水平定位誤差為3.9 cm，圖6 中最大高程定位誤差為1.5 cm。從圖5 中可以看出，水平軌跡在75 架、90 架以及100 架等軌跡曲率變化較大的地方，呈現誤差變大的現象，且曲率變化越大誤差也越大。由于慣導系統航向保持誤差主要與導航時間有關系，跟軌跡形狀無關，經數據處理分析，認為在采煤機經過曲率變化較大軌跡段時，慣性測量坐標系相對編碼器所表征的虛擬采煤機坐標系的航向安裝角發生了變化，本質上慣組與采煤機是固聯安裝，因此也可以理解為采煤機與刮板運輸機之間存在的航向安裝間隙導致航向安裝角發生了變化，產生了額外的水平定位誤差。這種誤差在工程實際中存在一定的隨機性，為不可避免的小量誤差。而高程定位精度受軌跡曲率影響小，這也間接說明采煤機與刮板運輸機之間的俯仰安裝間隙相對較小。即使扣除安裝間隙的影響，圖5 和圖6 仍然反映出高程定位精度整體優于水平定位精度，這一點符合式(12)和式(13)中相對測量方法定位誤差理論分析結果。主要原因是俯仰角保持誤差與航向角保持誤差在捷聯慣性導航中呈現不同的傳播規律。

圖6 采煤機空刀試驗的高程定位軌跡Fig.6 Shearer height positioning path in empty cutting test

從試驗結果還可以看出相對測量方法能適應采煤機斷電的情形，其在60架和116架附近并沒有因為斷電重啟產生定位誤差放大的情形。整體而言，在高達5 h 的工作時間內，采煤機三維軌跡的相對定位精度達到厘米級，能夠達到與國外LASC 系統相當的水平。

4 結 論

基于高精度慣導和編碼器，本文提出了一種無源自主的采煤機三維相對定位方法，解決了綜采工作面平直度精確測量問題，水平方向最大位置誤差為3.9 cm，高程方向最大位置誤差為1.5 cm。該方法簡單、可靠、環境適應性強，能夠滿足工程實際中工作面直線度的智能調整，液壓支架自動跟機移架、采煤機自適應智能調高等的需求，有利于推進井下綜采工作面向安全、高效、綠色、智能化、少人化方向發展。