基于自適應(yīng)卡爾曼濾波的雙慣導(dǎo)采煤機(jī)定位方法

楊金衡， 宋單陽(yáng)， 田慕琴， 宋建成， 許春雨

0 引言

采煤機(jī)定位是智能綜采工作面的關(guān)鍵技術(shù)之一，可解決依靠人工根據(jù)煤層分布狀況對(duì)采煤機(jī)的綜采作業(yè)路徑和參數(shù)進(jìn)行調(diào)整的問(wèn)題，能夠?yàn)閰f(xié)調(diào)采煤機(jī)、刮板輸送機(jī)及液壓支架的聯(lián)動(dòng)運(yùn)行打下基礎(chǔ)。目前常用的采煤機(jī)定位技術(shù)主要有基于紅外信號(hào)定位[1]、基于超聲波信號(hào)定位[2]、基于里程計(jì)定位[3]、基于無(wú)線信號(hào)定位[4]、基于慣導(dǎo)定位[5]等。其中基于紅外信號(hào)、超聲波信號(hào)、里程計(jì)、無(wú)線信號(hào)定位等技術(shù)具有實(shí)現(xiàn)簡(jiǎn)單、可操作性強(qiáng)的優(yōu)點(diǎn)，但存在紅外信號(hào)定位實(shí)時(shí)性弱，超聲波信號(hào)衰減嚴(yán)重，里程計(jì)定位維度低且存在累計(jì)誤差，無(wú)線信號(hào)定位易受井下復(fù)雜信道傳播環(huán)境影響而失效等缺點(diǎn)。慣導(dǎo)在井下工作時(shí)具有定位維度高、實(shí)時(shí)性好、自主性高、抗環(huán)境干擾能力強(qiáng)、數(shù)據(jù)全面、安裝條件簡(jiǎn)單等優(yōu)點(diǎn)，具有非常好的綜合定位性能[6]。然而慣導(dǎo)存在定位誤差累計(jì)、姿態(tài)角及位置漂移這2個(gè)固有缺陷，無(wú)法長(zhǎng)時(shí)間獨(dú)立工作。因此，有學(xué)者提出引入誤差補(bǔ)償技術(shù)和多傳感器組合定位技術(shù)。

誤差補(bǔ)償技術(shù)是對(duì)慣導(dǎo)在測(cè)量過(guò)程中產(chǎn)生的誤差進(jìn)行相應(yīng)補(bǔ)償。Fan Qigao等[7]提出了一種基于慣導(dǎo)的采煤機(jī)定位誤差分析方法，首先建立誤差傳遞模型，再通過(guò)擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)估計(jì)慣導(dǎo)失準(zhǔn)角和采煤機(jī)速度并進(jìn)行補(bǔ)償，但補(bǔ)償效果依賴(lài)于誤差傳遞模型的準(zhǔn)確度。韓勇強(qiáng)等[8]提出了一種慣導(dǎo)零速修正算法，使用Sage-Husa卡爾曼濾波結(jié)合高階擬合的方法對(duì)徑向速度殘差導(dǎo)致的定位誤差進(jìn)行補(bǔ)償，但需要在較長(zhǎng)時(shí)間的停車(chē)情況下進(jìn)行，影響工作效率。Chen Yuming等[9]基于采煤機(jī)動(dòng)力學(xué)模型和振動(dòng)特性，得到初始姿態(tài)角并對(duì)惡劣環(huán)境引起的線性振動(dòng)進(jìn)行補(bǔ)償，有效提高了初始對(duì)準(zhǔn)的收斂速度和精度，但無(wú)法解決慣導(dǎo)定位誤差累計(jì)的問(wèn)題。Wang Shibo等[10]利用里程表輔助慣導(dǎo)，結(jié)合長(zhǎng)壁采煤工作面采煤機(jī)封閉路徑的運(yùn)動(dòng)特性和卡爾曼濾波器，提出了一種慣導(dǎo)定位的最優(yōu)估計(jì)模型，降低了慣導(dǎo)航向角漂移對(duì)定位精度的影響，但無(wú)法補(bǔ)償慣導(dǎo)俯仰角和橫滾角的漂移誤差。

多傳感器組合定位技術(shù)通過(guò)融合慣導(dǎo)和其他傳感器輸出信息，實(shí)現(xiàn)定位誤差修正。應(yīng)葆華等[11]基于慣導(dǎo)與無(wú)線傳感器網(wǎng)絡(luò)的耦合模型，每隔一定時(shí)間利用無(wú)線定位對(duì)慣導(dǎo)的定位結(jié)果進(jìn)行校正，能夠有效減小平均定位誤差，但在井下建立無(wú)線傳感器網(wǎng)絡(luò)需要在多點(diǎn)布置無(wú)線傳感器，成本較高。周開(kāi)平[12]利用加速度計(jì)和陀螺儀組合的慣導(dǎo)解算出采煤機(jī)位置信息，結(jié)合地圖匹配技術(shù)進(jìn)一步降低了慣導(dǎo)在長(zhǎng)時(shí)間工作中出現(xiàn)的累計(jì)誤差，但考慮到工作面地形復(fù)雜，難以獲取精準(zhǔn)地理模型，該方法實(shí)現(xiàn)的技術(shù)難度較高。

本文提出了一種基于自適應(yīng)卡爾曼濾波的雙慣導(dǎo)采煤機(jī)定位方法。利用2套慣導(dǎo)系統(tǒng)的位置信息構(gòu)成雙慣導(dǎo)定位模型，克服了單慣導(dǎo)定位誤差累計(jì)的缺點(diǎn)；但雙慣導(dǎo)定位模型可能存在狀態(tài)突變，導(dǎo)致定位模型準(zhǔn)確度降低，因此將雙慣導(dǎo)定位模型和自適應(yīng)卡爾曼濾波算法結(jié)合，以減小狀態(tài)突變對(duì)定位精度的影響。

1 定位坐標(biāo)系建立

2套慣導(dǎo)系統(tǒng)在采煤機(jī)上的安裝位置如圖1所示[13]。在采煤機(jī)機(jī)身表面的幾何中心位置安裝慣導(dǎo)1，在同一平面內(nèi)安裝慣導(dǎo)2，慣導(dǎo)2與慣導(dǎo)1之間的距離為一定值r且相互之間呈一定夾角α。定義采煤機(jī)行進(jìn)方向?yàn)閄b軸，工作面推進(jìn)方向?yàn)閅b軸，采煤機(jī)機(jī)身的豎直方向?yàn)閆b軸。

圖1 雙慣導(dǎo)系統(tǒng)安裝位置

定位坐標(biāo)系如圖2所示。以采煤機(jī)機(jī)身上表面的幾何中心為坐標(biāo)原點(diǎn)O，建立采煤機(jī)的載體坐標(biāo)系(O-XbYbZb)，如圖2所示。選擇東-北-天坐標(biāo)系(O-XnYnZn)作為參考坐標(biāo)系，Xn指向北方向，Yn指向東方向，Zn垂直于Xn軸和Yn軸。ψ，θ，φ分別為采煤機(jī)機(jī)身的橫滾角、俯仰角及航向角。

圖2 定位坐標(biāo)系

2 雙慣導(dǎo)定位模型

慣導(dǎo)系統(tǒng)在采樣周期內(nèi)輸出加速度和姿態(tài)角，對(duì)加速度積分可得該采樣周期內(nèi)的采煤機(jī)速度。由于慣導(dǎo)系統(tǒng)輸出的加速度數(shù)據(jù)是離散形式的，可采用Lagrange插值多項(xiàng)式對(duì)一段時(shí)間內(nèi)的離散加速度函數(shù)進(jìn)行插值得到插值函數(shù)，再對(duì)插值函數(shù)積分[14]。

取3次加速度采樣數(shù)據(jù)得到3個(gè)插值點(diǎn)(k-2,ak-2)，(k-1,ak-1)，(k,ak)，其中k為當(dāng)前時(shí)刻，ak為當(dāng)前時(shí)刻的加速度。插值函數(shù)為

(1)

式中x為采樣時(shí)刻。

對(duì)Lk(x)在(k-1，k)區(qū)間上進(jìn)行積分，可得采樣周期內(nèi)的速度增量。上一時(shí)刻的速度加上采樣周期內(nèi)的速度增量即為當(dāng)前時(shí)刻的速度。

(2)

式中：vk為當(dāng)前時(shí)刻的速度；vk-1為上一時(shí)刻的速度；T為采樣周期。

由于采樣周期非常小，可將當(dāng)前時(shí)刻的速度作為1個(gè)采樣周期內(nèi)的平均速度，其與采樣周期的乘積為采煤機(jī)在采樣周期內(nèi)的位移增量ΔS。根據(jù)采煤機(jī)的實(shí)際運(yùn)動(dòng)特點(diǎn)(采煤機(jī)沿Xb軸運(yùn)動(dòng))，ΔS在參考坐標(biāo)系上的投影為

(3)

(4)

根據(jù)航跡推算原理，采煤機(jī)上的2套慣導(dǎo)系統(tǒng)在參考坐標(biāo)系下的位置為

(5)

根據(jù)式(5)，選取X(k)=[N1(k)E1(k)U1(k)N2(k)E2(k)U2(k)]T作為狀態(tài)量，可得狀態(tài)方程：

X(k)=A(k)X(k-1)+D(k)+W(k)

(6)

式中：A(k)為狀態(tài)轉(zhuǎn)移矩陣；D(k)為控制矩陣；W(k)為過(guò)程噪聲。

(7)

選取2套慣導(dǎo)系統(tǒng)之間的距離r和夾角α作為卡爾曼濾波器的觀測(cè)量Z(k)，r和α與狀態(tài)量之間的關(guān)系如下：

(8)

由式(8)結(jié)合狀態(tài)量X(k)，可得非線性的觀測(cè)方程：

Z(k)=f(X(k))+V(k)

(9)

式中：f()為表示狀態(tài)量和觀測(cè)量之間非線性關(guān)系的函數(shù)；V(k)為觀測(cè)噪聲。

用一階泰勒展開(kāi)式對(duì)式(9)進(jìn)行線性化處理[15]，得到線性化后的觀測(cè)方程：

Z(k)=H(k)X(k)+V(k)

(10)

式中H(k)為雅可比矩陣。

(11)

式中d=(N1(k)-N2(k))2+(E1(k)-E2(k))2。

整理可得EKF過(guò)程：

(1) 計(jì)算一步預(yù)測(cè)值：

(12)

(2) 計(jì)算一步預(yù)測(cè)均方誤差：

(13)

式中:P(k-1)為上一時(shí)刻狀態(tài)估計(jì)誤差的均方誤差矩陣；Q(k)為W(k)的協(xié)方差矩陣。

(3) 更新濾波增益：

(14)

式中R(k)為V(k)的協(xié)方差矩陣。

(4)k時(shí)刻的狀態(tài)估計(jì)更新：

(15)

(5)k時(shí)刻的均方誤差更新：

[I-K(k)H(k)]T+K(k)R(k)KT(k)

(16)

式中I為單位矩陣。

3 自適應(yīng)卡爾曼濾波

卡爾曼濾波器在迭代更新時(shí)的殘差為

(17)

在濾波過(guò)程中，當(dāng)狀態(tài)量無(wú)誤差且觀測(cè)量無(wú)較大偏差時(shí)，濾波器輸出的殘差服從標(biāo)準(zhǔn)正態(tài)分布。根據(jù)卡爾曼濾波的正交性原理[16]，殘差的馬氏距離的平方服從卡方分布，因此可構(gòu)造基于殘差的卡方檢驗(yàn)值：

(18)

式中：ξk服從自由度為m的卡方分布，即ξk～χ2(m)，本文中m為觀測(cè)量的維數(shù)；Crk為殘差rk的協(xié)方差矩陣。

(19)

當(dāng)狀態(tài)突變時(shí)，濾波器狀態(tài)方程的估計(jì)值偏離真實(shí)狀態(tài)較遠(yuǎn)，過(guò)程噪聲增加。狀態(tài)突變程度越大，卡方檢驗(yàn)值ξk越大。因此，引入三段模糊判別函數(shù)調(diào)整過(guò)程噪聲的協(xié)方差矩陣Q(k)，減少狀態(tài)估計(jì)不確定性對(duì)于濾波結(jié)果的影響。三段模糊判別函數(shù)通過(guò)設(shè)置不同的卡方檢驗(yàn)臨界值，設(shè)定狀態(tài)檢測(cè)閾值的上界Bmax、下界Bmin，用來(lái)調(diào)整不同程度的狀態(tài)突變。Bmax，Bmin分別取置信概率為99%和90%時(shí)所對(duì)應(yīng)的卡方臨界值，查表可得Bmax=9.21，Bmin=4.61。定義如下三段模糊判別函數(shù)，用于實(shí)時(shí)更新Q(k)，從而達(dá)到自適應(yīng)濾波的目的。

(20)

式中：Qmin為雙慣導(dǎo)定位模型未出現(xiàn)狀態(tài)突變時(shí)的Q(k)；Qmax可取Qmin的m倍。

4 方法驗(yàn)證

4.1 仿真分析

仿真總時(shí)長(zhǎng)為2 000 s，周期為1s，在采煤機(jī)運(yùn)動(dòng)到第500，1 000，1 500 s時(shí)分別對(duì)俯仰角、航向角及橫滾角進(jìn)行改變，采煤機(jī)運(yùn)行軌跡如圖3所示。

圖3 采煤機(jī)運(yùn)行軌跡

2套慣導(dǎo)系統(tǒng)之間的距離r和夾角α分別為1 m和45°。姿態(tài)角噪聲均設(shè)置為由0.2°/h的常值誤差、0.5°為基數(shù)的高斯白噪聲組成的混合噪聲，加速度噪聲設(shè)置為由0.1g(g為重力加速度)的常值誤差、0.05g為基數(shù)的高斯白噪聲組成的混合噪聲。

為驗(yàn)證本文方法的有效性，制造雙慣導(dǎo)定位模型狀態(tài)突變的外部環(huán)境，在第500，1 000，1 500 s時(shí)放大慣導(dǎo)1的過(guò)程噪聲，增加狀態(tài)估計(jì)不確定性。

雙慣導(dǎo)定位模型下引入自適應(yīng)卡爾曼濾波前后的卡方檢驗(yàn)值如圖4所示。在引入自適應(yīng)卡爾曼濾波前，第500，1 000 s附近存在較大的卡方檢驗(yàn)值；在引入自適應(yīng)卡爾曼濾波后，第500，1 000 s的卡方檢驗(yàn)值均小于引入自適應(yīng)卡爾曼濾波前的卡方檢驗(yàn)值，且低于檢測(cè)閾值；在第1 500 s放大過(guò)程噪聲時(shí)，引入自適應(yīng)卡爾曼濾波前后的卡方檢驗(yàn)值并無(wú)明顯變化且低于檢測(cè)閾值，表明自適應(yīng)卡爾曼濾波有效減小了狀態(tài)突變時(shí)的估計(jì)誤差，雙慣導(dǎo)定位模型的準(zhǔn)確度得到了一定改善。

(a) 引入自適應(yīng)卡爾曼濾波前

對(duì)比單慣導(dǎo)定位、基于EKF的雙慣導(dǎo)定位和基于自適應(yīng)卡爾曼濾波的雙慣導(dǎo)定位方法下采煤機(jī)在北、東、天方向上的定位誤差，結(jié)果如圖5所示，具體誤差范圍見(jiàn)表1?？煽闯?種雙慣導(dǎo)定位方法的定位誤差均小于單慣導(dǎo)定位方法；但基于EKF的雙慣導(dǎo)定位方法在采煤機(jī)存在較大機(jī)動(dòng)和狀態(tài)突變時(shí)，抗干擾能力較弱，各方向上的定位誤差會(huì)有明顯增加；而基于自適應(yīng)卡爾曼濾波的雙慣導(dǎo)定位方法通過(guò)動(dòng)態(tài)調(diào)整過(guò)程噪聲的協(xié)方差矩陣，降低了過(guò)程噪聲的協(xié)方差矩陣在濾波增益中的比重，可以很好地抑制狀態(tài)突變帶來(lái)的影響，使導(dǎo)航精度保持在良好的范圍內(nèi)。

(a) 慣導(dǎo)1位置誤差

表1 不同定位方法的誤差范圍

4.2 實(shí)驗(yàn)分析

單慣導(dǎo)定位、基于自適應(yīng)卡爾曼濾波的雙慣導(dǎo)定位方法下定位結(jié)果如圖6、圖7所示?？煽闯鲭S著運(yùn)行距離增加，與理想軌跡相比，單慣導(dǎo)的定位誤差逐漸增大，而基于自適應(yīng)卡爾曼濾波的雙慣導(dǎo)定位方法的定位誤差趨于收斂，無(wú)明顯的誤差累計(jì)現(xiàn)象，定位軌跡與理想軌跡重合度較好；基于自適應(yīng)卡爾曼濾波的雙慣導(dǎo)定位方法的定位誤差比單慣導(dǎo)的定位誤差在各方向上均有所減小。

(a) 水平面定位軌跡

(a) 水平面定位軌跡

5 結(jié)語(yǔ)

提出了一種基于自適應(yīng)卡爾曼濾波的雙慣導(dǎo)采煤機(jī)定位方法。在采煤機(jī)上固定安裝2套慣導(dǎo)系統(tǒng)，以慣導(dǎo)系統(tǒng)的位置為狀態(tài)量，慣導(dǎo)系統(tǒng)之間的距離和夾角為觀測(cè)量，構(gòu)建雙慣導(dǎo)定位模型；利用自適應(yīng)卡爾曼濾波算法對(duì)雙慣導(dǎo)定位模型狀態(tài)進(jìn)行評(píng)估和調(diào)整，即通過(guò)卡方檢驗(yàn)評(píng)估雙慣導(dǎo)定位模型是否發(fā)生狀態(tài)突變，并利用三段模糊判別函數(shù)動(dòng)態(tài)調(diào)整過(guò)程噪聲的協(xié)方差矩陣，從而降低雙慣導(dǎo)定位模型狀態(tài)突變對(duì)定位精度的影響。仿真和實(shí)驗(yàn)結(jié)果表明，自適應(yīng)卡爾曼濾波相比EKF的抗干擾能力更強(qiáng)，能抑制狀態(tài)突變帶來(lái)的影響；基于自適應(yīng)卡爾曼濾波的雙慣導(dǎo)采煤機(jī)定位方法的定位誤差比單慣導(dǎo)的定位誤差在各方向上均有所減小。