基于差分式慣性傳感組件的采煤機位姿解算法?

司 壘，王忠賓，譚 超，梁 斌，2，萬 淼

（1.中國礦業(yè)大學機電工程學院 徐州，221116）

（2.中國礦業(yè)大學徐海學院 徐州，221008）

關健詞采煤機；位姿解算；慣性導航；精確定位

引言

煤炭是我國的主體能源和重要的工業(yè)原料。2019年，全國原煤產(chǎn)量38.5億噸，同比增長4%［1］。近年來，受經(jīng)濟增速放緩、能源結構調(diào)整等因素影響，我國煤炭需求量有所下降。然而，我國富煤、貧油、少氣的能源賦存特點，決定了煤炭將在一次性能源生產(chǎn)和消費中占據(jù)主導地位且長期不會改變。近年來，國家逐步加大煤炭智能化開采的力度，煤炭安全形勢逐年好轉。2019年，我國煤炭開采百萬噸死亡人數(shù)0.083人，仍高于世界先進產(chǎn)煤國家。國家發(fā)改委、國家能源局聯(lián)合發(fā)布的《能源技術革命創(chuàng)新行動計劃（2016-2030）》中明確指出：提升煤炭開采效率和智能化水平，到2030年重點煤礦區(qū)基本實現(xiàn)工作面無人化。采煤機作為綜采工作面的核心裝備之一，其智能化是綜采工作面能否實現(xiàn)無人化開采的關鍵技術，而準確定位是實現(xiàn)采煤機智能化的基礎［2］。

由于煤礦井下的環(huán)境特殊、工況惡劣，地面常用的無線電導航、衛(wèi)星定位等技術無法穿透地面對采煤機進行定位?，F(xiàn)有的采煤機定位技術主要包括紅外、超聲波、齒輪計數(shù)、無線以及慣導定位技術等。國外學者對采煤機的定位研究較早，Jobes等［3］提出了一種采煤機的遠程定位系統(tǒng)，并給出了計算位置和航向的方法。文獻［4-5］通過軸編碼器計算電動機的轉速，進而換算出采煤機的前進距離。文獻［6-7］利用超聲波傳感器測量煤壁返回的強回聲從而感知采煤機的位置。在國內(nèi)，隨著紅外技術的快速發(fā)展，劉清等［8］在采煤機上安裝紅外發(fā)射裝置發(fā)射廣角脈沖，通過液壓支架上的紅外接收裝置接收信號，對接收信號的強弱進行分析，從而判斷采煤機具體位置。張連昆等［9］將超聲波發(fā)射裝置安裝在工作面巷道中，根據(jù)采煤機反射的超聲波確定采煤機和巷道的相對位置關系。隨著無線技術的日益成熟，文獻［10-11］提出了移動無線傳感網(wǎng)絡來進行采煤機的定位，通過仿真研究無線測距誤差、錨節(jié)點密度和錨節(jié)點基準坐標漂移方向等因素對采煤機定位精度的影響。

由于不依賴外界信息、也不向外輻射能量的優(yōu)點，慣性導航技術逐漸成為自主導航的重要技術之一，國內(nèi)外一些學者對基于慣性導航系統(tǒng)的采煤機定位技術進行了研究。澳大利亞聯(lián)邦科學與工業(yè)研究組織推出了基于陀螺儀導向定位的自動化采煤方法（longwall automation steering committee，簡 稱LASC），LASC技術采用高精度光纖陀螺儀和定制的定位導航算法，解決了慣性導航系統(tǒng)與采煤機高度通信、采煤機起點校準、截割曲線生成和支架推移調(diào)整控制等難題。文獻［12-18］分別對捷聯(lián)慣導的偏差角校準算法、初始對準方法、姿態(tài)解算算法及其融合定位方法進行深入研究，在一定程度上提高了采煤機的位姿檢測精度。

通過對國內(nèi)外采煤機定位方法的調(diào)研和分析，國內(nèi)外學者在該領域進行了大量且有意義的研究工作，但仍然存在以下問題：

1）傳統(tǒng)的紅外、無線、超聲波及齒輪計數(shù)等采煤機常規(guī)定位方法，不能測量采煤機的航向，無法真正實現(xiàn)采煤機運動軌跡的監(jiān)測；

2）采煤機慣性導航定位誤差分析不全面，建立的位姿解算方法沒有充分考慮采煤機的實際運動工況，影響實際應用的精度，難以滿足采煤機的定位要求；

3）慣性導航系統(tǒng)容易隨時間產(chǎn)生累積誤差，減少慣性導航系統(tǒng)累積誤差的方法主要集中在改進慣性導航算法和利用輔助定位手段方面，缺少從慣性系統(tǒng)結構上抑制累積誤差的方法。

為了減少采煤機慣性導航定位的累積誤差，提高采煤機的定位精度，筆者分析采煤機慣性導航定位產(chǎn)生的確定性漂移和非確定性漂移誤差，研究基于差分式慣性傳感組件的采煤機位姿差分解算方法，進而提高采煤機位姿解算精度，實現(xiàn)采煤機的準確定位和定姿。

1 差分式慣性傳感組件融合模型建立

1.1 坐標系建立及轉換過程

采煤機慣性導航定位利用慣性傳感器固連在采煤機機身上，通過慣性傳感器的陀螺儀和加速度計分別測量采煤機的角速度和加速度信息，導航計算機根據(jù)這些測量信息解算出采煤機在導航坐標系中的位姿信息?；咀鴺讼到⑷鐖D1所示。

圖1 坐標系建立Fig.1 Coordinate system construction

在圖1中：慣性坐標系用i表示，該坐標系是適用牛頓運動定律的參考坐標系；地球坐標系用e表示，固連在地球上，原點位于地球中心，也稱為地心坐標系；導航坐標系用n表示，原點位于導航起始點處，文中設為采煤機運行的起始位置；x，y，z分別指向東、北、天方向，也叫東北天坐標系；計算導航坐標系用t表示，通過計算獲得的導航坐標系，和真實的導航坐標系存在一定的誤差，該坐標系的定義是為了進行采煤機慣性導航定位的誤差分析；載體坐標系用b表示，固連在采煤機上，原點位于其重心。

采煤機的位姿信息是基于導航坐標系獲取的，而采煤機的慣性導航系統(tǒng)感知采煤機的運動特性是在載體坐標系中，因此需要建立坐標系的變換矩陣。各坐標系可以通過繞3個方位軸旋轉而得到：導航坐標系Oxnynzn繞z軸轉動φ角度，獲得坐標系Ox1y1z1；之后坐標系Ox1y1z1繞x1軸 轉動θ角度，獲得坐標系Ox2y2z2；最后坐標系Ox2y2z2繞y2軸轉動γ角度，獲得了載體坐標系Oxbybzb。為了方便獲取采煤機的位姿信息，定義φ角為航向角，θ角為俯仰角，γ為橫滾角。

1.2 差分布局方法

采煤機在進行慣性導航定位時，由于溫度、震動等外界因素會導致安裝在采煤機機身上的慣性傳感器發(fā)生漂移誤差。文中將慣性傳感器發(fā)生的漂移誤差歸為兩類：確定性漂移和非確定性漂移。確定性漂移指的是方向和大小確定的常值漂移，非確定性漂移指的是方向或大小不確定的隨機漂移。為了消除確定性漂移誤差和減少非確定性漂移，筆者設計一種基于差分式傳感組件的誤差消除方法，該方案主要采用4個慣性傳感器構成差分布置，利用差分式慣性傳感組件進行數(shù)據(jù)融合，通過位姿差分解算算法對融合后的數(shù)據(jù)進行位姿解算，得到采煤機準確的位姿參數(shù)。差分式慣性傳感組件的布局方法如圖2所示。

由圖2可知，設計的差分式傳感組件采用4個慣性單元構成差分結構，每相鄰的兩個慣性單元之間均有兩個坐標軸反向。理論上，差分式慣性單元能夠完全消除確定性漂移，降低非確定性漂移。

1.3 數(shù)據(jù)融合方程

1.3.1 角速度融合方程

圖2 差分式慣性傳感組件布局方法Fig.2 Differential inertial sensor module layout

當單個慣性單元發(fā)生繞某個軸轉動時，其輸出的角速度ω包括真實的角速度ωr、角速度確定性漂移ωc、角速度非確定性漂移ωu的矢量和

差分式慣性傳感組件下的角速度可以表示為

由于角速度確定性漂移ωc的大小和方向一致，因此在各個坐標軸方向可以相互抵消，而角速度非確定性漂移ωu由于大小和方向的不確定性不能相互抵消，根據(jù)式（1）和式（2）可獲得角速度的數(shù)據(jù)融合方程

從式（3）可以分析出，確定性漂移可以通過差分方法完全消除，而非確定性漂移的相同部分可相互抵消，不同部分通過求其均值避免誤差擴大。

1.3.2 比力加速度融合方程

當單個慣性傳感器發(fā)生平動時，其輸出的加比力速度f包括了真實比力加速度fr、比力加速度確定性漂移fc及比力加速度非確定性漂移fu

差分式慣性傳感組件下的比力加速度可以表示為

基于式（4）和式（5）獲得比力加速度的數(shù)據(jù)融合方程

同樣，分析式（6）可以得出融合后的比力加速度可以消除確定性漂移，降低非確定性漂移。

2 采煤機位姿差分解算算法

2.1 采煤機姿態(tài)差分解算算法

由于四元數(shù)法求解姿態(tài)矩陣實時性強，精度高，沒有退化和奇點等現(xiàn)象，本研究采用四元數(shù)法求解采煤機的姿態(tài)變換矩陣，可以求得采煤機的姿態(tài)角

其中：q0，q1，q2，q3分別為四元數(shù)微分方程中的系數(shù)。

通過式（7）求得各姿態(tài)角的值域為-90°～90°，而通常定義的航向角范圍為0°～360°，因此需要將航向角的值進行轉換，得到航向角如下

2.2 采煤機速度差分解算算法

在導航坐標系下，采煤機的速度更新方程可表示為

由于速度更新周期T較短，可以認為姿態(tài)變換矩陣Cnb對應的旋轉矢量非常微小。因此對等式兩邊積分可以得到tk時刻在導航坐標系內(nèi)的速度為

2.3 采煤機位置差分解算算法

為了方便計算，采煤機的位置方程可用其一階近似表達式表示

其中：l，L分別為當?shù)亟?jīng)度和緯度；dE，dN，dU分別為采煤機在東、北、天三個方向上的位移變量；RM，RN分別為子午圈和卯酉圈曲率半徑；h為地球曲率。

由于tk時刻的vn k已經(jīng)求得，在位置更新過程中采用平均速度值，可以得到位置的表達式為

3 位姿差分解算算法仿真分析

3.1 采煤機運行工況分析

為了驗證文中提出的采煤機位姿差分解算算法在不同運動工況下的位姿解算精度，需要獲取不同運動工況下慣性單元的原始輸出數(shù)據(jù)用于位姿差分解算算法的輸入，并利用位姿差分解算法解算后的位姿信息和參考位姿信息進行對比。由于采煤機實際工作過程中環(huán)境比較復雜，條件有限，無法獲取采煤機運行的實際參考運動數(shù)據(jù)。因此，筆者采用數(shù)值仿真法來模擬采煤機的不同運動工況。

采煤機的運動工況有直線運動、S彎運動、爬坡運動及起伏運動4種。其中直線運動和S彎運動在采煤過程中最為常見，爬坡運動是當?shù)装瀹a(chǎn)生了一定坡度時的采煤機運動工況，起伏運動是由于底板產(chǎn)生了起伏導致的運動工況。為了方便運動工況的模擬，需要定義一個采煤機運動軌跡坐標系m，x軸沿運動軌跡水平向右，y軸沿運動軌跡切線指向采煤機前進方向，z軸符合右手螺旋原則垂直于xOy面指向上，文中模擬運動工況時的加速度a（t）和角速度w（t）的定義均在此坐標系內(nèi)。采煤機工作過程中，其基本的運動狀態(tài)變化就是角速度w（t）以及加速度a（t）變化引起的。采煤機運動微分方程以采煤機運動工況分析中所述的角速度w（t）和加速度a（t）作為輸入，直接積分求解便可獲取各種運動參數(shù)。

3.2 仿真結果分析

利用文中提出的差分解算算法和原始解算算法獲取的采煤機位姿信息，并與模擬出的采煤機參考位姿信息進行對比，得到位姿信息的誤差曲線，以此驗證提出的采煤機位姿差分解算算法的性能。由于在模擬起伏運動時，包含了直線運動、S彎運動和爬坡運動，因此限于篇幅，文中不單獨進行直線運動、S彎運動和爬坡運動的模擬仿真。

起伏運動本質(zhì)是俯仰角發(fā)生二次變化的運動，文中模擬的起伏運動包括凸起、凹坑和連續(xù)凸起凹坑3部分。起伏運動仿真中，采煤機初始姿態(tài)角α0=［0 0 0］，初始速度v0=［0 0 0］，初始位置p0=［0 0 0］，初始位移為d0=［0 0 0］，采煤機慣性導航輸出頻率（采樣頻率）fs=100 Hz。設置采煤機沿y軸方向（工作面前進方向）運動60 m，沿z軸方向（高度方向）運動最大為0.052 3 m。根據(jù)以上參數(shù)，結合采煤機運動工況模擬方法，通過計算便可獲得起伏運動的仿真參數(shù)。圖3為起伏運動3種軌跡對比。

圖3 起伏運動的對比結果Fig.3 Comparative results of undulating motion

由圖3可以直觀看出，在起伏運動仿真中，差分解算算法能夠更好跟蹤基準軌跡，性能較好；而原始解算算法獲得的運動軌跡偏差較大，在東向發(fā)生了明顯的漂移，跟蹤性能較差。

圖4 為起伏運動模擬姿態(tài)角、速度、位移的解算誤差對比結果，起伏運動是由于俯仰角多次變化導致，因此俯仰角的模擬曲線發(fā)生了多次突變。從圖4（a）～（c）看出，原始解算算法在航向角最終累積誤差為0.505 6°，俯仰角最終累積誤差為-0.500 4°，橫滾角最終累積誤差為-0.484 6°；而差分解算算法在航向角最終累積誤差為0.028 8°，俯仰角最終累積誤差為0.014 6°，橫滾角最終累積誤差為-0.021 1°，差分解算算法明顯具有更好的解算效果。從俯仰角的誤差可以看出，筆者提出的差分解算算法具有較小的誤差，說明在俯仰角突變的工況下，文中算法能夠更好地抑制慣性傳感器的漂移誤差，從而減少了誤差的累積。

圖4 起伏運動模擬姿態(tài)角、速度、位移解算誤差對比Fig.4 The error comparison of undulating motion simulated attitude angle,velocity and displacement

從圖4（d）～（f）看出，差分解算算法在東向速度上解算效果優(yōu)勢明顯，同樣，在北向速度和天向速度上，兩個算法的最大誤差方面幾乎一致。同時，在天向由于俯仰角的多次變化產(chǎn)生了速度的突變，導致兩種算法在天向上的跟蹤性能相對較差，但是文中算法沒有發(fā)生明顯的漂移，在速度突變的工況下，具有更好的穩(wěn)定性。

從圖4（g）～（i）看出，原始解算算法在東向位移最終累積誤差為-2.052 4 m，北向位移最終累積誤差為2.347 5 m，天向位移最終累積誤差為-0.042 3 m，而差分解算算法在東向位移最終累積誤差為-0.107 2 m，北向位移最終累積誤差為0.152 3 m，最大北向位移誤差為0.237 2 m，小于原始算法的北向最大位移誤差，天向位移最終累積誤差為-0.002 0 m，最大天向位移誤差為-0.005 7 m，明顯小于原始解算算法的天向位移誤差最大值。由上述仿真結果可以看出，筆者提出的采煤機位姿差分解算算法在不同工況下具有更高的解算精度。

4 實驗驗證

4.1 實驗平臺搭建

為了對采煤機位姿解算方法進行驗證，文中搭建了采煤機慣性導航定位實驗平臺，并在礦山智能采掘裝備省部共建協(xié)同創(chuàng)新中心進行地面實驗，如圖5所示。差分式慣性傳感組件成差分布置固定在定位板上，定位板固聯(lián)在采煤機機身上，差分式慣性傳感組件和樹莓派Raspberry Pi 3B通過杜邦線連接，樹莓派Raspberry Pi 3B和上位機之間通過USB轉TTL模塊連接。

圖5 實驗平臺Fig.5 Experimental platform

4.2 姿態(tài)定位實驗

設置采煤機沿北向加速至3 m/min后減速至0 m/min后停止運行，運行時間總共為200 s。設定航向角、俯仰角和橫滾角的初始對準角度分別為-0.792°，-0.064°和0.091°，以此作為動態(tài)定姿實驗的初始值進行實驗。為了驗證文中所提方法的可靠性，采用高精度的慣性測量單元ADIS16448得到觀測值進行對比，圖6為動態(tài)定姿實驗的采煤機航向角解算結果，文中所提方法在航向角的誤差較大，最大誤差為1.745°，平均誤差為1.043°。同時，通過計算可以得出，文中所提方法在俯仰角和橫滾角方面誤差漂移均小于1°，整體漂移較小，具有較高的精度。

圖6 采煤機航向角角解算結果Fig.6 Heading angle calculation of shearer

4.3 位置定位實驗

考慮到實驗條件的限制，設置采煤機沿北向往返運動，單程運行范圍內(nèi)同樣先加速至3 m/min后減速運動，設置采煤機運行3次，單程位移為5 m，總位移為15 m。圖7為動態(tài)定姿實驗的采煤機東向位移解算結果。

圖7 采煤機東向位移解算結果Fig.7 Eastward displacement calculation of shearer

由圖7可知，隨著時間的推移，文中提出的采煤機姿態(tài)解算方法東向位移跟蹤上發(fā)生了一定的漂移，東向平均誤差為0.157 m。同時，經(jīng)過計算可以得到北向平均誤差為0.409 m，天向平均誤差為0.096 m。這是由于采煤機的位移需要經(jīng)過對加速度兩次積分才能獲得，因此慣性傳感器的漂移誤差經(jīng)過不斷積分后被累積放大，特別是在東向和北向方面比較明顯。

4.4 運動工況模擬實驗

本研究根據(jù)實際情況，搭建了移動載體慣性導航定位實驗臺，進行S彎、爬坡、起伏3種運動工況的模擬實驗，如圖8所示。

圖8 運動工況模擬實驗臺Fig.8 Motion conditions simulation experiment platform

為了更好地模擬采煤機的S彎運動、爬坡運動和起伏運動3種運動，其模擬運動軌跡設置如圖9所示，P0為起始點，P1～P4為模擬S彎階段，設置沿y軸方向（工作面前進方向）運動0.9 m，沿x軸方向（工作面推進方向）運動0.1 m。P5～P8為爬坡階段，爬坡高度設置0.04 m；起伏階段為P9～P16，P9～P12設置為凹坑階段，凹陷深度設置為0.01 m，P9～P16設置為凸起階段，凸起高度設置為0.01 m；P17作為終點。在實驗中，本研究可以準確獲得各關鍵點的姿態(tài)信息和位置信息。

圖9 模擬運動軌跡Fig.9 Simulated trajectory

為了進一步分析文中所提方法的有效性，根據(jù)以上節(jié)點的位姿數(shù)據(jù)擬合出真實的位姿曲線，并利用文中所提算法對姿態(tài)角和位置進行結算，對比結果如圖10所示。

圖10 姿態(tài)解算對比結果Fig.10 Comparison of attitude angle and position

由圖10（a）可以分析得出，航向角的平均誤差為0.183°，俯仰角的平均誤差為0.123°，橫滾角的平均誤差為0.020°，三個方向的姿態(tài)角誤差都控制在0.2°之內(nèi)。由圖10（b）可以分析得出，在東向和北向方面，文中方法獲得的位移后期發(fā)生了一定范圍內(nèi)的漂移，而在天向的誤差最小，這是符合運動規(guī)律的，因為在北向和東向運動距離較大導致產(chǎn)生一定漂移，而在天向運動距離小，因此漂移也較小。在東向平均誤差為0.007 m，北向的平均誤差為0.114 m，天向的平均誤差為0.003 m。由此可見，文中提出方法在S彎運動、爬坡運動及起伏運動等工況下仍能保持具有較好的解算精度。

5 結論

1）為了減少采煤機慣性導航定位的累積誤差，建立了采煤機差分式慣性傳感組件數(shù)據(jù)融合模型，提出了一種采煤機位姿差分解算算法，并進行了不同運動工況下的仿真分析。仿真結果表明：在S彎運動、爬坡運動及起伏運動中，文中提出的差分位姿解算算法在累積誤差和最大誤差方面均小于原始位姿解算算法。

2）搭建了采煤機慣性導航定位實驗平臺，并進行采煤機直線運動實驗，采煤機航向角、俯仰角和橫滾角的平均誤差分別為1.043°，0.251°，0.120°，其東向、北向和天向的平均誤差分別為0.157，0.409及0.096 m，以上誤差均滿足采煤機定位精度要求，驗證了采煤機位姿解算方法的可行性和有效性。

3）搭建了移動載體慣性導航定位實驗臺，并進行了S彎、爬坡、起伏3種運動工況的采煤機模擬運動實驗。實驗結果表明：在包含上述3種工況的路線運動時，移動載體的航向角、俯仰角、橫滾角的平均誤差分別為0.183°，0.123°，0.020°，東向、北向、天向位移的平均誤差分別為0.007，0.114，0.003 m。實驗結果驗證了所提算法的適用性和可行性。