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

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

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

0 引言

在當今世界,自主移動機器人技術的發展正以前所未有的速度蔓延到多個領域,如自動駕駛汽車、無人機、掃地機器人等多種自主移動導航系統[1],這些機器人需要準確的導航和地圖構建技術,以適應各種環境完成任務。SLAM是指移動機器人在未知環境中從未知位置移動[2],它在移動過程中根據位姿估計和傳感器數據進行自我定位,同時創建增量地圖,SLAM技術的研究已經成為移動機器人定位導航領域的熱點,基于多傳感器融合的定位方法引起了廣泛研究,也是實現機器人真正自主運動的關鍵。

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

針對上述問題,本文以履帶式機器人為對象,建立了機器人運動學模型,提出了通過線性變換函數的方法和用IMU角速度作為里程計角速度的方法來減小履帶運動誤差,利用擴展卡爾曼濾波融合里程計和IMU數據的方法減小機器人累積誤差。最后搭建了里程計的誤差測試實驗和不同場景下的SLAM建圖實驗,本文改進后的里程計誤差降低了91.72%,且SLAM地圖更加完整準確,驗證了本文算法的可行性與可靠性,提高了機器人定位精度。

1 算法設計

1.1 履帶式機器人運動學模型

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

圖1 履帶機器人運動學模型

機器人運動線速度v的解算如式 (1)。

(1)

機器人左輪運動速度vL、右輪運動速度vR的求解如式 (2)。

(2)

由式 (2)可以得到機器人運動角速度ω如式 (3) 。

(3)

式 (1)和式 (3)為機器人正運動學公式,寫成矩陣形式為:

(4)

對式 (4)中系數矩陣求逆運算可以得到機器人的逆運動學公式,如式 (5)。

(5)

在機器人運動控制過程中,正運動學公式可以描述為根據左右輪轉速計算得到機器人的線速度和角速度,逆運動學公式可以描述為向機器人發送線速度和角速度數值,解算成左右輪轉速。里程計是在正逆運動學基礎上,配合航跡推演算法來確定機器人的位姿信息P,包括了x軸坐標X、y軸坐標Y和航向角θ,與機器人線速度v、角速度ω的關系由式 (6)確定。

(6)

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

Pt=Pt-1+PΔt

(7)

1.2 線性變換法

針對直線運動過程中的履帶滑移現象,本文采用實驗逆推法求取線性變換函數。首先移動機器人,分別記錄下機器人里程計值和使用激光測距儀所測的實際位移,然后利用MATLAB函數擬合工具箱,將機器人里程計值作為輸入x,實際位移作為輸出y,進行直線擬合,最后擬合得到的函數圖像如圖2所示。

圖2 線性變換函數擬合

手動剔除掉離群點,最終確定的線性增益函數如式 (8)所示。

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

(8)

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

(9)

1.3 EKF多傳感器融合

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

系統狀態方程描述為:

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

(10)

其中,xk是系統在時刻k的狀態向量,f為非線性狀態轉移函數,uk為控制輸入量,wk為過程噪聲。系統測量方程描述為:

zk=h(xk)+vk

(11)

其中,zk是在時刻t測量得到的觀測向量,h是非線性觀測模型函數,vk是測量噪聲。初始狀態估計為:

(12)

(13)

(14)

其中,Fk為狀態轉移矩陣的雅可比矩陣,Qk為過程噪聲的協方差矩陣。k時刻的卡爾曼增益Kk為:

(15)

其中,Hk是觀測模型的雅可比矩陣,Rk是測量噪聲的協方差矩陣。

最后,更新卡爾曼濾波狀態,將計算得的最優狀態向量xk設為下一次計算中的xk-1,協方差矩陣Pk也進行同步更新,更新過程中的最優狀態和協方差矩陣可以表示為:

(16)

(17)

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

步驟1 擬合一個線性變換函數來修正機器人直線移動中的里程計誤差;使用IMU濾波后的角速度值作為里程計的角速度,發布初步處理后的里程計。

步驟2 通過里程計數據和IMU對卡爾曼濾波狀態進行聯合預測。

步驟3 計算卡爾曼增益并計算最優狀態向量與協方差。

步驟4 更新卡爾曼濾波器狀態與里程計。

2 實驗設計與結果

2.1 硬件與軟件平臺

圖3 硬件組成

圖4 硬件組成機器人實物

軟件開發環境使用以Linux為內核的Ubuntu 18.04系統,安裝ROS Melodic版本機器人開發環境,Jetson TX2與本地PC通過SSH建立遠程通信,運行機器人底盤與傳感器等設備,完成SLAM地圖構建并通過rviz可視化顯示。

2.2 里程計誤差實驗

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

圖5 里程計誤差

2.3 復雜室內SLAM實驗

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

圖6 場景一:未使用傳感器融合的SLAM地圖

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

圖7 場景一:使用本文算法后的SLAM地圖

圖8 場景二:未使用傳感器融合的SLAM地圖

圖9 場景二:使用本文算法后的SLAM地圖

3 結語

本文針對履帶式機器人在SLAM過程中的里程計誤差導致定位丟失問題,提出了基于線性變換函數和擴展卡爾曼濾波融合改進方法。經實驗表明,該方法里程計誤差最大減少91.72%,提高了SLAM過程的機器人定位精度。在未來,利用多傳感器融合進行不斷改進算法將是SLAM的發展方向,提高SLAM精度,才能實現真正可靠的自主導航。