近日，高仙機器人SLAM團隊的研究成果在機器人領域頂級學術會議IROS （IEEE/RSJ International Conference on Intelligent Robots and Systems 智能機器人與系統國際會議）2021上發表。該論文提出了一種應對動態環境變化的長期定位與建圖方法，引入了一種高效的動態地圖更新機制，用于解決在變化環境下（如超市、商場、車庫等）的定位跳動和丟失問題，從而大大提高了機器人在動態變化場景的適用性。
（高仙機器人SLAM團隊持續招聘中）
文章簡介
近些年刊登了不少有關于SLAM研究的最新成果，ICRA(IEEE International Conference on Robotics and Automation：機器人與自動化國際會議)是目前機器人和人工智能領域最重要的國際會議之一，高仙機器人SLAM團隊對其中相關的文獻進行追蹤，并發表文章《從機器人落地角度談談ICRA2020對SLAM研究的啟發》，筆者介紹了SLAM的落地難點和相關進展，其中由于環境的變化，當前幀點云與預先建立的地圖無法正確匹配建立約束(constraint)，引入錯誤的約束信息或者無法建立有效約束，從而導致定位偏斜和定位丟失，因此極大地影響機器人的正常使用。
為了克服環境變化，高仙機器人提出了一種lifelong SLAM的方法?；谕ㄓ玫膱D優化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給出了整個稀疏化的流程（具體符號含義可以參考“符號介紹“小節）：
位姿圖優化
在完成一次地圖稀疏化后，需要進行一次全局優化，即PGR的Pose Graph Optimization的過程。該部分方法與其他SLAM后端差異不大，本質是創建一個無約束優化問題，優化的目標是調整submap和node的位姿，使得公式1的代價函數最?。?/p>
實驗結果
為了驗證算法的有效性，本文在北京某商場進行了長達一個月的有效性測試，構建了超過1萬平的地圖，其中包含550個submap，用于后續的地圖更新測試，測試的機器人如圖4所示，型號為高仙Scrubber 50清潔機器人：
圖4. 用于測試的高仙Scrubber 50商用清潔機器人
地圖更新情況
圖5. 商超地圖前后變化圖
圖6. 實際變化與地圖對比
圖5給出了運行前后一個月的地圖變化，(a)為建圖時地圖，(b)為經過一個月地圖更新運行后的地圖，(c)為兩者的差異。綠色點為環境中實際消失的環境特征，紅色點為新增環境特征?？梢钥吹奖疚乃惴ㄔ陂L期運行下，能夠有效記錄環境變化，同時地圖未出現重影和錯誤。
定位性能效果
表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.H. 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.W. 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技術充滿熱情的全職或實習算法工程師、科學家、產品經理和項目經理，以創新拓展技術邊界，引領行業發展。
工作地點：
北京中關村/上海張江/杭州/新加坡/其他
應聘條件（滿足其一即可）：
·有SLAM或相關崗位任職經歷
·有SLAM或機器人、計算機視覺相關研究成果
·有豐富的C++項目開發經驗，且對SLAM有興趣
我們重視：
·嚴謹的邏輯思維和溝通能力
·在特定領域有深入研究和獨立見解
·代碼品味和質量追求
