基于自適應卡爾曼濾波的RFID/SINS組合導航研究

張一康，陳燚濤，劉 芳

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

0 引言

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

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

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

1 RFID/SINS組合導航

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

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

1.1 RFID輔助導航系統

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

1.2 組合導航系統數學建模

在設計組合導航系統之前,需要建立動態的特性狀態方程[13]。本研究為二維平面內的導航定位,不需要考慮三維立體空間的參數變量。因此基于慣性導航的狀態方程有12個變量,分別為SINS系統位置誤差(δφ,δλ)、速度誤差(δVE,δVN)、姿態角誤差(φx,φy,φz)、三軸陀螺儀零漂(εx,εy,εz)和東、北向加速度零漂(?x,?y)。組合導航系統的狀態變量如式(1)所示。系統噪聲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組合導航系統的狀態方程可以表示為式(3)。在組合導航系統中分別可以用式(4)和式(5)表示。

(3)

(4)

(5)

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

系統的量測噪聲主要反映量測量和狀態量之間的關系,在RFID定位系統中主要參數有東、北向的位置信息(xER,yNR),并與慣導系統中的位置信息(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優化分析

2.1 KF參數更新

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

先驗狀態估計值:

(10)

狀態一步預測均方誤差:

(11)

濾波增益:

(12)

后驗狀態估計值:

(13)

后驗狀態估計均方誤差:

(14)

2.2 AKF優化

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

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

(15)

(16)

AKF流程如圖2所示。

圖2 AKF流程Fig.2 AKF flowchart

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

3 仿真實驗

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

表1 仿真參數

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

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

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

(a)KF緯度方向誤差

(b)KF經度方向誤差

(a)AKF緯度方向誤差

(a)KF緯度方向誤差

(a)AKF緯度方向誤差

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

表2 單次導航平均定位誤差統計

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

表3 不同濾波下定位誤差均方根值統計

圖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的組合導航其導航穩定性得到了較大提升,并且AKF組合導航定位誤差均方根比較集中、上下浮動較小。因此,基于AKF的RFID/SINS組合導航系統在單次導航過程和多次導航過程中,其定位精度和穩定性都有一定的提升。綜上所述,AKF在RFID/SINS組合導航系統中有更好的導航性能。

4 實驗驗證

4.1 實驗環境

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

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

圖11 組合導航AGV結構簡圖Fig.11 Sketch of combined navigation AGV structure

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

4.2 實驗結果分析

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

(a)KF緯度方向誤差

(b)KF經度方向誤差

(a)AKF緯度方向誤差

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

表4 組合導航系統位置誤差及其均方差輸出

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

5 結束語

本文基于AKF的RFID/SINS組合導航系統,有效解決了室內慣性導航系統誤差發散和濾波中噪聲參數不斷變化的問題。

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

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