趙 蕓，趙 敏

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

0 引言

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

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

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

1 Cartographer SLAM 算法

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

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

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

其中位姿及約束關(guān)系如圖2 所示。

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

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

設(shè)eij(x)遵守高斯分布，F(xiàn)ij(x)即目標函數(shù)，可用如下公式表示：

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

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

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

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

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

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

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

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

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

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

2 Cartographer SLAM 算法優(yōu)化策略

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

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

Fig.3 Similarity of data between frames圖3 幀間數(shù)據(jù)相似情況

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

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

2.1 Map to Map 回環(huán)檢測設(shè)計

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

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

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

Fig.4 Grid map圖4 柵格地圖

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

由此構(gòu)建的子地圖m擁有激光點數(shù)為：

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

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

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

2.2 延時決策設(shè)計

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

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

Fig.6 Delay decision圖6 延時決策

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

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

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

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

Fig.8 Map comparison of algorithm construction under similar structure environment圖8 結(jié)構(gòu)相似環(huán)境下算法構(gòu)建地圖對比

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

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

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

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

Fig.9 Map constructed by Cartographer before improvement圖9 Cartographer 未改進前構(gòu)建的地圖

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

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

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

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

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

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

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

5 結(jié)語

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