動態環境下的地圖自主更新和定位方法

文｜秦金龍 董敏杰 陳放 王瑋琪

一、引言

靜態場景下的機器人導航定位技術目前已經比較成熟。為了解決動態場景下機器人定位問題，本文介紹了一種動態環境下機器人自主更新地圖和定位方法。

本文提出的方法是在cartographer框架基礎上改進優化的。本文主要貢獻有：①通過不同軌跡內的nodes和submaps建立的約束個數作為機器人定位狀態是否正常的依據。②通過機器人定位狀態來判斷是否保留新增的submaps來更新當前區域的地圖數據。③刪除舊的地圖數據，保留新的地圖數據，防止動態環境下地圖數據持續增大。

二、建圖算法原理

Cartographer是目前主流的基于圖優化的激光SLAM算法。激光雷達掃描得到的點云數據代表障礙物與雷達的距離，其出現的位置表示該位置有障礙物，通過柵格的概率值表示該位置點存在障礙物的概率。

（一） 前端匹配

Cartographer前端是激光雷達數據與柵格地圖匹配進行位姿估計。主要方法是通過暴力匹配，得到最高得分的解即為最優解，獲取匹配程度最高的位姿就是激光雷達與柵格地圖的相對位姿。

經過掃描匹配會得到激光雷達與submap相對位姿的估計值；如果在障礙物特征相似的環境如長走廊，掃描匹配效果較差；為了解決該問題，在每幀激光雷達數據插入到submap之前，cartographer引入Ceres掃描匹配器進一步優化node與submap之間的局部位姿，該優化器不僅考慮激光雷達數據與submap之前的概率匹配，還加入其他傳感器預測權重，如里程計預測位置和角度，如果在特征相似的環境中可以把里程計預測的位姿權重增大，這樣就可以避免陷入局部最優的情況。

（二）后端優化

全局優化需要進行回環檢測建立閉環約束；回環檢測是指識別曾到過的環境，使得地圖符合閉環的條件。閉環約束由同一條軌跡的node和submap形成的位姿約束；回環檢測實時檢測激光雷達數據與已知的submap的匹配結果，如果找到高于閾值的匹配就把其添加到閉環約束中；全局優化通過node、submap、閉環約束等信息形成位姿圖，通過圖優化的方式使整體約束之間的殘差最小，殘差即約束與真實值之差。

全局約束建立的匹配方法與前端掃描匹配的方法原理相同；回環檢測是判斷機器人是否與歷史submap匹配，由于誤差累計的影響會導致位置不確定性大，搜索范圍也擴大，所以后端采用了分支定界的方法加快匹配速度。

Cartographer在動態環境下的建圖和定位效果較差，隨著運行次數增多，其submap數量會持續增加影響機器人運行效率。為了解決上述問題，本文提出了一種動態環境下的地圖自主更新和定位方法。

三、動態環境下地圖自主更新和定位方法

為了保證在動態環境下機器人定位不丟失，本文通過機器人當前閉環約束的情況判斷當前環境是否發生變換，同時判斷是否保留當前node和submap更新當前地圖信息。

（一）定位軌跡的建立

在定位模式下，會創建一條新的軌跡，把建圖生成的軌跡記作trajectory0，定位生成的軌跡記作trajectory1；定位模式下，trajectory1的node會與trajectory0的submap進行特征匹配，如果匹配評分大于閾值則認定形成了閉環，兩者會形成約束（constraint）；同時如果trajectory1新生成一個submap，則該submap會與trajectory0的node進行特征匹配，如果匹配評分大于一定值也會建立constraint；當前trajectory1新生成一個submap后，會對之前建立的所有constraint進行最小二乘法圖優化。

隨著機器人不間斷運行，會生成較多的node和submap，會降低計算量和保證定位效果；因此定位軌跡只保留最新的4個submap以及對應的node，會把其他多余的submap及其node裁剪掉；當新增的constraint的數量大于閾值時則認定機器人定位正確；該定位方法不僅考慮機器人當前位姿的約束狀態，還考慮之前一定時間內的約束狀態，而且當前的定位狀態依賴于之前所有約束的優化效果，所以該定位方法具有較好的魯棒性和穩定性。

（二）地圖更新方法

為了解決變化環境下地圖實時更新的問題，本文會對定位軌跡trajectory1的submap及其node進行選擇性保留，當任務執行完成后，會把trajectory0和trajectory1的相關信息一起保存到數據庫中得到最新的地圖文件。

機器人執行任務中，當新增的constraint數量小于閾值時，可以認定定位效果差，為了保留當前更新后的實際環境地圖數據，不再對trajectory1的submap及其node進行裁剪，直到新增的constraint總數大于一定的值，這時說明trajectory1有足夠的constraint進行閉環；后續對trajectory1的submap及其node進行裁剪時保留n個之前記錄的submap及其node和最新的4個submap及其node，把中間多余的submap及其node進行裁剪；當機器人執行任務結束后，把所有軌跡的submap及其node一同保留到地圖數據庫，這樣就保證變化環境信息可以實時保存到地圖數據中，從而保證機器人長時間運行下的定位準確性。

（三）裁剪重復子圖及約束重建

當某些區域環境變化時，本文方法會保存當前submap直到機器人建立足夠的閉環約束。既然變化的區域已經保存為最新的submap，就可以把之前的submap及其node刪除掉。

因為所刪除的submap及其node會與其他存在的node或submap存在約束關系，如果直接忽略這些約束關系則會導致地圖錯亂；為了解決上述問題，本文利用稠密圖稀疏化的方式重建這些約束。

本文把與需要裁剪的submap有約束關系的submap和node分別放在不同的vector中，通過整數0-99和100-200分別映射與被刪除submap有約束關系的submap和node，把這些整數當做圖的頂點，兩兩結合當做圖的邊，通過邊的距離設置圖的weight，這樣就形成了以submap及node為頂點，兩者直接的相對位姿為邊的稠密圖；然后通過圖稀疏化的方式減少邊的個數，最后根據邊的編號提取處對應的submap和node，然后根據兩者的相對位姿關系形成約束，保證剪裁重復的submap時不會導致其他submap的閉環約束丟失。

四、實驗結果對比

通過機器人在實際動態環境中運行數據來對比本文提出的動態環境下的地圖自主更新和定位方法的有效性。首先設置兩組對比實驗，第一組為正常未添加障礙物的地圖，第二組為添加了障礙物的地圖，且障礙物添加在機器人運行路線中。實驗中發現，機器人可實時檢測路徑上的障礙物，并根據障礙物的變化實時更新submap。

根據第一組與第二組的對比，在走廊區域增加了某些動態障礙物導致地圖數據出現變化。本文通過不同位置處激光雷達實時檢測的環境數據分別于上述兩組進行定位匹配，得到的概率匹配評分如表1。

表1 機器人定位匹配評分對比

由表1中的匹配評分數據可知，動態更新后的匹配評分更高，可以有效減少機器人定位丟失問題。

實驗數據表明，對重復子圖進行裁剪時，未對裁剪的submap及其node進行約束重建處理的地圖會出現重影或錯亂問題，進行約束重建處理后則不會出現地圖重影、錯亂問題（見圖1）。

圖1 結果展示 左-約束前地圖 右-約束構建后地圖

五、結論

為了解決變化環境下機器人定位問題，本文提出了一種動態環境下的地圖自主更新和定位方法；機器人在執行任務過程中可以自主判斷哪些區域需要動態更新并保證后續執行任務時有準確的定位信息。本文在主流的激光雷達SLAM cartographer框架中增加地圖動態更新和定位功能，增加重復子圖刪除和約束重建功能，保證機器人長時間運行效率。通過實際測試數據驗證了該方法在動態環境中有較穩定的運行效果，不需要人為更新變化區域的地圖數據。