融合反光柱的2D 激光SLAM 和高精度定位系統

周凱月，張建偉

（四川大學視覺合成圖形圖像技術重點學科實驗室，成都 610041）

0 引言

現在，機器人已經廣泛應用在工生產和家居生活等環境中，例如工業生產中的AGV 和幫助家庭潔凈的掃地機等。機器人在未知環境中自主導航，首先需要依靠自身攜帶的傳感器構建環境的地圖。目前，研究人員提出了許多基于激光傳感器的2D SLAM 建圖系統，如 GMapping[1]、Hector SLAM[2]和 Cartographer[3]等。基于已經重建好的地圖，定位系統在環境中的位姿（此時只定位，不建圖）稱為純定位。在過去的幾十年中，研究人員提出了許多基于濾波的純定位方法，如AMCL[4]、NDT-MCL[5]和 Kalman Filter[6]等。在這些系統中，采用KLD 方法的AMCL 被廣泛使用。上述基于激光的2D SLAM 建圖和定位系統，在特征豐富的場景中，可以穩定地工作，但是，在幾何特征較少或者玻璃等特殊環境中，往往不能取得讓人滿意的效果。

為了提高2D 激光SLAM 系統的魯棒性和精度，我們提出了一個新的融合反光柱的2D 激光SLAM 和純定位。我們的系統建立在2D 激光SLAM 系統Cartographer[3]之上，我們采用TSDF 地圖表示重建的2D 空間[7]。在建圖時，我們在獲取的激光點中，根據反光柱的反射率分離反光柱上的點，并且將反光柱作為位姿圖中的節點，在位姿圖中和姿態節點以及激光關鍵幀節點一起優化。在定位時，不同于傳統的基于濾波的方法，我們基于系統生成的TSDF 柵格圖，采用聯合優化激光匹配誤差和反光柱匹配誤差的方式做純定位。對于系統輸入的每一幀激光幀，我們首先根據反光柱的反射率分離激光幀中的反光柱點，并且將反光柱點聚類得到每個反光柱的中心點坐標，然后，我們采用最近鄰算法將聚類得到的反光柱點和地圖中的反光柱節點做關聯，最后通過聯合優化反光柱的匹配誤差和激光點在地圖中的匹配誤差計算激光幀的位姿。實驗表明，建圖時，添加反光柱可以大幅度增強系統在幾何特征少或者玻璃等場景中定位的穩定性和精度；同時，在系統做純定位時，融合反光柱信息可以大幅度提高系統定位的精度。

1 相關研究

通常SLAM 系統用來構建未知環境的地圖，如果機器人工作在已經建好圖的環境中，這時只需要純定位模塊，純定位模塊作用是根據事先重建好的地圖，定位機器人在環境中的位姿和姿態。激光SLAM 系統通常通過將當前幀和地圖進行配準[2-3，7]，當前幀和地圖配準又稱為Scan-to-Map；或者將當前幀和上一幀配準的方式（Scan-to-Scan）計算激光幀的位姿[8]。目前，現有的2D 激光建圖系統通常采用概率地圖表示構建的環境[1-3]。Gmapping[1]采用粒子濾波的方式構建環境的地圖，粒子濾波系統根據其他傳感器（如里程計、IMU 等）的觀測產生可能的粒子狀態，并且通過匹配獲取的激光點和2D 地圖評價預測的粒子狀態，而從減小系統的定位和建圖誤差。Hector SLAM[3]和Cartographer[2]通過Scan-to-Map 的方式，通過優化激光點云在地圖匹配誤差計算激光幀位姿。為了增大定位優化的收斂范圍，Hector SLAM 構建多分辨率地圖，并且采用多層金字塔逐級優化。Cartographer[2]在建圖時構建子圖，并且采用分支定界的方式計算激光幀和子圖之間的閉環，并且采用位子圖優化減小系統的累計誤差。PL-ICP[8]是ICP 算法的一個變種，PL-ICP 算法首先在兩個激光幀找匹配點，然后通過優化匹配點的點到線距離計算位姿。

目前，柵格圖被廣泛應用在2D 機器人定位和導航中，在柵格圖中，系統重建的環境由固定大小的方格表示，每個方格存儲的值表示方格是障礙物的可能性。當前，基于柵格圖和AMCL 算法的定位系統是最常見的2D 定位系統，我們構建SDF 地圖，并且采用由SDF地圖生成的柵格圖做純定位，近年來SDF 地圖在基于RGB-D 的實時重建系統中被廣泛采用[15-16]。不同于傳統純定位系統中基于概率格子地圖生成的柵格圖，通過SDF 地圖生成的柵格圖，每個格子點保存的是格子到障礙物的距離。

通常，基于柵格圖和AMCL 的純定位系統，重復定位精度在2～5cm，Rowekamper 等人[14]提出的全局定位基于AMCL，局部到點定位基于PL-ICP 的算法，局部到點精度依靠PL-ICP 算法可以達到5mm。我們融合反光柱的定位系統，在玻璃等場景下，也可以達到5mm的定位效果。接下來，在第二節中，我們首先介紹融合反光柱的2D 激光SLAM 系統，然后再第三節中介紹融合反光柱的純定位系統。

2 融合反光柱的2D SLAM系統

我們的SLAM 和純定位系統建立在Cartographer基礎上，Cartographer 實現了 SDF 建圖方法[17]，我們采用Cartographer SDF 地圖表示形式。我們融合反光柱的激光SLAM 系統框圖如圖1 所示。

圖1 融合反光柱的激光SLAM系統框圖

對于輸入的激光幀，我們首先根據激光點的反射強度分離反光柱上的激光點，通常，反光柱上返回的激光點的反射強度比普通障礙物高很多，我們首先將激光點強度大于一定閾值（對于R2000，閾值設為1000）的激光點分離出來，并且插入到待處理的隊列Q1中，因為建圖系統插入到后端位姿圖中的激光幀節點是選擇性插入的（當機器人位移、角度或者時間大于一定閾值才插入新的節點），我們根據反光柱的時間戳，和位姿圖中激光節點的時間戳，判斷待處理的反光柱隊列中的反光柱，在激光節點中是否存在前后節點，如果存在前后節點，則根據反光柱的時間戳和前后激光節點時間戳，以及激光節點的位姿插值得到反光柱點的位置，并且將插值得到位置的反光柱點插入到隊列Q2中。

在構建地圖時，一幀激光幀可能可以觀測到多個反光柱，我們假設在擺放反光柱時，反光柱之間的距離大于一定閾值，實驗中大于1m。我們將隊列Q2中的反光柱點聚類，聚類得到每個反光柱上返回的點，聚類做法如下。假設聚類得到的各反光柱上的點為{Ci}，各反光柱的中心點坐標為{Mi}，我們依次處理Q2中的點，判斷當前處理的點和已經聚類點中心之間的距離，如果距離大于一定閾值(1m)，則將新處理的點初始化新的聚類點，如果當前處理的點和其中一個聚類中心Mi距離小于閾值，則將當前處理的點插入到Ci中，并且重新計算類的中心點坐標Ci，如此得到聚類后的各反光柱的中心點坐標點。

聚類得到各反光柱上點Ci以及各反光柱中心點坐標Mi后，因為激光點在障礙物深度不連續的地方可能產生外點，我們采用迭代聚類的方式去除外點，重新計算每個聚類的中心坐標Mi，方法如下：

（1）對于聚類Ci中的點，假設所有點均為內點，我們計算聚類Ci點的中心Mi。

（2）對于所有的內點計算坐標的均值μ和方差Σ。

（4）將高斯方程值小于閾值的點從內點中移除。

（5）對于 2-5 步，重復執行 3-5 次，直到 Σ 中所有值小于10-4，或者Σ 的逆變得不穩定，或者內點的數量小于4。

（6）最后，對于每個聚類Ci，計算點的中心點作為反光柱的最終位置。

聚類得到每個反光柱的中心點Mi后，聚類得到的反光柱在位姿圖中已有反光柱，按照距離找匹配，對于找到匹配的反光柱，將當前幀計算得到的反光柱中心作為一個的觀測；如果新聚類得到的反光柱在地圖中找不到匹配，則根據聚類得到的反光柱中心點，在位姿圖中初始化一個新的反光柱節點，位姿圖如圖2 所示，其中黑色圓圈表示激光幀節點，紅色圓圈表示子圖節點，五角星表示反光柱節點，黑色方框表示激光幀對于反光柱的觀測。

圖2 融合反光柱的激光SLAM系統位姿圖示意圖

3 融合反光柱的純定位方法

在純定位模式，我們采用聯合優化激光點在地圖中匹配誤差和反光柱匹配誤差計算位姿，我們采用由TSDF 地圖生成的柵格圖作為純定位的地圖，由SDF 地圖生成的柵格圖，每個格子點保存了格子到障礙物的距離為F(ci)，我們還同時保存了建圖時SDF 融合的權重W(ci)，我們聯合優化的目標函數表達式如下：

其中，di為當前幀激光點位置，Rj為激光幀聚類得到的反光柱中心點，P為待優化的激光幀位姿，Rj為根據估計的當前幀位姿計算的反光柱位置，Mj為地圖中匹配的反光柱節點位置。

在純定位模式，對于輸入的每幀激光幀，同章節2中的建圖流程，首先根據激光點的反射強度提取反光柱點，提取的反光柱點，根據上一幀位姿（或者融合里程計、IMU 等傳感器數據）預測的位姿，計算反光柱點在地圖中的位置，然后根據計算的位置，在地圖中選擇最近的反光柱節點作為匹配點。

4 實驗

我們首先基于仿真數據測試了融合反光柱的建圖系統，使用Robot Operating System（ROS）可視化得到的建圖結果如圖3 所示。圖中不同顏色的圓圈表示的是場景中的反光柱，白色部分為空白區域，灰色部分為有物體的遮擋區域，藍色線條為機器人運動軌跡。從圖中可以看出，我們的建圖系統工作良好，系統準確地構建出了反光柱在地圖中的位置。

圖3 融合反光柱SLAM系統仿真建圖結果

我們還測試了融合反光柱的純定位系統，首先在真實環境中布置反光柱，然后通過融合反光柱的建圖系統重建帶有反光柱的2D 地圖，建圖結果如圖4 中的（a）所示，因為構建的環境右上角存在玻璃，系統構建的地圖右上角SDF 地圖雜亂，在此區域的定位效果較差，融入反光柱以后因為有了額外的約束，會顯著提升此區域的定位結果。

圖4 融合反光柱的真實場景建圖結果

圖5 重復定位精度對比

基于建圖測試的機器人重復定位精度如圖4 所示。重復定位首先在建圖結果中指定兩點，然后讓機器人從一點移動到另一點，通過到達指定點的誤差來判斷重復定位精度，圖中是重復測量30 次重復定位機器人到達終點的落點分布，尺度為mm，可以看到，如圖5（a）所示，在沒有融合反光柱的情況下，機器人的重復定位落點誤差在20mm 的范圍內，在融合了反光柱以后（圖5（b）），重復定位落點誤差縮小到了5mm。

5 結語

在本文中，我們在現有的2D 激光SLAM 系統Cartographer 基礎上，將反光柱融合進其中，在系統的建圖和定位過程中加入反光柱約束，進一步優化機器人位姿。仿真以及試驗結果表明，本系統在無幾何特征以及玻璃等場景下具有更高的魯棒性和精度，具有很強的實用價值。