基于慣性／航跡推測的煤礦掘錨機組合導航方法

倫華江，徐 楓，黃 喆，許東旭，張曉日

(1.中國航天科技集團有限公司物聯網技術應用研究院，北京100094；2.力信測量(上海）有限公司，上海201300；3.天津科技大學電子信息與自動化學院，天津300222)

0 引言

煤炭是我國主體能源，在我國能源消費總量中占比60%[1]。煤炭的獲取依賴于安全開采[2]，而實現工作面安全開采離不開先進、可靠的掘進裝備[3]，其中掘錨機因其掘進速度快、安全性好和巷道成形效率高等特點而廣泛應用于我國礦井煤巷施工中[4-5]。煤礦巷道中使用激光指示和人工觀察相結合的方法來指導掘進，但這種方法勞動強度大，工人安全得不到有效保障，極大降低了施工質量和掘進效率。因此，研究高可靠性、高精度的導航定位方法在掘錨機煤巷施工中具有很高的工程應用價值。

近年來，國內學者在煤礦掘進機導航定位方面取得了較多研究成果。王以忠等[6]提出了一種基于空間矢量約束的實時位姿組合測量方法，通過構建空間四邊形的方式實現了煤礦掘進機的六自由度測量，但是該方法不能解算空間四邊形中有兩邊平行的特殊情況。田原[7-8]基于四點式光靶的掘進機定位方法實現了機器視覺和慣性的組合導航，但該方法在巷道進尺方向精度較差。而后該學者從掘進機特點出發，提出了一種基于零速修正的掘進機慣性導航定位方法，解決了環境適應性差、慣性導航長時定位精度低等問題。薛光輝等[9]針對慣導誤差累積的特點提出了一種基于激光靶向跟蹤的掘進機位姿測量方法，可以在標靶單點布站的情況下完成掘進機所有位姿參數的測量，但該方法受限于煤塵問題，不能長距離使用。就目前技術而言，由于掘進機工作環境復雜，基于單一傳感器的導航定位技術存在局限性，多傳感多信息融合的導航技術更有希望解決掘進機的導航定位問題[10]。文獻[11]～文獻[15]進行了里程計＋捷聯慣導組合導航技術在管道、自動駕駛及井下車輛等場景中的應用研究。掘進機定位技術關鍵在于建立掘進機的實時坐標[16]，然而上述文獻中未給出載體坐標的解算方法。基于零位測量實時更新載體坐標和姿態的方法在盾構導向中應用比較成熟[17]，其基于載體與傳感器之間的剛體關系，在已知傳感器坐標和姿態角的情況下實時解算載體坐標。

為了實現掘錨機定位測量和掘進導向，進一步豐富導航參數，本文基于對現有的技術研究，提出了基于慣性／航跡推測的組合導航方法。通過對慣導實時位姿測量，結合掘錨機剛體零位標定數據、全站儀測量數據和計劃線數據等，解算出掘錨機實時坐標及導向參數，實現了精確導向功能。

1 定位和偏差算法

1.1 坐標系定義

位姿測量系統坐標系的定義如圖1所示。OnXnYnZn(簡稱n系)為參考坐標系，OnXn軸指向正東方向，OnYn軸指向正北方向(方位角為零度的方向)，OnZn軸垂直向上，原點由城市規劃設計部門設定；ObXbYbZb(簡稱b系)為掘錨機坐標系，原點位于機體中線上，ObXb軸指向機體右側，ObYb軸由機尾指向機頭，ObZb軸滿足正交坐標右手系；OmXmYmZm(簡稱m系)為慣性測量單元(Inertial Measurement Unit，IMU)坐標系，原點在 IMU中心，坐標軸為IMU自身出廠的坐標軸。

圖1 測量系統坐標系定義及掘錨機與設計軸線空間位置關系Fig.1 Definition of the measurement system coordinate system and spatial position relationship between the roadheader and the design line

在掘錨機施工階段，進行測量的主要任務是獲取導向參數，即測量機體與計劃線的偏差量，從而控制機器前進方向。圖1描述了掘錨機與設計軸線在空間中的位置關系，過機頭中心點和機尾中心點分別做垂線垂直于設計軸線，則垂線段在水平面上的投影為水平偏移量，在豎直面上的投影為垂直偏移量。始終將水平偏差和垂直偏差控制在規定的范圍，就可以使掘錨機沿著計劃線前進，獲得良好的直線度。

為了求解掘錨機相對于計劃軸線的絕對位置偏差，需要實時解算機頭中心點和機尾中心點的坐標，然后根據點與直線的相對位置關系計算偏差量。解算的數據流程圖如圖2所示，主要分三步來實現偏差量計算：

圖2 數據流程示意圖Fig.2 Flowchart of data

第一步為零位標定，即確定掘錨機與IMU之間的相對位置關系和姿態關系。其中，位置關系通過標定機頭中心點和機尾中心點在n系下的坐標確定，姿態關系通過標定b系和m系的旋轉矩陣確定。

第二步為坐標解算，即根據里程計和IMU獲取機頭中心點和機尾中心點在n系下的坐標。

第三步為偏差計算，即根據空間中點到直線最短距離原理計算水平和垂直偏差值。

1.2 零位標定

在零位處獲取機尾、機頭和IMU在n系下的坐標Pa0、Pb0和P0，由IMU解算出自身三個姿態角，即俯仰角α0、橫滾角β0和航向角γ0。 則可得機尾、機頭在m系下的坐標Qa0和Qb0，表達式如下

由于IMU與掘錨機是剛性連接的，所以在以后的機體運動過程中，Qa0、Qb0以及保持不變。

1.3 坐標解算

分兩個步驟進行求解：1)基于里程計測量的位移數據和姿態信息計算IMU實時坐標；2)結合IMU實時姿態角數據和零位數據計算機頭機尾實時坐標。

在掘錨機運動過程中，里程計輸出空間位移數據，給定初始位置，可以推算實時位置信息。設里程計第i采樣間隔的里程增量在b系下的矢量表示為ΔSbi， IMU姿態角構成的旋轉矩陣為，根據航位推算原理，IMU實時坐標可表示為[19-20]

式(5)中，Pat為機尾中心在n系下的坐標，Pbt為機頭中心在n系下的坐標。

1.4 偏差計算

機尾、機頭偏差量求解原理如圖3所示。將掘錨機機尾中心A和機頭中心B在平面XnOnYn和平面ZnOnYn投影， 得到投影點A′、A″、B′和B″，將設計軸線起點C和終點D在平面XnOnYn和平面ZnOnYn投影， 得到投影點C′、C″、D′和D″， 平面A′AA″交線段CD、C′D′和C″D″于點E、E′和E″，平面B′BB″交線段CD、C′D′和C″D″于點F、F′和F″。則按照本文偏差量的定義，A′E′即為機尾中心點水平偏差量，A″E″為機尾中心點垂直偏差量，B′F′為機頭中心點水平偏差量，B″F″為機頭中心點垂直偏差量。

圖3 機尾、機頭偏差量求解原理Fig.3 Principle of solving the deviation of the tail and head

根據空間點與直線的位置關系[21]，容易得到

計劃線起點和終點在n系下坐標已知，記為C(xc，yc，zc)和D(xd，yd，zd)， 則可得C′(xc，yc，0)、C″(0，yc，zc)、D′(xd，yd， 0)、D″(0，yd，zd)。掘錨機機頭、 機尾坐標可由式求出，記為A(xa，ya，za) 和B(xb，yb，zb)，則可 得A′(xa，ya， 0)、A″(0，ya，za)、B′(xb，yb， 0)、B″(0，yb，zb)。 將上述點坐標帶入式(6)，得到

2 導航系統實驗

為了驗證組合導航方法的準確性，將慣性測量單元(IMU)和里程計安裝在移動小車上進行實驗，實驗原理如圖4所示。

圖4 實驗原理Fig.4 Diagram of experiment principle

讓測試小車沿著設定好的計劃線前進，在小車運行到觀察位置點時，用全站儀測量小車位置，同時記錄慣導＋里程計組合導航系統的定位坐標。結合初始零位及計劃線參數，計算機頭、機尾在水平方向和豎直方向上的偏移量，通過比較解算值和實測值，獲得系統測量誤差。本次實驗所用的設備如表1所示。

表1 實驗設備Table 1 List of experiment apparatus

實驗步驟如下：

1)組裝實驗裝置和搭建實驗環境。采用鋁合金型材、行走輪組裝成測試小車主體；在里程計上加裝計米輪并通過安裝支架固連到車體尾部，其中安裝支架中帶彈簧機構保證計米輪貼地；在IMU上加裝反射棱鏡，并通過螺絲固連到車體的右前部位；將全站儀架設在視野寬闊的位置，調平并設站定向。實驗裝置和實驗環境如圖5所示。

圖5 實驗裝置與實驗環境Fig.5 Diagram of experiment devices and experiment environment

2)標定剛體零位和初始化線路數據。通過全站儀測量小車上的特征點以及陀螺特征點坐標，得到零位坐標數據和零位姿態數據，如表2、表3所示。根據小車運動范圍設定沿正北方向150m為前進軸線，得到計劃線參數，如表4所示。

表2 零位坐標數據Table 2 Data of coordinate at zero-position

表3 零位姿態數據Table 3 Data of attitude angle at zero-position

表4 計劃線數據Table 4 Data of design line

3)根據安裝偏差校準算法[19]獲取補償誤差：航向角偏差為0.196°，俯仰角偏差為－0.002°，并補償到零位的安裝偏差中。

4)推動小車向前移動，每間隔約2m用全站儀測量慣導特征點坐標，同時記錄慣導自身解算坐標，得到的IMU軌跡如圖6所示。通過零位關系求解移動小車上機頭、機尾點與計劃線的偏差，結合全站儀測量得到的偏差計算誤差，得到的偏差變化曲線和誤差曲線如圖7所示。

圖6 慣導定位軌跡與全站儀定位軌跡比較Fig.6 Comparison between inertial navigation positioning track and total station positioning track

圖7 解算與實測偏差對比及誤差曲線Fig.7 Comparison of calculated and measured deviations and error curves

由圖6可知，小車在前進過程中，北向和東向位移增大，天向位移逐漸減小，對比全站儀測量坐標和慣導解算坐標，兩者的變化趨勢一致。但隨著時間和距離的變化，誤差逐漸增大，在東北天方向的最大定位誤差分別為37.9mm、46.0mm和29.5mm。在圖7的解算與實測偏差比較曲線中，機頭機尾中心與計劃線偏移量隨著里程變化逐漸增大，且頭尾變化保持一致。從解算誤差曲線圖中可以發現，誤差隨著時間和距離變化累積，在里程108.6481m處，水平方向上機頭中心和機尾中心的偏差量誤差分別為－44mm和－44mm，在里程114.2266m處，豎直方向上機頭中心和機尾中心的偏差量誤差分別為32mm和30mm，如表5所示。

表5 實驗小車與計劃線偏差的最大測量誤差Table 5 Maximum measurement error of the deviation between the experiment car and the design line

本次實驗中，由于在組裝時行走輪與機身軸線有偏角，加之推行小車的力位于小車左后方，小車并沒有完全沿著預定的計劃線前進，而是朝北偏東方向前進，所以小車在前進過程中東坐標和北坐標逐漸增大，且機頭機尾中心向右偏離計劃線；計劃線設定在高程為8.82m的水平面上，而實際小車下坡前進，故機頭機尾中心向下偏離計劃線。此外，由于慣性系統存在累積誤差，全站儀實測值與組合導航系統解算值之間存在誤差，直接體現在慣導定位誤差中，間接影響測試小車機頭機尾偏離計劃線的解算誤差。

該組合導航系統的現場應用如圖8所示，先在廠里做完零位標定，待掘進裝備運到施工現場后做零位復測。某煤礦應用中，巷道長度約300m，掘進過程中對機頭水平偏差解算誤差進行驗證，在同一掘進里程下使用全站儀人工測量機頭與設計線的水平偏差，記錄測量系統解算的機頭水平偏差，并統一編號作對比。表6為掘進里程位于122m～127m區間內的三組人工復核數據和測量系統解算數據。可由表6知，比對誤差在130mm以內，滿足應用需求。

圖8 測量系統現場應用Fig.8 Field application of measurement system

表6 機頭中心水平偏差實測值與計算值比較Table 6 Comparison of measured value and calculated value of head center horizontal deviation

上述結果驗證了本文提出的組合導航方法的可行性，在實驗條件下，該方法定位精度達到46mm(116m前進距離)，最終計算的位置偏差精度為：偏差量誤差／前進里程＝0.044／116.4287＝0.038%。在實踐應用中，水平位置偏差精度約為：0.129／300＝0.043%。相信將該導航方法應用于掘錨機施工中，將為機器設備的導航定位工作提供有力保障。另一方面，為了減小慣性誤差累積的影響，保證定位精度和位置偏差精度，通常在載體前進100m左右要做一次坐標修正。

3 結論

基于煤礦巷道施工現場掘進導向的迫切需求，本文提出了一種基于慣性導航和航跡推測的導航定位方法。該方法根據慣導姿態數據、里程計位移數據以及載體零位數據，解算機頭／機尾相對于計劃線在水平和豎直方向上的偏移量，實現了掘錨機的導向測量功能。在實驗環境下進行了定位精度和偏差精度測試，同時進行了在現場施工環境下的應用測試。實驗結果表明，該方法偏差解算精度為0.038D%(D為前進距離)，具有很好的工程推廣和應用價值。在接下來的工作中，需要研究慣導、里程計的標定方法以及振動環境下的穩定性測試，以提高系統精度。