基于捷聯慣性導航的礦用單軌吊機車定位算法

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

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

0 引言

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

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

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

1 定位流程

捷聯慣性導航系統(Strapdown Inertial Navigation System，SINS)是一個將IMU固定于載體上的獨立定位系統[7]，已廣泛應用于需要定位的各種工程領域[8]。基于SINS的礦用單軌吊機車定位流程如圖1所示。

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

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

2 數據采集與預處理

2.1 數據采集

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

2.2 數據預處理

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

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

a=a0+Kar+R+P

(1)

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

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

2.2.1 零偏濾除方法

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

2.2.2 尖峰噪聲濾除方法

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

(2)

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

2.2.3 刻度因數計算方法

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

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

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

(3)

式中KY為Y軸的刻度因數。

3 重力分量濾除方法

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

設方向余弦矩陣為C，C為3階方陣：

(4)

設機車所在的坐標系為載體坐標系OXbYbZb(b系)，用于定位的坐標系為導航坐標系OXnYnZn(n系)，導航坐標系的方向為東北天，分別對應XYZ軸[15-16]。從導航坐標系到載體坐標系可視為3個有序的旋轉過程，每個旋轉過程用繞單軸旋轉的余弦矩陣來表示[17]。

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

(5)

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

(6)

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

(7)

假定從導航坐標系到載體坐標系依次繞Z,X,Y軸旋轉，相應的姿態矩陣為

(8)

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

(9)

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

4 穩態誤差修正算法與測距算法

穩態誤差修正算法包括穩態加速度誤差修正算法和穩態速度誤差修正算法，測距算法包括Lagrange插值多項式測距算法和測距結果定時修正算法，這2部分算法對定位精度起決定性作用。

4.1 穩態誤差修正算法

4.1.1 穩態加速度誤差修正算法

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

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

加速度修正前后曲線比較如圖2所示。從圖2可看出,在15～20 s，加速度修正前,機車幾乎是減速運動，而此時機車實際是近似勻速運動；修正后的加速度始終為0，修正了穩態加速度誤差。

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

4.1.2 穩態速度誤差修正算法

機車的速度由加速度積分得來，因此，機車在行駛過程中的加速度誤差會導致其停車時的速度不為0[19]，即存在穩態速度誤差。根據停車時的特點，本文提出零速修正法修正穩態速度誤差。

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

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

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

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

由圖3和圖4可知，無零速修正時，停車時的速度為負值，無法正確反映機車的運動狀態；有零速修正時，停車時的速度為0，可正確反映機車的運動狀態。

4.2 測距算法

4.2.1 Lagrange插值多項式測距算法

測距算法是實現準確定位的核心，它的優劣將直接導致累計誤差的產生，因此，本文基于Lagrange插值多項式測距算法[21]，提出了含有4個插值節點的Lagrange插值多項式測距算法。

設4個插值節點分別為(x0,y0),(x1,y1),(x2,y2),(x3,y3)，對應的插值多項式如下：

(10)

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

(11)

式中：vno為當前時刻機車速度；vla為上一時刻機車速度。

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

(12)

式中：hno為當前時刻的行駛距離；hla為上一時刻的行駛距離。

4.2.2 測距結果定時修正算法

為了提高測距精度，防止測距時出現過大的誤差，提出測距結果定時修正法來修正測距誤差。因為單軌吊機車的行駛速度通常為1～2 m/s，所以可通過實驗數據對其在給定時間內的測距結果進行估計。如果給定時間內的測距結果與其估計值的差的絕對值大于閾值hth(hth為常數，此處取0.8 m)，則可將測距結果修正為估計值,否則保持原測距結果。

(13)

式中：ΔH為給定時間內的測距結果；Δhes為給定時間內的測距結果估計值。

5 測距實驗

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

5.1 實驗方案

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

圖5 實驗現場

5.2 測距結果及分析

將10次實驗測距結果進行統計并計算出相應的測距誤差，結果見表1。由表1可知，有4次測距誤差范圍為0.1～1.3 m；4次測距誤差范圍為12.3～16.7 m；有2次測距誤差分別為3.90 m和8.60 m，誤差波動范圍較大。

表1 測距結果

5.3 測距結果定時修正

為了提高測距精度，采用測距結果定時修正法修正測距誤差。通過分析大量實驗數據發現，當給定時間為27.5 s時,機車行駛距離約為30 m,因此設Δhes=30 m,以此為基礎，應用測距結果定時修正法對表1中的測距結果進行修正，結果見表2。

表2 修正后的測距結果

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

6 結論

(1)利用加速度變化率閾值法對穩態加速度誤差進行修正，提高了加速度數據的準確性。

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

(3)實驗結果表明，當汽車行程為30.8 m時，測距誤差基本在0.65 m以內，占總行程的2%，定位精度較高。

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