基于模糊AKF地磁輔助導航的采煤機定位方法*

毛 君, 鐘 聲,3, 馬 英

(1.遼寧工程技術(shù)大學 機械工程學院，遼寧 阜新 123000;2.天地科技股份有限公司 開采設(shè)計事業(yè)部，北京 100013;3.北京天地瑪珂電液系統(tǒng)控制有限公司,北京 100013)

0 引 言

根據(jù)煤礦安全、高效生產(chǎn)的要求，以及機械自動化技術(shù)的發(fā)展，綜采工作面實現(xiàn)少人或無人開采是必然趨勢，其較差的自動控制水平將直接影響綜采“無人化”進程的推進[1]。而采煤機在工作面的位置姿態(tài)的準確定位是目前研究的熱點問題之一[2，3]。樊啟高、趙靜等人采用了慣性導航方法對采煤機的位置姿態(tài)進行定位[4，5]。宋宇、翁新武提出了一種基于光流和慣性導航的小型無人機定位方法[6]。姜華、何風行等人提出了一種基于超聲和射頻融合的無線傳感器網(wǎng)絡(luò)網(wǎng)格定位方法[7]。李振浩、李英娜提出了基于免疫記憶人工魚群算法的局部放電超聲定位方法[8]等。

本文在此基礎(chǔ)之上提出了一種基于模糊自適應(yīng)卡爾曼濾波(adaptive Kalman filtering,AKF)慣性地磁導航的采煤機動態(tài)定位方法，可有效避免慣導定位的誤差累積。

1 采煤機慣性導航定位

將慣性導航固定于采煤機，通過三軸加速度計和三軸陀螺儀進行實時反饋采煤機的空間加速度信號和姿態(tài)信息，根據(jù)采煤機初始位置，可實現(xiàn)對采煤機的動態(tài)位置信息實時解算。

圖1 慣性導航計算流程

為了降低誤差累積，采用迭代最近等值點(iterative closest contour point，ICCP)算法將慣導與地磁輔助技術(shù)組合[9]。通過地磁輔助慣性導航所指示位置的P(P1,P2,…,PN)，而實際輸出的位置為P＇(P＇1，P＇2，…P＇N)，通過地磁傳感器對地磁場值進行測量，則得到地磁場等值線HN(H1,H2,…,HN)。通過歐氏距離法在該地磁場等值線上尋找距離P(P1,P2,…PN)點最近距離MN(M1,M2,…,MN)，通過旋轉(zhuǎn)矩陣R和平移矩陣t對其進行修正，反復(fù)進行上述過程，直至達到迭代終止條件。

為了對慣性導航實時性校正，采用模糊自適應(yīng)的卡爾曼濾波方法與地磁輔助慣性導航組合[10]。其效果通過系統(tǒng)狀態(tài)向量協(xié)方差的變換進行評價。

卡爾曼濾波中，測量新息定義為

rk+1=Zk+1-Hψk+1,kXk

(1)

其測量殘差方差實際值為[11]

(2)

式中m0=mN-N+1;N為測量方差。

卡爾曼濾波的測量殘差方差理論值為[12]

(3)

式(2)與式(3)的比值為

(4)

式中 tr(·)為矩陣的跡。

由于受到測量噪聲的影響，Rr值偏離理想值，則通過模糊控制量β進行自適應(yīng)調(diào)節(jié)

Rk+1=βRk

(5)

圖2 導航系統(tǒng)計算流程

采煤機模糊AKF的地磁輔助慣性導航得到姿態(tài)更新

(6)

采煤機速度矢量更新

(7)

式中vx,vy,vz分別為采煤機在x,y,z方向的速度;ωie為地球自轉(zhuǎn)角速率;RN為采煤機所在地經(jīng)線圈切線方向地球的曲率半徑;f=(fxfyfz)T為加速度計測量的比力；gn為采煤機重力加速度的大小。

采煤機位置更新微分方程

(8)

式中L為當?shù)鼐暥?h為當?shù)馗叨?RM為采煤機所在地緯線圈切線方向地球的曲率半徑。

通過式(6)可得出采煤機實時姿態(tài)角，通過對式(8)進行數(shù)值積分，可得到采煤機的所在位置。

2 仿真分析

基于本文所推導的采煤機導航方程，對采用模糊AKF慣性地磁導航的采煤機系統(tǒng)方案進行仿真分析。首先模擬加速度計以及陀螺儀的數(shù)據(jù)輸出，對產(chǎn)生的加速度計信號和陀螺儀信號進行實時采樣，并通過慣性處理法計算出采煤機的位移及速度信息。在地理坐標系下采煤機起始位置設(shè)定為[0 0 0]T，初始方位角設(shè)定為0°，初始速度設(shè)定為v0=0.5 m/s。

在采煤機仿真平臺中，設(shè)定X表示采煤機平行工作面方向，運行10 m，Y表示工作面推移方向，行進0.4 m，Z則表示垂直工作面方向，其中采煤機在X方向4～6 m處進行斜切進刀，仿真時間設(shè)定20 s，結(jié)果如圖3所示，分析可知，采用捷聯(lián)慣導方法定位結(jié)果在理想軌跡附近進行小幅波動，極大地減小了隨時間積累的定位誤差。

圖3 定位結(jié)果

3 結(jié)束語

針對采煤機煤礦井下定位受環(huán)境惡劣以及干擾較多等問題，提出了基于模糊AKF地磁輔助慣性導航的采煤機定位方法，推導了采煤機位置及姿態(tài)更新微分方程，在此基礎(chǔ)上對系統(tǒng)進行了仿真研究，結(jié)果表明：本文方法克服了慣導定位誤差隨時間累積的缺點，且實現(xiàn)了系統(tǒng)高精度實時定位。

[1] 蘇秀蘋,李 威,樊啟高.采煤機滾筒調(diào)高滑模變結(jié)構(gòu)控制策略[J].煤炭學報,2012, 37 (12):2107-21,11.

[2] 方新秋，何 杰，張 斌，等.無人工作面采煤機自主定位系統(tǒng)[J].西安科技大學學報，2008,28(2):349-353.

[3] 吳立新，殷作如，鄧智毅，等.論21世紀的礦山—數(shù)字礦山[J].煤炭學報，2000,25(4):337-342.

[4] 趙 靖.基于卡爾曼濾波算法的采煤機慣導定位方法[J].工況自動化,2014,40(10):29-32.

[5] 樊啟高，李 威，王禹橋，等.一種采用捷聯(lián)慣導的采煤機動態(tài)定位方法[J].煤炭學報，2011, 36(10):1758-1761.

[6] 宋 宇，翁新武，郭昕剛.基于光流和慣性導航的小型無人機定位方法[J].傳感器與微系統(tǒng)，2015,34(1):13-16.

[7] 姜 華，何風行，陳文權(quán)，等.一種基于超聲和射頻融合的無線傳感器網(wǎng)絡(luò)網(wǎng)格定位方法[J].傳感器與微系統(tǒng),2015,34(3):24-27.

[8] 李振浩，李英娜，張長勝，等.基于免疫記憶人工魚群算法的局部放電超聲定位方法[J].傳感器與微系統(tǒng),2017,36(3):73-76.

[9] 康 崇，張曉峻，樊黎明.基于ICCP算法的地磁匹配輔助導航[J].應(yīng)用基礎(chǔ)與工程科學學報，2014,22(3):598-604.

[10] 符 拯，王書滿，劉丙杰.自適應(yīng)卡爾曼濾波的最新進展[J].戰(zhàn)術(shù)導彈技術(shù)，2009(6)：62-66.

[11] Escamilla-Ambrosio P J,Mort N.Multisensor data fusion architecture based on adaptive Kalman filters and fuzzy logic performance assessment[C]∥Proceedings of the Fifth International Conference on Information Fusion，F(xiàn)USION 2002,Annapolis，USA，2002:1542-1549.

[12] 徐田來，游文虎，崔平遠.基于模糊自適應(yīng)卡爾曼濾波的INS/GPS組合導航系統(tǒng)算法研究[J].宇航學報，2006,26(5):571-575.