基于優(yōu)化Fast-SLAM的煤礦水泵房巡檢機器人研究

邱建都，吳 娟，金書奎 ，金 言，石軍杰

(1.太原理工大學 機械工程學院，山西 太原 030024；2.礦山流體控制國家地方聯(lián)合工程實驗室，山西 太原 030024；3.上海交通大學 機械系統(tǒng)與振動國家重點實驗室，上海 200240)

伴隨著我國礦山工業(yè)智能化的不斷推進以及機器人技術(shù)的進步與發(fā)展，煤礦水泵房巡檢機器人已經(jīng)逐漸成為我國在實現(xiàn)智慧礦山建設(shè)進程中一個迫切而必須探索和開拓的技術(shù)領(lǐng)域。

最初，人們通過將已知地圖存儲在巡檢機器人中以實現(xiàn)自主定位和導航，近幾年則主要利用在巡檢環(huán)境上方架設(shè)軌道來實現(xiàn)機器人的定位導航[1]。然而這種方式限制了機器人只能對已知的地圖進行導航或者只能按照預先鋪設(shè)的軌道進行巡檢，不能隨著巡檢環(huán)境的變化做出相應的變化。為了解決這種工作方式的局限性，同步定位與地圖構(gòu)建技術(shù)由此應運而生。SLAM技術(shù)最早于20世紀80年代被研究者提出，之后人們逐漸提出了卡爾曼濾波、擴展卡爾曼濾波、無跡卡爾曼濾波、粒子濾波、Fast-Slam、Gmapping等算法[2]，以上研究成果為我國煤礦水泵房巡檢機器人激光SLAM算法的深入研究奠定了堅實的基礎(chǔ)。

目前如何在煤礦水泵房內(nèi)進行精確定位和建圖是實現(xiàn)水泵房巡檢機器人自主巡檢所面臨的重要問題之一。因此對激光SLAM算法進行了研究，通過利用激光雷達快速獲取周圍障礙物的信息，并對里程計進行標定、多傳感器融合來減少定位誤差，然后通過對Gmapping算法進行優(yōu)化，將采樣的區(qū)域限制在一個比較小的地方，可以用更少的粒子來覆蓋機器人的概率分布，減少了重采樣的次數(shù)，在滿足機器人實時定位需求的同時也提高了建圖精度。

1 SLAM的概率模型

基于概率模型的SLAM問題原理如圖1所示。在該原理圖中，mk代表外界環(huán)境地圖；Uk代表k時刻里程計數(shù)據(jù)。Xk=(xk，yk，φk)代表移動機器人在k時刻下的位姿，其中(xk，yk)為移動機器人在環(huán)境地圖上的二維坐標，φk為移動機器人自身相對于環(huán)境地圖的轉(zhuǎn)角坐標；Zk提供k時刻激光雷達觀測到的數(shù)據(jù)。

圖1 SLAM問題原理圖

SLAM問題可以描述為：SLAM 就是通過根據(jù)起始狀態(tài)下的里程計數(shù)據(jù)以及激光雷達的觀測數(shù)據(jù)獲得移動機器人的整體路徑以及構(gòu)建地圖的最優(yōu)估計。用數(shù)學表達式進行表示為：

p(x1：t，m|u1：t，z1：t)

=p(x1：t|u1：t，z1：t)p(m|x1：t，u1：t，z1：t)

=p(x1：t|u1：t，z1：t)p(m|x1：t，z1：t)

(1)

式中，x1：t為機器人位姿；m為環(huán)境地圖；u1：t為里程計數(shù)據(jù)；z1：t為激光雷達的觀測數(shù)據(jù)；p(x1：t|u1：t，z1：t)為估計的機器人位姿；p(m|x1：t，z1：t)為已知機器人位姿和激光雷達觀測數(shù)據(jù)情況下估計環(huán)境地圖。

2 傳感器數(shù)據(jù)預處理

2.1 慣性測量單元數(shù)據(jù)和編碼器數(shù)據(jù)融合

在機器人移動時，車輪打滑或者顛簸等會造成編碼器數(shù)據(jù)失真，從而影響到輪式里程計數(shù)據(jù)，并最終導致機器人建圖效果不理想。而為了克服這些狀況，采用EKF算法將輪式里程計數(shù)據(jù)與IMU進行了融合。

圖2 EKF信息融合流程圖

2.2 里程計標定

對于每組數(shù)據(jù)，可得：

令

則矯正矩陣：

X=(ATA)-1ATb

(4)

將矯正矩陣乘于里程計數(shù)據(jù)，使得里程計測得的機器人運動信息和激光雷達測得的機器人運動信息相對應。

3 機器人同步定位與地圖構(gòu)建

3.1 基于粒子濾波算法的自身定位

在將外部傳感器獲得的環(huán)境信息和機器人的運動信息轉(zhuǎn)換為世界坐標系下后，即可獲得當前狀態(tài)的里程計數(shù)據(jù)和激光雷達數(shù)據(jù)，之后就可確定機器人處于世界坐標系中的位置即機器人的自身定位。采用粒子濾波算法去估計巡檢機器人的位姿。其算法流程如圖3所示。

圖3 基于粒子濾波的定位流程圖

粒子濾波算法的proposal分布即在機器人里程計運動學模型中進行采樣，設(shè)t-1時刻第i個粒子的位姿xit-1=(xt-1，yt-1，φt-1)，t-1時刻的里程計數(shù)據(jù)ut=(Δx，Δy，Δφ)，t時刻第i個粒子的位姿xit=(xt，yt，φt)，噪聲均值為0的高斯分布(白噪聲)分別為(Nx，Ny，Nφ)，因此：

如果沒有觀測，單純通過里程計去預測每個粒子的位姿，由于里程計中帶有噪聲，因此越傳播，粒子擴散越大，最終所有粒子的分布呈現(xiàn)均勻分布。而權(quán)重即為機器人傳感器的觀測模型，權(quán)重表示機器人的proposal分布與實際分布的差，計算公式為：

bel(xt)=ηp(zt|xt)p(x|xt-1，ut)bel(xt-1)

(7)

因此權(quán)重為：

=ηp(zt|xt)

(9)

根據(jù)權(quán)重對粒子群進行重采樣，去除低權(quán)值的粒子，復制高權(quán)值的粒子，得到需要的真實狀態(tài)xt。而這些重采樣之后的粒子就代表了真實狀態(tài)的概率分布，即可獲得機器人當前位置與姿態(tài)的準確估計。

3.2 覆蓋柵格建圖算法

當通過粒子濾波算法來估計得出機器人從運動到某一時刻t的位姿估計X1：t之后，便能夠建立全局環(huán)境地圖mt。其數(shù)學描述為以已知機器人位姿和激光雷達觀測到的數(shù)據(jù)為基礎(chǔ)估計環(huán)境地圖：

地圖估計問題表示為：

p(m|x1：t，z1：t)=∏p(mi|x1：t，z1：t)

(11)

mi是一個二元隨機變量，結(jié)合條件貝葉斯公式：

p(mi|x1：t，z1：t)

同理對于mi，兩者之比為：

對于P(x)，定義對應的log-Odd項：

等式兩邊取log得：

l(mi|x1：t，z1：t)

=l(mi|xt，zt)+l(mi|x1：t-1，z1：t-1)-l(mi)

(16)

式中，l(mi|xt，zt)表示激光雷達得逆觀測模型；l(mi|x1：t-1，z1：t-1)表示柵格mi在t-1時刻得狀態(tài)，l(mi)表示柵格mi的先驗值，該值對所有的柵格相同。

4 Fast-SLAM算法現(xiàn)有問題及其優(yōu)化

4.1 Fast-SLAM現(xiàn)有問題

Fast-SLAM算法以粒子濾波算法為基礎(chǔ)，在給定傳感器數(shù)據(jù)的情況下用粒子去估計機器人的位姿和地圖，其將對機器人的軌跡x1：t的估計轉(zhuǎn)換為一個增量估計的問題，并根據(jù)估計的位姿構(gòu)建地圖。Fast-SLAM算法存在以下幾個問題：

1)粒子耗散問題。為了使粒子群分布近似后驗概率分布，需要對粒子群進行多次重采樣，這就導致粒子的多樣性減少，最終所有粒子都由一個粒子或者幾個復制得到，這對于建圖是致命的。

2)維數(shù)災難。當空間的維數(shù)增大時，所需要的粒子數(shù)也隨之增大，由于每一個粒子都包含自己的柵格地圖，當環(huán)境大時、里程計精度不高時、預測與實際分布相差很大時都會造成內(nèi)存保證。

3)proposal分布。當里程計精度較差時，即proposal分布很廣，需要很多的粒子才能將機器人的后驗概率表達清楚。

4.2 Fast-SLAM算法優(yōu)化

Gmapping算法是目前應用最為廣泛的2D激光SLAM算法，它在較小的環(huán)境中能實現(xiàn)較好的建圖效果，即能較好的應用與煤礦水泵房環(huán)境。但Gmapping算法在Fast-SLAM算法的基礎(chǔ)上只對粒子濾波的前兩個問題進行了優(yōu)化。因此在Gmapping算法的基礎(chǔ)上，對第三個問題進行研究和優(yōu)化，提高機器人的定位和建圖精度。

為了減少粒子數(shù)，采用激光雷達匹配的方法對proposal分布進行優(yōu)化，將其限制在一個比較小的范圍，可以用更少的粒子覆蓋機器人的概率分布，如圖4所示，從而達到減少采樣次數(shù)來保持粒子多樣性。

圖4 proposal分布

機器人在t時刻的proposal分布位姿概率為：

p(xt|xt-1，ut，zt，m)=ηp(zt|xt，m)p(xt|xt-1，ut)

(17)

式中，p(zt|xt，m)在自己的區(qū)域(L(i))占主導地位，此時p(xt|xt-1，ut)的值不再重要，令其為常數(shù)，因此：

p(xt|xt-1，ut，zt，m)=ηp(zt|xt，m)xt∈L(i)

(18)

proposal分布從里程計觀測模型變成了激光雷達觀測模型，激光雷達觀測模型的方差較小，假設(shè)其服從高斯分布：

式中，xj為第j個粒子下機器人估計的位姿，Δ為一個微小量，對這k個位姿進行打分p(zt|xj，m)，并認為這k個位姿服從高斯分布，即可求得高斯分布表達式為：

Proposal分布變?yōu)楦咚狗植糔(μ，Σ)，分差較小，即可以用更少的粒子覆蓋機器人的概率分布。

因此對權(quán)重的計算可以化簡為：

5 水泵房巡檢機器人平臺搭建和實驗

5.1 平臺搭建和系統(tǒng)總體架構(gòu)

搭建以輪式底盤為車體結(jié)構(gòu)的水泵房巡檢機器人，車體內(nèi)部的單片機與工控機通過RS232串口通信，小車選用的工控機為凌華無風扇工控機，它采用英特爾酷睿i5系列處理器，8G內(nèi)存，32G硬盤，裝有ROS機器人操作系統(tǒng)。機器人由輪式機器人本體、工控機、激光雷達、慣導元件等硬件構(gòu)成，如圖5所示。

圖5 水泵房巡檢機器人

其中激光雷達無接觸遠距離測量周圍環(huán)境的障礙物信息，為了保證地圖構(gòu)建的質(zhì)量，小車使用深圳玩智商科技有限公司研發(fā)的YDLIDAE TG50激光雷達，雷達參數(shù)見表1。

表1 YDLIDAE TG50技術(shù)參數(shù)

輪式機器人通過兩輪差速驅(qū)動，為了對里程計的誤差進行補償，巡檢機器人的角速度由IMU測量獲得，采用GY85慣性測量模組來測量機器人運動的實時角速度，慣性測量模組參數(shù)見表2。水泵房巡檢機器人的平臺框架如圖6所示。

表2 GY85慣性測量模組參數(shù)

圖6 水泵房巡檢機器人平臺框架

5.2 里程計標定實驗

為了驗證里程計標定后的數(shù)據(jù)和原始數(shù)據(jù)之間的效果，使用移動機器人試驗平臺沿著室內(nèi)試驗場地進行了試驗。里程計標定的程序代碼在ROS Kinetic版本上運行，訂閱/odom、/scan和TF三個話題，其中/odom話題為里程計位姿，/scan話題為激光雷達的數(shù)據(jù)，TF為各個關(guān)節(jié)之間的坐標轉(zhuǎn)換關(guān)系，通過PI-ICP算法計算激光雷達預測的位姿。在輸入兩組數(shù)據(jù)后構(gòu)建最小二乘需要的超定方程組Ax=b，求解并返回得到的矯正矩陣：

在獲得矯正矩陣x后，乘于里程計的位姿，便可以獲得較里程計更為準確的矯正位姿。在ROS中使用rviz可視化插件實時顯示原始里程計位姿和激光雷達估計位姿，在控制巡檢機器人小車在室內(nèi)試驗場地走一段距離后獲得修正矩陣：

將小車行走獲得的矯正矩陣乘里程計數(shù)據(jù)即為矯正后的里程計標定位姿，其效果圖在matlab中顯示，如圖7所示。

圖7 里程計標定效果圖

由圖7可知，隨著機器人行駛距離的增大，里程計中的位移分量和激光雷達估計位姿中的位移分量幾乎相等，但姿態(tài)相差越來越大。由于激光雷達有環(huán)境的觀測信息作為后驗分布，所以激光里程計較輪式里程計更為精確。圖中經(jīng)矯正后的里程計標定位姿更靠近激光里程計位姿，所以經(jīng)過里程計標定后能保證巡檢機器人具有更高的定位精度。

5.3 慣性測量單元數(shù)據(jù)和編碼器數(shù)據(jù)融合

為了分析慣性測量單元數(shù)據(jù)和輪式里程計數(shù)據(jù)融合后的實際效果，利用圖5的機器人平臺進行實驗，記錄實際位姿和里程計位姿并進行對比。

由于機器人的零部件在加工、安裝、尺寸測量等過程中使機器人存在系統(tǒng)誤差，所以測試機器人在EKF算法融合里程計和IMU數(shù)據(jù)后行駛1m、3m和5m距離中偏離直線的大小和角度，測試試驗結(jié)果見表3。

機器人的定位誤差計算公式如下：

由表3數(shù)據(jù)分析可知，機器人向前行駛的距離越長，偏離直線的大小和角度也會隨之增大越大。但定位誤差Er< 0.1，滿足水泵房定位和建圖的要求。

表3 直線行駛測測試結(jié)果

轉(zhuǎn)彎試驗共分為轉(zhuǎn)過π/4、π/2、3π/4、π共4種情況進行。記錄實際轉(zhuǎn)角和里程計轉(zhuǎn)角的誤差，測試試驗結(jié)果見表4。

表4 轉(zhuǎn)彎試驗測試結(jié)果

機器人的控制精度計算公式如下：

在機器人建圖過程中，輪胎會與地面發(fā)生摩擦，并且摩擦力會隨著機器人速度的增加而增加，所以機器人實際轉(zhuǎn)動的角度會比指令發(fā)送的角度小，但融合里程計和IMU數(shù)據(jù)之后，機器人的控制精度ω<2°/rad，保證其定位軌跡更加貼近理想軌跡。

5.4 Fast-SLAM的優(yōu)化試驗

利用如圖5所示的機器人巡檢小車在經(jīng)過上述里程計標定和融合里程計和IMU數(shù)據(jù)之后，模擬井下水泵房內(nèi)無GPS的情況，分別采用傳統(tǒng)的Gmapping建圖算法和Fast-SLAM優(yōu)化后的Gmapping建圖算法進行同步定位與地圖構(gòu)建的對比試驗。建圖程序代碼在工控機上運行，操作系統(tǒng)為Ubuntu 16.04，使用VNC進行遠程控制機器人運動，并用Rviz可視化插件顯示建圖效果。

使用傳統(tǒng)的Gmapping算法在室內(nèi)環(huán)境構(gòu)建的柵格地圖能基本描繪出試驗場地的地圖，障礙物探測準確，但出現(xiàn)了失真、變形，而經(jīng)過里程計標定、融合慣性元件、優(yōu)化后的給Gmapping算法較之前建圖邊界各位平滑，異常點減少，且直線度更好。同時，激光雷達建圖仍存在一些不足，受雷達高度的影響只能檢測同一高度的障礙物，速度過快會引起地圖失真、變形、斷裂等問題。

6 結(jié) 語

通過對激光雷達的同步定位與建圖進行研究，分析了SLAM概率模型，對里程計進行標定并融合慣性元件IMU，實現(xiàn)巡檢機器人更為精確的定位。然后對Gmapping算法進行優(yōu)化以縮小采樣空間，用更少的粒子表示機器人的后驗概率，通過減少重采樣粒子的數(shù)量來提高建圖精度。仿真和實驗結(jié)果表明：經(jīng)過里程計標定、融合慣性元件、優(yōu)化算法后的巡檢機器人能夠在水泵房中進行同步定位和建圖且滿足實際工況需求。下一步研究工作的一個重點任務是加入視覺閉環(huán)檢測，提取關(guān)鍵幀并判斷閉環(huán)檢測數(shù)據(jù)來幫助系統(tǒng)有效消除累計誤差，使該算法構(gòu)建的二維柵格地圖能夠擁有更高的精度和魯棒性。