基于RFID的集裝箱碼頭AGV定位誤差補(bǔ)償方法

王 帆，翟 琳，褚建新，魏 偉

（上海海事大學(xué) 科學(xué)研究院，上海 201306）

新一代自動(dòng)化集裝箱碼頭的岸橋與堆場軌道吊之間的集裝箱運(yùn)輸是由自動(dòng)引導(dǎo)載運(yùn)車AGV（automation guided vehicle）完成。AGV是一種可以實(shí)現(xiàn)無人駕駛、自動(dòng)導(dǎo)航的智能載運(yùn)車。在AGV運(yùn)載集裝箱過程中，首先需對AGV進(jìn)行位置檢測才能完成自動(dòng)運(yùn)輸。

在AGV定位研究中主要為慣性導(dǎo)航定位，通過安裝在AGV上的傳感器，估計(jì)AGV的位置和運(yùn)動(dòng)姿態(tài)信息。估計(jì)期間會(huì)出現(xiàn)累積誤差，所以引入其他定位技術(shù)對慣性導(dǎo)航的估計(jì)值進(jìn)行補(bǔ)償。GPS定位能夠?yàn)锳GV提供較準(zhǔn)確的位置信息，但是惡劣天氣條件下或者在有障礙物遮擋時(shí)，GPS信號(hào)不穩(wěn)定，將會(huì)影響AGV的位置檢測[1]。基于視覺的碼頭集裝箱AGV定位誤差補(bǔ)償需要設(shè)定參照物確定位置信息[2]，由此不能靈活地設(shè)定路徑。還有利用磁阻傳感器作為AGV定位誤差的補(bǔ)償方法，但實(shí)現(xiàn)較為困難[3]。

本文采用基于射頻識(shí)別RFID（radio frequency identification）技術(shù)的AGV定位誤差補(bǔ)償方法。首先，以堆場某點(diǎn)建立一平面坐標(biāo)系（XY），每間隔一定距離埋設(shè)一個(gè)無源RFID標(biāo)簽，并將位置信息存儲(chǔ)在標(biāo)簽中。通過安裝在AGV車身（車前及車后）下方的RFID閱讀器讀取堆場地埋RFID電子標(biāo)簽信息，獲取AGV位置信息補(bǔ)償定位誤差，這種方法能不受惡劣的環(huán)境影響高效運(yùn)行。因此，本文利用AGV上的傳感器與RFID定位進(jìn)行信息融合，補(bǔ)償慣性定位所產(chǎn)生的固有誤差，提高對AGV位置檢測的精確度。

1 基于慣性導(dǎo)航的AGV定位

基于AGV的慣性導(dǎo)航定位主要是通過安裝在AGV上的各類傳感器獲取AGV的實(shí)時(shí)運(yùn)動(dòng)方向和運(yùn)動(dòng)姿態(tài)信息，通過建立AGV的運(yùn)動(dòng)模型，由此估算出AGV下一時(shí)刻的運(yùn)動(dòng)方向和運(yùn)動(dòng)姿態(tài)信息。本文選用裝載在AGV上的電子陀螺儀以及安裝在AGV車輪上的光電編碼器和驅(qū)動(dòng)輪轉(zhuǎn)向軸上的絕對位置編碼器對AGV進(jìn)行慣性導(dǎo)航定位。電子陀螺儀用來測量AGV的角速度；光電編碼器用來測定驅(qū)動(dòng)輪的行駛距離；轉(zhuǎn)向軸上的絕對位置編碼器主要測量AGV的方位角。通過以上傳感器獲取的信息估算AGV下一時(shí)刻的位置、方位角等運(yùn)動(dòng)狀態(tài)。根據(jù)以上傳感器獲取的AGV在堆場上的運(yùn)動(dòng)狀態(tài)，可以獲得其在堆場上圓弧運(yùn)動(dòng)的運(yùn)動(dòng)關(guān)系圖，如圖1所示。

圖1 AGV圓弧運(yùn)動(dòng)關(guān)系Fig.1 AGV arc motion diagram

在堆場坐標(biāo)系中，為了確定AGV的位置，AGV的驅(qū)動(dòng)輪為其2個(gè)前輪。設(shè)定AGV的方向角度以y軸為0°，方向旋轉(zhuǎn)角度用θ表示，順時(shí)針為正，逆時(shí)針為負(fù)。由此可知，判定AGV姿態(tài)的主要影響因素為 AGV 的坐標(biāo)位置以及方向旋轉(zhuǎn)角度（x，y，θ），設(shè)定為AGV當(dāng)前的運(yùn)動(dòng)狀態(tài)矩陣，如圖1所示，AGV在時(shí)刻k-1時(shí)的姿態(tài)為坐標(biāo)（x（k-1），y（k-1）），方位角為 θ（k-1）；而在 k 時(shí)刻的姿態(tài)為坐標(biāo)（x（k），y（k）），方位角為 θ（k）。 由此，得到的 AGV運(yùn)動(dòng)模型方程為

式中：w（k-1）為噪聲模型，且為均值為0的高斯白噪聲[4]。

2 基于RFID技術(shù)的AGV位置檢測方法

基于RFID技術(shù)的位置檢測方法，首先，在堆場上建立RFID電子標(biāo)簽陣列，并將位置信息存儲(chǔ)在電子標(biāo)簽中，利用安裝在AGV前后兩端的RFID閱讀器讀取地埋RFID電子標(biāo)簽的存儲(chǔ)信息，通過讀取標(biāo)簽信息對AGV進(jìn)行位置檢測[5]。如圖2所示，以集裝箱碼頭堆場為平面建立坐標(biāo)系，在堆場內(nèi)每間隔等距離（如間隔4 m或2 m）埋設(shè)一個(gè)無源RFID標(biāo)簽。安裝在AGV前后兩端的RFID閱讀器讀取標(biāo)簽信息，以此確定AGV的位置信息。由于AGV主要工作為集裝箱的托運(yùn)，所以安裝在AGV前后2個(gè)RFID閱讀器之間的距離設(shè)定為12 m。

圖2 堆場坐標(biāo)系示意Fig.2 Sketch map of yard coordinate system

AGV位置檢測工作流程為當(dāng)AGV位置檢測開始后，通過電子標(biāo)簽獲取當(dāng)前位置信息，直到獲取新的電子標(biāo)簽信息時(shí)，AGV的位置檢測信息才得以更新。

在基于RFID技術(shù)的位置檢測中，本文設(shè)定的RFID閱讀器的檢測范圍模型為邊長4 m的矩形模型，因?yàn)镽FID閱讀器的閱讀范圍較大，而電子標(biāo)簽較小，所以在一個(gè)RFID閱讀器的閱讀范圍內(nèi)檢測到的電子標(biāo)簽所得到的位置信息與實(shí)際位置信息有較大誤差，并且此誤差為固有誤差[6]。具體如圖3所示。

圖3 RFID閱讀器閱讀范圍內(nèi)的電子標(biāo)簽Fig.3 RFID electronic tag reader reading range

由圖3可知，RFID閱讀器讀取范圍內(nèi)的電子標(biāo)簽個(gè)數(shù)可以分為以下3種情況：①讀取到1個(gè)電子標(biāo)簽；②讀取到2個(gè)電子標(biāo)簽；③未能讀取到電子標(biāo)簽。以上3種情況對AGV的位置檢測有著不同的影響，在①中，RFID閱讀器讀取到的坐標(biāo)信息（xb，yb）作為該閱讀器的位置檢測估計(jì)信息（xe，ye）。在②中，RFID閱讀器讀取的2個(gè)電子標(biāo)簽信息為（x1，y1）、（x2，y2），為了減小位置檢測過程中固有誤差的影響，設(shè)定該閱讀器的位置檢測估計(jì)信息（xe，ye）為兩電子標(biāo)簽信息的中點(diǎn)，則估計(jì)信息（xe，ye）的計(jì)算方程為

在③中，因?yàn)樵赗FID閱讀器閱讀范圍內(nèi)未讀取到任何標(biāo)簽。所以位置檢測估計(jì)信息（xe，ye）可以由最后一次檢測到的標(biāo)簽信息（x（n-1），y（n-1））進(jìn)行估計(jì)，則該閱讀器的位置估計(jì)信息 （xe，ye）為該RFID閱讀器最后一次檢測到的標(biāo)簽信息（x（n-1），y（n-1））。

根據(jù)以上3種情況下根據(jù)RFID閱讀器安裝在AGV前后的位置，確定AGV的行駛位置，進(jìn)一步處理可以得到AGV當(dāng)前的速度、方位角相關(guān)信息，具體計(jì)算方法如下：

1）AGV的位置信息估計(jì)

首先，根據(jù)安裝在AGV前后兩端的RFID閱讀器讀取的電子標(biāo)簽信息分別得到2個(gè)閱讀器的位置估計(jì)信息為前端閱讀器位置估計(jì)信息（xfe，yfe）和后端閱讀器位置估計(jì)信息（xbe，ybe），則AGV的位置估計(jì)信息（x，y）方程為

2）AGV的速度信息估計(jì)

AGV當(dāng)前的行駛速度v是由AGV上一時(shí)刻位置估計(jì)信息（x（k-1），y（k-1））和當(dāng)前時(shí)刻位置估計(jì)信息（x（k），y（k））獲得，速度信息估計(jì)方程為

式中，T為2次位置檢測的循環(huán)周期。

3）AGV的方位角信息

AGV當(dāng)前的方位角，以堆場坐標(biāo)系的y軸為基準(zhǔn)方向。由AGV前后2個(gè)RFID閱讀器的位置估計(jì)信息獲得方位角信息估計(jì)方程為

綜上所述，可以確定在堆場中AGV的運(yùn)動(dòng)狀態(tài)，運(yùn)動(dòng)狀態(tài)主要為（x，y，v，θ）。 （x，y，v，θ）分別為AGV當(dāng)前的位置信息、速度信息以及方位角信息。

3 AGV在堆場中的路徑設(shè)定

AGV在堆場坐標(biāo)系中的運(yùn)動(dòng)路徑由AGV的起始位置與目標(biāo)位置確定，并且需要合理設(shè)定控制點(diǎn)與加速點(diǎn)，控制點(diǎn)是AGV的轉(zhuǎn)向點(diǎn)，減速點(diǎn)則是到達(dá)目標(biāo)位置的減速控制點(diǎn)。

本文對AGV路徑的設(shè)定如圖4所示，起始位置設(shè)定在坐標(biāo)（0，0）上，以 90°方位角行駛至控制點(diǎn)1，經(jīng)過2次轉(zhuǎn)向行駛至控制點(diǎn)2，再次以90°的方位角行駛至控制點(diǎn)3，再經(jīng)過2次轉(zhuǎn)向行駛至控制點(diǎn)4，然后通過減速點(diǎn)到達(dá)目標(biāo)位置。

圖4 AGV在堆場坐標(biāo)系中的運(yùn)動(dòng)路徑Fig.4 Motion path of AGV in the yard coordinate system

根據(jù)以上設(shè)定的路徑，由式（1）和式（2）可知，當(dāng)AGV按照圖4路徑行駛時(shí)，AGV的位置檢測估計(jì)信息與路徑規(guī)定的實(shí)際位置信息存在固有差值，其計(jì)算方程為

式中：（xr，yr）為路徑上的真實(shí)位置坐標(biāo)信息；（x，y）為位置檢測估計(jì)信息坐標(biāo)點(diǎn)；d為偏差距離。本文利用Matlab仿真可以得出設(shè)定路徑的誤差關(guān)系圖，如圖5所示。

圖5 誤差關(guān)系Fig.5 Error relation diagram

4 多傳感器的信息融合與仿真

根據(jù)建立的運(yùn)動(dòng)模型以及慣性導(dǎo)航位置估計(jì)公式估計(jì)AGV的位置，本文選用卡爾曼濾波算法對RFID技術(shù)以及AGV上的電子陀螺儀和安裝在AGV車輪上的光電編碼器和驅(qū)動(dòng)輪轉(zhuǎn)向軸上的絕對位置編碼器的信息進(jìn)行信息融合，從而補(bǔ)償多傳感器慣性導(dǎo)航的定位誤差。通過以上傳感器獲取的信息估算AGV下一時(shí)刻的位置、方位角等運(yùn)動(dòng)狀態(tài)。運(yùn)動(dòng)狀態(tài)估計(jì)期間出現(xiàn)的累積誤差通過RFID技術(shù)反饋AGV運(yùn)動(dòng)狀態(tài)，從而對以上傳感器估算的運(yùn)動(dòng)狀態(tài)值進(jìn)行校正。位置狀態(tài)估計(jì)如圖6所示。

圖6 位置狀態(tài)估計(jì)Fig.6 Position state estimation

綜上所述，AGV的位置校正狀態(tài)量是由RFID技術(shù)所得到的AGV運(yùn)動(dòng)方向和運(yùn)動(dòng)姿態(tài)信息，得到［x，y，θ］的狀態(tài)校正方程，且各個(gè)狀態(tài)量的誤差等效為高斯白噪聲。

卡爾曼濾波算法是一種最優(yōu)化自回歸數(shù)據(jù)處理算法。首先引入一個(gè)離散運(yùn)動(dòng)估計(jì)過程的系統(tǒng)。表達(dá)式為

得出上一狀態(tài)對現(xiàn)一狀態(tài)的的預(yù)測，表達(dá)式為

而現(xiàn)狀態(tài)的預(yù)測值對應(yīng)的協(xié)方差則會(huì)更新為

式中：P（k∣k-1）為預(yù)測值對應(yīng)的協(xié)方差；Q為系統(tǒng)過程的方差[7]。式（1）的運(yùn)動(dòng)模型作為預(yù)測模型，測量值為RFID的位置估算值，和每次的AGV實(shí)際位置一一對應(yīng)。所以，當(dāng)前狀態(tài)的最優(yōu)化估算值X（k∣k）的表達(dá)式為

式中，Kg（k）為卡爾曼濾波增益：

同時(shí)，為了使卡爾曼濾波不斷循環(huán)，所以現(xiàn)狀態(tài)下的 X（k∣k）的協(xié)方差 P（k∣k）為

由此可以得出AGV的位置狀態(tài)估計(jì)方程為

利用Matlab進(jìn)行仿真實(shí)驗(yàn)，可以得到單獨(dú)使用AGV上的電子陀螺儀以及安裝在AGV車輪上的光電編碼器和驅(qū)動(dòng)輪轉(zhuǎn)向軸上的絕對位置編碼器進(jìn)行定位的距離誤差曲線，如圖7所示。

圖7 單獨(dú)使用車載傳感器的定位誤差曲線Fig.7 Positioning error curve of the vehicle sensor

同時(shí)，也可以得到經(jīng)過對RFID位置估計(jì)、AGV上的電子陀螺儀以及安裝在AGV車輪上的光電編碼器和驅(qū)動(dòng)輪轉(zhuǎn)向軸上的絕對位置編碼器的信息融合得到的定位距離誤差曲線，如圖8所示。

圖8 多傳感器信息融合后誤差分布Fig.8 Error distribution of multi sensor information fusion

綜上所述，由以上仿真結(jié)果可知，單獨(dú)使用RFID技術(shù)定位時(shí)，其誤差只能維持在2.1 m以內(nèi)，平均誤差在1 m左右。單獨(dú)使用AGV上的車載傳感器進(jìn)行定位時(shí)，誤差僅維持在0.6 m以內(nèi)，平均誤差在0.3 m左右。對RFID技術(shù)、AGV上的電子陀螺儀和安裝在AGV車輪上的光電編碼器和驅(qū)動(dòng)輪轉(zhuǎn)向軸上的絕對位置編碼器進(jìn)行信息融合的定位結(jié)果的定位誤差值縮小至0.15 m以內(nèi)，平均誤差值也維持在0.13 m左右。結(jié)果表明，車載傳感器與RFID技術(shù)利用卡爾曼濾波進(jìn)行信息融合后達(dá)到了對固有誤差的補(bǔ)償，提高了定位精度。

5 結(jié)語

本文采用了多傳感器融和算法將RFID定位技術(shù)以及AGV上的電子陀螺儀以及安裝在AGV車輪上的光電編碼器和驅(qū)動(dòng)輪轉(zhuǎn)向軸上的絕對位置編碼器所采集的信息進(jìn)行信息融合后對AGV進(jìn)行了定位，與傳統(tǒng)的RFID技術(shù)定位和單獨(dú)使用車載傳感器定位相比，采用信息融合后的定位精度大大提高。

由于實(shí)驗(yàn)條件限制，只是利用仿真效果進(jìn)行論證。并且在RFID閱讀器的閱讀范圍上采用的矩形模型過于簡單。在下一階段的研究中應(yīng)該利用具體實(shí)驗(yàn)來驗(yàn)證方法的可行性。

[1]張曉霞，韓劉柱，李曉琳，等.AGV用GPS/DR組合導(dǎo)航動(dòng)融濾波技術(shù)[J].沈陽建筑大學(xué)學(xué)報(bào)：自然科學(xué)版，2012，28（3）：543-548.

[2]姜涌，曹杰，杜亞玲.基于視覺的碼頭集裝箱AGV導(dǎo)引系統(tǒng)[J].南京航空航天大學(xué)學(xué)報(bào)，2006，38（5）：628-633.

[3]朱政，張世平，孫金瑋.基于磁檢測的AGV導(dǎo)引新方法[J].宇航計(jì)測技術(shù)，2008，28（4）：12-15.

[4]郭建明，周華，劉清.基于EKF的移動(dòng)機(jī)器人RFID定位系統(tǒng)研究[J].武漢理工大學(xué)學(xué)報(bào)：信息與管理工程版，2006，31（6）：908-921.

[5]孫瑜，范平志.射頻識(shí)別技術(shù)及其在室內(nèi)定位中的應(yīng)用[J].計(jì)算機(jī)應(yīng)用，2005，25（5）：1205-1208.

[6]T Wada，N Uchitomi，Y Ota，et al.A novel scheme for spatial localization of passive RFID tags：communication range recognition（CRR）scheme[C]//IEEE International Conference on Communications，2009.

[7]CH Huang，LH Lee，CC Ho，et al.Real-time RFID indoor positioning system based on kalman-filter drift removal and heronbilateration location estimation[J].IEEE Transactions on Instrumentation&Measurement，2015，64（3）：728-739.