坐標(biāo)系在動(dòng)態(tài)SLAM中究竟有多重要？

01 摘要

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

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

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

02 介紹

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

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

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

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

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

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

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

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

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

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

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

03 背景

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

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



04 表示

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

4.1 SLAM前端



4.2 以世界為中心的表示

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

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

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



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



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

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

審核編輯：劉清