2021年11月23日

近日，高仙機器人SLAM團隊的研究成果在機器人領域頂級學術會議IROS 2021上發表。

IROS全稱 IEEE/RSJ International Conference on Intelligent Robots and Systems （智能機器人與系統國際會議），主要展示人工智能和機器人領域的最新研究進展（https://www.iros2021.org/）。

本期高仙技術家向大家分享這篇入選IROS的SLAM論文，該論文提出了一種應對動態環境變化的長期定位與建圖方法，該方法引入一種高效的動態地圖更新機制，用于解決在變化環境下（如超市、商場、車庫等）的定位跳動和丟失問題。從而大大提高了機器人在動態變化場景的適用性。

論文基本信息

標題：《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

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

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

文章簡介

在前一篇文章從機器人落地角度談談ICRA2020對SLAM研究的啟發，筆者介紹了SLAM的落地難點和相關進展，其中由于環境的變化，當前幀點云與預先建立的地圖無法正確匹配建立約束(constraint)，引入錯誤的約束信息或者無法建立有效約束，從而導致定位偏斜和定位丟失，因此極大地影響機器人的正常使用。

為了克服環境變化，我們提出了一種lifelong SLAM的方法。基于通用的圖優化SLAM框架，增加了動態地圖自動更新機制。更新的地圖與實際場景保持同步，從而保證在定位過程中點云和地圖始終建立有效的約束，從而克服定位偏斜和丟失。

基于上述問題和考慮，本文提出了一種通用的lifelong SLAM框架，相對于已有的lifelong方法，主要的貢獻點包括：

• 完整且穩定的lifelong SLAM框架，通過了實際場景的長期測試，并且得到大規模落地部署；

• 基于Chow-Liu Tree的最大互信息submap稀疏化和優化方案，在維持常量計算復雜度的同時，達到了厘米級的定位精度；

• 測試過程采集了包含激光雷達、IMU、輪式編碼器在內的lifelong SLAM數據，并開源了該數據集，便于之后學術界和工業界的研究。

系統架構

圖1. lifelong SLAM 系統框圖

圖1給出了系統架構圖，主要包含6大模塊，分別為：

• 輸入（Input）：聽取傳感器數據，包含IMU、輪式編碼計、激光雷達；

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

• 局部激光里程計（Local LiDAR Odometry，簡稱LLO）：SLAM前端，接收預處理模塊的傳感器數據，并進行連續幀匹配，同時生成submap；

• 全局激光匹配（Global LiDAR Matching，簡稱GLM）：SLAM后端，接收前端的激光數據并與地圖數據中的submap進行匹配，構建約束信息，并送入PGR模塊；

• 位姿圖優化（Pose Graph Refinement，簡稱PGR）：SLAM后端，也是本文核心模塊，負責submap的刪除和添加，根據最大互信息原則進行位姿圖的稀疏化，之后啟動全局位姿圖優化；

• 輸出（Output）：實時輸出計算得到的定位信息，TF轉換、位姿和地圖；

地圖更新

地圖更新過程

圖2. 地圖更新過程圖示

本文針對lifelong SLAM問題引入session概念，具體更新過程如圖2所示。第一次建圖屬于session 0，此時系統處于建圖階段（mapping stage），構建了當前環境的地圖描述，并存入數據庫。在機器人執行任務時，進入定位模式，每次定位會從數據庫讀取地圖數據，并加載到內存，創建新session（session 1~n），定位過程中根據圖1的算法，不斷將新生成的submap添加到當前session，同時刪除舊session的submap。在刪除舊submap時，觸發位姿圖的稀疏化和位姿優化。最后在結束定位任務時，調用接口保存更新的地圖，并傳入數據庫，從而完成一次地圖更新。經過多次地圖更新后，地圖中包含多條session，每條session由一定數量的node、submap和約束信息構成。

位姿圖稀疏化和優化

submap稀疏化

由于地圖更新需要考慮刪除舊submap，簡單粗暴的方法是直接在位姿圖中，將submap和附屬的node、約束信息刪除，其帶來的問題是整個圖將會丟失該submap的所有信息，因此在刪除數據時，需要盡可能的在刪除該submap時，將信息轉移并保存到位姿圖中，并用于后面的優化。

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

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

位姿圖優化

在完成一次地圖稀疏化后，需要進行一次全局優化，即PGR的Pose Graph Optimization的過程。該部分方法與其他SLAM后端差異不大，本質是創建一個無約束優化問題，優化的目標是調整submap和node的位姿，使得公式1的代價函數最小：

公式1

其中，，由下式得到：

公式2

符號介紹

使用，，分別表示節點(node)的位姿、submap位姿和約束信息。是的集合，為node的ID，表示session ID。類似的，submap構成。對于，本文出現三種約束：

1.：node到submap的約束，即node到submap的相對位姿觀測，由點云到submap匹配計算得到，為約束的協方差矩陣，表示觀測的不確定性；

2.：node到node的約束，即node到node的相對位姿觀測，由位姿圖稀疏化時獲得，為約束的協方差矩陣；

3.：submap到submap的約束，即submap到submap的相對位姿觀測，由位姿圖稀疏化時得到，為約束的協方差矩陣；

實驗結果

為了驗證算法的有效性，本文在北京某商場進行了長達一個月的有效性測試，構建了超過1萬平的地圖，其中包含550個submap，用于后續的地圖更新測試，測試的機器人如圖4所示，型號為高仙Scrubber 50清潔機器人：

圖4. 用于測試的高仙Scrubber 50商用清潔機器人

地圖更新情況

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

圖6. 實際變化與地圖對比

圖5給出了運行前后一個月的地圖變化，(a)為建圖時地圖，(b)為經過一個月地圖更新運行后的地圖，(c)為兩者的差異。綠色點為環境中實際消失的環境特征，紅色點為新增環境特征。可以看到本文算法在長期運行下，能夠有效記錄環境變化，同時地圖未出現重影和錯誤。

定位性能效果

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

表1給出了執行了25次定位任務和地圖更新，分別比較了純里程計、無地圖更新、地圖更新三種情況下的定位性能，對于每次定位，藍點表示初始化成功，藍線的長短表示定位任務執行的里程數，中間間斷表示定位丟失。表中左側百分號指標為正確軌跡的里程比值（參考論文中MRCL，通過激光與地圖匹配情況計算獲得），右下側為初始化成功率（參考論文CRI，通過統計初始化成功次數獲得），可以看到開啟地圖更新后，定位精度和定位丟失概率相對于未開啟地圖更新得到顯著提高。

表2. 平均匹配分數比較

表2基于25次地圖更新數據，與不帶地圖更新的文獻[2]比較了平均匹配分數，可以看到由于地圖更新機制的引入，使得點云和地圖匹配情況更好。

圖6為本文方法和[2]進行比較，可以看到本文方法的定位精度更高，未出現定位跳動等問題。

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

計算量評估

圖7和圖8顯示了在多次地圖更新后，地圖數據的變化和計算量變化情況。在超過18次的更新后，submap和node數量得到收斂，同時CPU和內存的占用保證在一定收斂范圍內，證明本文方法適用于長期的現場部署。

圖8. 多次更新后地圖數據中的submap、node數量變化

圖9. 多次更新后CPU和內存的占用變化

結語

為了解決長期運行時環境變化導致的定位問題，本文提出了一種完整的lifelong SLAM框架，基于Chow-Liu Tree互信息最大的submap更新方法，保證地圖數據與環境特征能夠實時同步，在取得高精度定位效果的同時，計算量也保證在一定范圍內，適用于大范圍的落地應用。

參考引用

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團隊招人啦！

SLAM團隊是高仙機器人核心算法研發部門之一，團隊多年來自主研發的多模態SLAM系統，，可運行于室內外全場景，已支持高仙機器人6大系列產品在全球43個國家和地區實現商業落地，累計運行超過1.5億公里。

團隊目前依托大量落地場景和數據，致力于攻克現實世界中最具挑戰的SLAM問題，并通過智能地圖服務持續提升用戶體驗。團隊成員全部來自國內985高校或海外知名高校，其中多人擁有博士學位，研究方向覆蓋激光SLAM、視覺SLAM和深度學習。

團隊與國內外多所知名高校展開學術合作，包括新加坡國立大學、清華大學、上海交通大學、哈爾濱工業大學、西安交通大學等。其中，與上海交通大學合作項目“低速無人系統定位導航技術與應用”榮獲2020年度上海市科學技術獎技術發明一等獎。

團隊長期招募對SLAM技術充滿熱情的全職或實習算法工程師、科學家、產品經理和項目經理，以創新拓展技術邊界，引領行業發展。