基于2D建圖算法的導航方法

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

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

0 引言

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

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

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

1 小車硬件平臺設計

首先需建立一輛智能監測系統小車，設計小車的實時建圖與路徑規劃系統的結構框架如圖1所示。

圖1 系統結構

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

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

圖2 小車平臺實物

2 基于Hector算法建圖

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

掃描匹配用當前雷達所傳回的一幀數據與以往所建地圖數據構造誤差函數：

(1)

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

(2)

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

圖3 Hector算法核心示意

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

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

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

3 基于Gmapping算法建圖

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

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

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

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

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

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

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

步驟6：使用多元正態分布計算出新的位姿，計算該位姿粒子的權重。

步驟7：更新地圖，更新粒子群并循環重復計算上一時刻的每一個粒子。

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

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

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

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

4 導航效果對比

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

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

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

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

5 結束語

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