李 娜，賀海育，賈 偉，雍龍泉

(1.陜西理工大學(xué) 數(shù)學(xué)與計算機科學(xué)學(xué)院，陜西 漢中723000；2.西安航空學(xué)院 電子工程學(xué)院，陜西 西安 710017)

0 引言

近年來，在智能控制方面，對于即時定位與地圖構(gòu)建(Simultaneous Localization and Mapping，SLAM)算法的研究是一大熱點[1-8]。

在2D-SLAM建圖與路徑規(guī)劃的應(yīng)用方面研究較多，文獻(xiàn)[9-11]中機器人的里程計信息進行坐標(biāo)變換后，利用Gmapping建圖算法可以實現(xiàn)在室內(nèi)小范圍地圖的建立;文獻(xiàn)[12]提出了一種基于多傳感器融合的室內(nèi)建圖和定位算法；文獻(xiàn)[13]中采用基于高斯牛頓法(Guass-Newton)的Hector建圖算法，不需要里程計的情況下可以建立出高精度的地圖；文獻(xiàn)[14]提出了一種實時定位和建圖前端優(yōu)化的解決方法；文獻(xiàn)[15-19]基于機器人操作系統(tǒng)(Robot Operating System,ROS)平臺,對導(dǎo)航機器人的整體架構(gòu)和路徑規(guī)劃進行研究。以上研究大多需要大量的傳感器，造成在實際應(yīng)用中成本過高，且常用建圖算法較多而鮮有對比。在實際應(yīng)用中，最佳方案應(yīng)根據(jù)不同的環(huán)境選取不同的建圖算法，實現(xiàn)精確定位與導(dǎo)航。

因此，本文使用ROS管理控制2D激光雷達(dá)和樹莓派來采集環(huán)境信息，對信息處理后分別利用Gmapping與Hector兩種算法建立二維柵格地圖,然后進行路徑規(guī)劃，實現(xiàn)小車對未知環(huán)境的自由探索。實驗證明，該系統(tǒng)不僅成本較低、結(jié)構(gòu)簡單，而且擴展性以及適用性較強。

1 小車硬件平臺設(shè)計

首先需建立一輛智能監(jiān)測系統(tǒng)小車，設(shè)計小車的實時建圖與路徑規(guī)劃系統(tǒng)的結(jié)構(gòu)框架如圖1所示。

圖1 系統(tǒng)結(jié)構(gòu)

當(dāng)小車處于未知環(huán)境時，首先在雷達(dá)節(jié)點中，計算機通過串口讀取到雷達(dá)所傳回的環(huán)境深度信息，然后在建圖節(jié)點中實時建立二維柵格地圖，并利用實時更新地圖節(jié)點不斷保存更新所建地圖。接著，將柵格地圖信息傳送到ROS系統(tǒng)中內(nèi)嵌的導(dǎo)航算法包(Navigation)進行實時路徑規(guī)劃，再將所計算出的里程計信息給到底層控制節(jié)點(STM32)，進行小車運動控制，同時讀取底層里程計信息，構(gòu)成閉環(huán)控制。最后，在PC端利用遠(yuǎn)程登陸到樹莓派并在rviz可視化環(huán)境下進行人機交互，可以在PC端設(shè)置目標(biāo)點，觀察和記錄小車以及外部環(huán)境。

本文所用的小車平臺由一個可解讀里程計信息的底盤、一個激光雷達(dá)和一臺Linux嵌入式計算機構(gòu)成，如圖2所示。激光雷達(dá)所使用的是杉川Delta-2A，其量程為8 m，掃面頻率6.2 Hz。所使用的小車主控制機器為Raspberry Pi 4B+，8 GB內(nèi)存版本。采用Ubuntu18.04操作系統(tǒng)，并配置了ROS。

圖2 小車平臺實物

2 基于Hector算法建圖

Hector無需里程計，通過掃描匹配的方法進行地圖創(chuàng)建，基于高斯牛頓法以求其最優(yōu)解，大大減少了算法所需的計算量。

掃描匹配用當(dāng)前雷達(dá)所傳回的一幀數(shù)據(jù)與以往所建地圖數(shù)據(jù)構(gòu)造誤差函數(shù)：

(1)

式中，ξ=(Px,Py,ψ)T是運動小車在世界坐標(biāo)系中xoy平面的位姿，Px和Py是在世界坐標(biāo)系的二維橫縱坐標(biāo)，ψ是以世界坐標(biāo)系y軸為參考的旋轉(zhuǎn)角度,ξ為上一時刻運動小車的位姿或初始位姿；ξ*為當(dāng)前時刻最優(yōu)位姿。經(jīng)激光掃描所體現(xiàn)外部深度信息的點云坐標(biāo)轉(zhuǎn)化到世界坐標(biāo)系后的坐標(biāo)為：

(2)

式中，si=(si,x,si,y)T是激光掃描端點在雷達(dá)自身坐標(biāo)系下的坐標(biāo)，si下標(biāo)序號代表激光點標(biāo)簽，可由激光雷達(dá)采集的原始數(shù)據(jù)的極坐標(biāo)轉(zhuǎn)化而來。其中右側(cè)第1個矩陣為旋轉(zhuǎn)矩陣，第3個矩陣為平移矩陣，此時用一組旋轉(zhuǎn)與平移即可完全描述出物體的運動，函數(shù)M(Pm)代表坐標(biāo)點Pm在占用柵格地圖中的占用值，式(1)中坐標(biāo)點Pm為Si(ξ)的占用值，其值越大匹配效果越好，算法核心示意如圖3所示。

圖3 Hector算法核心示意

在Hector算法中，坐標(biāo)變換則顯得無比重要。根據(jù)傳感器傳回第1幀圖像來確定世界坐標(biāo)系。通過所采樣的第n幀圖像依次通過雷達(dá)運動來推算出每一幀圖像的坐標(biāo)變換，通過特征點檢查和疊加建立出完整的地圖，如圖4所示。

(a)正常的室內(nèi)二維地圖

利用Hector算法在教室中(矩形無障礙區(qū)域)進行建圖實驗，可以發(fā)現(xiàn)，在不使用里程計的情況下，當(dāng)傳感器有大幅度角度運動或行進速度過快時，圖像會出現(xiàn)運動畸變且Hector算法沒有地圖回環(huán)，一旦定位丟失，Hector算法無法重新定位，也無法繼續(xù)建圖。

3 基于Gmapping算法建圖

Gmapping是基于濾波SLAM框架的常用開源算法。利用粒子濾波來實現(xiàn)定位[3]和建圖，在室內(nèi)或較小環(huán)境中的建圖效果更好。粒子濾波是基于蒙特卡羅方法的小車定位算法，其缺點是需要很多的樣本，算法復(fù)雜度很高，所需計算量較大，但卻可以適應(yīng)不同的誤差分布方式。

在SALM中要解決的問題就是由控制數(shù)據(jù)和觀測數(shù)據(jù)來求位姿和地圖的聯(lián)合分布。即先進行定位再進行建圖。此算法會將第1幀所得到的數(shù)據(jù)作為小車的初始點，然后小車進行移動時用所接收到的雷達(dá)數(shù)據(jù)與上一時刻所建的地圖進行匹配，這樣可以對小車定位，用此時小車的位置可以建立出這一時刻的地圖。這樣重復(fù)下去即可一幀一幀地迭代建立出完整的地圖。算法核心流程如下：

步驟1：輸入上一時刻的粒子群、最近時刻的里程計和雷達(dá)掃描信息，同時初始化當(dāng)前時刻的粒子群。

步驟2：遍歷并獲取上一時刻粒子群中粒子所攜帶的位姿、權(quán)重和地圖信息。

步驟3：通過里程計進行位姿更新，用極大似然估計法可以求得局部極值。如果沒有找到局部極值則提議分布。

步驟4：更新粒子位姿狀態(tài)，然后使用觀測模型對位姿權(quán)重更新。若找到局部極值在其附近取k個位姿并認(rèn)為這k個位姿服從高斯分布。

步驟5：計算這k個位姿的權(quán)重與均值，將均值歸一化后，求出k位姿的方差并將方差歸一化。

步驟6：使用多元正態(tài)分布計算出新的位姿，計算該位姿粒子的權(quán)重。

步驟7：更新地圖，更新粒子群并循環(huán)重復(fù)計算上一時刻的每一個粒子。

步驟8：計算所有粒子權(quán)重的離散程度，并判斷是否超過閾值，是否進行重采樣。

在PC端中的rviz可視化環(huán)境下，可以觀察到建立的地圖，如圖5所示。

(a)里程計不精準(zhǔn)時建圖效果

在實驗中，使用Gmapping算法對教室進行建圖。從實際結(jié)果可以看出，在有里程計信息的(矩形無障礙區(qū)域)情況下，當(dāng)里程計不精準(zhǔn)時建圖效果并不理想，在地圖的后端與前端發(fā)生重影現(xiàn)象，但是可以精確計算里程計時(矩形有障礙區(qū)域)是可以建立精準(zhǔn)地圖的。由此可知，Gmapping算法嚴(yán)重依賴?yán)锍逃?，而無法適應(yīng)復(fù)雜的地面環(huán)境，且此算法在沒有里程計的情況下是無法建立地圖的。

4 導(dǎo)航效果對比

基于上述2種算法在不同的環(huán)境中，使用ROS中內(nèi)嵌的導(dǎo)航算法包(Navigation)進行導(dǎo)航實驗，其中導(dǎo)航包要求輸入小車定位、地圖信息、里程計信息和雷達(dá)測量的點云數(shù)據(jù)以及小車、雷達(dá)與世界坐標(biāo)系的轉(zhuǎn)換關(guān)系。有了這些必需信息，導(dǎo)航包即可提供全局與局部的路徑規(guī)劃，從而實現(xiàn)導(dǎo)航。

在配置好導(dǎo)航包后，分別在路面平整與路面不平整的情況下進行建圖導(dǎo)航實驗，實驗效果如圖6所示。

(a)自由探索時最佳路徑

經(jīng)實驗驗證，可以建立出精準(zhǔn)地圖并成功在圖中定位小車位置，且做出最優(yōu)路徑規(guī)劃(圖6中箭頭指向的綠線)。當(dāng)自由探索時，小車可以自動按照當(dāng)前地圖邊界向外界擴展，并實時更新路徑，當(dāng)規(guī)定目標(biāo)點時，小車可以進行避障，且在此基礎(chǔ)上尋找最優(yōu)路徑。

5 結(jié)束語

通過2種算法進行建圖導(dǎo)航，所設(shè)計的系統(tǒng)不僅成本較低，結(jié)構(gòu)簡單，而且擴展性以及適用性較強。2種方法各有優(yōu)缺點，其中Gmapping算法嚴(yán)重依賴?yán)锍逃嫞瑹o法適應(yīng)無人機在三維情況下飛行時的定位建圖及地面不平坦的區(qū)域，無回環(huán)即無法重新定位。在室外大場景粒子較多的情況下，非常消耗計算資源，很依賴計算力會造成在實際應(yīng)用中的成本過高，其更加適合于路面情況不復(fù)雜的巡檢工作。Hector算法基于解最小二乘問題，只用求出函數(shù)極值而不需要里程計，而且可以適應(yīng)空中，手持實時記錄地圖信息或者地面不平坦的情況，但缺點在于對于雷達(dá)掃描頻率和幀率要求很高(40 Hz)，并且需要估計三維空間下的6自由度機器人的位姿，這使得算法復(fù)雜度大幅增加。