磁修正加權(quán)EKF多傳感器組合導(dǎo)航算法研究

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

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

0 引言

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

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

為了提高系統(tǒng)的可靠性,需要采用同類多傳感器對同一目標(biāo)點進(jìn)行觀測,從而形成冗余數(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)值分配原則。上述研究人員選用同類型多傳感器測量目標(biāo)點用以降低信息誤差波動,然而該方法不能對自身傳感器進(jìn)行校準(zhǔn)。

針對同類型多傳感器無法校準(zhǔn)自身信息這一問題,WANG等[15]提出了一種基于慣性測量單元陣列-全球定位系統(tǒng)(IMA-GPS)三層數(shù)據(jù)融合的新型無人機(jī)定位系統(tǒng)。使用GPS數(shù)據(jù)對處理后的多IMU最優(yōu)測量值進(jìn)行補(bǔ)償,使得整體定位精度提高;Bancroft等[16]提出了一種集中式濾波器結(jié)構(gòu),將多個IMU的導(dǎo)航解與GPS提供的信息進(jìn)行融合以提高系統(tǒng)的導(dǎo)航精度。Rhudy等[17]研究在非線性遞歸濾波器預(yù)測階段中,使用的3種不同的冗余多傳感器數(shù)據(jù)融合方法,與使用單個IMU的 GPS/INS傳感器融合的結(jié)果相比,這3種融合方法均能有效減少橫搖和俯仰誤差。然而GPS對使用環(huán)境等有一定的要求,不能滿足室內(nèi)移動機(jī)器人的定位導(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ù)測信息與觀測信息,得到當(dāng)前時刻最優(yōu)狀態(tài)信息;最后利用冗余磁導(dǎo)航傳感器(magnetic guidance sensor,MGS)修正系統(tǒng)累計誤差,以確保移動機(jī)器人長時間穩(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分別安裝在移動機(jī)器人的上表面與下表面,3個磁釘埋藏在地下與3個MGS一一對應(yīng),1個RFID安裝在中心偏前,與中心MGS相鄰。

圖1 冗余傳感器布局

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

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

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

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

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

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

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

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

現(xiàn)有的定位導(dǎo)航系統(tǒng)主要補(bǔ)償系統(tǒng)的確定性誤差即系統(tǒng)誤差,針對隨機(jī)誤差沒有很好的解決方法。本文針對組合導(dǎo)航系統(tǒng)的不確定度進(jìn)行研究,采用磁修正加權(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的高斯白噪聲。

移動機(jī)器人系統(tǒng)狀態(tài)為[x,y,θ]T,其中x、y為移動機(jī)器人在世界坐標(biāo)系中的位置,θ為移動機(jī)器人的航向角。系統(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ù)上一時刻的位姿信息以及當(dāng)前時刻的速度信息對系統(tǒng)的狀態(tài)進(jìn)行預(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觀測解算出的移動機(jī)器人中心點位姿信息。

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

(12)

進(jìn)一步更新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ù)進(jìn)行多IMU觀測,依照各個IMU自身的性能,以最小均方根誤差為準(zhǔn)則選取各個傳感器的加權(quán)系數(shù)。使用多IMU加權(quán)融合降低EKF觀測數(shù)據(jù)的不確定度,進(jìn)而降低整個系統(tǒng)誤差的不確定度。

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

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

(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ù)進(jìn)行加權(quán)融合得到最優(yōu)觀測信息。最優(yōu)觀測信息的均方誤差取得最小值為

(18)

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

2.3 磁定位修正累計誤差

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

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

圖3 算法流程圖

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

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

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

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

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

3 算法仿真驗證

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

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

表2 各傳感器采樣頻率 Hz

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

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

4 結(jié)論

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