動態(tài)環(huán)境下的地圖自主更新和定位方法

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

一、引言

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

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

二、建圖算法原理

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

（一） 前端匹配

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

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

（二）后端優(yōu)化

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

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

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

三、動態(tài)環(huán)境下地圖自主更新和定位方法

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

（一）定位軌跡的建立

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

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

（二）地圖更新方法

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

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

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

當某些區(qū)域環(huán)境變化時，本文方法會保存當前submap直到機器人建立足夠的閉環(huán)約束。既然變化的區(qū)域已經保存為最新的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的閉環(huán)約束丟失。

四、實驗結果對比

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

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

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

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

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

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

五、結論

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