2021年11月23日

近日，高仙機(jī)器人SLAM團(tuán)隊(duì)的研究成果在機(jī)器人領(lǐng)域頂級(jí)學(xué)術(shù)會(huì)議IROS 2021上發(fā)表。

IROS全稱(chēng) IEEE/RSJ International Conference on Intelligent Robots and Systems （智能機(jī)器人與系統(tǒng)國(guó)際會(huì)議），主要展示人工智能和機(jī)器人領(lǐng)域的最新研究進(jìn)展（https://www.iros2021.org/）。

本期高仙技術(shù)家向大家分享這篇入選IROS的SLAM論文，該論文提出了一種應(yīng)對(duì)動(dòng)態(tài)環(huán)境變化的長(zhǎng)期定位與建圖方法，該方法引入一種高效的動(dòng)態(tài)地圖更新機(jī)制，用于解決在變化環(huán)境下（如超市、商場(chǎng)、車(chē)庫(kù)等）的定位跳動(dòng)和丟失問(wèn)題。從而大大提高了機(jī)器人在動(dòng)態(tài)變化場(chǎng)景的適用性。

論文基本信息

標(biāo)題：《A General Framework for Lifelong Localization and Mapping in Changing Environment》

作者：Min Zhao, Xin Guo, Le Song, Baoxing Qin, Xuesong Shi, Gim Hee Lee, Guanghui Sun

來(lái)源：2021 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS2021)

論文鏈接：https://arxiv.org/abs/2111.10946

文章簡(jiǎn)介

在前一篇文章從機(jī)器人落地角度談?wù)処CRA2020對(duì)SLAM研究的啟發(fā)，筆者介紹了SLAM的落地難點(diǎn)和相關(guān)進(jìn)展，其中由于環(huán)境的變化，當(dāng)前幀點(diǎn)云與預(yù)先建立的地圖無(wú)法正確匹配建立約束(constraint)，引入錯(cuò)誤的約束信息或者無(wú)法建立有效約束，從而導(dǎo)致定位偏斜和定位丟失，因此極大地影響機(jī)器人的正常使用。

為了克服環(huán)境變化，我們提出了一種lifelong SLAM的方法。基于通用的圖優(yōu)化SLAM框架，增加了動(dòng)態(tài)地圖自動(dòng)更新機(jī)制。更新的地圖與實(shí)際場(chǎng)景保持同步，從而保證在定位過(guò)程中點(diǎn)云和地圖始終建立有效的約束，從而克服定位偏斜和丟失。

基于上述問(wèn)題和考慮，本文提出了一種通用的lifelong SLAM框架，相對(duì)于已有的lifelong方法，主要的貢獻(xiàn)點(diǎn)包括：

• 完整且穩(wěn)定的lifelong SLAM框架，通過(guò)了實(shí)際場(chǎng)景的長(zhǎng)期測(cè)試，并且得到大規(guī)模落地部署；

• 基于Chow-Liu Tree的最大互信息submap稀疏化和優(yōu)化方案，在維持常量計(jì)算復(fù)雜度的同時(shí)，達(dá)到了厘米級(jí)的定位精度；

• 測(cè)試過(guò)程采集了包含激光雷達(dá)、IMU、輪式編碼器在內(nèi)的lifelong SLAM數(shù)據(jù)，并開(kāi)源了該數(shù)據(jù)集，便于之后學(xué)術(shù)界和工業(yè)界的研究。

系統(tǒng)架構(gòu)

圖1. lifelong SLAM 系統(tǒng)框圖

圖1給出了系統(tǒng)架構(gòu)圖，主要包含6大模塊，分別為：

• 輸入（Input）：聽(tīng)取傳感器數(shù)據(jù)，包含IMU、輪式編碼計(jì)、激光雷達(dá)；

• 預(yù)處理模塊（Pre-Process）：包含IMU和輪式編碼器的數(shù)據(jù)融合、激光去畸變；

• 局部激光里程計(jì)（Local LiDAR Odometry，簡(jiǎn)稱(chēng)LLO）：SLAM前端，接收預(yù)處理模塊的傳感器數(shù)據(jù)，并進(jìn)行連續(xù)幀匹配，同時(shí)生成submap；

• 全局激光匹配（Global LiDAR Matching，簡(jiǎn)稱(chēng)GLM）：SLAM后端，接收前端的激光數(shù)據(jù)并與地圖數(shù)據(jù)中的submap進(jìn)行匹配，構(gòu)建約束信息，并送入PGR模塊；

• 位姿圖優(yōu)化（Pose Graph Refinement，簡(jiǎn)稱(chēng)PGR）：SLAM后端，也是本文核心模塊，負(fù)責(zé)submap的刪除和添加，根據(jù)最大互信息原則進(jìn)行位姿圖的稀疏化，之后啟動(dòng)全局位姿圖優(yōu)化；

• 輸出（Output）：實(shí)時(shí)輸出計(jì)算得到的定位信息，TF轉(zhuǎn)換、位姿和地圖；

地圖更新

地圖更新過(guò)程

圖2. 地圖更新過(guò)程圖示

本文針對(duì)lifelong SLAM問(wèn)題引入session概念，具體更新過(guò)程如圖2所示。第一次建圖屬于session 0，此時(shí)系統(tǒng)處于建圖階段（mapping stage），構(gòu)建了當(dāng)前環(huán)境的地圖描述，并存入數(shù)據(jù)庫(kù)。在機(jī)器人執(zhí)行任務(wù)時(shí)，進(jìn)入定位模式，每次定位會(huì)從數(shù)據(jù)庫(kù)讀取地圖數(shù)據(jù)，并加載到內(nèi)存，創(chuàng)建新session（session 1~n），定位過(guò)程中根據(jù)圖1的算法，不斷將新生成的submap添加到當(dāng)前session，同時(shí)刪除舊session的submap。在刪除舊submap時(shí)，觸發(fā)位姿圖的稀疏化和位姿優(yōu)化。最后在結(jié)束定位任務(wù)時(shí)，調(diào)用接口保存更新的地圖，并傳入數(shù)據(jù)庫(kù)，從而完成一次地圖更新。經(jīng)過(guò)多次地圖更新后，地圖中包含多條session，每條session由一定數(shù)量的node、submap和約束信息構(gòu)成。

位姿圖稀疏化和優(yōu)化

submap稀疏化

由于地圖更新需要考慮刪除舊submap，簡(jiǎn)單粗暴的方法是直接在位姿圖中，將submap和附屬的node、約束信息刪除，其帶來(lái)的問(wèn)題是整個(gè)圖將會(huì)丟失該submap的所有信息，因此在刪除數(shù)據(jù)時(shí)，需要盡可能的在刪除該submap時(shí)，將信息轉(zhuǎn)移并保存到位姿圖中，并用于后面的優(yōu)化。

為了達(dá)到稀疏化的目的，考慮將submap和附屬的node視為因子圖(factor graph)的一個(gè)節(jié)點(diǎn)(node)，constraint視為因子圖的因子(factor)，在刪除submap時(shí)，對(duì)被刪的submap及其附屬數(shù)據(jù)進(jìn)行一次邊緣化（marginalization），同時(shí)為了消除邊緣化帶來(lái)的dense fill in問(wèn)題，我們根據(jù)Chow-Liu Tree最大互信息原則保留了信息量最多的變量對(duì)，同時(shí)剔除其他的變量。圖3給出了整個(gè)稀疏化的流程（具體符號(hào)含義可以參考“符號(hào)介紹“小節(jié)）：

圖3. 基于Chow-Liu Tree最大互信息的地圖稀疏化流程

位姿圖優(yōu)化

在完成一次地圖稀疏化后，需要進(jìn)行一次全局優(yōu)化，即PGR的Pose Graph Optimization的過(guò)程。該部分方法與其他SLAM后端差異不大，本質(zhì)是創(chuàng)建一個(gè)無(wú)約束優(yōu)化問(wèn)題，優(yōu)化的目標(biāo)是調(diào)整submap和node的位姿，使得公式1的代價(jià)函數(shù)最小：

公式1

其中，，由下式得到：

公式2

符號(hào)介紹

使用，，分別表示節(jié)點(diǎn)(node)的位姿、submap位姿和約束信息。是的集合，為node的ID，表示session ID。類(lèi)似的，submap構(gòu)成。對(duì)于，本文出現(xiàn)三種約束：

1.：node到submap的約束，即node到submap的相對(duì)位姿觀(guān)測(cè)，由點(diǎn)云到submap匹配計(jì)算得到，為約束的協(xié)方差矩陣，表示觀(guān)測(cè)的不確定性；

2.：node到node的約束，即node到node的相對(duì)位姿觀(guān)測(cè)，由位姿圖稀疏化時(shí)獲得，為約束的協(xié)方差矩陣；

3.：submap到submap的約束，即submap到submap的相對(duì)位姿觀(guān)測(cè)，由位姿圖稀疏化時(shí)得到，為約束的協(xié)方差矩陣；

實(shí)驗(yàn)結(jié)果

為了驗(yàn)證算法的有效性，本文在北京某商場(chǎng)進(jìn)行了長(zhǎng)達(dá)一個(gè)月的有效性測(cè)試，構(gòu)建了超過(guò)1萬(wàn)平的地圖，其中包含550個(gè)submap，用于后續(xù)的地圖更新測(cè)試，測(cè)試的機(jī)器人如圖4所示，型號(hào)為高仙Scrubber 50清潔機(jī)器人：

圖4. 用于測(cè)試的高仙Scrubber 50商用清潔機(jī)器人

地圖更新情況

圖5. 商超地圖前后變化圖

圖6. 實(shí)際變化與地圖對(duì)比

圖5給出了運(yùn)行前后一個(gè)月的地圖變化，(a)為建圖時(shí)地圖，(b)為經(jīng)過(guò)一個(gè)月地圖更新運(yùn)行后的地圖，(c)為兩者的差異。綠色點(diǎn)為環(huán)境中實(shí)際消失的環(huán)境特征，紅色點(diǎn)為新增環(huán)境特征。可以看到本文算法在長(zhǎng)期運(yùn)行下，能夠有效記錄環(huán)境變化，同時(shí)地圖未出現(xiàn)重影和錯(cuò)誤。

定位性能效果

表1. 地圖更新前后的定位性能比較

表1給出了執(zhí)行了25次定位任務(wù)和地圖更新，分別比較了純里程計(jì)、無(wú)地圖更新、地圖更新三種情況下的定位性能，對(duì)于每次定位，藍(lán)點(diǎn)表示初始化成功，藍(lán)線(xiàn)的長(zhǎng)短表示定位任務(wù)執(zhí)行的里程數(shù)，中間間斷表示定位丟失。表中左側(cè)百分號(hào)指標(biāo)為正確軌跡的里程比值（參考論文中MRCL，通過(guò)激光與地圖匹配情況計(jì)算獲得），右下側(cè)為初始化成功率（參考論文CRI，通過(guò)統(tǒng)計(jì)初始化成功次數(shù)獲得），可以看到開(kāi)啟地圖更新后，定位精度和定位丟失概率相對(duì)于未開(kāi)啟地圖更新得到顯著提高。

表2. 平均匹配分?jǐn)?shù)比較

表2基于25次地圖更新數(shù)據(jù)，與不帶地圖更新的文獻(xiàn)[2]比較了平均匹配分?jǐn)?shù)，可以看到由于地圖更新機(jī)制的引入，使得點(diǎn)云和地圖匹配情況更好。

圖6為本文方法和[2]進(jìn)行比較，可以看到本文方法的定位精度更高，未出現(xiàn)定位跳動(dòng)等問(wèn)題。

圖7. 與[2]方法的軌跡比較

計(jì)算量評(píng)估

圖7和圖8顯示了在多次地圖更新后，地圖數(shù)據(jù)的變化和計(jì)算量變化情況。在超過(guò)18次的更新后，submap和node數(shù)量得到收斂，同時(shí)CPU和內(nèi)存的占用保證在一定收斂范圍內(nèi)，證明本文方法適用于長(zhǎng)期的現(xiàn)場(chǎng)部署。

圖8. 多次更新后地圖數(shù)據(jù)中的submap、node數(shù)量變化

圖9. 多次更新后CPU和內(nèi)存的占用變化

結(jié)語(yǔ)

為了解決長(zhǎng)期運(yùn)行時(shí)環(huán)境變化導(dǎo)致的定位問(wèn)題，本文提出了一種完整的lifelong SLAM框架，基于Chow-Liu Tree互信息最大的submap更新方法，保證地圖數(shù)據(jù)與環(huán)境特征能夠?qū)崟r(shí)同步，在取得高精度定位效果的同時(shí)，計(jì)算量也保證在一定范圍內(nèi)，適用于大范圍的落地應(yīng)用。

參考引用

1. Kretzschmar and C. Stachniss, “Information-theoretic compression of pose graphs for laser-based slam,” The International Journal of Robotics Research, vol. 31, no. 11, pp. 1219–1230, 2012.
2. Hess, D. Kohler, H. Rapp, and D. Andor, “Real-time loop closure in 2d lidar slam,” in 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016.

高仙SLAM團(tuán)隊(duì)招人啦！

SLAM團(tuán)隊(duì)是高仙機(jī)器人核心算法研發(fā)部門(mén)之一，團(tuán)隊(duì)多年來(lái)自主研發(fā)的多模態(tài)SLAM系統(tǒng)，，可運(yùn)行于室內(nèi)外全場(chǎng)景，已支持高仙機(jī)器人6大系列產(chǎn)品在全球43個(gè)國(guó)家和地區(qū)實(shí)現(xiàn)商業(yè)落地，累計(jì)運(yùn)行超過(guò)1.5億公里。

團(tuán)隊(duì)目前依托大量落地場(chǎng)景和數(shù)據(jù)，致力于攻克現(xiàn)實(shí)世界中最具挑戰(zhàn)的SLAM問(wèn)題，并通過(guò)智能地圖服務(wù)持續(xù)提升用戶(hù)體驗(yàn)。團(tuán)隊(duì)成員全部來(lái)自國(guó)內(nèi)985高校或海外知名高校，其中多人擁有博士學(xué)位，研究方向覆蓋激光SLAM、視覺(jué)SLAM和深度學(xué)習(xí)。

團(tuán)隊(duì)與國(guó)內(nèi)外多所知名高校展開(kāi)學(xué)術(shù)合作，包括新加坡國(guó)立大學(xué)、清華大學(xué)、上海交通大學(xué)、哈爾濱工業(yè)大學(xué)、西安交通大學(xué)等。其中，與上海交通大學(xué)合作項(xiàng)目“低速無(wú)人系統(tǒng)定位導(dǎo)航技術(shù)與應(yīng)用”榮獲2020年度上海市科學(xué)技術(shù)獎(jiǎng)技術(shù)發(fā)明一等獎(jiǎng)。

團(tuán)隊(duì)長(zhǎng)期招募對(duì)SLAM技術(shù)充滿(mǎn)熱情的全職或?qū)嵙?xí)算法工程師、科學(xué)家、產(chǎn)品經(jīng)理和項(xiàng)目經(jīng)理，以創(chuàng)新拓展技術(shù)邊界，引領(lǐng)行業(yè)發(fā)展。