坐標系在動態(tài)SLAM中究竟有多重要？

01 摘要

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

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

本文對各種動態(tài)SLAM公式進行深入分析，確定解決該問題的最佳方法?；诖四康模疚囊胍环N使用GTSAM的與前端無關的框架——用于評估各種動態(tài)SLAM公式。

02 介紹

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

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

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

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

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

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

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

本文的貢獻如下：

1）本文引入了一組詳細的數(shù)學公式和圖結構，用于在SLAM問題中估計自身運動和跟蹤動態(tài)目標；

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

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

03 背景

3.1 參考坐標系和符號

3.2 位姿變換和坐標系變化



04 表示

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

4.1 SLAM前端



4.2 以世界為中心的表示

4.3 以目標為中心的表示

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

圖3 展示了以目標為中心表示的因子



4.4 目標運動學因子



05 總結與未來展望

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

審核編輯：劉清