鄔興， 賀春林， 石義紅

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

?

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

鄔興， 賀春林， 石義紅

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

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

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

0　引言

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

圖1　系統(tǒng)簡圖

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

1　理論分析及建模

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

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

(1)

Zk=HkXk+Vk(2)

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

現(xiàn)有的實驗設(shè)備為一兩軸運(yùn)動臺，可模擬小車在X軸、Z軸方向的振動?？紤]系統(tǒng)狀態(tài)量選取為航向角和小車在空間三維坐標(biāo)系中X、Y、Z軸方向的坐標(biāo)：[βk,SXk,SYk,SZk]T，但受運(yùn)動平臺的限制，βk和SYk都不會發(fā)生變化，均設(shè)置為0，只有SXk和SZk會隨著運(yùn)動平臺的振動規(guī)律的影響而變化。若按式(1)、(2)來建立系統(tǒng)方程進(jìn)行濾波，由于運(yùn)動平臺僅模擬小車的振動規(guī)律，k-1時刻到k時刻的狀態(tài)量遞推方程不易求得，狀態(tài)轉(zhuǎn)移矩陣也就不易獲得。而且因為包含三角函數(shù)，需要選用擴(kuò)展卡爾曼濾波代替標(biāo)準(zhǔn)卡爾曼濾波，對三角函數(shù)進(jìn)行泰勒展開來近似，由此造成的誤差會進(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)動平臺的振動規(guī)律設(shè)置為：X軸振幅為8 cm，Z軸振幅也為8 cm，每次均設(shè)置運(yùn)動平臺的運(yùn)動初始位置為空間三維坐標(biāo)系的0點和X軸振動的負(fù)最大值點以及Z軸振動的0點，但振動規(guī)律的周期和初相角隨著運(yùn)動平臺的運(yùn)動時刻發(fā)生變化。故筆者考慮由相鄰兩個采樣點的X坐標(biāo)和Z坐標(biāo)來推算出每個采樣點處的振動規(guī)律的周期和初相角。振動規(guī)律用簡諧振動方程來模擬：

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

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

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

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

(7)

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

(9)

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

(11)

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

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

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

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

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

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

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

圖2　通信連接圖

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

圖3　模擬實驗圖

圖4　二維運(yùn)動平臺

圖5　基站轉(zhuǎn)臺

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

圖6　程序流程圖

圖7　X軸方向跟蹤效果圖

3　結(jié)論

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

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

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

[3]周宏仁，敬忠良，王培德.機(jī)動目標(biāo)跟蹤[M].北京：國防工業(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]張國良，曾靜.組合導(dǎo)航原理與技術(shù)[M].西安：西安交通大學(xué)出版社，2008.

[7]趙亦林著，譚國真譯.車輛定位與導(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]付夢印，鄧志紅，閆莉萍. 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ī)學(xué)院碩士研究生，主要研究方向：卡爾曼濾波器設(shè)計、基于計算機(jī)的網(wǎng)絡(luò)應(yīng)用；賀春林(1971-)，男，四川廣安人，西華師范大學(xué)計算機(jī)學(xué)院教授，碩士生導(dǎo)師，主要研究方向：基于計算機(jī)的網(wǎng)絡(luò)應(yīng)用。

2016-03-02