基于優化Hector-SLAM算法的機器人自主導航系統設計

汪建華，黃 磊，石雨婷，張曉倩，祁良劍

（南京林業大學 機械電子工程學院，江蘇 南京 210037）

自主移動機器人必須具備自主導航和路徑規劃能力，而構建環境地圖是實現路徑規劃的前提[1-5]。SLAM（simultaneous localization and mapping，實時定位與建圖）是機器人自主導航中的一項關鍵技術[6-7]，其是指機器人在未知環境中移動時，利用傳感器定位自身位姿，并基于位姿增量式構建地圖。近年來，隨著激光雷達和中央處理器的性能不斷提升，學者們通過不斷改進和優化SLAM算法來滿足這些設備的性能需求。支奕琛等[8]提出了一種基于ZBar 算法和融合Cartographer-SLAM 與Hector-SLAM算法的方法，以實現對二維碼目標點的識別和標記。該方法先采用ZBar算法識別機器人的二維碼信息，再采用Cartographer算法生成Geotiff地圖，最后采用Hector-SLAM 算法在地圖上繪制目標點。經實驗驗證，該方法能識別遠距離目標點的信息且精度較高，但對目標點的預估定位存在誤差。Guan等[9]提出了一種集成Gmapping建議分布與Kullback-Leibler散度的粒子數適應算法。經實驗驗證，該算法能夠采用較少的粒子實現較高的定位精度，但是其對單個粒子的計算負載較大，所耗費的計算時間較長。蘇易衡等[10]提出了一種適用于低端激光雷達的優化Hector-SLAM算法：先采用插值法獲取高精度的柵格地圖，再通過圖像相減與矩陣變換來減少系統對激光雷達刷新率的依賴，最后對SLAM中的關鍵幀判定機制進行優化，以消除旋轉時所獲取數據的干擾。經實驗驗證，該算法在搭載低精度激光雷達條件下也能構建清晰的地圖。周燕[11]提出了一種利用卡爾曼濾波算法將Hector-SLAM與人工地標視覺定位結果相融合的組合定位算法。該算法可抑制Hector-SLAM所導致的累積誤差，削弱人工地標視覺定位引發的跳變異常，提高了定位精度。

綜上所述，二維SLAM算法已被廣泛應用于實際場景。目前，常用的激光SLAM算法包括Gmap‐ping、Karto-SLAM、Cartographer 和Hector-SLAM等。其中，Hector-SLAM 是一種基于高斯-牛頓（Gauss-Newton）掃描匹配的算法[12]，該算法在構建地圖時不需要里程計數據，更適用于無人機以及行駛在不平坦地面上的車輛的定位和建圖，但其對激光雷達的精度和刷新率的要求較高。例如：美國Innovusion公司研制的獵鷹（Falcon）激光雷達，其探測距離高達500 m，測距精度為±2 cm，角分辨率為0.05°×0.05°，每秒發出的激光點云數高達100萬個，但成本高昂；上海思嵐科技有限公司面向低端消費電子產品推出了非接觸式激光掃描測距雷達（型號為RPLIDAR-A1），其探測距離為0.15～12 m，最高測距精度為±3 cm，在5～12 m時的測距精度為實際距離的2.5%，角分辨率為1°，每秒發出的激光點云數僅為8 000 個。顯然，RPLIDAR-A1 激光掃描測距雷達的精度遠遠不及獵鷹激光雷達，但勝在價格低廉。一般情況下，激光雷達的精度越高，價格越昂貴，在實際應用中會受到限制。但Hector-SLAM算法在使用低精度激光雷達數據建圖時會產生大量累積誤差，在導航定位時易引發故障。針對上述問題，筆者擬作如下改進：1）優化Hector-SLAM算法中的插值函數，即采用雙三次插值法構建高精度的柵格地圖，以提高掃描匹配的精準度；2）采用擴展卡爾曼濾波算法對里程計和慣性測量單元（inertial measurement unit, IMU）的數據進行融合，通過組合定位方法來降低漂移效應引起的累積誤差，以提高定位的準確性；3）提出里程計輔助法與PL-ICP（point to line iterative closest points，點到線迭代最近點）配準法相融合的方法，以實現運動估計過程的快速收斂，從而達到校正運動畸變的目的；4）設置IMU 的傾斜角閾值，以消除機器人旋轉時出現的地圖重影現象。在此基礎上，利用優化的Hector-SLAM算法設計一種低成本、高性能的自主導航系統，并以AGV（automated guided vehicle，自動導引車）為例開展建圖實驗和自主導航實驗，以驗證系統的可行性。

1 優化的Hector-SLAM算法

傳統Hector-SLAM 算法的建圖過程分為3個階段[13]：占據柵格地圖、掃描匹配和多分辨率地圖映射。具體流程如下：首先，利用激光雷達獲取環境信息，并采用雙線性插值法對離散數據進行處理，以得到連續的柵格地圖；然后，將激光雷達獲取的實時數據與已知地圖進行對齊匹配，構造誤差函數，并采用高斯-牛頓法求解最優位姿與偏差；最后，采用多種分辨率不同的柵格地圖對激光點云進行映射，以避免匹配結果陷入局部最小值。

1.1 柵格地圖占據

柵格地圖[14-17]可有效反映環境信息，其將外部環境劃分為多個尺寸相同的柵格，每一個柵格具有3種狀態：空閑、占用或未知。在占據柵格地圖時，通過數值0 和1 來表示柵格單元被障礙物占據的概率，數值0 表示沒有障礙物，數值1 表示出現障礙物。由于Hector-SLAM算法構建的柵格地圖是連續的，且掃描匹配也需要連續的數據進行偏導運算，但激光雷達獲取的是離散數據，因此須采用插值法對離散數據進行處理。傳統的Hector-SLAM算法采用雙線性插值法完成柵格地圖的構建，但該插值法僅考慮采樣點周圍4個直接相鄰點的像素值變化對采樣點的影響，具有低通濾波器的性質。此外，采用雙線性插值法計算后，圖像會丟失高頻分量，導致邊緣不清晰。為彌補上述缺陷，本文采用雙三次插值法，將采樣點所有鄰近點的像素值變化的影響均考慮在內。同時，由于低精度激光雷達的掃描頻率較低，其獲取的可用數據較少，采用雙三次插值法進行處理，可降低數據量不足對建圖精度的影響，獲得梯度和精度更高的柵格地圖，從而達到減少系統噪聲、降低邊緣誤差以及提高掃描匹配準確度的目的。

如圖1所示，雙三次插值法通過對采樣點周圍16個鄰近點的像素值進行加權平均處理來獲得采樣點的像素值[18]。其中：采樣點的坐標為(x+i,y+j)，其中x、y為整數部分，i、j為小數部分。

圖1 雙三次插值模型示意Fig.1 Schematic of bicubic interpolation model

雙三次插值的基函數S(x)可表示為：

式中：a為待定系數。

經多次實驗后發現，參數a取-0.5 時的插值效果最佳。利用雙三次插值法進行計算，可得采樣點的像素值f(x+i,y+j)：

其中：

柵格地圖中插值函數的改變會導致高斯-牛頓法掃描匹配所使用的地圖梯度發生改變。對式(2)求X、Y方向的偏導，可得：

將A、B、C代入式(3)中的第1個方程，化簡可得：

同理，將式(4)中的i改為j即可求得Y方向的偏導函數。

1.2 掃描匹配

獲得柵格地圖后，將激光雷達采集的實時數據與已知的柵格地圖進行對齊匹配[19]。掃描匹配的原理是利用最小二乘法求解機器人的最優位姿ξ*：

其中：

式中：ξ=[PxPyψ]Τ為機器人在世界坐標系的二維平面XOY內的初始位姿，其中Px、Py、ψ分別為機器人位置的橫、縱坐標和旋轉角度；M(Sq(ξ))為障礙概率；Sq(ξ)為機器人在世界坐標系中的位置；Sq,x、Sq,y為激光點在激光雷達坐標系中的位置坐標。

機器人在當前時刻的最優位姿ξ*與上一時刻初始位姿ξ存在如下關系：ξ+Δξ=ξ*。若要減小激光掃描當前幀與柵格地圖對齊過程中的誤差，則要確保所有激光點在柵格地圖中的總占用偏差盡可能小，即Δξ應滿足以下條件：

將式(6)在Δξ處泰勒展開并求偏導，可得：

其中：

式中：H為Hessian矩陣；?M(Sq(ξ))為地圖梯度。

1.3 運動畸變校正

在跟隨機器人運動的過程中，激光雷達每一幀數據中的激光點并不是在同一位姿下瞬時獲得的，即不同激光點的坐標不一致。但激光雷達在封裝每一幀數據時，默認該幀數據中的所有激光點均是在同一時刻、同一位姿下采集得到的，這會導致運動畸變[20]。當激光雷達的掃描頻率較低時，激光束之間的誤差較大，點云數據會出現“變形”現象。本文所使用的激光雷達的掃描頻率為5.5 Hz，每一幀數據的掃描用時約為180 ms。設置機器人的線速度為0.5 m/s，每采集一幀數據后，機器人移動0.09 m。由此可知，本文所搭載的激光雷達在實際運動過程中的運動畸變不可忽略。

在機器人快速移動的過程中，里程計易發生位置漂移以及無法有效檢測滑動，而IMU的輸出速率較快，測量誤差會隨時間累積，無法實現長時間的精準定位[21]。為了減小累積誤差，采用擴展卡爾曼濾波算法對里程計和IMU 的數據進行融合濾波處理。

假設t時刻機器人通過IMU 獲得加速度at，由此可得t時刻機器人的速度：

式中：vxt和vyt、axt和ayt分別為t時刻機器人沿X、Y方向的速度和加速度。

則在t時刻，由IMU 獲得的機器人的狀態向量X1t可表示為：

式中：ωt為IMU坐標系下機器人的Z向角速度。

在t時刻，由里程計獲得的機器人的狀態向量X2t可表示為：

式中：xt、yt為t時刻機器人沿X、Y方向的位移；θt為t時刻機器人的旋轉角度。

在利用里程計和IMU 獲取機器人的狀態向量后，將基于里程計數據的機器人軌跡推導過程視作系統狀態方程的計算過程，以IMU和里程計的輸出結果作為更新方程的輸入，在擴展卡爾曼濾波算法的基礎上進行觀測更新，以實現對里程計與IMU的位姿數據的融合。由此可得，t時刻機器人的狀態向量Xt可表示為：

鑒于機器人所搭載的里程計為雙輪差速里程計，在t至t+1 時刻，機器人的狀態轉移矩陣可表示為：

式中：W0為機器人的狀態噪聲，服從高斯分布，即W0～N(0,Q0)，其中Q0為預測狀態下的協方差矩陣。

則機器人的狀態觀測模型更新如下：

式中：Zt為機器人狀態更新矩陣；Ht為機器人的狀態觀測矩陣；V0為觀測噪聲，服從高斯分布，即V0～(0,R0)，其中R0為觀測噪聲的協方差矩陣。

在此基礎上，按照擴展卡爾曼濾波融合流程，計算卡爾曼增益并更新機器人狀態及對應的協方差矩陣，具體公式如下：

其中：

式中：Ft為雅克比矩陣，Kt為卡爾曼增益，h(Xt)為機器人狀態測量模型，Pt/t為狀態向量的協方差矩陣，I為單位矩陣。

通過擴展卡爾曼濾波算法對里程計和IMU的數據進行融合后，有效減小了誤差，解決了機器人在轉彎時檢測不精準的問題，但這遠遠無法達到機器人定位與導航精度的要求。因此，須結合里程計輔助法與PL-ICP配準法來對機器人的位姿進行進一步標定，以實現運動畸變的校正。首先，利用里程計中的編碼器獲取機器人在當前時刻與上一時刻的位姿差，并將該位姿差作為PL-ICP配準法的輸入初始值，以獲取機器人在當前時刻的對應位姿。然后，基于當前時刻下里程計獲取的位姿差和PL-ICP配準法計算得到的位姿，構建位移誤差方程。最后，將位移誤差均分至下一時刻的機器人位姿，以修正其位姿差，并再次使用誤差方程對修正后的位姿差進行多次迭代計算，不斷縮小機器人當前時刻與上一時刻的位移誤差。當誤差函數收斂或達到最大迭代次數時，迭代終止，最終獲得里程計標定的位姿真值。

1.4 地圖重影優化

由于機器人的質量較小，其重心在運動過程中會因地形變化而上移，使得運動姿態不穩定，這會對激光雷達的掃描產生干擾，造成地圖重影。另外，由于本文采用的是二維激光雷達，在構建地圖時將實際環境轉換到二維平面上時可能會出現多平面重疊在同一位置處的情況，同樣會造成地圖重影。與此同時，常用的特征匹配算法的魯棒性也會影響單應性矩陣的估計誤差，導致圖像變換不準確，使得圖像拼接時在重疊區域出現明顯的對齊誤差，進而造成地圖模糊[22]。除此之外，當機器人作旋轉運動時，由于數據更新較慢，掃描匹配時會存在時間誤差，也會造成地圖重影。針對上述問題，可通過以下方式來消除地圖重影：

1）增大機器人的質量，以減少地形對機器人運動姿態的干擾。

2）融合里程計和IMU 的數據，以減小因機器人打滑而導致的里程計測量誤差，同時減小機器人運動過程中產生的漂移誤差，從而提升定位精度。

3）在IMU 中設置傾斜角閾值，本文設傾斜角閾值為45°，當傾斜角大于45°時，系統自動忽略激光雷達采集的數據，進而過濾在機器人傾斜角過大情況下所采集的數據。

2 AGV驗證實驗

基于優化的Hector-SLAM算法，設計自主導航系統，并以AGV為例，配合Rviz 三維可視化平臺對系統的可行性進行驗證[23]。本次實驗中所使用的AGV的軟硬件設備如表1所示。

表1 AGV的軟硬件設備Table 1 Hardware and software equipment of AGV

AGV底盤及其自主導航系統搭載的激光雷達的實物分別如圖2和圖3所示。

圖2 AGV底盤實物圖Fig.2 Physical diagram of AGV chassis

圖3 AGV自主導航系統搭載的激光雷達Fig.3 Lidar equipped on AGV autonomous navigation system

2.1 建圖實驗

在ROS（robot operating system，機器人操作系統）中訂閱“/scan”激光雷達數據命令話題，由此發布AGV的位姿和柵格地圖信息。隨后，啟動launch腳本功能，以構建相應的柵格地圖。優化前后Hector-SLAM算法的建圖效果對比如圖4所示。

圖4 Hector-SLAM 算法優化前后的建圖效果對比Fig.4 Comparison of mapping effects before and after Hector-SLAM algorithm optimization

由于激光雷達的精度不高，且里程計和IMU在實際應用時會產生漂移誤差，因此激光雷達的各掃描幀之間偏移量會不斷增大。觀察圖4(a)可知，采用優化前Hector-SLAM算法建圖時，地圖左上方出現了重影，同時存在大量噪聲，導致邊緣處出現毛刺現象。觀察圖4(b)可知，Hector-SLAM 算法優化后，地圖左上方的重影和邊緣的毛刺現象得到了明顯改善，地圖邊緣更加清晰。由此說明，優化后的Hector-SLAM算法有效地提高了建圖精度。

基于優化后的Hector-SLAM算法，選取南京林業大學教九樓（簡稱南林教九樓）二樓和逸夫科技實驗樓（簡稱南林逸夫樓）三樓的走廊為實驗場地，開展建圖實驗。具體流程如下：首先，將AGV放置在走廊地面的某一位置處，以放置位置作為AGV的運行起點；然后，調用“roslaunch bringup.launch”命令，運行ROS 中的多個節點；接著，調用“ro‐slaunch lidar_slam.launch”命令，啟動激光雷達；最后，利用鍵盤控制命令來控制AGV的移動。由于AGV速度過快時，其驅動輪會出現打滑現象，影響建圖效果，因此設置AGV的最大線速度為0.5 m/s，角速度為0.3 rad/s，以保證建圖精度。將構建好的地圖保存到固態硬盤中，等待系統調用。

南林教九樓二樓走廊為封閉的“日”字形區域，其在百度地圖中的外圍輪廓如圖5所示，對應的自主導航系統建圖結果如圖6所示。

圖5 百度地圖中的南林教九樓二樓走廊Fig.5 Second floor corridor in Ninth Teaching Build‐ing of NJFU in Baidu map

圖6 南林教九樓二樓走廊建圖結果Fig.6 Mapping results of second floor corridor in Ninth Teaching Building of NJFU

南林逸夫樓三樓走廊呈扇形，其在百度地圖中的外圍輪廓如圖7所示，對應的自主導航系統建圖結果如圖8所示。

圖7 百度地圖中的南林逸夫樓三樓走廊Fig.7 Third floor corridor in Yifu Building of NJFU in Baidu map

圖8 南林逸夫樓三樓走廊建圖結果Fig.8 Mapping results of third floor corridor in Yifu Building of NJFU

為了定量分析基于優化Hector-SLAM 算法的AGV自主導航系統在真實環境中的映射精度，選取教九樓二樓的東西走廊和南北走廊的長度為例，來驗證優化后Hector-SLAM算法的建圖精度，并與優化前Hector-SLAM算法進行對比。

本文實驗采用成都景瑞特科技有限公司研制的高精度激光測距儀（最大探測距離為150 m，誤差為3 mm）對教九樓二樓的走廊長度進行多次測量，并求平均值作為最終的實測值。通過測量和計算可得，教九樓二樓東西走廊的長度約為46 m，南北走廊的長度約為91 m?；谠摂祿?，使用Visio 軟件等比例繪制教九樓二樓走廊示意圖，各走廊分別用編號a、b、c、d表示，如圖9所示。

圖9 南林教九樓二樓走廊示意圖Fig.9 Schematic of second floor corridor in Ninth Teaching Building of NJFU

使用Rviz平臺中的測量函數獲取圖6所示地圖中各走廊的長度（同樣是多次測量并求平均值），并與利用激光測距儀獲取的走廊長度實測值進行比較，結果如表2所示。從表2中可以看出，Hector-SLAM算法優化前，AGV自主導航系統的平均建圖相對誤差為0.50%左右；Hector-SLAM 算法優化后，系統的平均建圖相對誤差約為0.44%，最小建圖誤差僅為0.236 m，相較于優化前減小了0.041 m，說明建圖精度明顯提升，驗證了本文算法的優化是可行的。

表2 優化前后AGV自主導航系統的建圖誤差對比Table 2 Comparison of mapping errors of AGV autonomous navigation system before and after optimization

2.2 自主導航實驗

本文分別使用A*算法和DWA（dynamic win‐dow approach，動態窗口法）來實現AGV的全局路徑規劃[24]和局部路徑規劃[25]。根據2.1 節構建的地圖，利用Rviz平臺開展自主導航實驗。由于教九樓的走廊比逸夫樓的長，且教室和透明玻璃更多，更能考驗AGV在復雜環境下的自主導航性能，因此在教九樓中開展自主導航實驗。在導航實驗開始前，將AGV放置在教九樓二樓走廊的地面上，并將此時AGV的位姿作為初始位姿；同時，根據AGV在真實環境中的位姿，調整Rviz 平臺中AGV的初始位姿，使其與實際位姿一致，如圖10所示。

圖10 Rviz平臺中AGV自主導航的起點Fig.10 Starting point of AGV autonomous navigation in Rviz platform

為了驗證所設計的AGV自主導航系統的定位精度和避障準確度，本次實驗共設置4 個目標點（第4 個目標點與起點重合）和多個障礙物，控制AGV從圖10所示的起點出發，按照A*算法規劃的路徑前往不同目標點，AGV沿地圖運行一周，最終返回至起點，具體過程如圖11所示。由圖11可知，AGV能夠按照A*算法規劃的最優路徑準確前往各個目標點。

圖11 Rviz平臺中AGV的自主導航過程Fig.11 Autonomous navigation process of AGV in Rviz plat‐form

在實際場景中，AGV自主導航時可能會發生緊急情況，如出現行人或障礙物。為避免AGV因緊急情況出現而影響正常運行，自主導航系統會調用DWA來重新規劃路徑，以躲避行人和障礙物，保證AGV最終到達目標點。通過2 次實驗來驗證AGV自主導航系統的緊急避障性能，結果分別如圖12和圖13所示。

圖12 實際場景中AGV的避障過程Fig.12 Obstacle avoidance process of AGV in actual scenario

圖13 Rviz平臺中AGV的避障過程Fig.13 Obstacle avoidance process of AGV in Rviz platform

從圖12 和圖13 中可以看出，當行人突然出現在AGV的前方時，自主導航系統能及時調用DWA來重新規劃AGV的行駛路徑，保證其在躲避行人和障礙物后準確到達目標點，實現自主導航功能。

3 總 結

本文對搭載低精度激光雷達的機器人自主導航系統進行了優化，解決了機器人在使用傳統Hector-SLAM算法時存在的建圖不清晰、定位不精準等問題。首先，利用雙三次插值法代替雙線性插值法來提高數據的連續性，使高斯-牛頓法掃描匹配地圖的精準度得以提升。然后，采用擴展卡爾曼濾波算法對里程計與IMU的數據進行了融合；同時，將里程計輔助法和PL-ICP配準法相結合，以使機器人的位姿標定更加準確，提高了定位的準確性。最后，通過設置IMU傾斜角閾值，消除了機器人旋轉時產生的地圖重影現象。實驗結果表明，Hector-SLAM算法優化后，自主導航系統的最小建圖誤差相比于優化前減小了0.041 m，其平均建圖相對誤差低于0.50%，建圖精度大大提升。但是，當建圖環境較大時，所設計的自主導航系統的建圖與定位效果仍存在不足，在后續研究中可考慮使用高精度的激光雷達來進一步提升系統的有效性。