李亞晶,岳 義,韋寶琛,崔國華,劉國興

(上海工程技術(shù)大學機械與汽車工程學院,上海 201620)

0 引言

高精度定位導(dǎo)航技術(shù)是移動機器人的關(guān)鍵技術(shù)之一[1]。目前,移動機器人的工作環(huán)境主要分為結(jié)構(gòu)化環(huán)境與非結(jié)構(gòu)化環(huán)境,常用的定位導(dǎo)航方式包括慣性導(dǎo)航、全球定位系統(tǒng)、磁釘/磁帶導(dǎo)航、二維碼導(dǎo)航、超寬帶無線通訊技術(shù)、毫米雷達、雙目視覺、激光雷達等[2]。多傳感器組合導(dǎo)航精度相比于單一傳感器更高,但仍舊存在著不確定度高的問題[3]。目前冗余多傳感器組合導(dǎo)航系統(tǒng)的研究很多,其本質(zhì)是對多元數(shù)據(jù)的融合處理。數(shù)據(jù)融合定義為把來自多個傳感器的數(shù)據(jù)進行關(guān)聯(lián)、估計和組合,以獲得目標位置估計[4]。20世紀70年代,國外主要在目標跟蹤、導(dǎo)航、信號處理和模式識別等領(lǐng)域進行數(shù)據(jù)融合的研究;20世紀80年代國內(nèi)也展開數(shù)據(jù)融合技術(shù)的研究,如1987年成功研制使用多傳感器數(shù)據(jù)融合技術(shù)的航空雷達系統(tǒng),1988年研制成功的氣象類雷達系統(tǒng)等[5]。

數(shù)據(jù)融合主要包括兩類方法:概率統(tǒng)計法和人工智能法。其中,人工智能法包括神經(jīng)網(wǎng)絡(luò)、模糊邏輯、專家系統(tǒng)、D-S 證據(jù)推理等;概率統(tǒng)計法包括貝葉斯估計、互補濾波、統(tǒng)計決策理論、卡爾曼濾波、假設(shè)檢驗等[6]。其中卡爾曼濾波因其簡便性與實時性被廣泛應(yīng)用于移動機器人的組合導(dǎo)航中。劉潔瑜等[7]設(shè)計了一款卡爾曼濾波器對陀螺儀陣列進行數(shù)據(jù)融合,對最優(yōu)估計過程進行了改進,降低了數(shù)據(jù)處理的復(fù)雜度和計算量; GUO等[8]使用聯(lián)合卡爾曼濾波器融合GPS和INS傳感器的多源并行實時數(shù)據(jù)。然而卡爾曼濾波在針對非線性系統(tǒng)進行濾波融合時會失效,因此利用泰勒展開對非線性方程組直接進行線性化的擴展卡爾曼濾波(extended Kalman filter,EKF)被提出。Sahlholm等[9]基于EKF及平滑算法對車載傳感器數(shù)據(jù)與GPS數(shù)據(jù)進行融合,利用分布式數(shù)據(jù)融合來確保在處理其他測量時,道路的存儲需求不會增加;Chae等[10]提出了一種結(jié)合GPS、編碼器和基于EKF的慣性傳感器(inertial measurement unit,IMU)的室外定位方法。上述的多傳感器數(shù)據(jù)融合沒有考慮到在某類單一傳感器故障后系統(tǒng)不確定度增高的問題,使得整個系統(tǒng)的魯棒性較低。

為了提高系統(tǒng)的可靠性,需要采用同類多傳感器對同一目標點進行觀測,從而形成冗余數(shù)據(jù)。Guerrier[11]通過仿真研究傳感器數(shù)量與精度之間的關(guān)系,并且確定了滿足系統(tǒng)要求所需的傳感器具體數(shù)量;程建興等[12]利用局部狀態(tài)估計值之間的冗余信息實現(xiàn)其權(quán)系數(shù)動態(tài)的合理分配;Wang等[13]使用16個MEMS IMU 組成一組冗余慣性測量系統(tǒng),理論上,該IMU陣列的導(dǎo)航精度能夠比單個IMU提高4倍,野外實驗數(shù)據(jù)表明導(dǎo)航精度提高了3.4倍,接近理論值;楊軍佳等[14]通過分析融合權(quán)重對融合精度的影響,確定了最優(yōu)權(quán)值分配原則。上述研究人員選用同類型多傳感器測量目標點用以降低信息誤差波動,然而該方法不能對自身傳感器進行校準。

針對同類型多傳感器無法校準自身信息這一問題,WANG等[15]提出了一種基于慣性測量單元陣列-全球定位系統(tǒng)(IMA-GPS)三層數(shù)據(jù)融合的新型無人機定位系統(tǒng)。使用GPS數(shù)據(jù)對處理后的多IMU最優(yōu)測量值進行補償,使得整體定位精度提高;Bancroft等[16]提出了一種集中式濾波器結(jié)構(gòu),將多個IMU的導(dǎo)航解與GPS提供的信息進行融合以提高系統(tǒng)的導(dǎo)航精度。Rhudy等[17]研究在非線性遞歸濾波器預(yù)測階段中,使用的3種不同的冗余多傳感器數(shù)據(jù)融合方法,與使用單個IMU的 GPS/INS傳感器融合的結(jié)果相比,這3種融合方法均能有效減少橫搖和俯仰誤差。然而GPS對使用環(huán)境等有一定的要求,不能滿足室內(nèi)移動機器人的定位導(dǎo)航要求。

綜上所述,針對現(xiàn)有的組合導(dǎo)航系統(tǒng)存在成本高、應(yīng)用環(huán)境受限、定位精度差、不確定度高等問題,本文提出了一種磁修正加權(quán)EKF算法用于處理冗余多傳感器數(shù)據(jù)。首先采用最優(yōu)加權(quán)算法處理冗余IMU數(shù)據(jù)得到最優(yōu)觀測信息;然后通過EKF算法融合系統(tǒng)預(yù)測信息與觀測信息,得到當前時刻最優(yōu)狀態(tài)信息;最后利用冗余磁導(dǎo)航傳感器(magnetic guidance sensor,MGS)修正系統(tǒng)累計誤差,以確保移動機器人長時間穩(wěn)定工作。經(jīng)過MATLAB仿真驗證該算法相比于傳統(tǒng)EKF算法能更好地改善導(dǎo)航系統(tǒng)的不確定度。

1 冗余組合導(dǎo)航系統(tǒng)

針對現(xiàn)有的定位導(dǎo)航系統(tǒng)存在成本高、安裝困難、應(yīng)用環(huán)境受限、定位導(dǎo)航精度低、不確定度高等問題,以 IMU、MGS和RFID(radio frequency identification)讀取器為測量元件,搭建了冗余組合導(dǎo)航系統(tǒng),如圖1所示。3個IMU、3個MGS分別安裝在移動機器人的上表面與下表面,3個磁釘埋藏在地下與3個MGS一一對應(yīng),1個RFID安裝在中心偏前,與中心MGS相鄰。

圖1 冗余傳感器布局

其中,IMU用于測量移動機器人3個方向上的加速度與角速度,用以解算其在世界坐標系中的位移量與偏轉(zhuǎn)角度;MGS用于測量移動機器人中軸線相對于磁釘?shù)臋M偏距離,解算其中心點位姿的絕對偏差,用以修正IMU的累計誤差;RFID用于檢測存儲在RFID標簽中的序號信息,用以提供磁釘?shù)奈恢镁幪枴Ｈ哂嘟M合導(dǎo)航系統(tǒng)的流程圖如圖2所示。具體實現(xiàn)過程如下:

圖2 冗余導(dǎo)航系統(tǒng)

(1)將3個IMU采集到的加速度與角速度信息轉(zhuǎn)換到世界坐標系下,分別解算出各個IMU的位姿信息。

(2)使用3個IMU位姿信息計算載體中心的位姿信息。并通過最優(yōu)加權(quán)融合將冗余信息進行處理,得到最優(yōu)位姿信息。

(3)將得到的最優(yōu)位姿信息作為觀測值與預(yù)測狀態(tài)進行EKF融合,得到濾波消噪后的系統(tǒng)狀態(tài)。

(4)對MGS檢測到的橫偏信息進行KF濾波后再進行位姿解算,依據(jù)3個MGS的方差信息進行最優(yōu)權(quán)系數(shù)的確定,加權(quán)融合冗余定位位姿信息。

(5)將MGS解算出來的定位信息用以修正IMU的累計誤差,從而輸出更高精度的定位導(dǎo)航信息。

2 磁修正加權(quán)EKF算法

現(xiàn)有的定位導(dǎo)航系統(tǒng)主要補償系統(tǒng)的確定性誤差即系統(tǒng)誤差,針對隨機誤差沒有很好的解決方法。本文針對組合導(dǎo)航系統(tǒng)的不確定度進行研究,采用磁修正加權(quán)EKF算法處理系統(tǒng)的不確定度,相比于傳統(tǒng)EKF處理單一傳感器數(shù)據(jù)的結(jié)果,不僅降低了系統(tǒng)定位導(dǎo)航誤差的不確定度,同時利用間歇性的磁定位信息修正了系統(tǒng)運行過程中長時間的累計誤差。

2.1 EKF融合單一IMU

考慮高斯白噪聲的非線性狀態(tài)方程:

xk+1=f(xk,wk)

(1)

(2)

簡化為

(3)

式中:Fk為狀態(tài)轉(zhuǎn)移矩陣;wk為零均值、協(xié)方差為Ωk的高斯白噪聲。

移動機器人系統(tǒng)狀態(tài)為[x,y,θ]T,其中x、y為移動機器人在世界坐標系中的位置,θ為移動機器人的航向角。系統(tǒng)的狀態(tài)方程為

(4)

式中v和ω分別為車體中心處的線速度與角速度。

將(4)式離散化得:

(5)

簡化為

Xk+1=FXXk+FUUk+wk

(6)

式中:FX為狀態(tài)轉(zhuǎn)移矩陣;FU為控制矩陣。

通過FX完成k+1時刻的狀態(tài)協(xié)方差矩陣的更新:

(7)

依據(jù)上一時刻的位姿信息以及當前時刻的速度信息對系統(tǒng)的狀態(tài)進行預(yù)測。

考慮具有高斯白噪聲的非線性觀測方程:

zk=h(xk,vk)

(8)

(9)

簡化為

(10)

式中:Hk+1為觀測矩陣;vk+1為零均值、協(xié)方差為Rk+1的高斯白噪聲。

由于IMU輸出的位姿是離散形式的,本文的觀測信息可描述為

Zk+1=Hk+1Xk+1+Vk+1

(11)

式中Zk+1=[xz,yz,θz]T為IMU觀測解算出的移動機器人中心點位姿信息。

k+1時刻的增益矩陣可描述為

(12)

進一步更新k+1時刻的狀態(tài)協(xié)方差矩陣P和最優(yōu)位姿值為

Pk+1|k+1=(I-Kk+1Hk+1)Pk+1|k

(13)

(14)

2.2 加權(quán)EKF融合冗余IMU

針對單一IMU誤差不確定度高等問題,通過最優(yōu)加權(quán)EKF處理冗余IMU信息以降低系統(tǒng)的不確定度。加權(quán)EKF是在原有的EKF基礎(chǔ)上對觀測數(shù)據(jù)進行多IMU觀測,依照各個IMU自身的性能,以最小均方根誤差為準則選取各個傳感器的加權(quán)系數(shù)。使用多IMU加權(quán)融合降低EKF觀測數(shù)據(jù)的不確定度,進而降低整個系統(tǒng)誤差的不確定度。

在任意時刻對多IMU進行加權(quán)的步驟如下:

(2)建立加權(quán)均方誤差表達式如下:

(15)

(3)確定最優(yōu)權(quán)系數(shù),令式(15)取最小值,構(gòu)建如下輔助函數(shù):

(16)

求得最優(yōu)權(quán)系數(shù)為

(17)

(4)通過最優(yōu)權(quán)系數(shù)對各個傳感器的觀測數(shù)據(jù)進行加權(quán)融合得到最優(yōu)觀測信息。最優(yōu)觀測信息的均方誤差取得最小值為

(18)

(5)將最優(yōu)觀測信息作為EKF的Zk+1觀測數(shù)據(jù) 提供到濾波算法中與狀態(tài)預(yù)測信息相結(jié)合推斷此刻移動機器人的最優(yōu)位姿。

2.3 磁定位修正累計誤差

冗余IMU僅能降低IMU自身數(shù)據(jù)的隨機波動,對其本身的累計誤差沒有修正作用。在移動機器人經(jīng)過磁釘上方時,MGS檢測磁釘中心相對于MGS中心即車體中心的相對偏差,通過3組兩兩MGS的偏差數(shù)據(jù)與其之間的相對位置結(jié)合解算車體航偏角,利用航偏角與橫向偏差解算車體中心的位置信息,提供給IMU系統(tǒng)用以清除累計誤差。

磁修正加權(quán)EKF算法流程如圖3所示,其解算步驟如下:

圖3 算法流程圖

(1)IMU輸入三軸加速度與角速度,通過積分解算得到當前時刻位姿信息。

(2)根據(jù)3個IMU方差信息確定各自權(quán)信息w1、w2和w3,得到最優(yōu)觀測信息。

(3)將最優(yōu)觀測信息與系統(tǒng)預(yù)測信息進行EKF融合得到當前時刻最優(yōu)位姿信息。

(4)車體經(jīng)過磁釘上方時,3個MGS分別檢測其中心與磁釘中心的橫向偏差,兩兩結(jié)合確定最優(yōu)偏差角,進一步解算得到移動機器人中心位姿。

(5)通過MGS系統(tǒng)提供的絕對定位信息修正3個IMU的累計誤差。

3 算法仿真驗證

本文提出磁修正加權(quán)EKF算法的目的在于降低冗余導(dǎo)航系統(tǒng)誤差的不確定度。因此,為驗證該算法的有效性,對移動機器人的傳感器數(shù)據(jù)進行了仿真實驗分析。利用MATLAB軟件生成移動機器人的動態(tài)軌跡并對傳感器真實誤差數(shù)據(jù)進行仿真建模。表1為所用傳感器的誤差參數(shù),表2為各個傳感器的采樣頻率。

表1 導(dǎo)航誤差參數(shù)

表2 各傳感器采樣頻率 Hz

設(shè)置一條包括前進、原地轉(zhuǎn)向等運動過程的移動機器人行駛路徑。移動機器人起始位置(0 m,0 m),規(guī)劃移動機器人向北行駛8 m,停止后原地旋轉(zhuǎn)90°再向東行駛8 m,原地旋轉(zhuǎn)90°后向南行駛4 m,原地旋轉(zhuǎn)90°后向西行駛4 m,最后原地旋轉(zhuǎn)90°后向北行駛2 m到達指定位置。

針對在同一軌跡上的位置與姿態(tài)誤差,分別進行了單一IMU的EKF定位導(dǎo)航仿真、3個IMU的加權(quán)EKF定位導(dǎo)航仿真以及磁修正冗余IMU定位導(dǎo)航仿真,圖4為單一IMU的EKF定位導(dǎo)航軌跡與位姿誤差,圖5為3個IMU的加權(quán)EKF定位導(dǎo)航軌跡與位姿誤差,圖6為磁修正冗余IMU定位導(dǎo)航軌跡與位姿誤差。

(a)行駛路徑

(a)行駛路徑

(a)行駛路徑

由圖4～圖6可知,在使用單一IMU作為EKF觀測信息與系統(tǒng)狀態(tài)進行融合時,位置姿態(tài)誤差隨著時間的累積嚴重漂移,同時IMU的觀測噪聲不能得到有效抑制,有害觀測信息被引入組合導(dǎo)航系統(tǒng);在使用3個IMU提供冗余觀測信息對系統(tǒng)進行修正時,數(shù)據(jù)不確定度降低了42.3%,但累計誤差并沒有得到改善,這是IMU傳感器本身的特性導(dǎo)致的,增加IMU的數(shù)量僅能降低其觀測噪聲,提高觀測信息的可靠性;通過磁修正的冗余IMU導(dǎo)航系統(tǒng)的仿真結(jié)果可以在移動機器人經(jīng)過磁釘上方時,MGS檢測到自身相對于磁釘?shù)奈蛔似?提供高精度的位姿參數(shù)用以修正IMU的累計誤差,從而達到高精度導(dǎo)航的目的。

4 結(jié)論

本文為降低移動機器人定位導(dǎo)航誤差的不確定度,建立了一套基于多傳感器的冗余測量系統(tǒng),針對該系統(tǒng)提出了一種磁修正加權(quán)EKF算法。對冗余IMU信息進行加權(quán)融合,相比于單一IMU為系統(tǒng)提供不確定度更低的觀測信息用以EKF狀態(tài)更新;以間歇性的磁信息作為輸入去修正冗余IMU系統(tǒng)的累計誤差。通過MATLAB軟件對系統(tǒng)進行仿真實驗,相比于傳統(tǒng)的EKF算法,本文的磁修正加權(quán)EKF算法不僅能使定位誤差的不確定度降低42.3%,同時能針對系統(tǒng)的累計誤差進行修正,以達到長時間穩(wěn)定工作。