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

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

一、引言

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

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

二、建圖算法原理

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

（一） 前端匹配

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

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

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

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

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

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

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

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

（一）定位軌跡的建立

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

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

（二）地圖更新方法

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

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

（三）裁剪重復(fù)子圖及約束重建

當(dāng)某些區(qū)域環(huán)境變化時(shí)，本文方法會(huì)保存當(dāng)前submap直到機(jī)器人建立足夠的閉環(huán)約束。既然變化的區(qū)域已經(jīng)保存為最新的submap，就可以把之前的submap及其node刪除掉。

因?yàn)樗鶆h除的submap及其node會(huì)與其他存在的node或submap存在約束關(guān)系，如果直接忽略這些約束關(guān)系則會(huì)導(dǎo)致地圖錯(cuò)亂；為了解決上述問(wèn)題，本文利用稠密圖稀疏化的方式重建這些約束。

本文把與需要裁剪的submap有約束關(guān)系的submap和node分別放在不同的vector中，通過(guò)整數(shù)0-99和100-200分別映射與被刪除submap有約束關(guān)系的submap和node，把這些整數(shù)當(dāng)做圖的頂點(diǎn)，兩兩結(jié)合當(dāng)做圖的邊，通過(guò)邊的距離設(shè)置圖的weight，這樣就形成了以submap及node為頂點(diǎn)，兩者直接的相對(duì)位姿為邊的稠密圖；然后通過(guò)圖稀疏化的方式減少邊的個(gè)數(shù)，最后根據(jù)邊的編號(hào)提取處對(duì)應(yīng)的submap和node，然后根據(jù)兩者的相對(duì)位姿關(guān)系形成約束，保證剪裁重復(fù)的submap時(shí)不會(huì)導(dǎo)致其他submap的閉環(huán)約束丟失。

四、實(shí)驗(yàn)結(jié)果對(duì)比

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

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

表1 機(jī)器人定位匹配評(píng)分對(duì)比

由表1中的匹配評(píng)分?jǐn)?shù)據(jù)可知，動(dòng)態(tài)更新后的匹配評(píng)分更高，可以有效減少機(jī)器人定位丟失問(wèn)題。

實(shí)驗(yàn)數(shù)據(jù)表明，對(duì)重復(fù)子圖進(jìn)行裁剪時(shí)，未對(duì)裁剪的submap及其node進(jìn)行約束重建處理的地圖會(huì)出現(xiàn)重影或錯(cuò)亂問(wèn)題，進(jìn)行約束重建處理后則不會(huì)出現(xiàn)地圖重影、錯(cuò)亂問(wèn)題（見(jiàn)圖1）。

圖1 結(jié)果展示 左-約束前地圖 右-約束構(gòu)建后地圖

五、結(jié)論

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