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

01 引言

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

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

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

我們的主要貢獻如下：

我們提出了一種能夠識別并記錄環(huán)境半動態(tài)特征的語義建圖方法；

我們構(gòu)建了一個穩(wěn)健的定位系統(tǒng)，可以根據(jù)觀察結(jié)果確定物體類別；

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

02 方法概述

本節(jié)介紹所提方法的體系結(jié)構(gòu)。根據(jù)[29]的分類，本文稍作修改如下：

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

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

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

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

A：建圖方法

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

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

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

B：定位方法

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

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

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

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

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

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

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

03實驗

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

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

A. 停車場場景中的地圖

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

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

B. 在非靜態(tài)環(huán)境中的定位

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

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

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

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

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

C.一些棘手的情況

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

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

04 總結(jié)與未來

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

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