基于龍門吊大車的定位算法研究

熊巧文,李文鋒,葛艷紅,董文濤

(武漢理工大學(xué)物流工程學(xué)院,湖北 武漢 430063)

露天貨場(chǎng)的業(yè)務(wù)運(yùn)營過程中,不可避免地存在貨物翻堆現(xiàn)象,這種現(xiàn)象在剪板加工區(qū)存放的熱卷上尤為頻繁.由于操作人員沒有及時(shí)記錄鋼卷位置,導(dǎo)致貨物實(shí)際位置與系統(tǒng)信息不一致,給查找貨物帶來不便.特別是對(duì)于翻堆次數(shù)多的鋼卷,可能很難將它找到[1],要設(shè)計(jì)一種貨物自動(dòng)定位系統(tǒng),實(shí)現(xiàn)信息系統(tǒng)和貨場(chǎng)的銜接.本文即基于此需求,研究一種基于龍門吊定位的定位算法,實(shí)現(xiàn)對(duì)龍門吊的準(zhǔn)確定位.

1 軌道式龍門吊大車運(yùn)行定位

軌道式龍門吊跨度較大,堆碼層數(shù)多,能充分利用場(chǎng)地面積,適應(yīng)于場(chǎng)地面積較小但較方正的倉庫專用堆場(chǎng).軌道式龍門吊是沿著場(chǎng)地上的軌道行走的,因此只能限制在設(shè)有軌道的場(chǎng)地范圍內(nèi)進(jìn)行作業(yè).軌道式龍門吊定位能力較強(qiáng),較易實(shí)現(xiàn)全自動(dòng)化裝卸.

龍門吊大車定位系統(tǒng)包括控制器、位置傳感器、無線發(fā)送模塊以及數(shù)據(jù)中心,實(shí)現(xiàn)對(duì)龍門吊位置信息實(shí)時(shí)采集和處理,并存儲(chǔ)數(shù)據(jù).核心控制器采用研華工控機(jī),位置傳感器選用NovAtel公司的GPS,安裝在龍門吊大梁一端.控制器與各個(gè)傳感器模塊之間采用串口通信,貨場(chǎng)前方與數(shù)據(jù)中心之間通過無線GPRS方式通信[2].數(shù)據(jù)中心部分主要完成數(shù)據(jù)庫信息管理及人機(jī)交互等功能.

2 GPS定位

2.1 差分GPS

差分GPS(Differential Global Positioning System,即DGPS),就是用兩臺(tái)接收機(jī)于兩個(gè)測(cè)站上同時(shí)測(cè)量來自相同GPS衛(wèi)星的導(dǎo)航定位信號(hào),以聯(lián)合測(cè)得移動(dòng)站用戶的精確位置.其中一個(gè)測(cè)站位于業(yè)已測(cè)定的已知點(diǎn),設(shè)在該已知點(diǎn)(基準(zhǔn)點(diǎn))的GPS信號(hào)接收機(jī)為基準(zhǔn)站接收機(jī).它和安裝在移動(dòng)載體上的GPS信號(hào)接收機(jī)(簡(jiǎn)稱為移動(dòng)站接收機(jī))同時(shí)測(cè)量來自相同GPS衛(wèi)星的導(dǎo)航定位信號(hào).基準(zhǔn)站接收機(jī)所測(cè)得的三維位置與該點(diǎn)已知值進(jìn)行比較,便可獲得GPS定位數(shù)據(jù)的改正值.如果及時(shí)將GPS改正值發(fā)送給若干臺(tái)共視衛(wèi)星用戶的移動(dòng)站接收機(jī),而改正后者所測(cè)得的實(shí)時(shí)位置,這叫做實(shí)時(shí)差分動(dòng)態(tài)定位;若用GPS改正值對(duì)若干臺(tái)共視衛(wèi)星用戶的動(dòng)態(tài)接收機(jī)所采集的定位數(shù)據(jù)進(jìn)行測(cè)后修正,稱之為后處理差分動(dòng)態(tài)定位.由于這種改正能將公共誤差(如衛(wèi)星軌道誤差、SA影響、大氣影響等)抵消,從而提高了定位精度,使實(shí)時(shí)定位精度從100 m提高到3～5 m[3].圖1為差分動(dòng)態(tài)定位的原理圖.

2.2 帶約束的卡爾曼濾波算法

偽距差分精度3～5 m還達(dá)不到區(qū)分每個(gè)鋼卷的使用要求,這就需要進(jìn)一步提高定位精度.通過分析,龍門吊在行駛過程中本身存在著一些約束條件,充分利用這些約束條件有可能提高GPS定位精度和系統(tǒng)的可靠性.以龍門吊軌道的直線方程作為定位濾波的約束條件,建立帶軌道約束條件的系統(tǒng)模型,并推導(dǎo)約束條件下的卡爾曼濾波方程.

圖1 差分動(dòng)態(tài)定位的原理圖

2.2.1 狀態(tài)方程 考慮龍門吊大車在平行軌道上運(yùn)行,取軌道面這個(gè)二維平面坐標(biāo)系建立運(yùn)動(dòng)方程.為了確定龍門吊的位置和速度,系統(tǒng)狀態(tài)變量可以選取:

這里SN,,VN和SE,VE分別為龍門吊在緯度和經(jīng)度方向上的位置和速度分量.

系統(tǒng)的狀態(tài)方程可以寫成:

其中:

這里wN和wE分別為均值為0,方差為q2N、q2E的高斯白噪聲.

離散化后得到離散狀態(tài)方程:

觀測(cè)方程為:

其中V為觀測(cè)噪聲,均值為0,方差為R.

引入約束條件軌道直線方程:y=d·x+m,那么約束方程可以表示為:

寫成矩陣形式為

2.2.2 卡爾曼濾波方程的推導(dǎo) 由于狀態(tài)方程存在約束條件D·X=M,濾波的目的就是在滿足

的條件下[4],使得D· X=M.由于有:

建立拉格朗日方程:

又因?yàn)榭柭鼮V波估值可以表示為 X= ∑X·P(X|Z),則有

求偏導(dǎo)得到

解得

得到帶約束條件下的卡爾曼濾波遞推公式:

3 實(shí)驗(yàn)結(jié)果

為了檢驗(yàn)上述方法的有效性,本文進(jìn)行了實(shí)測(cè)數(shù)據(jù)試驗(yàn).實(shí)驗(yàn)數(shù)據(jù)資料取自2010年7月19日在武漢某物流公司實(shí)際運(yùn)行所紀(jì)錄的數(shù)據(jù),共500個(gè)歷元,圖2是實(shí)測(cè)濾波軌跡曲線圖.為了分析方便,對(duì)圖中縱坐標(biāo)作了放大.其中曲線1為真實(shí)軌跡,曲線2為觀測(cè)軌跡,曲線3為帶約束條件的卡爾曼濾波后的軌跡.

圖2 實(shí)測(cè)濾波軌跡曲線

從圖2可以看出,觀測(cè)結(jié)果受觀測(cè)噪聲影響較大,與真實(shí)路線出現(xiàn)較大偏差.采用帶約束條件的卡爾曼濾波算法后,觀測(cè)結(jié)果偏離真實(shí)路線的現(xiàn)象得到了改善.

圖3是誤差曲線,可見采用帶約束條件的卡爾曼濾波算法后,濾波精度明顯提高.

圖3 誤差曲線

4 結(jié)束語

本文研究了一種基于龍門吊大車定位的定位算法,其中定位系統(tǒng)采用GPS和絕對(duì)式旋轉(zhuǎn)編碼器等位置傳感器,實(shí)現(xiàn)對(duì)龍門吊大車定位,是實(shí)現(xiàn)信息系統(tǒng)和貨場(chǎng)銜接的基礎(chǔ).通過運(yùn)用帶約束條件的Kalman濾波算法,提高了差分GPS的定位精度,實(shí)現(xiàn)了對(duì)龍門吊大車的準(zhǔn)確定位.

[1]梁紅貞.鋼鐵企業(yè)物流管控系統(tǒng)研究與實(shí)現(xiàn)[D].長(zhǎng)沙:湖南大學(xué)圖書館,2006.

[2]王俊佳.基于GPRS的工業(yè)現(xiàn)場(chǎng)數(shù)據(jù)控制器研究[J].計(jì)算機(jī)應(yīng)用研究,2005(11):92-93.

[3]房建成.GPS動(dòng)態(tài)濾波新方法[J].中國慣性技術(shù)學(xué)報(bào),1997(5):4-7.

[4]Samer S Saab.Discrete-Time Kalman Filter under Incorrect Noise Covariances[C]//Proceedings of the A-merican Control Conference,Jun.1995:1152-1156.