低成本IMU 與RFID 技術(shù)結(jié)合的AGV 實(shí)時(shí)定位方法研究

王 爽，石 朝，曾大懿，鄒益勝

（西南交通大學(xué)先進(jìn)設(shè)計(jì)與制造技術(shù)研究所，四川 成都 610031）

1 引言

隨著智能制造的提出，中國工業(yè)生產(chǎn)將進(jìn)一步朝著智能化、柔性化和高度集成化的方向發(fā)展[1]，AGV 作為智能工廠中實(shí)現(xiàn)物料自動(dòng)運(yùn)輸?shù)囊苿?dòng)機(jī)器人，對其進(jìn)行實(shí)時(shí)定位是物料精準(zhǔn)配送的關(guān)鍵。

近年來，基于紅外線、超聲波、藍(lán)牙、無線射頻（RFID）、超寬帶（UWB）等技術(shù)的各種室內(nèi)定位方法不斷興起，被廣泛應(yīng)用于智能家居等領(lǐng)域，并且具有較高的定位精度[2]。文獻(xiàn)[3]提出了一種多目視覺與激光組合導(dǎo)航的精確定位方法，但是成本相對較高。文獻(xiàn)[4]利用安裝于AGV 兩側(cè)的超聲波傳感器為其實(shí)現(xiàn)定位，但是該方法受視距影響嚴(yán)重。以上定位技術(shù)均表現(xiàn)出獨(dú)立性弱、有視距傳輸要求等問題，需要借助外界設(shè)備裝置進(jìn)行輔助，并通過這些硬件設(shè)備進(jìn)行數(shù)據(jù)傳輸，一旦外界環(huán)境無法滿足條件，定位方法也將失效[5-6]。針對復(fù)雜的工廠加工環(huán)境，以上方法難以實(shí)現(xiàn)精準(zhǔn)定位，而基于自身傳感器進(jìn)行定位的慣性導(dǎo)航定位方法成為解決問題的關(guān)鍵。

慣性定位是不依賴外部信息，也無需輻射能量，因其獨(dú)立性好，面對復(fù)雜的室內(nèi)環(huán)境，具有絕對優(yōu)勢。但市面上成熟的慣性導(dǎo)航器件成本都很高[7]，若使用低成本傳感器，會出現(xiàn)采集到的數(shù)據(jù)靜止加速度零偏嚴(yán)重，運(yùn)動(dòng)加速度噪聲偏大，不同測量速度下測得偏轉(zhuǎn)角度精度不同以及計(jì)算所得位移漂移嚴(yán)重等問題；另外，單獨(dú)使用慣性定位方法解算AGV 的位置，得到的誤差會存在隨時(shí)間的累積效應(yīng)，一定時(shí)間后，定位誤差將會超出可接受范圍，定位也將失去意義。為解決以上問題，采用兩種方法：一是參考軌道交通中廣泛應(yīng)用的應(yīng)答器方法，利用RFID 技術(shù)對慣性導(dǎo)航定位進(jìn)行位置校正，通過兩種方法數(shù)據(jù)的組合解算AGV 位置，解決慣性導(dǎo)航累計(jì)誤差的問題；二是提出一套基于低成本IMU 的實(shí)時(shí)定位誤差修正方法，緩解低成本慣性傳感器在試驗(yàn)中測量誤差大等問題，使定位結(jié)果滿足工業(yè)室內(nèi)定位的要求。

2 室內(nèi)定位系統(tǒng)模型及改進(jìn)

2.1 慣性定位原理

捷聯(lián)式慣導(dǎo)系統(tǒng)的優(yōu)勢在于省去復(fù)雜的固定的慣性平臺，將傳感器安裝在移動(dòng)體上。一個(gè)捷聯(lián)慣性測量單元包括加速度計(jì)和陀螺儀，用于跟蹤移動(dòng)體的平移和旋轉(zhuǎn)過程，對于車間環(huán)境中的AGV，主要考慮二維平面運(yùn)動(dòng)，利用加速度計(jì)采集AGV 的移動(dòng)加速度（X 向和Y 向），以及利用陀螺儀采集AGV 的偏航角（繞Z 軸所轉(zhuǎn)角度）。

通過以上變換，將b 系下的加速度信息轉(zhuǎn)換到n 系下。

2.2 基于卡爾曼濾波的位姿更新算法

IMU 采集到的數(shù)據(jù)摻雜噪聲，對AGV 的實(shí)時(shí)定位要求算法能夠?qū)崟r(shí)對產(chǎn)生的數(shù)據(jù)進(jìn)行處理，普遍使用的濾波算法對實(shí)時(shí)性無法保證，卡爾曼濾波算法（UKF）作為一種純時(shí)域的估計(jì)算法很好的解決了這個(gè)問題[9-10]。

狀態(tài)方程：在INS 定位系統(tǒng)中，由b 系下的傳感器獲得的信息得到n 系下AGV 的位置和速度更新，其模型為：

由位姿更新模型和狀態(tài)方程可知，狀態(tài)轉(zhuǎn)移矩陣：

由量測方程可知，觀測矩陣H=［0 0 0 0 1 1］。

應(yīng)用卡爾曼濾波基本方程：

該部分計(jì)算由MATLAB 實(shí)現(xiàn)，最終得到濾波后的AGV 速度與位移更新，得到AGV 的實(shí)時(shí)位置（X，Y）。

2.3 RFID 校正原理

RFID 定位系統(tǒng)主要包含閱讀器、電子標(biāo)簽和天線，在本實(shí)驗(yàn)環(huán)境下選擇無源電子標(biāo)簽，由于RFID 設(shè)備不具備通信能力，需要結(jié)合ZigBee 技術(shù)來解決RFID 定位的無線通信問題。讀寫器安裝在AGV 小車底部，設(shè)置閱讀器的讀寫距離為15cm，AGV 在移動(dòng)時(shí)具有軌跡，軌跡中設(shè)定位置已知的標(biāo)簽作為參考節(jié)點(diǎn)，閱讀器去識別標(biāo)簽以確定當(dāng)前位置。

2.4 組合定位

INS 系統(tǒng)的主要誤差來源是低成本傳感器自身精度不高，系統(tǒng)誤差大，加速度計(jì)和陀螺儀存在零位誤差且計(jì)算結(jié)果隨時(shí)間漂移嚴(yán)重。因此采用RFID 技術(shù)與慣性定位相組合的定位方法，取長補(bǔ)短，滿足用戶的需求。

組合定位系統(tǒng)的基本原理：加速度計(jì)和陀螺儀進(jìn)行數(shù)據(jù)采集后，輸入到導(dǎo)航解算單元進(jìn)行速度、位置的解算，最終利用誤差修正算法對導(dǎo)航結(jié)果進(jìn)行反饋修正，達(dá)到對AGV 精確定位的目的。

在AGV 上安裝低成本IMU，選擇九軸姿態(tài)測量傳感器。對于采集的加速度信號，經(jīng)過坐標(biāo)轉(zhuǎn)換計(jì)算和姿態(tài)解算算法得到AGV 的位置信息。針對常規(guī)慣性定位方法存在累積誤差問題，通過RFID 閱讀器采集的信號與慣性傳感信息進(jìn)行組合定位，在特定參考節(jié)點(diǎn)進(jìn)行位置信息的校正，進(jìn)而獲得AGV 的實(shí)時(shí)精確位置信息。

設(shè)定軌跡上N 個(gè)電子標(biāo)簽的位置坐標(biāo)為（x1，y1），（x2，y2），……，（xn，yn）；

融合定位位置更新方程：

式中：（X，Y）—AGV 的位置坐標(biāo)；Xk—卡爾曼濾波更新得到的位置。基于INS／RFID 融合定位整體流程，如圖1 所示。

圖1 IMU/RFID 組合定位流程Fig.1 IMU/RFID Integrated Positioning Process

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

為了驗(yàn)證提出的誤差修正定位方法的準(zhǔn)確性，進(jìn)行了幾組實(shí)測實(shí)驗(yàn)。在實(shí)驗(yàn)中，采用維特智能的JY9001 姿態(tài)傳感器為測量模塊，電壓（3.5～5）V，測量維度為加速度3 維，磁場3 維，角度3維，穩(wěn)定性加速度為0.01g，角速度0.05°/s，數(shù)據(jù)輸出頻率（0.1～200）Hz，數(shù)據(jù)接口為串口。IMU 安裝在AGV 上，通過藍(lán)牙與上位機(jī)進(jìn)行數(shù)據(jù)的傳輸，信號采集頻率設(shè)為200Hz。實(shí)驗(yàn)裝置，如圖2所示。

圖2 實(shí)驗(yàn)裝置圖Fig.2 Experimental Installation Diagram

3.1 零速檢測與誤差修正方法

低成本IMU 在應(yīng)用中存在一些問題，在采集數(shù)據(jù)前傳感器存在一定的誤差，包括加速度和偏航角度，需要對采集數(shù)據(jù)進(jìn)行預(yù)處理。以200Hz 為采樣頻率對傳感器靜止時(shí)的數(shù)據(jù)進(jìn)行采集，并對其結(jié)果進(jìn)行誤差的補(bǔ)償。為解決加速度的初始偏移問題，對靜止時(shí)的加速度數(shù)據(jù)進(jìn)行分析，得知需要對IMU 的加速度補(bǔ)償值為X 方向-0.0253，Y 方向0.0137。誤差補(bǔ)償前后的加速度對比圖，如圖3 所示。實(shí)驗(yàn)發(fā)現(xiàn)未進(jìn)行補(bǔ)償處理的數(shù)據(jù)嚴(yán)重偏移，加速度值在0.025 附近波動(dòng)，補(bǔ)償后的結(jié)果，如圖3（b）所示。比較符合實(shí)際情況。

圖3 靜止加速度補(bǔ)償前后對比圖Fig.3 Static Acceleration Contrast Diagram

零加速檢測目的是對采集的數(shù)據(jù)進(jìn)行模糊處理，對于在AGV 運(yùn)動(dòng)中采集到數(shù)據(jù)（加速度和偏轉(zhuǎn)角度），若兩次采集的數(shù)據(jù)變化量小于指定閾值，則認(rèn)為其加速度為零或角度偏轉(zhuǎn)為零。通過對實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析總結(jié)，認(rèn)為當(dāng)加速度變化量小于0.002時(shí)認(rèn)為其沒有發(fā)生變化。角度偏轉(zhuǎn)誤差變化比較復(fù)雜，在下面進(jìn)行說明。實(shí)驗(yàn)中發(fā)現(xiàn)，IMU 在不同運(yùn)行速度下的角度偏轉(zhuǎn)誤差有所不同，針對這一問題做了一系列實(shí)驗(yàn)并提出改進(jìn)方法。實(shí)驗(yàn)對AGV 運(yùn)行速度為（0～6000）mm/min 的直線運(yùn)動(dòng)進(jìn)行測量，得到60組角度偏轉(zhuǎn)的誤差數(shù)據(jù)，將AGV 運(yùn)行速度作為輸入，利用支持向量機(jī)（SVM）方法對角度偏轉(zhuǎn)誤差進(jìn)行訓(xùn)練與測試，得出AGV 運(yùn)行速度與角度偏差的模型，根據(jù)模型可對AGV 的角度偏差依據(jù)不同運(yùn)行速度進(jìn)行實(shí)時(shí)補(bǔ)償。運(yùn)行速度與角度偏轉(zhuǎn)誤差的關(guān)系曲線，如圖4 所示。

圖4 運(yùn)行速度與角度偏差曲線Fig.4 Running Speed and Angle Deviation Curve

因此，在定位算法中，對不同運(yùn)行速度的AGV 需要按照角度偏差曲線動(dòng)態(tài)的采取不同的角度補(bǔ)償值，使定位算法更加符合實(shí)際，同時(shí)提高定位算法精度。

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

分別做了直線運(yùn)動(dòng)和任意軌跡運(yùn)動(dòng)的實(shí)驗(yàn)。采用常規(guī)二次積分解算方法以及改進(jìn)后的誤差修正方法對實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析。直線測試實(shí)驗(yàn)結(jié)果，如圖5 所示。實(shí)測直線運(yùn)動(dòng)下，采用常規(guī)方法與采用修正定位算法得到的定位軌跡對比圖，常規(guī)方法X 方向誤差21.5%，Y 方向誤差5.5%，誤差修正方法X 方向誤差4.1%，Y方向誤差3.4%，因此提出的誤差修正算法表現(xiàn)更為優(yōu)異。

圖5 直線測試結(jié)果Fig.5 Straight Line Test Results

任意軌跡的實(shí)測實(shí)驗(yàn)結(jié)果，如圖6 所示。實(shí)驗(yàn)結(jié)果表明，開始的直線運(yùn)動(dòng)修正定位方法與常規(guī)定位方法差別不大，隨著時(shí)間的累積，常規(guī)定位方法的漂移愈發(fā)嚴(yán)重，位移2.5m 后，已經(jīng)難以達(dá)到精度要求，但是修正定位算法的優(yōu)勢明顯。此實(shí)驗(yàn)中，常規(guī)方法誤差達(dá)40%，修正定位方法定位誤差最大9.75%，精度較常規(guī)方法提高31%。通過實(shí)驗(yàn)分析得知，在使用JY9001 姿態(tài)傳感器條件下，運(yùn)動(dòng)位移與實(shí)驗(yàn)誤差之間存在圖7 關(guān)系，用戶可根據(jù)對AGV 的實(shí)際定位精度要求選擇適合的布局方式。以現(xiàn)有的AGV 尺寸為考慮因素，若要求定位精度為0.5m，則參考節(jié)點(diǎn)每隔7.8m 布置一次，若精度為1.0m，則參考節(jié)點(diǎn)每隔10m 布置一次。利用以上方法進(jìn)行系統(tǒng)位置校正，保證定位誤差始終在可接受范圍內(nèi)。

圖6 曲線測試結(jié)果Fig.6 Curve Test Result

圖7 誤差與位移曲線Fig.7 Error and Displacement Curve

4 結(jié)語

成本昂貴或定位精度低是室內(nèi)復(fù)雜環(huán)境下AGV 實(shí)時(shí)精準(zhǔn)定位現(xiàn)階段普遍存在的問題，綜合考慮，針對低成本IMU 與RFID 技術(shù)的組合定位方法進(jìn)行研究。針對低成本IMU 測量數(shù)據(jù)的誤差特征，提出實(shí)時(shí)定位誤差修正方法，通過卡爾曼濾波算法對INS 位置進(jìn)行解算，并與RFID 電子標(biāo)簽信息進(jìn)行結(jié)合，推導(dǎo)出組合定位的位置更新方程，獲得室內(nèi)AGV 位姿信息。通過實(shí)測實(shí)驗(yàn)驗(yàn)證以上算法的正確性，結(jié)果表明，基于低成本IMU 與RFID 技術(shù)的實(shí)時(shí)定位方法能夠達(dá)到實(shí)際定位要求，較單純使用INS 穩(wěn)定性和可靠性更高，定位精度提高31%，適用于室內(nèi)AGV實(shí)時(shí)定位。