在非靜態環境中進行地圖繪制和定位是機器人學的基本問題。之前的方法大多關注靜態和高度動態的物體，但在半動態環境中，如果不考慮低動態物體（如停放的汽車和行人），可能會導致定位失敗。我們提出了語義建圖和終身定位方法，以識別非靜態環境中的半動態物體，并提出了一個通用框架，將主流物體檢測算法與建圖和定位算法集成在一起。建圖方法結合物體檢測算法和SLAM算法來檢測半動態物體，并構建只包含這些對象的語義地圖。在導航期間，定位方法可以對靜態和非靜態對象進行分類觀測，并評估半動態對象是否移動，以減少無效觀測權重和位置波動。實驗證明所提出的方法可以提高移動機器人在非靜態環境下的定位精度。

01 引言

在過去幾十年中，人們努力將SLAM的應用范圍從實驗室場景擴展到戶外場景，但由于場景動態程度的差別。戶外環境中，除了靜態物體外，還有著更多的動態和半動態物體，這可能會導致定位系統產生波動，以對地圖構建和定位系統提出了重要挑戰。為此，研究人員開始打破靜態世界的假設，但在識別高度動態和半動態物體方面仍然存在挑戰。通過對動態物體進行建模和過濾，在高度動態的環境中實現了精確的地圖和定位結果。然而，在包含許多半動態物體的環境中，地圖和定位問題仍未解決，因為靜態與半動態物體具有相似的特性。

為了解決這個問題，通過選擇那些隨時間變化較小的特征，并利用語義信息進行定位的方法證明是有效的，且已經得到廣泛應用。因此，本文提出了一種相關方法，用于識別和記錄環境中的靜態和半動態物體，可以分為兩部分：分別構建靜態地圖和半動態地圖，并基于兩者進行機器人定位。

本文的建圖算法擴展了赫斯等人[15]引入的OccupancyGrid技術，用于記錄環境的半動態部分。該算法能夠檢測環境中的靜態和半動態物體，并生成兩張地圖：靜態地圖和半動態地圖。在建圖過程中，通過過濾動態物體，靜態地圖包含環境中的靜態和半動態物體，而半動態地圖只包含半動態物體。本文的定位算法同時使用兩張地圖優化粒子濾波算法。它允許對觀測進行分類（靜態或半動態物體），并確定半動態物體移動的距離。然后通過調整不同物體的權重，可以在半動態環境中獲得更精確的定位結果。

我們的主要貢獻如下：

我們提出了一種能夠識別并記錄環境半動態特征的語義建圖方法；

我們構建了一個穩健的定位系統，可以根據觀察結果確定物體類別；

我們引入了一個通用框架，結合主流的物體檢測和SLAM算法，無論半動態物體位置如何變化，都能達到高精度定位的目標。

02 方法概述

本節介紹所提方法的體系結構。根據[29]的分類，本文稍作修改如下：

靜態物體：保持固定位置的物體，如墻壁、貨架；

半動態物體：在建圖過程中保持靜止，但在定位過程中可能移動的物體，如椅子、停放的汽車；

動態物體：指位置頻繁變化的物體，例如行人和移動車輛。

圖2展示了建圖和定位框架。在建圖過程中（紅色虛線表示），選擇一個對象檢測算法（Yolov3
[37]）來識別半動態對象。在SLAM線程中，我們利用SLAM算法（Cartographer
[15]）生成靜態地圖，然后將激光雷達點投影到圖像中。給出的半動態位置的全球坐標可以幫助創建半動態地圖從靜態地圖。靜態地圖包含靜態和半動態對象的信息，而半動態地圖僅包含半動態對象的信息。此外，半動態地圖是在線生成的，不會增加建圖過程的復雜性。在定位過程中（藍色虛線表示），同時將和加載到定位系統中。通過比較觀察距離，我們可以獲得與靜態對象（黃色點）和半動態對象（紅色點）對應的觀察信息。同時從和中獲取信息來定位機器人的姿態，取決于對象是否移動以及移動了多遠。通過降低半動態物體觀測值的權重，可以實現精確定位。

A：建圖方法

1）靜態地圖更新：基于物體出現在相同位置的可能性來實現對動態物體的識別。具體來說，如果某個區域的狀態在占用和空閑之間發生變化，則該區域被視為動態區域。在本文中，采用概率方法將問題定義為：

2)半動態地圖更新：觀測數據被分為靜態對象觀測數據和半動態對象觀測數據。為此步驟，應用了Yolov3來獲取分類信息。通過校準激光雷達與攝像頭之間的外部參數，可以將激光雷達點投影到這些矩形框中。然而，由于激光雷達點在矩形框邊緣可能對應于其他物體，因此無法提供準確的尺寸信息。為了解決這個問題，只采用矩形框中心的點來代表半動態對象的位置信息，從而排除了尺寸信息。有了位置信息，就可以在靜態地圖的基礎上進行連接區域的計算，從而生成更精確的半動態地圖。

圖3中的藍色圓點表示半動態位置。一個或多個相連的區域表示一個單一物體。為了補充半動態物體的維度，我們使用較低的閾值來獲取二進制靜態地圖，然后計算相連區域。將半動態位置和相連區域結合起來生成半動態地圖。在生成靜態地圖時，僅獲取半動態位置，圖3中的其他過程是離線的，不會增加建圖過程的計算復雜度。有關更多實驗結果，請參見第四部分。

B：定位方法

對于定位過程，我們使用蒙特卡洛方法通過使用odometry和觀察數據?進行遞歸驗證來提出該想法。通過同時加載和，我們可以識別并減少與移動的半動態物體對應的觀測值的權重。圖4展示了所提定位方法的圖形模型，其輸入包括傳感器觀測數據、靜態地圖、半動態地圖、odometry和時間的姿態。定位問題可以表示為：

通過將貝葉斯公式應用于方程2，可以表示為：

其中為歸一化常數。為了進一步減少移動物體的權重，應根據和計算觀測值的分類。方程3可進一步定義為：

我們的方法主要改進在于同時使用兩張地圖對半動態物體和靜態物體的觀測進行分類。通過比較每個激光點與兩張地圖之間的最近距離來計算分類，設代表點到靜態地圖中相鄰網格的距離，代表點i到半動態地圖中相鄰網格的距離。如果-的差小于一個閾值，則當前激光點對應于半動態物體，否則為靜態物體。定位過程中給出的結果反映了半動態物體（紅色激光點）和靜態物體（黃色激光點）。根據半動態物體移動的距離，相應觀測的權重會相應地減少。減少的比例可以定義為，該函數的結果在0到1之間，表示當前觀測的權重應減少多少。在本工作中，這些半動態物體的位置被假設遵循高斯分布。用F表示遞減比率，其定義如下：

其中，是一個相對較小的距離，在本文中取0.1。是判斷一個物體是否為半動態的閾值，在本文中取0.3。因此，當前位置的最終權重可以計算如下：

其中，表示觀測模型。在本文中，選擇似然觀測模型來計算當前位置的權重。通過同時使用靜態地圖和半動態地圖，可以使姿態估計更加準確。計算當前姿態權重的算法總結在算法1中。

該框架的三個主要組件——目標檢測、地圖構建和定位算法——都可以被其他相應的主流算法替換。

03實驗

為了驗證本文所提出的方法，我們使用一臺機器人進行了實驗測試。該機器人（如圖6所示）配備了一臺16束Velodyne激光雷達（在地圖構建和定位過程中僅使用一束激光束）、攝像頭、慣性測量單元和編碼器。

該停車庫被選為實驗場景，因為每個個體運行時停車庫都會發生顯著變化。它由四個房間組成，地圖框架中的左側房間通常比其他房間停放更多的車輛。機器人從左側房間開始，繞著停車庫移動，然后回到原地。由于實驗場景的特殊性，很難直接獲得真實姿態。我們在地圖構建過程中記錄了機器人的姿態作為真實姿態，并在其他時間構建的地圖上記錄了定位過程中的姿態。通過比較未使用和使用所提方法的定位精度，驗證了所提方法的有效性。在發生改變的環境中使用所提方法的定位精度也與在未改變的環境中不使用所提方法的定位精度進行了比較。

A. 停車場場景中的地圖

對于建圖過程，我們讓機器人每隔幾天進行一次數據采集。在單次運行中收集的數據用于構建.，該文件是通過使用靜態對象的位置和速度信息在線下構建。

最終構建的和如圖5所示。在這些靜態地圖中，由于車輛的移動，某些區域顯得較為明顯，而其他部分則始終保持靜態。包含環境中大部分半動態物體，缺失的半動態物體對應于較少被觀察到的區域，這對定位精度影響較小。通過應用逆觀測模型，在地圖構建過程中可以自動去除高度動態的物體，如移動的行人和車輛。

B. 在非靜態環境中的定位

我們比較了在未修改和修改的半動態環境中使用和不使用我們的算法的定位精度（圖7ABCD）。此外，我們還展示了在使用純靜態地圖的情況下，原始蒙特卡洛定位方法的定位精度（圖7E）。所有半動態對象，如停放的汽車，都是人工移除的。在建圖過程中選擇的姿態作為真值，在定位過程中記錄的姿態也使用在其他日子構建的地圖中。算法Evo（[38]）被用于離線評估我們的方法的精度。圖7展示了定位精度的定性結果，表I列出了姿態誤差的定量比較。MCL+-和MCL++分別表示在未修改和修改的環境中使用蒙特卡洛算法時，是否使用了我們的方法（第一個加減號指示定位過程中環境是否改變，第二個加減號指示定位過程中是否使用了我們的方法）。因此，MCL--和MCL-+分別表示在未修改的環境中使用蒙特卡洛算法時，是否使用了我們的方法。MCL表示使用蒙特卡洛算法，但不使用我們的方法，使用純靜態地圖。純靜態地圖如圖8所示，其中半動態對象從地圖中人工移除。此外，圖7ABCDE分別對應表1中的MCL+-、MCL++、MCL--、MCL-+和MCL。

在圖7A和7B中，我們的算法在改變的環境中顯著降低了偏差，給出了更低的平均誤差（0.083米相對于0.129米）和最大誤差（0.336米相對于0.516米）。根據圖5中的地圖和機器人的路線，它在前300秒和最后200秒期間遇到更多的半動態物體（如圖7A所示），這些地方的定位精度顯著波動，這證明原始的蒙特卡洛算法在改變的環境中性能較低。相反，應用了我們的算法后，結果更加準確（如圖7B所示）。

我們還進行了實驗來評估算法在靜態環境中的表現。如圖7C和7D所示，我們的算法也可以應用于未改變的環境中，與傳統的蒙特卡洛定位方法相比，其精度相當（0.327米對0.336米，0.065米對0.064米），這證明了所提方法在具有靜態和半動態物體的環境中的通用性。

此外，我們還進行了實驗，測試原始蒙特卡洛定位算法在修改后的環境中的定位性能，但將半動態物體從地圖中人工移除。因此，地圖僅包含環境的靜態物體（如圖8所示）。機器人利用環境中的靜態物體來確定自身的位置。如圖7E所示，由于所有半動態信息都被丟棄，因此其定位精度較低，是合理的。

此外，是在線獲取的。因此，在定位過程中，機器人只需要比較兩幅地圖之間的距離，所以計算復雜度可以忽略不計。值得注意的是，蒙特卡洛算法的絕對精度并不是本文的重點。我們的目標是將該方法引入主流的地圖和定位算法，以獲得更準確的結果而不犧牲效率。實驗結果證明，我們的方法在靜態和半動態環境中具有更好的魯棒性、更高的精度和通用性。

C.一些棘手的情況

在獲得半動態位置的過程中，可能會出現一些半動態位置擊中錯誤位置的情況（如圖9A和B所示）。但是，錯誤的位置不會影響生成，因為它沒有對應的連接區域。

在計算連通區域的過程中，某些靜態區域可能會被誤認為是半動態區域。如圖9C所示，柱子被誤認為是半動態物體，然而，這些物體（柱子和墻）的靜態特性使得該區域的重量保持不變。因此，錯誤的連通區域不會影響定位精度。

04 總結與未來

本文提出了一種新穎的語義地圖生成方法和能夠處理半動態環境的定位方法。通過融合攝像頭和激光雷達，所提出的方法可以在地圖構建期間自主檢測和標記半動態物體，利用這些關鍵信息，能夠構建用于后續導航的2D半動態地圖，并設計了基于后驗分布的姿態估計。我們還對配備了攝像頭、激光雷達和IMU的移動平臺在地下停車場進行了多項評估。結果表明，我們的方法在大多數情況下都能正常工作。基于此工作，我們的框架可以輕松擴展到3D模式。

在進一步擴展中，我們將研究使用其他類型的地圖和不同的定位算法，例如點云地圖和NDT定位算法。