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

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

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

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

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

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

1 SLAM的概率模型

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

圖1 SLAM問題原理圖

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

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

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

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

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

圖2 EKF信息融合流程圖

2.2 里程計標(biāo)定

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

令

則矯正矩陣：

X=(ATA)-1ATb

(4)

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

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

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

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

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

粒子濾波算法的proposal分布即在機(jī)器人里程計運(yùn)動學(xué)模型中進(jìn)行采樣，設(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φ)，因此：

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

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

(7)

因此權(quán)重為：

=ηp(zt|xt)

(9)

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

3.2 覆蓋柵格建圖算法

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

地圖估計問題表示為：

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

(11)

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

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

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

對于P(x)，定義對應(yīng)的log-Odd項(xiàng)：

等式兩邊取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)表示激光雷達(dá)得逆觀測模型；l(mi|x1：t-1，z1：t-1)表示柵格mi在t-1時刻得狀態(tài)，l(mi)表示柵格mi的先驗(yàn)值，該值對所有的柵格相同。

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

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

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

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

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

3)proposal分布。當(dāng)里程計精度較差時，即proposal分布很廣，需要很多的粒子才能將機(jī)器人的后驗(yàn)概率表達(dá)清楚。

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

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

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

圖4 proposal分布

機(jī)器人在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))占主導(dǎo)地位，此時p(xt|xt-1，ut)的值不再重要，令其為常數(shù)，因此：

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

(18)

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

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

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

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

5 水泵房巡檢機(jī)器人平臺搭建和實(shí)驗(yàn)

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

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

圖5 水泵房巡檢機(jī)器人

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

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

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

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

圖6 水泵房巡檢機(jī)器人平臺框架

5.2 里程計標(biāo)定實(shí)驗(yàn)

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

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

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

圖7 里程計標(biāo)定效果圖

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

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

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

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

機(jī)器人的定位誤差計算公式如下：

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

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

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

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

機(jī)器人的控制精度計算公式如下：

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

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

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

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

6 結(jié) 語

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