移動(dòng)機(jī)器人CartographerSLAM 建圖改進(jìn)研究

趙 蕓，趙 敏

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

0 引言

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

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

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

1 Cartographer SLAM 算法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Fig.4 Grid map圖4 柵格地圖

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

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

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

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

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

2.2 延時(shí)決策設(shè)計(jì)

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

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

Fig.6 Delay decision圖6 延時(shí)決策

3 改進(jìn)前后的Cartographer SLAM 算法對比實(shí)驗(yàn)

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

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

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

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

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

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

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

Fig.10 Improved Cartographer SLAM map圖10 改進(jìn)后的Cartographer SLAM 地圖

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

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

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

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

Fig.12 Comparison of CPU occupancy rate before andafter the improvement of Cartographer algorithm圖12 改進(jìn)前后Cartographer 算法運(yùn)行時(shí)CPU 占用率對比

5 結(jié)語

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