基于捷聯(lián)慣性導(dǎo)航的礦用單軌吊機(jī)車定位算法

郭梁， 宋建成， 寧振兵， 王明勇， 吝伶艷， 黃健琦

(1.太原理工大學(xué) 電氣與動力工程學(xué)院，山西 太原 030024；2.太原理工大學(xué) 礦用智能電器技術(shù)國家地方聯(lián)合工程實(shí)驗(yàn)室，山西 太原 030024；3.太原礦機(jī)電氣股份有限公司，山西 太原 030032)

0 引言

單軌吊機(jī)車是煤礦井下用來運(yùn)送材料、設(shè)備和人員的先進(jìn)輔助運(yùn)輸設(shè)備。與傳統(tǒng)的輔助運(yùn)輸方式相比，單軌吊機(jī)車具有運(yùn)輸效率高、坡度大、不受巷道地面影響等特點(diǎn)。盡管這種運(yùn)輸方式優(yōu)點(diǎn)突出，但其定位方式落后，目前仍以定位精度較低的射頻識別(Radio Frequency Identification，RFID)定位和接收信號強(qiáng)度指示(Received Signal Strength Indication，RSSI)定位為主，難以滿足工業(yè)應(yīng)用需求。據(jù)2013—2017年我國煤礦事故統(tǒng)計(jì)，運(yùn)輸事故數(shù)占煤礦事故總數(shù)的17.48%，運(yùn)輸事故死亡人數(shù)占煤礦事故總死亡人數(shù)的10.85%[1]，這與礦井輔助運(yùn)輸機(jī)車無法精確定位有直接關(guān)系。此外，在不確定機(jī)車位置的情況下，無法實(shí)現(xiàn)機(jī)車的統(tǒng)一調(diào)度，會出現(xiàn)多輛單軌吊機(jī)車擁堵的情況，運(yùn)輸效率較低。因此，開發(fā)單軌吊機(jī)車高精度定位系統(tǒng)具有重要的現(xiàn)實(shí)意義。

目前，應(yīng)用于煤礦井下的定位技術(shù)有RFID、RSSI、超寬帶(Ultra Wide Band，UWB)、捷聯(lián)慣性導(dǎo)航等定位技術(shù)。文獻(xiàn)[2]設(shè)計(jì)了一種基于RFID和ZigBee技術(shù)的礦井機(jī)車精確定位系統(tǒng)，該系統(tǒng)通過RFID技術(shù)實(shí)現(xiàn)位置的更新，以ZigBee技術(shù)實(shí)現(xiàn)數(shù)據(jù)的傳輸，電子標(biāo)簽密度越高，定位精度越高，但這會導(dǎo)致成本上升。文獻(xiàn)[3]設(shè)計(jì)了一種基于ZigBee的鎢礦井下礦車定位監(jiān)測系統(tǒng)，該系統(tǒng)利用RSSI測距模型和加權(quán)質(zhì)心定位算法實(shí)現(xiàn)了礦車定位，與極大似然估計(jì)算法相比，定位精度較高，但該系統(tǒng)需要較多的參考節(jié)點(diǎn)，定位成本高。文獻(xiàn)[4]提出了一種基于RSSI的礦山井下機(jī)車定位算法，利用最小二乘法和chan算法實(shí)現(xiàn)機(jī)車的定位，最小定位誤差為0.4 m，但該方法也需要較多參考節(jié)點(diǎn)才能實(shí)現(xiàn)精確定位，定位成本高。文獻(xiàn)[5]設(shè)計(jì)了一種基于UWB的礦井機(jī)車定位系統(tǒng)，該系統(tǒng)利用UWB技術(shù)和到達(dá)時間差(Time Difference of Arrival，TDOA)定位算法實(shí)現(xiàn)機(jī)車定位，定位精度達(dá)10 cm，但該系統(tǒng)在實(shí)現(xiàn)某一區(qū)域的定位時，需要1臺同步控制器和多臺定位基站，成本較高。文獻(xiàn)[6]設(shè)計(jì)了基于慣性導(dǎo)航的煤礦移動車輛定位系統(tǒng)，該系統(tǒng)采用六軸慣性測量單元(Inertial Measurement Unit，IMU)實(shí)現(xiàn)對車輛的定位，然而，六軸IMU解算出的航向角誤差較大，難以對機(jī)車進(jìn)行長期、準(zhǔn)確的定位。

針對以上定位方法存在的問題，本文提出了一種基于捷聯(lián)慣性導(dǎo)航的礦用單軌吊機(jī)車定位算法，該算法不僅提高了單軌吊機(jī)車的定位精度，而且降低了定位成本。

1 定位流程

捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System，SINS)是一個將IMU固定于載體上的獨(dú)立定位系統(tǒng)[7]，已廣泛應(yīng)用于需要定位的各種工程領(lǐng)域[8]?；赟INS的礦用單軌吊機(jī)車定位流程如圖1所示。

圖1 基于SINS的礦用單軌吊機(jī)車定位流程

利用九軸IMU(包括三軸加速度傳感器、三軸陀螺儀、三軸磁力計(jì))采集機(jī)車數(shù)據(jù)，解算并輸出姿態(tài)角和加速度值。為了濾除加速度數(shù)據(jù)中的確定性誤差和尖峰噪聲，需對其進(jìn)行預(yù)處理。由于加速度數(shù)據(jù)中含有重力分量，當(dāng)機(jī)車在斜坡上行駛時該分量會對測距造成巨大干擾，因而采用方向余弦矩陣法濾除該分量。在積分測距的過程中，通過加速度變化率閾值法和零速修正法修正穩(wěn)態(tài)誤差，根據(jù)機(jī)車測距結(jié)果和初始位置實(shí)現(xiàn)定位。

2 數(shù)據(jù)采集與預(yù)處理

2.1 數(shù)據(jù)采集

采用WT931模塊采集數(shù)據(jù)，該模塊集成了高精度的微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System，MEMS)加速度計(jì)、陀螺儀、磁力計(jì)，可分別采集機(jī)車的三軸加速度、三軸角速度、三軸磁場強(qiáng)度數(shù)據(jù)。對加速度積分可以得到機(jī)車的行駛距離。以加速度、角速度、磁場強(qiáng)度數(shù)據(jù)為基礎(chǔ)，利用卡爾曼濾波算法可以精準(zhǔn)地輸出機(jī)車的實(shí)時姿態(tài)角。

2.2 數(shù)據(jù)預(yù)處理

盡管MEMS加速度傳感器具有體積小、質(zhì)量小、功耗低、成本低、易集成等優(yōu)點(diǎn)[9]，但其采集到的加速度數(shù)據(jù)中仍存在誤差和噪聲，所以，必須對加速度數(shù)據(jù)進(jìn)行預(yù)處理。

MEMS加速度傳感器采集到的加速度數(shù)據(jù)中的誤差和噪聲主要包括確定性誤差、隨機(jī)誤差、尖峰噪聲等[10]。誤差模型為

a=a0+Kar+R+P

(1)

式中：a為加速度測量值；a0為零偏；K為刻度因數(shù)；ar為加速度理論值；R為隨機(jī)噪聲；P為尖峰噪聲。

WT931模塊中集成了高精度的BMI160六軸傳感器，該傳感器可輸出高精度的三軸加速度和三軸角速度數(shù)據(jù)，加速度中所含的隨機(jī)噪聲很小，因此，可不考慮其影響。零偏、刻度因數(shù)、尖峰噪聲均會對加速度測量值造成較大干擾。本文分別采用均值法和限幅濾波法濾除零偏和尖峰噪聲，采用翻滾法計(jì)算刻度因數(shù)。

2.2.1 零偏濾除方法

零偏是指加速度傳感器輸入為0時的輸出量，它是由加速度傳感器本身的制作工藝引起的誤差。對于零偏，可采用均值法濾除。以Y軸為例，首先將WT931模塊置于水平臺上(Y軸平行于臺面)，連續(xù)采集2 000個加速度數(shù)據(jù)并求出均值aav，此后，用采集到的加速度數(shù)據(jù)減去aav即可濾除零偏[11]。

2.2.2 尖峰噪聲濾除方法

尖峰噪聲是由傳感器本身的技術(shù)問題引起的，它具有幅值大、持續(xù)時間短等特點(diǎn)。本文根據(jù)限幅濾波的思想來濾除尖峰噪聲。已知加速度傳感器的量程為±2g(g=9.8 m/s2)，假設(shè)相鄰2次采樣值為an-1和an，如果an的絕對值超過了2g，則令an等于前一時刻的值an-1；如果an的絕對值小于或等于2g，則不對an進(jìn)行處理[12]。

(2)

式中an為n時刻的加速度。

2.2.3 刻度因數(shù)計(jì)算方法

刻度因數(shù)是指加速度傳感器輸出量與輸入量之比。由于MEMS加速度傳感器的折疊梁加工尺寸不一致和微機(jī)械結(jié)構(gòu)質(zhì)心偏離平衡位置等原因[13]，刻度因數(shù)為未知的常數(shù)。本文根據(jù)12位置重力翻滾法的思想，采用翻滾法來求取刻度因數(shù)。

以Y軸為例，將WT931模塊置于水平臺上，將Y軸與水平臺的夾角依次調(diào)整至90°和-90°，待數(shù)據(jù)穩(wěn)定后分別采集2 000個Y軸加速度數(shù)據(jù)并求取均值aY1和aY2，根據(jù)均值可以求出刻度因數(shù)，計(jì)算公式為

KY=(|aY1|+|aY2|)/2g

(3)

式中KY為Y軸的刻度因數(shù)。

3 重力分量濾除方法

慣性模塊WT931中的加速度傳感器既能檢測到靜態(tài)加速度(重力加速度)，又能檢測到動態(tài)加速度(運(yùn)動加速度)[14]。當(dāng)單軌吊機(jī)車在斜坡上運(yùn)動時，重力加速度(即重力分量)的存在會對運(yùn)動加速度產(chǎn)生干擾，這會嚴(yán)重影響單軌吊機(jī)車的定位精度。因此，本文采用方向余弦矩陣法濾除重力分量。

設(shè)方向余弦矩陣為C，C為3階方陣：

(4)

設(shè)機(jī)車所在的坐標(biāo)系為載體坐標(biāo)系OXbYbZb(b系)，用于定位的坐標(biāo)系為導(dǎo)航坐標(biāo)系OXnYnZn(n系)，導(dǎo)航坐標(biāo)系的方向?yàn)闁|北天，分別對應(yīng)XYZ軸[15-16]。從導(dǎo)航坐標(biāo)系到載體坐標(biāo)系可視為3個有序的旋轉(zhuǎn)過程，每個旋轉(zhuǎn)過程用繞單軸旋轉(zhuǎn)的余弦矩陣來表示[17]。

繞X軸旋轉(zhuǎn)α(俯仰角)得到的余弦矩陣為

(5)

繞Y軸旋轉(zhuǎn)γ(橫滾角)得到的余弦矩陣為

(6)

繞Z軸旋轉(zhuǎn)θ(航向角)得到的余弦矩陣為

(7)

假定從導(dǎo)航坐標(biāo)系到載體坐標(biāo)系依次繞Z,X,Y軸旋轉(zhuǎn)，相應(yīng)的姿態(tài)矩陣為

(8)

由此可得出重力加速度在載體坐標(biāo)系各軸上的分量為

(9)

用采集到的加速度值減去重力加速度在載體坐標(biāo)系各軸上的分量gx,gy,gz，即可濾除重力分量。

4 穩(wěn)態(tài)誤差修正算法與測距算法

穩(wěn)態(tài)誤差修正算法包括穩(wěn)態(tài)加速度誤差修正算法和穩(wěn)態(tài)速度誤差修正算法，測距算法包括Lagrange插值多項(xiàng)式測距算法和測距結(jié)果定時修正算法，這2部分算法對定位精度起決定性作用。

4.1 穩(wěn)態(tài)誤差修正算法

4.1.1 穩(wěn)態(tài)加速度誤差修正算法

由于MEMS加速度傳感器的精度有限，當(dāng)其靜止或勻速運(yùn)動時的加速度值總在0附近小幅波動，即存在穩(wěn)態(tài)加速度誤差。通過分析數(shù)據(jù)發(fā)現(xiàn)，傳感器處于靜止與變速運(yùn)動時的加速度變化率幅值有較大差異，因此，本文提出了加速度變化率閾值法，用以修正此誤差。

設(shè)a1，a2，…，a10為連續(xù)10個加速度值，u為這10個數(shù)據(jù)的均值，T為采樣周期。以10個數(shù)據(jù)為1組，用組內(nèi)相鄰2個數(shù)據(jù)求出1個加速度變化率并取絕對值，由此可得：b1=|(a2-a1)|/T，b2=|(a3-a2)|/T，…，b9=|(a10-a9)|/T。用d表示9個數(shù)據(jù)中大于w(w為經(jīng)驗(yàn)值，此處取0.1 m/s3)的個數(shù)，d最大為9。如果d<8且|u|<0.25 m/s2，則將這10個數(shù)據(jù)置0；否則，對其不做任何處理[18]。

加速度修正前后曲線比較如圖2所示。從圖2可看出,在15～20 s，加速度修正前,機(jī)車幾乎是減速運(yùn)動，而此時機(jī)車實(shí)際是近似勻速運(yùn)動；修正后的加速度始終為0，修正了穩(wěn)態(tài)加速度誤差。

圖2 加速度修正前后曲線比較

4.1.2 穩(wěn)態(tài)速度誤差修正算法

機(jī)車的速度由加速度積分得來，因此，機(jī)車在行駛過程中的加速度誤差會導(dǎo)致其停車時的速度不為0[19]，即存在穩(wěn)態(tài)速度誤差。根據(jù)停車時的特點(diǎn)，本文提出零速修正法修正穩(wěn)態(tài)速度誤差。

設(shè)an為n時刻的加速度，vn為n時刻的速度，n=0,1,…，vco為一個速度常數(shù)(此處取0.005 m/s)。如果vn≤vco且an，an-1，an-2均小于0，則令vn=0，否則不對vn做處理[20]。

機(jī)車運(yùn)行時要經(jīng)歷靜止、變速運(yùn)動、靜止3個階段。無零速修正與有零速修正時的速度變化曲線分別如圖3、圖4所示。

圖3 無零速修正時的速度變化曲線

圖4 有零速修正時的速度變化曲線

由圖3和圖4可知，無零速修正時，停車時的速度為負(fù)值，無法正確反映機(jī)車的運(yùn)動狀態(tài)；有零速修正時，停車時的速度為0，可正確反映機(jī)車的運(yùn)動狀態(tài)。

4.2 測距算法

4.2.1 Lagrange插值多項(xiàng)式測距算法

測距算法是實(shí)現(xiàn)準(zhǔn)確定位的核心，它的優(yōu)劣將直接導(dǎo)致累計(jì)誤差的產(chǎn)生，因此，本文基于Lagrange插值多項(xiàng)式測距算法[21]，提出了含有4個插值節(jié)點(diǎn)的Lagrange插值多項(xiàng)式測距算法。

設(shè)4個插值節(jié)點(diǎn)分別為(x0,y0),(x1,y1),(x2,y2),(x3,y3)，對應(yīng)的插值多項(xiàng)式如下：

(10)

在計(jì)算機(jī)車速度時，相應(yīng)插值節(jié)點(diǎn)分別為(0,a0),(T,a1),(2T,a2),(3T,a3)，a0—a3為按時間順序采集到的4個加速度，a3為當(dāng)前時刻的加速度。將這4個插值節(jié)點(diǎn)代入f(x)中，得到函數(shù)A(x)，對A(x)進(jìn)行積分即可計(jì)算出機(jī)車速度。

(11)

式中：vno為當(dāng)前時刻機(jī)車速度；vla為上一時刻機(jī)車速度。

在計(jì)算機(jī)車行駛距離時，相應(yīng)的插值節(jié)點(diǎn)分別為(0,v0),(T,v1),(2T,v2),(3T,v3)，v0—v3為按時間順序求得的4個速度，v3為當(dāng)前時刻的速度。將這4個插值節(jié)點(diǎn)代入f(x)中，得到函數(shù)V(x)。對V(x)進(jìn)行積分即可計(jì)算出機(jī)車的行駛距離。

(12)

式中：hno為當(dāng)前時刻的行駛距離；hla為上一時刻的行駛距離。

4.2.2 測距結(jié)果定時修正算法

為了提高測距精度，防止測距時出現(xiàn)過大的誤差，提出測距結(jié)果定時修正法來修正測距誤差。因?yàn)閱诬壍鯔C(jī)車的行駛速度通常為1～2 m/s，所以可通過實(shí)驗(yàn)數(shù)據(jù)對其在給定時間內(nèi)的測距結(jié)果進(jìn)行估計(jì)。如果給定時間內(nèi)的測距結(jié)果與其估計(jì)值的差的絕對值大于閾值hth(hth為常數(shù)，此處取0.8 m)，則可將測距結(jié)果修正為估計(jì)值,否則保持原測距結(jié)果。

(13)

式中：ΔH為給定時間內(nèi)的測距結(jié)果；Δhes為給定時間內(nèi)的測距結(jié)果估計(jì)值。

5 測距實(shí)驗(yàn)

為了驗(yàn)證定位算法的準(zhǔn)確性，以WT931模塊和汽車為實(shí)驗(yàn)平臺，將WT931模塊安裝于汽車上用于采集運(yùn)行數(shù)據(jù)，以汽車在平坦路面的行駛來模擬單軌吊機(jī)車在平直軌道上的行駛。

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

由于單軌吊機(jī)車的最大行駛速度約為2 m/s，所以，本實(shí)驗(yàn)將汽車的最高速度控制在2 m/s左右；加速度傳感器采樣頻率設(shè)為200 Hz；行駛路線為30.8 m的直線路段，汽車經(jīng)歷靜止、變速運(yùn)動、靜止3個階段。實(shí)驗(yàn)現(xiàn)場如圖5所示。反復(fù)實(shí)驗(yàn)10次，實(shí)時采集加速度數(shù)據(jù),利用Matlab軟件計(jì)算測距結(jié)果。

圖5 實(shí)驗(yàn)現(xiàn)場

5.2 測距結(jié)果及分析

將10次實(shí)驗(yàn)測距結(jié)果進(jìn)行統(tǒng)計(jì)并計(jì)算出相應(yīng)的測距誤差，結(jié)果見表1。由表1可知，有4次測距誤差范圍為0.1～1.3 m；4次測距誤差范圍為12.3～16.7 m；有2次測距誤差分別為3.90 m和8.60 m，誤差波動范圍較大。

表1 測距結(jié)果

5.3 測距結(jié)果定時修正

為了提高測距精度，采用測距結(jié)果定時修正法修正測距誤差。通過分析大量實(shí)驗(yàn)數(shù)據(jù)發(fā)現(xiàn)，當(dāng)給定時間為27.5 s時,機(jī)車行駛距離約為30 m,因此設(shè)Δhes=30 m,以此為基礎(chǔ)，應(yīng)用測距結(jié)果定時修正法對表1中的測距結(jié)果進(jìn)行修正，結(jié)果見表2。

表2 修正后的測距結(jié)果

由于表2中前2次實(shí)驗(yàn)測距時間小于27.5 s，所以，不對其進(jìn)行修正。第4次測距誤差較大,為4.46 m，這是由汽車急停導(dǎo)致的。其余7次測距誤差范圍為0.10～0.65 m。與現(xiàn)有的定位方法相比，該算法不僅有較高的定位精度，而且定位成本較低。

6 結(jié)論

(1)利用加速度變化率閾值法對穩(wěn)態(tài)加速度誤差進(jìn)行修正，提高了加速度數(shù)據(jù)的準(zhǔn)確性。

(2)測距結(jié)果定時修正法可以補(bǔ)償大量測距誤差，提高測距精度與定位精度。

(3)實(shí)驗(yàn)結(jié)果表明，當(dāng)汽車行程為30.8 m時，測距誤差基本在0.65 m以內(nèi)，占總行程的2%，定位精度較高。

(4)算法未對機(jī)車急停時帶來的加速度誤差進(jìn)行分析,在后續(xù)工作中，將著重對其進(jìn)行分析與研究。