基于自適應卡爾曼濾波的雙慣導采煤機定位方法

楊金衡， 宋單陽， 田慕琴， 宋建成， 許春雨

0 引言

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

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

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

本文提出了一種基于自適應卡爾曼濾波的雙慣導采煤機定位方法。利用2套慣導系統的位置信息構成雙慣導定位模型，克服了單慣導定位誤差累計的缺點；但雙慣導定位模型可能存在狀態突變，導致定位模型準確度降低，因此將雙慣導定位模型和自適應卡爾曼濾波算法結合，以減小狀態突變對定位精度的影響。

1 定位坐標系建立

2套慣導系統在采煤機上的安裝位置如圖1所示[13]。在采煤機機身表面的幾何中心位置安裝慣導1，在同一平面內安裝慣導2，慣導2與慣導1之間的距離為一定值r且相互之間呈一定夾角α。定義采煤機行進方向為Xb軸，工作面推進方向為Yb軸，采煤機機身的豎直方向為Zb軸。

圖1 雙慣導系統安裝位置

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

圖2 定位坐標系

2 雙慣導定位模型

慣導系統在采樣周期內輸出加速度和姿態角，對加速度積分可得該采樣周期內的采煤機速度。由于慣導系統輸出的加速度數據是離散形式的，可采用Lagrange插值多項式對一段時間內的離散加速度函數進行插值得到插值函數，再對插值函數積分[14]。

取3次加速度采樣數據得到3個插值點(k-2,ak-2)，(k-1,ak-1)，(k,ak)，其中k為當前時刻，ak為當前時刻的加速度。插值函數為

(1)

式中x為采樣時刻。

對Lk(x)在(k-1，k)區間上進行積分，可得采樣周期內的速度增量。上一時刻的速度加上采樣周期內的速度增量即為當前時刻的速度。

(2)

式中：vk為當前時刻的速度；vk-1為上一時刻的速度；T為采樣周期。

由于采樣周期非常小，可將當前時刻的速度作為1個采樣周期內的平均速度，其與采樣周期的乘積為采煤機在采樣周期內的位移增量ΔS。根據采煤機的實際運動特點(采煤機沿Xb軸運動)，ΔS在參考坐標系上的投影為

(3)

(4)

根據航跡推算原理，采煤機上的2套慣導系統在參考坐標系下的位置為

(5)

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

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

(6)

式中：A(k)為狀態轉移矩陣；D(k)為控制矩陣；W(k)為過程噪聲。

(7)

選取2套慣導系統之間的距離r和夾角α作為卡爾曼濾波器的觀測量Z(k)，r和α與狀態量之間的關系如下：

(8)

由式(8)結合狀態量X(k)，可得非線性的觀測方程：

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

(9)

式中：f()為表示狀態量和觀測量之間非線性關系的函數；V(k)為觀測噪聲。

用一階泰勒展開式對式(9)進行線性化處理[15]，得到線性化后的觀測方程：

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

(10)

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

(11)

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

整理可得EKF過程：

(1) 計算一步預測值：

(12)

(2) 計算一步預測均方誤差：

(13)

式中:P(k-1)為上一時刻狀態估計誤差的均方誤差矩陣；Q(k)為W(k)的協方差矩陣。

(3) 更新濾波增益：

(14)

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

(4)k時刻的狀態估計更新：

(15)

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

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

(16)

式中I為單位矩陣。

3 自適應卡爾曼濾波

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

(17)

在濾波過程中，當狀態量無誤差且觀測量無較大偏差時，濾波器輸出的殘差服從標準正態分布。根據卡爾曼濾波的正交性原理[16]，殘差的馬氏距離的平方服從卡方分布，因此可構造基于殘差的卡方檢驗值：

(18)

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

(19)

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

(20)

式中：Qmin為雙慣導定位模型未出現狀態突變時的Q(k)；Qmax可取Qmin的m倍。

4 方法驗證

4.1 仿真分析

仿真總時長為2 000 s，周期為1s，在采煤機運動到第500，1 000，1 500 s時分別對俯仰角、航向角及橫滾角進行改變，采煤機運行軌跡如圖3所示。

圖3 采煤機運行軌跡

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

為驗證本文方法的有效性，制造雙慣導定位模型狀態突變的外部環境，在第500，1 000，1 500 s時放大慣導1的過程噪聲，增加狀態估計不確定性。

雙慣導定位模型下引入自適應卡爾曼濾波前后的卡方檢驗值如圖4所示。在引入自適應卡爾曼濾波前，第500，1 000 s附近存在較大的卡方檢驗值；在引入自適應卡爾曼濾波后，第500，1 000 s的卡方檢驗值均小于引入自適應卡爾曼濾波前的卡方檢驗值，且低于檢測閾值；在第1 500 s放大過程噪聲時，引入自適應卡爾曼濾波前后的卡方檢驗值并無明顯變化且低于檢測閾值，表明自適應卡爾曼濾波有效減小了狀態突變時的估計誤差，雙慣導定位模型的準確度得到了一定改善。

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

對比單慣導定位、基于EKF的雙慣導定位和基于自適應卡爾曼濾波的雙慣導定位方法下采煤機在北、東、天方向上的定位誤差，結果如圖5所示，具體誤差范圍見表1。可看出2種雙慣導定位方法的定位誤差均小于單慣導定位方法；但基于EKF的雙慣導定位方法在采煤機存在較大機動和狀態突變時，抗干擾能力較弱，各方向上的定位誤差會有明顯增加；而基于自適應卡爾曼濾波的雙慣導定位方法通過動態調整過程噪聲的協方差矩陣，降低了過程噪聲的協方差矩陣在濾波增益中的比重，可以很好地抑制狀態突變帶來的影響，使導航精度保持在良好的范圍內。

(a) 慣導1位置誤差

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

4.2 實驗分析

單慣導定位、基于自適應卡爾曼濾波的雙慣導定位方法下定位結果如圖6、圖7所示。可看出隨著運行距離增加，與理想軌跡相比，單慣導的定位誤差逐漸增大，而基于自適應卡爾曼濾波的雙慣導定位方法的定位誤差趨于收斂，無明顯的誤差累計現象，定位軌跡與理想軌跡重合度較好；基于自適應卡爾曼濾波的雙慣導定位方法的定位誤差比單慣導的定位誤差在各方向上均有所減小。

(a) 水平面定位軌跡

(a) 水平面定位軌跡

5 結語

提出了一種基于自適應卡爾曼濾波的雙慣導采煤機定位方法。在采煤機上固定安裝2套慣導系統，以慣導系統的位置為狀態量，慣導系統之間的距離和夾角為觀測量，構建雙慣導定位模型；利用自適應卡爾曼濾波算法對雙慣導定位模型狀態進行評估和調整，即通過卡方檢驗評估雙慣導定位模型是否發生狀態突變，并利用三段模糊判別函數動態調整過程噪聲的協方差矩陣，從而降低雙慣導定位模型狀態突變對定位精度的影響。仿真和實驗結果表明，自適應卡爾曼濾波相比EKF的抗干擾能力更強，能抑制狀態突變帶來的影響；基于自適應卡爾曼濾波的雙慣導采煤機定位方法的定位誤差比單慣導的定位誤差在各方向上均有所減小。