移動機器人CartographerSLAM 建圖改進研究

趙 蕓，趙 敏

（上海理工大學光電信息與計算機工程學院，上海 200093）

0 引言

即時定位與建圖（Simultaneous Localization and Mapping，SLAM）最早由Smith 等［1］在1988 年提出，它是機器人技術研究方向。機器人在應用中能自己移動的前提是需要掃描當下環境并生成地圖。實現機器人SLAM 需要知道其在當前地圖中的姿勢和位置。因此，SLAM 是移動機器人領域的一個關鍵問題。

早期SLAM 研究多用激光雷達作為主要傳感器，將擴展卡爾曼濾波（EKF）應用于機器人位姿估計，但效果不理想。對于一些強非線性系統，這種方法會帶來更多的截斷誤差，從而導致定位和映射不準確。2007 年，Grisetti 等［2］提出改進的Rao-Blackwellized particle filter（RBPF）激光雷達Gmapping 方法，這在SLAM 領域堪稱里程碑通過改進所提出的分布和自適應采樣技術，提高了定位精度，降低了計算復雜度；優化方法作為概率方法的一種有效替代，近年得到廣泛應用；2010 年Konolige 等［3］提出一種具有代表性的方法Karto-SLAM，該方法利用稀疏位姿調整解決非線性優化中的矩陣直接求解問題；2011 年提出的Hector SLAM［4］采用高斯—牛頓法解決掃描匹配問題，該方法不需要里程表信息，但需要高精度的激光雷達；2016 年，谷歌提出Cartographer［5］，通過對子圖和全局圖同時使用激光閉環，減少累積誤差。

本文針對Cartographer SLAM 算法在回環匹配時使用Scan to Map（幀與子圖）方法容易在環境相似的地方或者長距離走廊等處出現匹配出錯高的缺點，提出改進方法，將Scan to Map 用Map to Map（子圖與子圖）方法替代，優化Cartographer SLAM 算法構建更精確的地圖；闡述了Map to Map 方法理論，利用實驗對比改進前后算法，對實驗結果進行分析。

1 Cartographer SLAM 算法

在分析Cartographer SLAM 算法之前先分析基于圖優化的SLAM 框架原理。圖優化的SLAM 是通過后端的回環檢測對前端位姿進行調整，最終得到當下最接近真實值的機器人位置和姿勢［6］。

圖1 是基于圖優化SLAM 的架構［7］，主要包括前端和后端兩個部分，其中數據關聯和閉環檢測由前端完成。數據關聯主要通過傳感器對機器人運動過程中的環境進行觀測獲得觀測值，對獲得的觀測值和已經在地圖中存在的特征值進行對比，如果是新的環境特征則將其添加到建立的地圖中，如果與已建立的地圖中的特征符合則通過對比不斷修正特征，解決匹配前幀和后幀問題并估計相關位置和姿勢，把控局部數據關聯［8］。閉環檢測利用傳感器獲得觀測量，比較當前位姿與之前位姿是否匹配，并不斷優化位姿，把控全局數據關系。

Fig.1 SLAM algorithm framework based on graph optimization圖1 基于圖優化的SLAM 算法框架

其中位姿及約束關系如圖2 所示。

Fig.2 Representation method of transforming robot pose into graph圖2 機器人位姿轉化為圖的表示方法

向量X=(xi,xj,…)T是圖中顯示點集，表示機器人位姿序列。其中，xi，xj為i節點和j節點的位姿；zij為i節點和j節點的相對位姿，表示不同時刻下的關聯信息；Ωij為i節點和j節點的信息矩陣；fij(x)為無干擾理想環境測量函數值；eij(x)為預測量與測量之間的差量，即誤差函數值。

設eij(x)遵守高斯分布，Fij(x)即目標函數，可用如下公式表示：

由公式（1）可知，當存在節點x*令目標函數Fij(x)取到最小值，則此時的解為最優，用公式表示如下：

因此，圖優化的SLAM 是將其變成求最優解，Cartographer SLAM 則通過對公式求解得到優化后的位姿。

Cartographer SLAM 是谷歌的開源算法，利用非線性最小二乘法將閉環檢測后的數據圖進行后端優化，從而獲得全局地圖一致性導航［9］。谷歌提供Cartographer SLAM 算法配備傳感設備包方式給出一種搭建室內地圖的即時解決方法，該包能產生r=5 cm 為單位的2D 柵格地圖。此算法結合單獨的本地方法和全局方法處理2D SLAM，兩種方法都優化了由LIDAR 觀測值的平移和旋轉組成的姿態。

ξx、ξy為移動機器人在x,y方向的平移，ξθ為2D 平面旋轉量，該姿態進一步稱為掃描。在穩定性不夠好的平臺如Cartographer 上，利用IMU 預估重力方向，把水平安裝的激光雷達掃描投影到2D 地圖中［10］。在局部法中，每個連續掃描都是針對地圖上一小部分進行匹配的，這種小塊區域稱為子圖M，非線性優化可將掃描與子圖對齊，這個過程稱為掃描匹配。掃描匹配會因為時間變長而累積錯誤，而全局方法會消除這些誤差。

子映射構造是對觀測數據幀和子映射坐標數據幀進行多次對齊的更替行為。激光掃描的起始點在0 ∈R2。將掃描數據記為

掃描數據幀的子映射位姿ξ變換為Tξ，將激光掃描點的掃描幀轉換到子圖坐標下，公式定義如下：

使用幾個連續的雷達掃描數據構建子映射，這些子映射采用概率網格M:rZ×rZ→形式，從給定分辨率r如5 cm 的離散網格點映射值，這些值可被認為是網格點被阻塞的概率。針對每個網絡點定義對應的像素，由最靠近該網絡點的所有點構成［11］。

無論什么時候把激光掃描幀插進概率網絡，都將計算得出一對命中（hit）網絡點和一組丟失（miss）網絡點。對于每一次hit柵格，把最近網絡點插入hit集合。針對每一個hit點插進與每一個像素相關的點，這些像素以及激光掃描數據點和每個掃描數據點之間的一條射線交叉，排除那些在hit集合中的網絡點［12］。對每個未被掃描到的網格點賦予一個概率值phit或pmiss。如果網格點x已被觀察到，那么命中和丟失的概率則變為：

將掃描數據幀插入到子映射前，掃描位姿ξ相對于當前時刻的子映射，需要通過Ceresbased［13］掃描匹配器進行優化。掃描設備查找一個掃描姿勢，該姿勢使子映射里掃描點的概率變得最大，即把這個變為一個非線性最小二乘［14］：

其中Tξ根據位姿掃描將hk從掃描幀格式轉換為子圖。函數Msmooth:R2→R是局部子圖中概率值的平滑版本，這里用雙立方插值。因此，區間[0,1]的值可能出現，但對結果沒有影響。IMU 能夠測量角速度用來估計移動機器人的旋轉角度θ和激光雷達之間的位姿［15］。

2 Cartographer SLAM 算法優化策略

Cartographer 算法利用回環檢測改善位姿進而消除累計誤差。機器人在運動時會把特征點以及地面標注點和之前觀測到的地面標注點以及特征點形成數據關聯的回環。因此，只要有新的特征點加入，都可使回環鏈添加邊緣，再構建新的回環鏈。閉環問題是數據關聯至關重要部分，利用閉環檢測判別機器人是不是已經走過現在的位置，優化已經完成的地圖，并且通過這個條件約束完成拓撲等價的路徑圖，激光掃描數據匹配相近性即為閉環檢測核心。

從上述原理可知匹配特征點非常重要，只要有錯誤出現就會使地圖構建的圖形發散。傳統Cartographer 回環檢測采用幀與子圖（Scan to Map）方式進行對比，消去構圖時出現的誤差積累，雖然幀與子圖的匹配方式可適當提高匹配效率，但這種方式容易因為一幀的量過少而使單個激光幀匹配時受限導致回環出錯。

Fig.3 Similarity of data between frames圖3 幀間數據相似情況

當兩幀數據比較相似，在進行匹配時會認為是一個閉環從而造成回環匹配錯誤，如圖3 虛線內所示。因此，本文設計一種子圖與子圖（Map to Map）回環檢測法，這種方式能優化激光幀少的缺陷，將激光雷達當前掃描的N 幀數據緩存起來形成一個局部子圖，通過該子圖和前階段的子圖進行再匹配［16］。

利用Cartographer 構建地圖時，在距離較長的走廊和環境地圖結構比較相似的地方很容易導致該算法在回環檢測中出錯。針對該問題本文設計并使用延時策略以保證回環檢測準確率及加速匹配速率。

2.1 Map to Map 回環檢測設計

針對回環檢測中幀—子圖的匹配問題，本文提出Map to Map 回環檢測策略。首先對激光數據坐標轉換進行分析。

以激光雷達為坐標系時，其自身轉一圈得到的間距值可提供雷達的一幀數值，根據轉角不一樣以及掃描斷電之間的計量間距，得出雷達自身與四周物障的間距［17］，但得到的數據以雷達自身為中心，需要轉化到世界坐標系下［18］。假設雷達掃描端點的某一個點以S(Sx,Sy)表示，那么它在世界坐標系的位置和姿態為U=(Ux,Uy,Uθ)，可通過U轉化矩陣TU將s轉化到世界坐標系中，見公式（10）。

將最近的幾幀掃描幀建立子圖，以二維柵格地圖方式顯示，地圖分辯率由柵格大小控制，每一個小柵格狀態用0和1 表示該空閑和占據［19］。通過公式（10）把雷達掃描幀轉化成全局坐標系，如圖4 所示。對于每一個柵格狀態，把紅色點當作激光雷達中心，掃描的物障信息為黑色的點。空白柵格代表雷達掃描范疇未發現的物障。

Fig.4 Grid map圖4 柵格地圖

將最近連續的幾幀掃描數據整合為子圖。若子地圖m由雷達數據l0至li組成，以N代表相應幀包括的雷達點數目，即l0至li包含的激光點數是：

由此構建的子地圖m擁有激光點數為：

將式（11）及式（12）經過柵格地圖化后須匹配運算的柵格數量為和Nm，因為相近幀之間的值存在多余成分，有＞Nm，因此可以利用子圖的構建去掉連續數據幀的冗余數據［20］，使Map to Map 匹配時需要運算的數據少于Scan to Map，以提高匹配效率。正是因為子地圖中含有多幀掃描數據，信息量較大，匹配范疇較廣，所以能很好地解決由于環境布局結構比較類似引起錯誤的回環匹配，如圖5 所示。

Fig.5 Sub-map example圖5 子地圖示例

圖5（a）和圖5（b）虛線內部分雖然相似度極高，但其它部分相似度較低，在加入Map to Map 回環檢測之后，整體匹配不會被判斷為一個閉環，從而提升回環匹配準確性。

2.2 延時決策設計

即使應用改進后的基于Map to Map 方式進行回環檢測匹配，但在較長的走廊和環境特征點極為相似時，由于激光雷達檢測到的數據幀極為相似，仍易出現回環錯誤，故本文使用延時決策處理策略，結合Map to Map 的回環匹配方法使改進后的算法能在任何環境下構建精確地圖。

由圖6 可知，移動機器人由a 點行駛到p 點過程中，當機器人走到i時發現i點和h點激光雷達觀測到了極為相似的環境數據，正常情況下會進行閉環優化，并將i點和h點連接起來，但一旦此時回環出錯，整個地圖會被一次錯的回環匹配破壞［21］。加入延時決策之后，當檢測到一個回環時不會立即進行回環檢測優化位姿，機器人繼續行駛直到產生下一個回環檢測點j和g點時進行位姿更新優化［22］。當第二次檢測到回環點后，假設這兩次檢測到的點都是正確的，那么兩次回環形成的4 條邊為T1、T2、T3、T4，根據圖優化SLAM 理論［23］可知T1?T2?T3?T4=I。

Fig.6 Delay decision圖6 延時決策

3 改進前后的Cartographer SLAM 算法對比實驗

為證明在多種環境如長距離、結構相似等環境下改進后的算法比改進前的算法具有更好的建圖能力，本文設計兩個實驗驗證改進后的Cartographer SLAM 算法。針對物理結構極其相似的環境和長距離、等寬度走廊，搭建簡單的物理布局，使用遮擋板擋住其兩端，依次使用未改進的Cartographer 算法和改進后的優化算法進行地圖構建。

圖7 為在狹長等寬走廊環境下改進前后算法實驗對比圖，圖8 是在結構相似環境下改進前后算法實驗對比。圖7（a）為使用未改進算法的建圖結果，從圖中可以看出，在較長走廊內，由于環境基本相似，構建出來的走廊的與實際環境中的走廊長度不符，且建圖寬度與實際環境不符，可以判斷是回環檢測匹配出現錯誤。相反，圖7（b）是使用改進后的優化算法構建的地圖，可以看出即使在狹長的走廊里也能構建出精確的地圖。從圖8（b）可以看出，使用改進優化后的算法進行地圖構建時，當物理結構環境極為相似時，該算法也能進行正確的回環檢測與匹配，從而生成精確地圖。反之，圖8（a）在使用未改進的算法描繪地圖時出現回環檢測錯誤，導致整個地圖與真實環境布局不匹配，無法進行下一步導航。

Fig.7 Map comparison of algorithm construction under narrow and equal width corridor environment圖7 狹長等寬走廊環境下算法構建地圖對比

Fig.8 Map comparison of algorithm construction under similar structure environment圖8 結構相似環境下算法構建地圖對比

4 改進前后的Cartographer SLAM 算法建圖對比實驗

將上述Cartographer SLAM 優化算法在實際環境中進行整體對比實驗，與未優化前的建圖結果進行對比分析，證明優化后的算法對SLAM 建圖確有效果。

通過上位機操控移動機器人在實驗環境中行駛，ROS（移動機器人操作系統平臺）將傳感器如里程計、激光雷達等掃描數據記錄下來，之后分別運行優化前后的Cartographer SLAM 算法得到建圖結果，對比兩種結果并分析優化效果。

圖9、圖10 是改進前后的Cartographer SLAM 算法構建地圖效果。

Fig.9 Map constructed by Cartographer before improvement圖9 Cartographer 未改進前構建的地圖

對比圖9 和圖10 可以很明顯看出，改進前的CartographerSLAM 算法繪制的地圖存在邊界毛刺不清晰情況，內部也有些許重影；而改進后的算法構建的地圖邊緣更加清晰，內部障礙物描畫也更具體，幾乎不存在邊界模糊或毛刺樣不清晰或重影情況。

Fig.10 Improved Cartographer SLAM map圖10 改進后的Cartographer SLAM 地圖

為了更好地對比改進前后算法優化程度，本文通過挑選并實際測量實驗環境中的10 個特征點，與在Rviz 中建圖的測量值進行比較，對這兩個算法的相對誤差絕對值數據、絕對誤差數據進行計算，并畫出兩個算法相對誤差的對比折線圖，如圖11 所示。

Fig.11 Comparison of absolute value of relative error of Cartographer algorithm before and after improvement圖11 改進前后Cartographer 算法相對誤差絕對值對比折線

實驗采用四核處理器的CPU，改進前后算法在運行過程中采集到的每核系統內存占用率對比如圖12 所示。

從圖11 可以看出，改進前的算法相對誤差普遍較大并且不穩定；改進后的算法相對誤差穩定，一般都未超過1%，對于室內環境地圖構建精度更高。由圖12 可以看出，改進前的CPU 占用率比改進后的高并呈上升趨勢，改進后的CPU 占用率較低，隨著時間增加趨于平穩。因此，在相同實驗環境下移動機器人使用Cartographer SLAM 算法效果更好。

Fig.12 Comparison of CPU occupancy rate before andafter the improvement of Cartographer algorithm圖12 改進前后Cartographer 算法運行時CPU 占用率對比

5 結語

本文闡述了主流Cartographer SLAM 算法，并針對以前Scan to Map 算法對子圖進行閉環檢測出現的匹配度不足問題，采用Map to Map 策略進行創新，降低了激光數據信息量少的缺點。利用創新的Map to Map 算法對子圖信息匹配作回環檢測，通過柵格地圖建圖，提高匹配度。針對回環檢測在相似度高的環境下匹配易出錯問題，將創新的Map to Map 算法與延時決策設計結合用以更新移動機器人位姿，提高回環檢測穩定性。后續研究將此算法與傳感器數據融合算法相結合，以進一步提高移動機器人SLAM 建圖精度。