01 摘要

大多同時定位于建圖（SLAM）系統(tǒng)在傳統(tǒng)上假定的都是靜態(tài)世界，這與現(xiàn)實世界的場景不符。為了使機(jī)器人能夠在動態(tài)環(huán)境中安全導(dǎo)航和規(guī)劃，必須采用能夠處理動態(tài)目標(biāo)的表示。動態(tài)SLAM是SLAM研究中的一個新興領(lǐng)域，它不僅提高了整體系統(tǒng)的精度，還提供了目標(biāo)運(yùn)動的額外估計。

最新的文獻(xiàn)提供了動態(tài)SLAM的兩種主要表示方法，將動態(tài)目標(biāo)點在世界坐標(biāo)系或者目標(biāo)坐標(biāo)系中表示。盡管在局部參考系中表達(dá)目標(biāo)點可能看上去很直觀，但是這可能無法獲得最精確、最魯棒的解決方案。

本文對各種動態(tài)SLAM公式進(jìn)行深入分析，確定解決該問題的最佳方法。基于此目的，本文引入一種使用GTSAM的與前端無關(guān)的框架——用于評估各種動態(tài)SLAM公式。

02 介紹

同時定位和建圖（SLAM）是一個已被研究三十余載的問題。SLAM系統(tǒng)使機(jī)器人能夠構(gòu)建環(huán)境的表示，同時還能在該環(huán)境中定位自身。當(dāng)下的許多SLAM解決方案是在假設(shè)環(huán)境大部分由靜態(tài)元素組成的前提下執(zhí)行的，很遺憾，這在動態(tài)目標(biāo)豐富的現(xiàn)實世界中可能不成立。

傳統(tǒng)上，SLAM系統(tǒng)將與運(yùn)動目標(biāo)相關(guān)聯(lián)的傳感器數(shù)據(jù)作為異常值，并且在估計過程中剔除它們，而忽略任何與動態(tài)目標(biāo)相關(guān)的有用信息。將目標(biāo)集成到SLAM框架中的優(yōu)勢是，生成的地圖可以直接向?qū)Ш胶腿蝿?wù)規(guī)劃系統(tǒng)通知待估計的目標(biāo)運(yùn)動和場景結(jié)構(gòu)，提高了機(jī)器人系統(tǒng)在復(fù)雜動態(tài)環(huán)境中的魯棒性。因此，SLAM中的一項新興策略是結(jié)合對場景中動態(tài)目標(biāo)的觀測結(jié)果，并且估計其運(yùn)動。在本文中，我們將這種系統(tǒng)稱為動態(tài)SLAM系統(tǒng)。

最近，已經(jīng)探索了多目標(biāo)視覺里程計和基于圖優(yōu)化的動態(tài)SLAM系統(tǒng)，以基于靜態(tài)點和動態(tài)點觀測結(jié)果來聯(lián)合定位機(jī)器人并且估計靜態(tài)結(jié)構(gòu)和場景中剛體目標(biāo)的運(yùn)動/軌跡。這些系統(tǒng)通常采用局部、滑動窗口或者批量優(yōu)化技術(shù)，文獻(xiàn)中給出了這些優(yōu)化問題中表示變量的各種方式。在設(shè)計SLAM系統(tǒng)時，選擇合適的表示是非常重要的，因為這決定了系統(tǒng)的魯棒性、精度和效率。因此，對不同表示進(jìn)行正規(guī)分析是至關(guān)重要的，這些表示清楚地描述了此類系統(tǒng)成功執(zhí)行的情況。

圖1 展示了以目標(biāo)為中心 vs 以世界為中心。圖1(a)展示了一種更直觀的方法，其中觀測到的動態(tài)點在其對應(yīng)目標(biāo)的局部坐標(biāo)系中表示，本文稱之為以目標(biāo)為中心。

局部表達(dá)的點可以被建模為相對于目標(biāo)坐標(biāo)系的靜態(tài)點，以增強(qiáng)剛體假設(shè)，因此可以由因子圖中的單個狀態(tài)變量來表示。然而，目標(biāo)坐標(biāo)系的位姿是無法直接觀測的，并且當(dāng)根據(jù)目標(biāo)的部分觀測進(jìn)行估計時可能是不可靠的。

一種替代方法是在已知的參考坐標(biāo)系中表達(dá)動態(tài)點，例如相機(jī)坐標(biāo)系或者地圖/世界坐標(biāo)系。我們先前的工作VDO-SLAM證明了SE(3)運(yùn)動可以在包含世界坐標(biāo)系的任何參考坐標(biāo)系中表達(dá)。根據(jù)這一點并且通過在世界坐標(biāo)系中表示動態(tài)目標(biāo)點，避免了估計目標(biāo)位姿，并且產(chǎn)生了精確的結(jié)果。本文將這種表示稱為以世界為中心，如圖1(b)所示。

本文探索了如何在動態(tài)SLAM系統(tǒng)中更好地表示目標(biāo)。為此，我們引入了一個基于因子圖的優(yōu)化框架，用于開發(fā)和測試不同的動態(tài)SLAM表示。基于最先進(jìn)的文獻(xiàn)，我們實現(xiàn)了以世界和目標(biāo)為中心的表示，嚴(yán)格分析了由此產(chǎn)生的SLAM系統(tǒng)的精度和魯棒性。基于這一分析，本文提出了動態(tài)SLAM表示，其最精確且最魯棒地估計相機(jī)位姿和目標(biāo)運(yùn)動。

本文的貢獻(xiàn)如下：

1）本文引入了一組詳細(xì)的數(shù)學(xué)公式和圖結(jié)構(gòu)，用于在SLAM問題中估計自身運(yùn)動和跟蹤動態(tài)目標(biāo)；

2）本文使用現(xiàn)實世界數(shù)據(jù)集嚴(yán)格分析、評估和測試各種表示；

3）本文提供了一個使用GTSAM的動態(tài)SLAM優(yōu)化框架，該框架實現(xiàn)了本文中提出的各種表示。

03 背景

3.1 參考坐標(biāo)系和符號

3.2 位姿變換和坐標(biāo)系變化



04 表示

本節(jié)引入了若干種表示，在基于因子圖的動態(tài)SLAM估計框架中定義變量并且建模這些變量之間的關(guān)系（因子），類似于最先進(jìn)的方法。本文將這些表示分為以世界為中心或者以目標(biāo)為中心。

4.1 SLAM前端



4.2 以世界為中心的表示

4.3 以目標(biāo)為中心的表示

以目標(biāo)為中心的方法估計世界坐標(biāo)系中的相機(jī)位姿、靜態(tài)點、目標(biāo)運(yùn)動和位姿。對應(yīng)的因子圖如圖3所示，其中我們強(qiáng)調(diào)了用于實驗的公式的不同變體。

圖3 展示了以目標(biāo)為中心表示的因子



4.4 目標(biāo)運(yùn)動學(xué)因子



05 總結(jié)與未來展望

本文對動態(tài)SLAM的多種解決方案進(jìn)行了全面分析，并且在現(xiàn)有現(xiàn)實世界數(shù)據(jù)集上評估了所提出的表示。為此，我們使用GTSAM開發(fā)了一個與前端無關(guān)的優(yōu)化框架，其可以輕松地實現(xiàn)并且測試不同的配置。根據(jù)動態(tài)目標(biāo)及其對應(yīng)點觀測在因子圖中的表示方式，這些表示分為以目標(biāo)為中心和以世界為中心。以目標(biāo)為中心的表示更為直觀，因為基于剛體假設(shè)，目標(biāo)點相對于目標(biāo)局部坐標(biāo)系是靜態(tài)的。然而，本文分析表明，以世界為中心的表示可以產(chǎn)生更精確的目標(biāo)運(yùn)動估計，同時在相機(jī)位姿估計方面也表現(xiàn)更好，并且在優(yōu)化過程中展現(xiàn)更好的穩(wěn)定性。在未來，我們計劃對研究成果進(jìn)行正式描述，也可以提前提供明確的指導(dǎo)方案。

審核編輯：劉清