毛 君, 鐘 聲,3, 馬 英

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

0 引 言

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

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

1 采煤機(jī)慣性導(dǎo)航定位

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

圖1 慣性導(dǎo)航計(jì)算流程

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

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

卡爾曼濾波中，測(cè)量新息定義為

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

(1)

其測(cè)量殘差方差實(shí)際值為[11]

(2)

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

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

(3)

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

(4)

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

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

Rk+1=βRk

(5)

圖2 導(dǎo)航系統(tǒng)計(jì)算流程

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

(6)

采煤機(jī)速度矢量更新

(7)

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

采煤機(jī)位置更新微分方程

(8)

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

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

2 仿真分析

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

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

圖3 定位結(jié)果

3 結(jié)束語(yǔ)

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

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

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

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

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

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

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

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

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

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

[10] 符 拯，王書滿，劉丙杰.自適應(yīng)卡爾曼濾波的最新進(jìn)展[J].戰(zhàn)術(shù)導(dǎo)彈技術(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] 徐田來(lái)，游文虎，崔平遠(yuǎn).基于模糊自適應(yīng)卡爾曼濾波的INS/GPS組合導(dǎo)航系統(tǒng)算法研究[J].宇航學(xué)報(bào)，2006,26(5):571-575.