基于自適應(yīng)卡爾曼濾波的RFID/SINS組合導(dǎo)航研究

張一康，陳燚濤，劉 芳

(武漢紡織大學(xué) 機(jī)械工程與自動(dòng)化學(xué)院，湖北 武漢 430200)

0 引言

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

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

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

1 RFID/SINS組合導(dǎo)航

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

圖1 RFID/SINS組合導(dǎo)航仿真流程Fig.1 RFID/SINS combination navigation simulation flowchart

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

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

1.2 組合導(dǎo)航系統(tǒng)數(shù)學(xué)建模

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

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

(1)

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

(2)

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

(3)

(4)

(5)

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

系統(tǒng)的量測(cè)噪聲主要反映量測(cè)量和狀態(tài)量之間的關(guān)系,在RFID定位系統(tǒng)中主要參數(shù)有東、北向的位置信息(xER,yNR),并與慣導(dǎo)系統(tǒng)中的位置信息(xE,yN)共同組成量測(cè)量。量測(cè)向量Z(t)則可以表示為式(6),對(duì)應(yīng)的量測(cè)方程如式(7)所示,其中H(t)為量測(cè)矩陣(式(8)),V(t)為量測(cè)噪聲,包含東、北向位置誤差(式(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)方程,通過(guò)系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù),對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法。傳統(tǒng)KF參數(shù)更新的流程為,預(yù)測(cè)階段用前一時(shí)刻的狀態(tài)估計(jì)值推算當(dāng)前時(shí)刻的狀態(tài),獲得先驗(yàn)狀態(tài)估計(jì)值,然后在更新階段對(duì)預(yù)測(cè)值進(jìn)行修正,獲得后驗(yàn)狀態(tài)估計(jì)值。通過(guò)不斷地進(jìn)行“預(yù)測(cè)—實(shí)測(cè)—修正”的更新順序,最終達(dá)到一個(gè)平衡狀態(tài)[15]。通過(guò)上述給出的系統(tǒng)狀態(tài)方程和量測(cè)方程,利用KF進(jìn)行參數(shù)的時(shí)間更新和量測(cè)更新,可以得到KF的基本方程如下[16]。

先驗(yàn)狀態(tài)估計(jì)值:

(10)

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

(11)

濾波增益:

(12)

后驗(yàn)狀態(tài)估計(jì)值:

(13)

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

(14)

2.2 AKF優(yōu)化

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

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

(15)

(16)

AKF流程如圖2所示。

圖2 AKF流程Fig.2 AKF flowchart

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

3 仿真實(shí)驗(yàn)

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

表1 仿真參數(shù)

圖3 不同路徑路線導(dǎo)航示意Fig.3 Different path route navigation schematic diagram

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

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

(a)KF緯度方向誤差

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

(a)AKF緯度方向誤差

(a)KF緯度方向誤差

(a)AKF緯度方向誤差

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

表2 單次導(dǎo)航平均定位誤差統(tǒng)計(jì)

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

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

圖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

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

4 實(shí)驗(yàn)驗(yàn)證

4.1 實(shí)驗(yàn)環(huán)境

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

圖10 RFID標(biāo)簽布置及參考點(diǎn)位置示意Fig.10 Schematic diagram of RFID tag arrangement and reference point location

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

圖12 實(shí)驗(yàn)場(chǎng)地和預(yù)定路徑示意Fig.12 Schematic diagram of the experimental site and intended path

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

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

(a)KF緯度方向誤差

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

(a)AKF緯度方向誤差

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

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

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

5 結(jié)束語(yǔ)

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

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

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