基于擴(kuò)展卡爾曼濾波的傳感器融合SLAM研究

杜俊峰,郁漢琪,劉義亭,張 昊

(南京工程學(xué)院,江蘇 南京 211167)

0 引言

在當(dāng)今世界,自主移動(dòng)機(jī)器人技術(shù)的發(fā)展正以前所未有的速度蔓延到多個(gè)領(lǐng)域,如自動(dòng)駕駛汽車(chē)、無(wú)人機(jī)、掃地機(jī)器人等多種自主移動(dòng)導(dǎo)航系統(tǒng)[1],這些機(jī)器人需要準(zhǔn)確的導(dǎo)航和地圖構(gòu)建技術(shù),以適應(yīng)各種環(huán)境完成任務(wù)。SLAM是指移動(dòng)機(jī)器人在未知環(huán)境中從未知位置移動(dòng)[2],它在移動(dòng)過(guò)程中根據(jù)位姿估計(jì)和傳感器數(shù)據(jù)進(jìn)行自我定位,同時(shí)創(chuàng)建增量地圖,SLAM技術(shù)的研究已經(jīng)成為移動(dòng)機(jī)器人定位導(dǎo)航領(lǐng)域的熱點(diǎn),基于多傳感器融合的定位方法引起了廣泛研究,也是實(shí)現(xiàn)機(jī)器人真正自主運(yùn)動(dòng)的關(guān)鍵。

尹皓等[3]提出一種基于高斯牛頓方程的多傳感器非線性融合定位算法,但存在運(yùn)算量大、運(yùn)行速度慢等問(wèn)題;朱忠祥等[4]提出了一種基于置信度加權(quán)的組合導(dǎo)航融合定位方法,但計(jì)算較復(fù)雜;何珍等[5]提出了一種視覺(jué)與激光雷達(dá)組合的導(dǎo)航定位系統(tǒng),有效提高了定位精度,但相機(jī)的工作環(huán)境要求較高且計(jì)算復(fù)雜。Zhang等[6-7]提出利用二維碼輔助定位,用激光雷達(dá)輔助避障算法,有效提高了定位精度,但是需要提前布置二維碼路標(biāo),應(yīng)用場(chǎng)景局限。

針對(duì)上述問(wèn)題,本文以履帶式機(jī)器人為對(duì)象,建立了機(jī)器人運(yùn)動(dòng)學(xué)模型,提出了通過(guò)線性變換函數(shù)的方法和用IMU角速度作為里程計(jì)角速度的方法來(lái)減小履帶運(yùn)動(dòng)誤差,利用擴(kuò)展卡爾曼濾波融合里程計(jì)和IMU數(shù)據(jù)的方法減小機(jī)器人累積誤差。最后搭建了里程計(jì)的誤差測(cè)試實(shí)驗(yàn)和不同場(chǎng)景下的SLAM建圖實(shí)驗(yàn),本文改進(jìn)后的里程計(jì)誤差降低了91.72%,且SLAM地圖更加完整準(zhǔn)確,驗(yàn)證了本文算法的可行性與可靠性,提高了機(jī)器人定位精度。

1 算法設(shè)計(jì)

1.1 履帶式機(jī)器人運(yùn)動(dòng)學(xué)模型

本文使用的機(jī)器人為履帶式結(jié)構(gòu),單側(cè)驅(qū)動(dòng)輪只有一個(gè),建立運(yùn)動(dòng)學(xué)模型時(shí)對(duì)機(jī)器人做出以下2個(gè)假設(shè):(1)機(jī)器人履帶運(yùn)動(dòng)時(shí)與地面不發(fā)生相對(duì)滑移。(2)機(jī)器人質(zhì)量分布均勻且質(zhì)心位于幾何中心;其運(yùn)動(dòng)學(xué)模型可以簡(jiǎn)化等效成兩輪差速驅(qū)動(dòng)機(jī)器人模型[8]。選取幾何中心作為原點(diǎn)O建立右手坐標(biāo)系,機(jī)器人向前運(yùn)動(dòng)方向?yàn)閤軸正方向,垂直x軸向左為y軸正方向,垂直xOy平面向上為z軸正方向,如圖1所示。其中機(jī)器人輪距為L(zhǎng),轉(zhuǎn)彎半徑為R,右輪到圓心距離為D(假設(shè)機(jī)器人此時(shí)向右做移動(dòng)轉(zhuǎn)向運(yùn)動(dòng)),機(jī)器人運(yùn)動(dòng)線速度為v,角速度為ω。

圖1 履帶機(jī)器人運(yùn)動(dòng)學(xué)模型

機(jī)器人運(yùn)動(dòng)線速度v的解算如式 (1)。

(1)

機(jī)器人左輪運(yùn)動(dòng)速度vL、右輪運(yùn)動(dòng)速度vR的求解如式 (2)。

(2)

由式 (2)可以得到機(jī)器人運(yùn)動(dòng)角速度ω如式 (3) 。

(3)

式 (1)和式 (3)為機(jī)器人正運(yùn)動(dòng)學(xué)公式,寫(xiě)成矩陣形式為:

(4)

對(duì)式 (4)中系數(shù)矩陣求逆運(yùn)算可以得到機(jī)器人的逆運(yùn)動(dòng)學(xué)公式,如式 (5)。

(5)

在機(jī)器人運(yùn)動(dòng)控制過(guò)程中,正運(yùn)動(dòng)學(xué)公式可以描述為根據(jù)左右輪轉(zhuǎn)速計(jì)算得到機(jī)器人的線速度和角速度,逆運(yùn)動(dòng)學(xué)公式可以描述為向機(jī)器人發(fā)送線速度和角速度數(shù)值,解算成左右輪轉(zhuǎn)速。里程計(jì)是在正逆運(yùn)動(dòng)學(xué)基礎(chǔ)上,配合航跡推演算法來(lái)確定機(jī)器人的位姿信息P,包括了x軸坐標(biāo)X、y軸坐標(biāo)Y和航向角θ,與機(jī)器人線速度v、角速度ω的關(guān)系由式 (6)確定。

(6)

將式 (6)在時(shí)間t上進(jìn)行積分就可以求出底盤(pán)實(shí)時(shí)位姿,也就是里程計(jì)信息。實(shí)際情況下,需要在離散時(shí)間域上進(jìn)行計(jì)算,由于底盤(pán)相鄰兩位姿相隔時(shí)間Δt很小,積分運(yùn)算可以用式 (7)的累加運(yùn)算來(lái)替代。

Pt=Pt-1+PΔt

(7)

1.2 線性變換法

針對(duì)直線運(yùn)動(dòng)過(guò)程中的履帶滑移現(xiàn)象,本文采用實(shí)驗(yàn)?zāi)嫱品ㄇ笕【€性變換函數(shù)。首先移動(dòng)機(jī)器人,分別記錄下機(jī)器人里程計(jì)值和使用激光測(cè)距儀所測(cè)的實(shí)際位移,然后利用MATLAB函數(shù)擬合工具箱,將機(jī)器人里程計(jì)值作為輸入x,實(shí)際位移作為輸出y,進(jìn)行直線擬合,最后擬合得到的函數(shù)圖像如圖2所示。

圖2 線性變換函數(shù)擬合

手動(dòng)剔除掉離群點(diǎn),最終確定的線性增益函數(shù)如式 (8)所示。

f(x)=1.155x-0.1857, (|x|≥0.1608)

(8)

里程計(jì)模型中的角速度ω由左右兩輪速度差和輪距計(jì)算得來(lái),但是機(jī)器人在實(shí)際轉(zhuǎn)向運(yùn)動(dòng)過(guò)程中,機(jī)器人質(zhì)量分布不均勻、與地面發(fā)生不同程度的滑移和編碼器精度較低等因素都會(huì)增大里程計(jì)航向角的累積誤差,嚴(yán)重影響機(jī)器人的定位。針對(duì)角速度ω的誤差修正,提出融合IMU的方法:先利用低通濾波對(duì)IMU原始數(shù)據(jù)進(jìn)行濾波,然后使用IMU角速度ω′作為里程計(jì)計(jì)算過(guò)程中的角速度ω,新的里程計(jì)位姿P′的計(jì)算方式如 (9)。

(9)

1.3 EKF多傳感器融合

為了進(jìn)一步減小里程計(jì)的累積誤差,提高機(jī)器人的定位精度,本文使用擴(kuò)展卡爾曼濾波對(duì)里程計(jì)和IMU進(jìn)行多傳感器融合,k-1時(shí)刻機(jī)器人里程計(jì)數(shù)據(jù)輸入的位姿估計(jì)為xk-1=[xk-1,yk-1,θk-1]T,由xOy平面上x(chóng)軸坐標(biāo)、y軸坐標(biāo)以及機(jī)器人的航向角確定。

系統(tǒng)狀態(tài)方程描述為:

xk=f(xk-1,uk)+wk

(10)

其中,xk是系統(tǒng)在時(shí)刻k的狀態(tài)向量,f為非線性狀態(tài)轉(zhuǎn)移函數(shù),uk為控制輸入量,wk為過(guò)程噪聲。系統(tǒng)測(cè)量方程描述為:

zk=h(xk)+vk

(11)

其中,zk是在時(shí)刻t測(cè)量得到的觀測(cè)向量,h是非線性觀測(cè)模型函數(shù),vk是測(cè)量噪聲。初始狀態(tài)估計(jì)為:

(12)

(13)

(14)

其中,Fk為狀態(tài)轉(zhuǎn)移矩陣的雅可比矩陣,Qk為過(guò)程噪聲的協(xié)方差矩陣。k時(shí)刻的卡爾曼增益Kk為:

(15)

其中,Hk是觀測(cè)模型的雅可比矩陣,Rk是測(cè)量噪聲的協(xié)方差矩陣。

最后,更新卡爾曼濾波狀態(tài),將計(jì)算得的最優(yōu)狀態(tài)向量xk設(shè)為下一次計(jì)算中的xk-1,協(xié)方差矩陣Pk也進(jìn)行同步更新,更新過(guò)程中的最優(yōu)狀態(tài)和協(xié)方差矩陣可以表示為:

(16)

(17)

本文使用的傳感器融合算法步驟可以總結(jié)為:

步驟1 擬合一個(gè)線性變換函數(shù)來(lái)修正機(jī)器人直線移動(dòng)中的里程計(jì)誤差;使用IMU濾波后的角速度值作為里程計(jì)的角速度,發(fā)布初步處理后的里程計(jì)。

步驟2 通過(guò)里程計(jì)數(shù)據(jù)和IMU對(duì)卡爾曼濾波狀態(tài)進(jìn)行聯(lián)合預(yù)測(cè)。

步驟3 計(jì)算卡爾曼增益并計(jì)算最優(yōu)狀態(tài)向量與協(xié)方差。

步驟4 更新卡爾曼濾波器狀態(tài)與里程計(jì)。

2 實(shí)驗(yàn)設(shè)計(jì)與結(jié)果

2.1 硬件與軟件平臺(tái)

圖3 硬件組成

圖4 硬件組成機(jī)器人實(shí)物

軟件開(kāi)發(fā)環(huán)境使用以Linux為內(nèi)核的Ubuntu 18.04系統(tǒng),安裝ROS Melodic版本機(jī)器人開(kāi)發(fā)環(huán)境,Jetson TX2與本地PC通過(guò)SSH建立遠(yuǎn)程通信,運(yùn)行機(jī)器人底盤(pán)與傳感器等設(shè)備,完成SLAM地圖構(gòu)建并通過(guò)rviz可視化顯示。

2.2 里程計(jì)誤差實(shí)驗(yàn)

移動(dòng)機(jī)器人從起點(diǎn)到距離15 m處,分別使用原始里程計(jì)模型和本文改進(jìn)融合后里程計(jì)算法,每次間隔1 m記錄一次激光測(cè)距儀測(cè)量機(jī)器人位置的真實(shí)值,與里程計(jì)話題數(shù)值相減得到里程計(jì)誤差,對(duì)比原始誤差和融合后誤差,重復(fù)10次實(shí)驗(yàn)并記錄平均值,最后的里程計(jì)誤差如圖5所示,里程計(jì)誤差最大降低了91.72%。

圖5 里程計(jì)誤差

2.3 復(fù)雜室內(nèi)SLAM實(shí)驗(yàn)

本文選取2個(gè)場(chǎng)景進(jìn)行實(shí)驗(yàn),其一為室內(nèi)實(shí)驗(yàn)大樓1樓,包含長(zhǎng)廊退化環(huán)境、多角點(diǎn)特征環(huán)境、大范圍空曠環(huán)境等;另一場(chǎng)景為半封閉實(shí)驗(yàn)樓,主要由4條長(zhǎng)走廊組成,形成閉環(huán)。在本地PC上利用SSH遠(yuǎn)程控制Jetson TX2運(yùn)行機(jī)器人程序,錄制包含所有設(shè)備話題的試驗(yàn)數(shù)據(jù)集,其中主要有:雷達(dá)話題/scan、IMU話題/imu/data、里程計(jì)話題/odom。然后,進(jìn)行SLAM對(duì)比實(shí)驗(yàn),使用的SLAM算法均為Cartographer,在傳感器方面,第一次使用未融合的里程計(jì)和激光雷達(dá),第二次實(shí)驗(yàn)使用經(jīng)本文算法融合改進(jìn)后的里程計(jì)和激光雷達(dá),所得到的實(shí)驗(yàn)樓地圖如圖6-9所示。

圖6 場(chǎng)景一:未使用傳感器融合的SLAM地圖

在場(chǎng)景一下,圖6區(qū)域1中,激光雷達(dá)應(yīng)對(duì)長(zhǎng)廊環(huán)境時(shí)發(fā)生退化,里程計(jì)累積誤差使機(jī)器人定位丟失,構(gòu)建的SLAM地圖發(fā)生嚴(yán)重的漂移,造成地圖不匹配;圖7中使用本文融合多傳感器改進(jìn)算法進(jìn)行定位和構(gòu)建地圖,機(jī)器人定位準(zhǔn)確,整體地圖完整,地圖信息構(gòu)建全面,對(duì)長(zhǎng)廊、多角點(diǎn)環(huán)境以及大范圍空曠地圖等能良好構(gòu)建出地圖。在場(chǎng)景二下,圖8構(gòu)建的地圖盡管能通過(guò)Cartographer后端回環(huán)進(jìn)行優(yōu)化,但仍然存在嚴(yán)重扭曲部分,無(wú)法滿足SLAM的要求;圖9中SLAM效果顯著,提高了里程計(jì)準(zhǔn)確性,對(duì)環(huán)境特征重現(xiàn)更加準(zhǔn)確。

圖7 場(chǎng)景一:使用本文算法后的SLAM地圖

圖8 場(chǎng)景二:未使用傳感器融合的SLAM地圖

圖9 場(chǎng)景二:使用本文算法后的SLAM地圖

3 結(jié)語(yǔ)

本文針對(duì)履帶式機(jī)器人在SLAM過(guò)程中的里程計(jì)誤差導(dǎo)致定位丟失問(wèn)題,提出了基于線性變換函數(shù)和擴(kuò)展卡爾曼濾波融合改進(jìn)方法。經(jīng)實(shí)驗(yàn)表明,該方法里程計(jì)誤差最大減少91.72%,提高了SLAM過(guò)程的機(jī)器人定位精度。在未來(lái),利用多傳感器融合進(jìn)行不斷改進(jìn)算法將是SLAM的發(fā)展方向,提高SLAM精度,才能實(shí)現(xiàn)真正可靠的自主導(dǎo)航。