基于2D建圖算法的導(dǎo)航方法

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

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

0 引言

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

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

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

1 小車硬件平臺(tái)設(shè)計(jì)

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

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

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

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

圖2 小車平臺(tái)實(shí)物

2 基于Hector算法建圖

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

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

(1)

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

(2)

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

圖3 Hector算法核心示意

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

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

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

3 基于Gmapping算法建圖

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 結(jié)束語

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