基于卡爾曼濾波的無人采礦設(shè)備狀態(tài)預(yù)測(cè)研究

鄔興， 賀春林， 石義紅

(西華師范大學(xué)　計(jì)算機(jī)學(xué)院， 四川　南充　637002)

?

基于卡爾曼濾波的無人采礦設(shè)備狀態(tài)預(yù)測(cè)研究

鄔興， 賀春林， 石義紅

(西華師范大學(xué)計(jì)算機(jī)學(xué)院， 四川南充637002)

為了對(duì)井下無人采礦設(shè)備的狀態(tài)進(jìn)行更準(zhǔn)確的預(yù)測(cè)，及時(shí)對(duì)設(shè)備狀態(tài)進(jìn)行跟蹤,提出了一種激光定位導(dǎo)航系統(tǒng)中的基于卡爾曼濾波的位置預(yù)測(cè)方法，并在一套試驗(yàn)系統(tǒng)上進(jìn)行了可行性驗(yàn)證。該系統(tǒng)用可水平旋轉(zhuǎn)、可俯仰的轉(zhuǎn)臺(tái)作為定位跟蹤基站，用可平動(dòng)和豎直運(yùn)動(dòng)的二維運(yùn)動(dòng)平臺(tái)模擬運(yùn)動(dòng)目標(biāo)，采用卡爾曼濾波技術(shù)對(duì)觀測(cè)到的運(yùn)動(dòng)目標(biāo)的坐標(biāo)信息進(jìn)行濾波和預(yù)測(cè)，預(yù)測(cè)值作為轉(zhuǎn)臺(tái)跟蹤的設(shè)定值，使基站發(fā)出的激光束能平穩(wěn)準(zhǔn)確地跟蹤運(yùn)動(dòng)目標(biāo)。

卡爾曼濾波；濾波去噪；預(yù)測(cè)跟蹤

0　引言

自動(dòng)化和智能化是21世紀(jì)采礦業(yè)所面臨的重要課題，將自動(dòng)化和智能化應(yīng)用于采礦行業(yè)中，可以起到改善作業(yè)環(huán)境、提高生產(chǎn)效率、降低生產(chǎn)成本以及保障現(xiàn)場(chǎng)工作人員的安全的作用[1]。本文的目的是為了對(duì)礦區(qū)無人設(shè)備的狀態(tài)進(jìn)行更準(zhǔn)確的預(yù)測(cè)，及時(shí)對(duì)設(shè)備狀態(tài)進(jìn)行跟蹤，從而對(duì)無人設(shè)備的運(yùn)動(dòng)實(shí)時(shí)地加以控制。圖1是擬開發(fā)的一套車輛定位導(dǎo)航系統(tǒng)[2]，對(duì)象為無人駕駛智能采礦設(shè)備，可以理解為無人鏟運(yùn)機(jī)或無人礦車。本文在此系統(tǒng)上運(yùn)用卡爾曼濾波算法實(shí)現(xiàn)對(duì)無人設(shè)備進(jìn)行機(jī)動(dòng)目標(biāo)跟蹤[3]，從而實(shí)現(xiàn)對(duì)設(shè)備將來的位姿進(jìn)行精確的預(yù)報(bào)。

圖1　系統(tǒng)簡(jiǎn)圖

卡爾曼濾波[4]技術(shù)是一個(gè)“最優(yōu)化自回歸數(shù)據(jù)處理算法”，它對(duì)于解決很多問題是最優(yōu)、效率最高的。它已經(jīng)應(yīng)用了很多年，且用途非常廣泛[5]，包括雷達(dá)系統(tǒng)、組合導(dǎo)航[6]、機(jī)器人控制、傳感器數(shù)據(jù)融合等。在計(jì)算機(jī)技術(shù)中也有較多應(yīng)用，如原子鐘馴服、高精度測(cè)量等。在車輛定位導(dǎo)航系統(tǒng)中也有重要的應(yīng)用[7-8]。

1　理論分析及建模

考慮如下離散時(shí)間系統(tǒng)[9]：?k≥1

Xk=φk,k-1Xk-1+Γk,k-1Wk-1

(1)

Zk=HkXk+Vk(2)

1.1運(yùn)動(dòng)模型的建立

現(xiàn)有的實(shí)驗(yàn)設(shè)備為一兩軸運(yùn)動(dòng)臺(tái)，可模擬小車在X軸、Z軸方向的振動(dòng)。考慮系統(tǒng)狀態(tài)量選取為航向角和小車在空間三維坐標(biāo)系中X、Y、Z軸方向的坐標(biāo)：[βk,SXk,SYk,SZk]T，但受運(yùn)動(dòng)平臺(tái)的限制，βk和SYk都不會(huì)發(fā)生變化，均設(shè)置為0，只有SXk和SZk會(huì)隨著運(yùn)動(dòng)平臺(tái)的振動(dòng)規(guī)律的影響而變化。若按式(1)、(2)來建立系統(tǒng)方程進(jìn)行濾波，由于運(yùn)動(dòng)平臺(tái)僅模擬小車的振動(dòng)規(guī)律，k-1時(shí)刻到k時(shí)刻的狀態(tài)量遞推方程不易求得，狀態(tài)轉(zhuǎn)移矩陣也就不易獲得。而且因?yàn)榘呛瘮?shù)，需要選用擴(kuò)展卡爾曼濾波代替標(biāo)準(zhǔn)卡爾曼濾波，對(duì)三角函數(shù)進(jìn)行泰勒展開來近似，由此造成的誤差會(huì)進(jìn)一步增大，所以筆者考慮將狀態(tài)轉(zhuǎn)移矩陣設(shè)置為4階單位矩陣，增加控制輸入向量，由控制輸入向量的變化來形成狀態(tài)量的遞推。則系統(tǒng)方程建立如下：?k≥1

Xk=φk,k-1Xk-1+Γk,k-1Wk-1+uk-1(3)

Zk=HkXk+Vk(4)

其中φk,k-1=I4×4，Γk,k-1=I4×4，Wk-1假設(shè)為4維白噪聲。控制輸入向量uk-1的確定方法如下。

二維運(yùn)動(dòng)平臺(tái)的振動(dòng)規(guī)律設(shè)置為：X軸振幅為8 cm，Z軸振幅也為8 cm，每次均設(shè)置運(yùn)動(dòng)平臺(tái)的運(yùn)動(dòng)初始位置為空間三維坐標(biāo)系的0點(diǎn)和X軸振動(dòng)的負(fù)最大值點(diǎn)以及Z軸振動(dòng)的0點(diǎn)，但振動(dòng)規(guī)律的周期和初相角隨著運(yùn)動(dòng)平臺(tái)的運(yùn)動(dòng)時(shí)刻發(fā)生變化。故筆者考慮由相鄰兩個(gè)采樣點(diǎn)的X坐標(biāo)和Z坐標(biāo)來推算出每個(gè)采樣點(diǎn)處的振動(dòng)規(guī)律的周期和初相角。振動(dòng)規(guī)律用簡(jiǎn)諧振動(dòng)方程來模擬：

S=Rcos(ωt+φ)(5)

假設(shè)前一個(gè)采樣點(diǎn)處推算出的周期為T1，初相角為fy1，后一個(gè)采樣點(diǎn)推算出的周期為T2，初相角為fy2，采樣間隔設(shè)為T。則可得控制輸入向量為：

1.2位置預(yù)測(cè)算法研究

對(duì)運(yùn)動(dòng)平臺(tái)的位姿的預(yù)測(cè)步驟如下：由相隔一個(gè)采樣周期的兩個(gè)相鄰狀態(tài)量(假設(shè)為上一時(shí)刻和當(dāng)前時(shí)刻)確定X軸、Z軸的運(yùn)動(dòng)模型方程，對(duì)系統(tǒng)的當(dāng)前時(shí)刻觀測(cè)值做濾波，得當(dāng)前時(shí)刻的最優(yōu)系統(tǒng)狀態(tài)量，再由預(yù)測(cè)方程預(yù)測(cè)一個(gè)采樣周期后或者好幾個(gè)采樣周期后的系統(tǒng)狀態(tài)量。狀態(tài)轉(zhuǎn)移矩陣、控制輸入矩陣、觀測(cè)方程確定之后，就能通過卡爾曼濾波獲得濾波值，就可以基于濾波值來做預(yù)測(cè)，最優(yōu)預(yù)測(cè)原理如下：所謂隨機(jī)線性離散系統(tǒng)的最優(yōu)預(yù)測(cè)，就是根據(jù)系統(tǒng)觀測(cè)方程所提供的觀測(cè)數(shù)據(jù)來對(duì)未來時(shí)刻的狀態(tài)向量Xk在給定假設(shè)條件下進(jìn)行最優(yōu)估計(jì)的問題。正交投影定理是研究預(yù)測(cè)算法的基礎(chǔ)。

(7)

將式(8)代入式(7)中：

(9)

所以，向前預(yù)測(cè)m步，綜合的濾波預(yù)測(cè)公式為：

(11)

2　實(shí)際硬件設(shè)備上的應(yīng)用

考慮用電機(jī)驅(qū)動(dòng)的兩自由度轉(zhuǎn)臺(tái)來跟蹤也由電機(jī)驅(qū)動(dòng)的二維運(yùn)動(dòng)平臺(tái)，驗(yàn)證濾波預(yù)測(cè)方法是否可行。

通信連接如圖2所示，全部采用CAN總線通信來實(shí)現(xiàn)信息的傳遞。

二維運(yùn)動(dòng)平臺(tái)DSP芯片CAN通信：二維運(yùn)動(dòng)平臺(tái)位置信息反饋。

轉(zhuǎn)臺(tái)ARM芯片CAN通信：從總線上接收激光測(cè)距傳感器的打開、關(guān)閉指令，將測(cè)距傳感器測(cè)距信息發(fā)送到總線上。

轉(zhuǎn)臺(tái)DSP芯片CAN通信：接收轉(zhuǎn)臺(tái)運(yùn)動(dòng)指令信息、接收測(cè)距傳感器測(cè)距信息，將測(cè)距信息和轉(zhuǎn)臺(tái)偏轉(zhuǎn)、俯仰角度進(jìn)行編碼并發(fā)出。

電腦CAN卡：接收二維運(yùn)動(dòng)平臺(tái)位置信息、坐標(biāo)編碼信息，發(fā)出轉(zhuǎn)臺(tái)運(yùn)動(dòng)指令信息(筆者的卡爾曼濾波預(yù)測(cè)程序使用C語言編寫，在電腦上運(yùn)行)。

圖2　通信連接圖

實(shí)驗(yàn)設(shè)計(jì)：由于僅在水平方向?qū)崿F(xiàn)了反饋裝置的安裝，故驗(yàn)證性實(shí)驗(yàn)考慮運(yùn)動(dòng)平臺(tái)僅在水平方向上做平動(dòng)的運(yùn)動(dòng)狀態(tài)。開始時(shí)手動(dòng)調(diào)整轉(zhuǎn)臺(tái)，使激光測(cè)距儀發(fā)射的激光通過反射鏡打到二維運(yùn)動(dòng)平臺(tái)的接收靶上，以此時(shí)二維運(yùn)動(dòng)平臺(tái)位置為坐標(biāo)系原點(diǎn)，建立空間三維坐標(biāo)系，并且以此時(shí)轉(zhuǎn)臺(tái)的俯仰角、偏轉(zhuǎn)角均為0點(diǎn)，轉(zhuǎn)臺(tái)到二維運(yùn)動(dòng)平臺(tái)距離為距離初始值。二維運(yùn)動(dòng)平臺(tái)開始振動(dòng)后，向PC機(jī)發(fā)送含有實(shí)時(shí)X、Z坐標(biāo)值的CAN消息，可計(jì)算出坐標(biāo)值和距離、俯仰角、偏轉(zhuǎn)角，且由相機(jī)及附帶的圖像處理板對(duì)偏轉(zhuǎn)角做修正，由實(shí)時(shí)坐標(biāo)值和距離、俯仰角、偏轉(zhuǎn)角做濾波和預(yù)測(cè)，給出轉(zhuǎn)臺(tái)下一步需要調(diào)整的偏轉(zhuǎn)角和俯仰角。當(dāng)轉(zhuǎn)臺(tái)完成調(diào)整后，激光再次打到接收靶上時(shí)，發(fā)回新的測(cè)距值及俯仰角、偏轉(zhuǎn)角，然后再得到新的實(shí)時(shí)運(yùn)動(dòng)平臺(tái)坐標(biāo)值，使整個(gè)跟蹤隨動(dòng)過程繼續(xù)下去。模擬實(shí)驗(yàn)圖如圖3所示，用到的二維運(yùn)動(dòng)平臺(tái)和基站轉(zhuǎn)臺(tái)如圖4、圖5所示。

圖3　模擬實(shí)驗(yàn)圖

圖4　二維運(yùn)動(dòng)平臺(tái)

圖5　基站轉(zhuǎn)臺(tái)

實(shí)驗(yàn)時(shí)所編寫的程序的流程圖及實(shí)驗(yàn)所得的X軸方向的跟蹤效果圖如圖6和圖7所示，可以看出隨著濾波方差矩陣的收斂，跟蹤逐漸穩(wěn)定。

圖6　程序流程圖

圖7　X軸方向跟蹤效果圖

3　結(jié)論

本文在簡(jiǎn)化的采礦設(shè)備運(yùn)動(dòng)模型的條件下，將卡爾曼濾波理論應(yīng)用于井下無人采礦設(shè)備的定位與導(dǎo)航,利用硬件設(shè)備搭建了一套實(shí)驗(yàn)定位導(dǎo)航系統(tǒng)，探究了這套系統(tǒng)在井下使用的可行性。最終在實(shí)驗(yàn)室環(huán)境下實(shí)現(xiàn)了利用擴(kuò)展卡爾曼濾波方法預(yù)測(cè)運(yùn)動(dòng)設(shè)備的位姿并使基站發(fā)射的激光束跟蹤運(yùn)動(dòng)設(shè)備，下一步即是移植到井下看能否使用成功。由于轉(zhuǎn)臺(tái)上實(shí)驗(yàn)了不同的激光定位跟蹤系統(tǒng)，使得轉(zhuǎn)臺(tái)的俯仰方向的機(jī)械結(jié)構(gòu)需要再次設(shè)計(jì)，由于時(shí)間原因，所以只做了水平方向激光定位跟蹤的實(shí)驗(yàn),實(shí)驗(yàn)結(jié)果如圖7。由于在實(shí)際情況下，井下作業(yè)的采礦設(shè)備的運(yùn)動(dòng)速度并不快，典型的鏟運(yùn)機(jī)的平均運(yùn)動(dòng)速度約為1 m/s，且可以通過各種方法使系統(tǒng)過程噪聲和測(cè)量噪聲盡量減小，由圖7可看出，在這樣的條件下，跟蹤效果較好，設(shè)計(jì)的算法可預(yù)報(bào)運(yùn)動(dòng)設(shè)備的位姿，實(shí)現(xiàn)定位與導(dǎo)航。下一步工作分為兩方面：一是由于CAN通信受線纜的限制，在井下應(yīng)用起來并不十分方便，考慮將采礦設(shè)備的運(yùn)動(dòng)信息包含到激光束中，由激光發(fā)射器和接收器組成激光通信系統(tǒng)，替代CAN通信；二是考慮將算法移植到基站轉(zhuǎn)臺(tái)的ARM芯片或DSP芯片中，提高算法的實(shí)時(shí)性，從而提高定位精度。

[1]戰(zhàn)凱，顧洪樞，周俊武等，地下遙控鏟運(yùn)機(jī)遙控技術(shù)和精確定位技術(shù)研究[J]．有色金屬，2009，61(1)：107-112.

[2]富立, 范耀祖. 車輛定位導(dǎo)航系統(tǒng)——智能交通系統(tǒng)[M]. 北京:中國(guó)鐵道出版社, 2004.

[3]周宏仁，敬忠良，王培德.機(jī)動(dòng)目標(biāo)跟蹤[M].北京：國(guó)防工業(yè)出版社，1991.

[4]Kalman B R E. A new approach to linear filtering and predition problems[J]. Journal of Basic Engineering, 2012,82(1):167 - 179.

[5]Maybeck P S. Stochastic models, estimation and control[ M] . NewYork: Academic Press, 1982: 129.

[6]張國(guó)良，曾靜.組合導(dǎo)航原理與技術(shù)[M].西安：西安交通大學(xué)出版社，2008.

[7]趙亦林著，譚國(guó)真譯.車輛定位與導(dǎo)航系統(tǒng)[M].北京：電子工藝出版社，1999.

[8]Ge L, Han S, Rizos C. Multipath mitigation of continuous GPS measurements using an adaptive filter[J].GPS Solution,2000,4(2):19-30.

[9]付夢(mèng)印，鄧志紅，閆莉萍. Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京：科學(xué)出版社，2003.

[Abstract]In order to make predictions more accurate about the position of the unmanned mining equipment and track the states of it in real time, a prediction method of the position of autonomous mining vehicle based on Kalman filter in the laser positioning and navigation system is proposed. To verify the feasibility, we make up a simple positioning system in our laboratory and carry out verification experiment within this system. The system uses a turntable which can horizontally rotate and tilt as the location tracking base station, two-dimensional motion platform which can play translational and vertical movement as the moving target (actual moving target is one kind of the unmanned mining vehicles). It applies Kalman filter technology to do filtering and prediction with the coordinate information of the moving target, uses the predicted value as the set value of the tracking parameter of the turntable, so as to make the Laser emitted from the base station steadily and accurately track the moving target.

[Key words]Kalman filter； filtering and denoising； prediction and tracking

[責(zé)任編輯劉景平]

Study of the State Prediction of Unmanned Mining Equipment Based on the Kalman Filter

WU Xing， HE Chun-lin， SHI Yi-hong

(School of Computer Science, China West Normal University, Nanchong, Sichuan 637002, China)

TP301

A

1672-9021(2016)02-0088-06

鄔興(1990-)，女，四川廣元人，西華師范大學(xué)計(jì)算機(jī)學(xué)院碩士研究生，主要研究方向：卡爾曼濾波器設(shè)計(jì)、基于計(jì)算機(jī)的網(wǎng)絡(luò)應(yīng)用；賀春林(1971-)，男，四川廣安人，西華師范大學(xué)計(jì)算機(jī)學(xué)院教授，碩士生導(dǎo)師，主要研究方向：基于計(jì)算機(jī)的網(wǎng)絡(luò)應(yīng)用。

2016-03-02