張一康，陳燚濤，劉 芳

(武漢紡織大學 機械工程與自動化學院，湖北 武漢 430200)

0 引言

自動導引車(Automated Guided Vehicle,AGV)作為自動倉儲和自動物流的關(guān)鍵設(shè)備能夠?qū)崿F(xiàn)無人運輸?shù)墓δ?其關(guān)鍵技術(shù)是利用導航系統(tǒng)提供較為精確的位置信息[1]。組合導航系統(tǒng)是將多個導航系統(tǒng)結(jié)合在一起,彌補各自的不足,從而實現(xiàn)更精準的導航定位。其中,GPS/捷聯(lián)慣導系統(tǒng)(Strap-down Inertial Navigation System,SINS)組合導航由于精度較高和定位范圍廣等優(yōu)點被廣泛運用[2]。但是AGV小車普遍應用在廠房或者室內(nèi)環(huán)境,無法穩(wěn)定地接收到GPS信號,甚至在復雜的室內(nèi)環(huán)境下完全無法接收到衛(wèi)星信號[3-4],導致組合導航系統(tǒng)完全由慣性測量單元(Inertial Measurement Unit,IMU)發(fā)揮作用,使整套定位導航系統(tǒng)精度迅速降低。

為了解決室內(nèi)接收不到GPS信號的問題,文獻[5]運用機器學習的方法彌補慣性導航運行一段時間后定位精度變差的問題,但是增加了整套導航系統(tǒng)的計算量和設(shè)備負擔,并且和室內(nèi)組合導航系統(tǒng)相比其定位精度較差。文獻[6]采用機器視覺輔助的方式替代原有的GPS信號組成新的組合導航系統(tǒng),通過將快速準確的圖像匹配應用到視覺里程計中,可以有效減少慣性導航系統(tǒng)的定位誤差。文獻[7]采用激光雷達和MEMS微慣性組合的方式進行室內(nèi)定位與導航,提出了以SLAM定位結(jié)果為觀測量的激光雷達/MEMS慣性的組合導航方案。

上述針對于室內(nèi)組合導航的改進方案,本質(zhì)都是尋找可以提供類似于GPS信號的定位裝置,實現(xiàn)室內(nèi)導航,但是改進方案都需要較高的成本。RFID室內(nèi)定位具有信號穿透性強、成本低廉等諸多優(yōu)點[8-10],因此,本文利用RFID將位置信息通過閱讀器和上位機傳輸?shù)綉T性導航系統(tǒng)中,并使用自適應卡爾曼濾波(Adaptive Kalman Filtering, AKF)[11]的方法,在降低整體導航成本的前提下,提高了導航系統(tǒng)的定位精度。

1 RFID/SINS組合導航

在RFID/SINS組合導航系統(tǒng)中, RFID定位系統(tǒng)作為輔助系統(tǒng),主要功能是提供對應時刻的位置信息,并與慣導系統(tǒng)的位置信息對比,通過卡爾曼濾波(Kalman Filtering, KF)進行過濾校正,其輸入為RFID和SINS解算位置之差[12]。本文采用的組合導航系統(tǒng)仿真流程如圖1所示。

圖1 RFID/SINS組合導航仿真流程Fig.1 RFID/SINS combination navigation simulation flowchart

1.1 RFID輔助導航系統(tǒng)

在RFID/SINS組合導航系統(tǒng)中,RFID定位系統(tǒng)主要提供AGV的位置坐標,定位設(shè)備主要包含RFID閱讀器和無源電子標簽。通過RFID閱讀器發(fā)射電磁波與標簽交互后,根據(jù)收到的反饋信號強度判斷AGV的位置,或者直接通過指紋定位算法估計AGV的位置坐標[8]。因此,通過將RFID定位系統(tǒng)獲取位置信息作為觀測量,并與慣性導航獲取的位置信息比較導入KF中,即可對原有的導航軌跡進行校正。

1.2 組合導航系統(tǒng)數(shù)學建模

在設(shè)計組合導航系統(tǒng)之前,需要建立動態(tài)的特性狀態(tài)方程[13]。本研究為二維平面內(nèi)的導航定位,不需要考慮三維立體空間的參數(shù)變量。因此基于慣性導航的狀態(tài)方程有12個變量,分別為SINS系統(tǒng)位置誤差(δφ,δλ)、速度誤差(δVE,δVN)、姿態(tài)角誤差(φx,φy,φz)、三軸陀螺儀零漂(εx,εy,εz)和東、北向加速度零漂(?x,?y)。組合導航系統(tǒng)的狀態(tài)變量如式(1)所示。系統(tǒng)噪聲W(t)主要為陀螺儀和加速度計的高斯白噪聲誤差,如式(2)所示:

X(t)=[δφ,δλ,δVE,δVN,φx,φy,φz,εx,εy,εz,?x,?y]12×1,

(1)

W(t)=[ωgx,ωgy,ωgz,ωax,ωay]。

(2)

整個RFID/SINS組合導航系統(tǒng)的狀態(tài)方程可以表示為式(3)。在組合導航系統(tǒng)中分別可以用式(4)和式(5)表示。

(3)

(4)

(5)

式中:F(t)為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣,對狀態(tài)變量的運動建模;G(t)為控制矩陣,將輸入轉(zhuǎn)化為狀態(tài);FN為系統(tǒng)動態(tài)矩陣,由慣性導航系統(tǒng)的2個位置誤差、2個速度誤差和3個姿態(tài)誤差組成[14]。

系統(tǒng)的量測噪聲主要反映量測量和狀態(tài)量之間的關(guān)系,在RFID定位系統(tǒng)中主要參數(shù)有東、北向的位置信息(xER,yNR),并與慣導系統(tǒng)中的位置信息(xE,yN)共同組成量測量。量測向量Z(t)則可以表示為式(6),對應的量測方程如式(7)所示,其中H(t)為量測矩陣(式(8)),V(t)為量測噪聲,包含東、北向位置誤差(式(9)):

(6)

Z(t)=H(t)X(t)+V(t),

(7)

(8)

V(t)=[EEEN]。

(9)

2 AKF優(yōu)化分析

2.1 KF參數(shù)更新

KF是一種利用線性系統(tǒng)狀態(tài)方程,通過系統(tǒng)輸入輸出觀測數(shù)據(jù),對系統(tǒng)狀態(tài)進行最優(yōu)估計的算法。傳統(tǒng)KF參數(shù)更新的流程為,預測階段用前一時刻的狀態(tài)估計值推算當前時刻的狀態(tài),獲得先驗狀態(tài)估計值,然后在更新階段對預測值進行修正,獲得后驗狀態(tài)估計值。通過不斷地進行“預測—實測—修正”的更新順序,最終達到一個平衡狀態(tài)[15]。通過上述給出的系統(tǒng)狀態(tài)方程和量測方程,利用KF進行參數(shù)的時間更新和量測更新,可以得到KF的基本方程如下[16]。

先驗狀態(tài)估計值:

(10)

狀態(tài)一步預測均方誤差:

(11)

濾波增益:

(12)

后驗狀態(tài)估計值:

(13)

后驗狀態(tài)估計均方誤差:

(14)

2.2 AKF優(yōu)化

AKF是一種能夠動態(tài)調(diào)整KF參數(shù)的算法,對于含有噪聲參數(shù)和不確定性信號的濾波有更好的估計性能。在RFID/SINS組合導航系統(tǒng)中,RFID定位系統(tǒng)用于提供離散的定位信息,慣性導航系統(tǒng)提供連續(xù)的運動狀態(tài)信息。RFID直接或間接依靠電磁波信號特征進行定位計算,由于在室內(nèi)復雜環(huán)境下存在各類障礙物,增加了電磁波信號噪聲和不確定性,進而降低了AGV組合導航系統(tǒng)的導航精度和穩(wěn)定性[17]。因此本文選用AKF融合導航信息。

AKF在進行狀態(tài)估計的同時可以通過量測輸出,在線實時的估計系統(tǒng)的噪聲參數(shù)Rk。在AKF中噪聲參數(shù)和量測輸出相互關(guān)聯(lián),可以通過式(15)得出,其中,βk為自適應參數(shù)(式(16)),b為自適應因子,一般取值0.9～0.999[18]。

(15)

(16)

AKF流程如圖2所示。

圖2 AKF流程Fig.2 AKF flowchart

在RFID/SINS組合導航實現(xiàn)過程中,初始位置和姿態(tài)分別通過RFID定位系統(tǒng)和IMU磁力計獲取計算得到,在AGV開始運動后采集RFID器件和慣性傳感器信息,通過位置定位算法計算推導RFID閱讀器的位置坐標,慣性導航系統(tǒng)則通過慣性導航姿態(tài)解算算法和運動狀態(tài)估計算法輸出導航參數(shù)。將2個導航系統(tǒng)的輸出參數(shù)輸入組合導航系統(tǒng),運用AKF對下一時刻預估參數(shù)進行糾偏和校正,從而適應不同的運動狀態(tài),得到更好的導航精度和穩(wěn)定性。

3 仿真實驗

為了驗證提出的AKF的效果,本文根據(jù)數(shù)學模型使用Matlab軟件進行仿真,部分仿真參數(shù)如表1所示。RFID標簽布置在AGV行駛的路徑上,依據(jù)其速度每隔一定的距離布置RFID標簽陣列。軌跡路線示意如圖3所示。慣性傳感器IMU采樣頻率為10 Hz,當AGV運動到RFID標簽陣列處時,進行RFID輔助定位獲取位置信息,并與IMU測量的位置信息進行比對,通過KF進行過濾和修正。在整個移動過程中,AGV每次經(jīng)過RFID標簽陣列時進行位置糾偏,直至AGV小車到達終點完成路徑運動。

表1 仿真參數(shù)

圖3 不同路徑路線導航示意Fig.3 Different path route navigation schematic diagram

本文針對于室內(nèi)復雜環(huán)境設(shè)計了2種不同路徑的仿真方案,仿真時間為300 s。AGV正常行駛速度設(shè)置為1 m/s,進行轉(zhuǎn)彎時,會以-0.2 m/s2的加速度將速度提前降到0.4 m/s,并且會在轉(zhuǎn)彎后恢復至原有速度。第一次仿真軌跡為直線軌跡,第二次仿真軌跡為曲線軌跡。

對于上述2種軌跡,分別使用標準KF和AKF的RFID/SINS組合導航系統(tǒng)進行仿真,并輸出導航系統(tǒng)在每個采樣時刻的定位誤差曲線。圖4和圖5分別為AGV小車沿直線軌跡行駛,傳統(tǒng)KF和AKF在經(jīng)緯方向上的定位誤差曲線,其中δλ為緯度距離誤差,δL為經(jīng)度距離誤差。圖6和圖7為曲線軌跡的定位誤差曲線。

(a)KF緯度方向誤差

(b)KF經(jīng)度方向誤差

(a)AKF緯度方向誤差

(a)KF緯度方向誤差

(a)AKF緯度方向誤差

通過上述仿真結(jié)果可以看出,AGV直線軌跡運行和曲線軌跡運行所表現(xiàn)的整體導航性能變化不大,主要的不同點是AGV在轉(zhuǎn)彎時會導致導航系統(tǒng)的定位誤差產(chǎn)生較大的變化。單次導航過程中AGV的平均定位誤差如表2所示。通過不同KF下的定位誤差均方根,可知AKF比標準KF在RFID/SINS組合導航系統(tǒng)中定位精度更高,其平均定位精度提高了9.36%,并且在單次導航過程中其定位誤差分布較為穩(wěn)定, AKF相比于KF穩(wěn)定性提升了6.64%。

表2 單次導航平均定位誤差統(tǒng)計

為了保證仿真的準確性和可靠性,本文后續(xù)進行蒙特卡洛實驗,以模擬AGV在復雜室內(nèi)環(huán)境下的導航定位情況。在相同的軌跡下進行30次仿真。分別記錄每次仿真的緯、經(jīng)度方向上的定位誤差,并計算平面內(nèi)的幾何定位誤差和誤差均方根值。不同濾波下的幾何定位誤差均方根曲線如圖8和圖9所示。為了清晰直觀地了解30次仿真數(shù)據(jù)的整體情況,表3列出了多次仿真不同軌跡下的定位誤差均方根平均值。

表3 不同濾波下定位誤差均方根值統(tǒng)計

圖8 直線軌跡幾何定位誤差均方根曲線 Fig.8 Root mean square curve of geometric positioning error of straight trajectory

圖9 曲線軌跡幾何定位誤差均方根曲線Fig.9 Root mean square curve of geometric positioning error of curve trajectory

通過圖8、圖9和表3可以看出,在直線和曲線軌跡下,AKF組合導航在多次導航過程的定位誤差均方差都比KF組合導航要小,說明基于AKF的組合導航其導航穩(wěn)定性得到了較大提升,并且AKF組合導航定位誤差均方根比較集中、上下浮動較小。因此,基于AKF的RFID/SINS組合導航系統(tǒng)在單次導航過程和多次導航過程中,其定位精度和穩(wěn)定性都有一定的提升。綜上所述,AKF在RFID/SINS組合導航系統(tǒng)中有更好的導航性能。

4 實驗驗證

4.1 實驗環(huán)境

為了驗證提出的基于AKF的RFID/SINS組合導航在實際使用過程中的準確性,本文設(shè)計并進行了實驗。在實驗中,采用的IMU測量模塊為WHEELTEC的N200系列九軸傳感器,其航向陀螺零偏穩(wěn)定性為2 (°)/h,加速度零偏穩(wěn)定性為0.000 2g。RFID閱讀器為圓極化天線,工作頻率為915 MHz,RFID標簽為H47型,實驗布置示意如圖10所示。IMU測量模塊和RFID閱讀器均通過USB接口和上位機進行數(shù)據(jù)傳輸,IMU數(shù)據(jù)采集頻率為10 Hz。實驗裝置如圖11所示,實驗場地示意如圖12所示。

圖10 RFID標簽布置及參考點位置示意Fig.10 Schematic diagram of RFID tag arrangement and reference point location

圖11 組合導航AGV結(jié)構(gòu)簡圖Fig.11 Sketch of combined navigation AGV structure

圖12 實驗場地和預定路徑示意Fig.12 Schematic diagram of the experimental site and intended path

4.2 實驗結(jié)果分析

在跑車實驗中,通過獲取RFID和慣導IMU傳感器的各項導航參數(shù),分別使用KF和AKF對AGV導航精度進行處理,可以繪制AGV小車在移動過程中的位置精度數(shù)據(jù)圖。KF和AKF下組合導航經(jīng)緯度方向位置誤差曲線如圖13和圖14所示。

(a)KF緯度方向誤差

(b)KF經(jīng)度方向誤差

(a)AKF緯度方向誤差

通過上述RFID/SINS組合導航系統(tǒng)的位置誤差曲線圖,可以計算該實驗環(huán)境下AGV的整個移動過程中的平均位置誤差和誤差穩(wěn)定性,其中位置誤差穩(wěn)定性通過位置誤差均方差值來反映位置誤差變化的幅度。組合導航系統(tǒng)輸出及數(shù)據(jù)處理結(jié)果如表4所示。L和λ分別表示地球坐標系中的緯度和經(jīng)度。KF和AKF下組合導航系統(tǒng)部分軌跡對比如圖15所示。

表4 組合導航系統(tǒng)位置誤差及其均方差輸出

實驗結(jié)果表明,AGV在整個導航運行過程中,除了在轉(zhuǎn)彎時會產(chǎn)生較大的誤差偏移,其余過程的位置誤差較小且穩(wěn)定。由表4和圖13可以看出,在KF和AKF條件下RFID/SINS組合導航系統(tǒng)平均定位誤差分別為0.08、0.072 m,在室內(nèi)環(huán)境下均能夠提供高精度的導航定位。應用AKF的組合導航,定位精度提升了10%,導航穩(wěn)定性提升了7.4%,驗證了上述仿真結(jié)果。

5 結(jié)束語

本文基于AKF的RFID/SINS組合導航系統(tǒng),有效解決了室內(nèi)慣性導航系統(tǒng)誤差發(fā)散和濾波中噪聲參數(shù)不斷變化的問題。

針對室內(nèi)環(huán)境無法接收GPS信號導致GPS/SINS組合導航精度變差和外界噪聲的不確定影響,通過RFID輔助定位系統(tǒng)替代原有的GPS信號,實現(xiàn)室內(nèi)組合導航與定位,應用AKF將噪聲參數(shù)與量測輸出參數(shù)關(guān)聯(lián)并實時獲取較為準確的噪聲參數(shù)。

通過實驗和仿真計算發(fā)現(xiàn),RFID/SINS組合導航系統(tǒng)能夠較好地實現(xiàn)室內(nèi)高精度的定位與導航,實驗結(jié)果顯示組合導航系統(tǒng)的平均定位誤差保持在0.07 m左右。應用AKF濾波其平均定位誤差減少了10%,穩(wěn)定性提升了7.4%。