01 摘要

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

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

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

02 介紹

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

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

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

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

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

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

本文探索了如何在動態SLAM系統中更好地表示目標。為此，我們引入了一個基于因子圖的優化框架，用于開發和測試不同的動態SLAM表示。基于最先進的文獻，我們實現了以世界和目標為中心的表示，嚴格分析了由此產生的SLAM系統的精度和魯棒性。基于這一分析，本文提出了動態SLAM表示，其最精確且最魯棒地估計相機位姿和目標運動。

本文的貢獻如下：

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

2）本文使用現實世界數據集嚴格分析、評估和測試各種表示；

3）本文提供了一個使用GTSAM的動態SLAM優化框架，該框架實現了本文中提出的各種表示。

03 背景

3.1 參考坐標系和符號

3.2 位姿變換和坐標系變化



04 表示

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

4.1 SLAM前端



4.2 以世界為中心的表示

4.3 以目標為中心的表示

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

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



4.4 目標運動學因子



05 總結與未來展望

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

審核編輯：劉清