周凱月，張建偉

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

0 引言

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

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

1 相關研究

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

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

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

2 融合反光柱的2D SLAM系統(tǒng)

我們的SLAM 和純定位系統(tǒng)建立在Cartographer基礎上，Cartographer 實現(xiàn)了 SDF 建圖方法[17]，我們采用Cartographer SDF 地圖表示形式。我們?nèi)诤戏垂庵募す釹LAM 系統(tǒng)框圖如圖1 所示。

圖1 融合反光柱的激光SLAM系統(tǒng)框圖

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

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

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

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

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

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

（5）對于 2-5 步，重復執(zhí)行 3-5 次，直到 Σ 中所有值小于10-4，或者Σ 的逆變得不穩(wěn)定，或者內(nèi)點的數(shù)量小于4。

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

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

圖2 融合反光柱的激光SLAM系統(tǒng)位姿圖示意圖

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

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

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

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

4 實驗

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

圖3 融合反光柱SLAM系統(tǒng)仿真建圖結(jié)果

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

圖4 融合反光柱的真實場景建圖結(jié)果

圖5 重復定位精度對比

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

5 結(jié)語

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