基于非完整性約束的采煤機(jī)定位方法

宋單陽， 楊金衡， 陶心雅， 盧春貴， 田慕琴， 宋建成

（1. 太原理工大學(xué) 礦用智能電器技術(shù)國家地方聯(lián)合工程實(shí)驗(yàn)室，山西 太原 030024；2. 太原科技大學(xué) 電子信息工程學(xué)院，山西 太原 030024）

0 引言

采煤機(jī)準(zhǔn)確定位是綜采工作面實(shí)現(xiàn)自動(dòng)化開采的關(guān)鍵技術(shù)之一，不僅可為記憶截割、工作面校直提供采煤機(jī)準(zhǔn)確位置，還可為工作面“三機(jī)”聯(lián)動(dòng)、自動(dòng)化采煤提供參考依據(jù)。目前采煤機(jī)定位方法大多依靠基于慣性測(cè)量單元（由三軸陀螺儀和三軸加速度計(jì)構(gòu)成）的慣性導(dǎo)航系統(tǒng)（以下稱慣導(dǎo)系統(tǒng)）[1]，但由于傳感器誤差、環(huán)境因素等影響，定位誤差會(huì)隨著時(shí)間而累計(jì)，所以需要引入輔助信息抑制慣導(dǎo)系統(tǒng)的誤差累計(jì)。文獻(xiàn)[2]結(jié)合慣導(dǎo)系統(tǒng)和綜采工作面精準(zhǔn)地理模型，利用地圖匹配技術(shù)約束定位過程中慣導(dǎo)系統(tǒng)的誤差，但建立綜采工作面精準(zhǔn)地理模型的難度過大，該方法的可行性有待確定。文獻(xiàn)[3]采用將里程計(jì)和慣導(dǎo)系統(tǒng)相結(jié)合的方法，通過采集里程計(jì)的速度或位移信息，校正慣導(dǎo)系統(tǒng)的定位結(jié)果，但里程計(jì)在定位過程中只對(duì)慣導(dǎo)系統(tǒng)的前進(jìn)速度進(jìn)行修正，無法對(duì)其側(cè)向和垂向速度進(jìn)行修正。文獻(xiàn)[4]在超聲波定位系統(tǒng)的基礎(chǔ)上，運(yùn)用卡爾曼濾波算法對(duì)慣導(dǎo)系統(tǒng)和超聲波傳感器的組合定位結(jié)果進(jìn)行過濾，但井下非視距環(huán)境會(huì)降低超聲波傳感器測(cè)量精度，進(jìn)而影響超聲波定位系統(tǒng)精度。文獻(xiàn)[5-6]構(gòu)建了慣導(dǎo)系統(tǒng)與超寬帶組合定位方案，能夠有效抑制慣導(dǎo)系統(tǒng)的誤差發(fā)散，但需要增加大量額外的超寬帶錨節(jié)點(diǎn)，增加了成本和復(fù)雜度。文獻(xiàn)[7]采用聯(lián)邦濾波將慣導(dǎo)系統(tǒng)分別與視覺相機(jī)、里程計(jì)構(gòu)成子濾波器，解決了傳統(tǒng)卡爾曼濾波中單個(gè)傳感器測(cè)量誤差降低整個(gè)定位系統(tǒng)定位精度的問題，但工作面粉塵大、光線弱、振動(dòng)強(qiáng)，影響視覺相機(jī)定位精度。文獻(xiàn)[8-9]以2套慣導(dǎo)系統(tǒng)之間的距離為約束條件建立卡爾曼濾波模型，能夠有效抑制單套慣導(dǎo)系統(tǒng)的誤差發(fā)散，但2套慣導(dǎo)系統(tǒng)的安裝條件嚴(yán)苛，影響模型定位精度。

采煤機(jī)在運(yùn)動(dòng)過程中滿足非完整性約束的特點(diǎn)，即在采煤機(jī)不發(fā)生跳躍和側(cè)滑的情況下，牽引齒輪和履帶連接處的側(cè)向和垂向速度為零。結(jié)合該特點(diǎn)，本文基于慣導(dǎo)系統(tǒng)和里程計(jì)組合定位，引入非完整性約束作為輔助信息，提出了一種基于非完整性約束的采煤機(jī)定位方法，可抑制慣導(dǎo)系統(tǒng)的誤差發(fā)散，提高定位精度。

1 采煤機(jī)定位方法

1.1 方法原理

基于非完整性約束的采煤機(jī)定位方法原理如圖1所示。安裝于采煤機(jī)機(jī)身中部的慣性測(cè)量單元輸出經(jīng)機(jī)械編排獲得采煤機(jī)姿態(tài)、速度和位置信息；安裝于采煤機(jī)牽引齒輪上的里程計(jì)輸出用于計(jì)算采煤機(jī)瞬時(shí)速度。將慣導(dǎo)系統(tǒng)的機(jī)械編排結(jié)果、里程計(jì)輸出及非完整性約束通過卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合，將卡爾曼濾波結(jié)果作為誤差反饋，修正慣導(dǎo)系統(tǒng)的機(jī)械編排結(jié)果，從而得到采煤機(jī)姿態(tài)、速度和位置信息的最優(yōu)估計(jì)。

圖1 基于非完整性約束的采煤機(jī)定位方法原理Fig. 1 Principle of shearer positioning method based on non-holonomic constraints

1.2 坐標(biāo)系定義

本文求解位置信息過程中用到的坐標(biāo)系包括參考坐標(biāo)系（n系）、采煤機(jī)機(jī)體坐標(biāo)系（m系）和慣性測(cè)量單元坐標(biāo)系（b系），如圖2所示。

圖2 坐標(biāo)系Fig. 2 Coordinate systems

參考坐標(biāo)系onxnynzn：使用當(dāng)?shù)厮阶鴺?biāo)系作為參考坐標(biāo)系，坐標(biāo)系原點(diǎn)on位于慣性測(cè)量單元的測(cè)量中心，xn軸 指向正北，yn軸指向正東，zn軸 垂直于xnonyn平面，向下為正。

采煤機(jī)機(jī)體坐標(biāo)系obxbybzb： 坐標(biāo)系原點(diǎn)ob為采煤機(jī)牽引齒輪與底板的接觸點(diǎn)，xm軸沿采煤機(jī)前進(jìn)方向，ym軸指向采煤機(jī)靠近液壓支架一側(cè)，zm軸垂直向下，與xm，ym軸共同構(gòu)成右手坐標(biāo)系。

慣性測(cè)量單元坐標(biāo)系omxmymzm：坐標(biāo)系原點(diǎn)om位于慣性測(cè)量單元的中心，xb，yb，zb軸構(gòu)成右手坐標(biāo)系，并分別平行于慣性測(cè)量單元的3個(gè)敏感軸。

1.3 卡爾曼濾波狀態(tài)方程

為提高卡爾曼濾波效果，慣導(dǎo)系統(tǒng)一般采用間接卡爾曼濾波進(jìn)行組合定位解算，即采用慣導(dǎo)系統(tǒng)的誤差作為狀態(tài)向量，可解決連續(xù)時(shí)間條件下定位系統(tǒng)非線性變化的問題。選擇慣導(dǎo)系統(tǒng)的部分誤差作為定位系統(tǒng)的狀態(tài)向量：

式中：φ為三維姿態(tài)誤差向量； δvn為 三維速度vn誤差向量； δrn為 三維位置rn誤差向量；bg為三軸陀螺儀的零偏向量；ba為三軸加速度計(jì)的零偏向量；sg為三軸陀螺儀的比例因子誤差向量；sa為三軸加速度計(jì)的比例因子誤差向量。

對(duì)慣導(dǎo)系統(tǒng)的誤差擾動(dòng)進(jìn)行分析，可得連續(xù)時(shí)間狀態(tài)下姿態(tài)、速度和位置的誤差微分方程：

三軸陀螺儀和三軸加速度計(jì)的零偏和比例因子誤差向量的變化可用一階高斯-馬爾科夫過程表示，其微分方程為

式中：t為時(shí)間；Tbg，Tba，Tsg，Tsa為一階高斯-馬爾科夫過程的相關(guān)時(shí)間；wbg，wba，wsg，wsa為一階高斯-馬爾科夫過程的驅(qū)動(dòng)白噪聲。

為方便計(jì)算，在連續(xù)時(shí)間下的狀態(tài)方程需要先離散化[11]。由式（2）-式（5）可得卡爾曼濾波器狀態(tài)方程在離散化之后的簡(jiǎn)化形式：

式中：F為慣導(dǎo)系統(tǒng)誤差轉(zhuǎn)移矩陣；G為噪聲驅(qū)動(dòng)矩陣；w為噪聲矩陣；k為時(shí)刻標(biāo)記。

建立狀態(tài)方程，可將慣導(dǎo)系統(tǒng)誤差作為狀態(tài)信息融入卡爾曼濾波器中，為卡爾曼濾波器提供預(yù)測(cè)值。

1.4 卡爾曼濾波觀測(cè)方程

采煤機(jī)截割過程中，慣性測(cè)量單元必然要受安裝角和采煤機(jī)振動(dòng)的影響，因此非完整性約束在慣性測(cè)量單元安裝處并不完全成立，但對(duì)于采煤機(jī)牽引齒輪和履帶的連接處，牽引齒輪緊緊地卡在履帶上，非完整性約束是成立的。因此將慣導(dǎo)系統(tǒng)經(jīng)過機(jī)械編排得到的速度投影到牽引齒輪和履帶的連接處，利用非完整性約束與里程計(jì)輸出的速度對(duì)采煤機(jī)構(gòu)成三維速度約束。

當(dāng)采煤機(jī)滿足非完整性約束時(shí)，牽引齒輪和履帶的連接處在采煤機(jī)機(jī)體坐標(biāo)系下的速度為

式中vt為采煤機(jī)真實(shí)前進(jìn)速度。

由慣導(dǎo)系統(tǒng)推算得到的速度投影到牽引齒輪和履帶的連接處時(shí)，需要考慮慣性測(cè)量單元的安裝角及牽引齒輪和履帶的連接處到慣性測(cè)量單元中心的桿臂，投影到牽引齒輪和履帶的連接處的速度為

式中：為由慣性測(cè)量單元坐標(biāo)系到采煤機(jī)機(jī)體坐標(biāo)系的姿態(tài)矩陣[12]，由慣性測(cè)量單元的安裝角確定；為帶有對(duì)準(zhǔn)誤差的由參考坐標(biāo)系到慣性測(cè)量單元坐標(biāo)系的姿態(tài)矩陣，由慣導(dǎo)系統(tǒng)推算得到的姿態(tài)角確定；為慣導(dǎo)系統(tǒng)推算得到的速度向量；為慣導(dǎo)系統(tǒng)推算得到的慣性測(cè)量單元坐標(biāo)系相對(duì)于參考坐標(biāo)系的轉(zhuǎn)動(dòng)角速度向量在慣性測(cè)量單元坐標(biāo)系下的投影；為牽引齒輪和履帶的連接處到慣性測(cè)量單元中心的位置向量在慣性測(cè)量單元坐標(biāo)系下的投影；為由參考坐標(biāo)系到慣性測(cè)量單元坐標(biāo)系的姿態(tài)矩陣；為慣性測(cè)量單元坐標(biāo)系下三軸陀螺儀角速度測(cè)量誤差向量。

里程計(jì)輸出的速度為

式中V為觀測(cè)噪聲。

由式（7）-式（9）確定卡爾曼濾波的觀測(cè)向量：

通過觀測(cè)向量建立觀測(cè)方程，可將非完整性約束與里程計(jì)輸出的速度作為觀測(cè)信息融入卡爾曼濾波器中，抑制慣導(dǎo)系統(tǒng)的定位誤差累計(jì)。

1.5 卡爾曼濾波算法

卡爾曼濾波算法是一種能夠融合預(yù)測(cè)信息和觀測(cè)信息，從而獲得系統(tǒng)下一時(shí)刻狀態(tài)最優(yōu)估計(jì)的算法。卡爾曼濾波算法分為預(yù)測(cè)和更新2個(gè)部分，預(yù)測(cè)算法為

更新算法為

式中：P為預(yù)測(cè)誤差的協(xié)方差矩陣；Q為狀態(tài)方程中系統(tǒng)噪聲的協(xié)方差矩陣；K為濾波增益矩陣；H為觀測(cè)矩陣；R為觀測(cè)噪聲的協(xié)方差矩陣；I為單位矩陣。

通過卡爾曼濾波算法可計(jì)算得到采煤機(jī)姿態(tài)、速度和位置信息的最優(yōu)估計(jì)。

2 實(shí)驗(yàn)驗(yàn)證

2.1 實(shí)驗(yàn)方案

為驗(yàn)證本文方法的可行性，與傳統(tǒng)慣導(dǎo)系統(tǒng)和里程計(jì)組合定位方法進(jìn)行對(duì)比。實(shí)驗(yàn)設(shè)備采用由ICM42605陀螺儀及ADXL355加速度計(jì)集成的慣性測(cè)量單元（參數(shù)見表1，其中g(shù)為重力加速度），并在采煤機(jī)的牽引齒輪上安裝E6H-CWZ6C編碼器作為里程計(jì)的觀測(cè)數(shù)據(jù)來源，編碼器分辨率為3 600 P/r。在整個(gè)實(shí)驗(yàn)過程中實(shí)時(shí)采集慣性測(cè)量單元和編碼器的數(shù)據(jù)，數(shù)據(jù)采集頻率為100 Hz，并在實(shí)驗(yàn)后對(duì)數(shù)據(jù)進(jìn)行處理。

表1 慣性測(cè)量單元性能參數(shù)Table 1 Performance parameters of inertial measurement unit

實(shí)驗(yàn)過程模擬采煤機(jī)在工作面中斜切進(jìn)刀及割三角煤的過程，進(jìn)刀距離為0.3 m，全程總長30 m。為便于對(duì)實(shí)驗(yàn)結(jié)果進(jìn)行觀察，同時(shí)建立工作面坐標(biāo)系oxyz，其以采煤機(jī)起始位置為原點(diǎn)o，采煤機(jī)前進(jìn)方向?yàn)閤軸，工作面推進(jìn)方向?yàn)閥軸，z軸符合右手定則，將定位結(jié)果從參考坐標(biāo)系轉(zhuǎn)換到工作面坐標(biāo)系。在采煤機(jī)行駛過程中，人為不定時(shí)地在采煤機(jī)移動(dòng)軌跡上做一系列標(biāo)記點(diǎn)，測(cè)量得到標(biāo)記點(diǎn)在工作面坐標(biāo)系中的坐標(biāo)，記錄標(biāo)記時(shí)刻并對(duì)標(biāo)記點(diǎn)進(jìn)行編號(hào)，從而得到采煤機(jī)實(shí)測(cè)移動(dòng)軌跡標(biāo)記點(diǎn)。將標(biāo)記點(diǎn)作為定位參考點(diǎn)，與本文方法解算所得軌跡對(duì)應(yīng)點(diǎn)坐標(biāo)進(jìn)行比較分析。

2.2 實(shí)驗(yàn)結(jié)果

采煤機(jī)在工作面坐標(biāo)系下的行駛軌跡如圖3所示。可看出傳統(tǒng)慣導(dǎo)系統(tǒng)和里程計(jì)組合定位方法（傳統(tǒng)方法）由于只有里程計(jì)對(duì)采煤機(jī)前進(jìn)速度起到一定的約束作用，誤差發(fā)散很快；本文方法加入非完整性約束后，由于在采煤機(jī)側(cè)向和垂向速度上增加了運(yùn)動(dòng)約束信息，定位誤差沒有隨時(shí)間發(fā)散，對(duì)實(shí)際軌跡具有良好的追蹤性能。

圖3 采煤機(jī)行駛軌跡Fig. 3 Shearer running trajectory

2種方法在x，y，z軸3個(gè)方向上的定位誤差如圖4所示。可看出在加入非完整性約束后，x，y，z軸方向上的誤差均大幅下降，分別降低了66%，62%，67%。

圖4 采煤機(jī)定位誤差Fig. 4 Shearer positioning error

各方向上的定位誤差最大值見表2。可看出加入非完整性約束的本文方法可大大提高采煤機(jī)在x，y，z軸方向上的定位精度，但在x軸方向上的定位誤差仍遠(yuǎn)大于y，z軸方向上的定位誤差，這是由于雖然有里程計(jì)可以輔助慣導(dǎo)系統(tǒng)對(duì)采煤機(jī)前進(jìn)速度進(jìn)行約束，但里程計(jì)本身存在刻度因數(shù)等誤差，這些誤差會(huì)通過濾波器的不斷迭代而累計(jì)，進(jìn)而疊加在采煤機(jī)前向定位誤差上。

表2 采煤機(jī)定位誤差最大值Table 2 The maximum positioning error of shearer m

3 結(jié)論

（1） 在傳統(tǒng)慣導(dǎo)系統(tǒng)和里程計(jì)組合定位方法的基礎(chǔ)上，根據(jù)采煤機(jī)工作過程中的運(yùn)動(dòng)信息，通過結(jié)合非完整性約束與里程計(jì)輸出的速度，對(duì)采煤機(jī)構(gòu)成三維方向的速度約束，抑制慣導(dǎo)系統(tǒng)的定位誤差累計(jì)。

（2） 實(shí)驗(yàn)結(jié)果表明，相較于傳統(tǒng)慣導(dǎo)系統(tǒng)和里程計(jì)組合定位方法，加入非完整性約束后，采煤機(jī)在前向、側(cè)向、垂向的定位誤差分別降低了66%，62%，67%，有效提高了采煤機(jī)定位精度。

（3） 采煤機(jī)在前向的定位誤差大于側(cè)向和垂向的定位誤差，考慮是里程計(jì)的刻度因數(shù)等誤差累計(jì)造成，今后可通過采用精度更高的里程計(jì)或增加新的傳感器來修正里程計(jì)的輸出，從而降低里程計(jì)誤差累計(jì)帶來的影響。