一種半動(dòng)態(tài)環(huán)境中的定位方法

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

01 引言

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

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

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

我們的主要貢獻(xiàn)如下：

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

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

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

02 方法概述

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

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

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

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

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

A：建圖方法

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

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

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

B：定位方法

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

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

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

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

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

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

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

03實(shí)驗(yàn)

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

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

A. 停車場場景中的地圖

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

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

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

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

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

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

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

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

C.一些棘手的情況

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

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

04 總結(jié)與未來

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

在進(jìn)一步擴(kuò)展中，我們將研究使用其他類型的地圖和不同的定位算法，例如點(diǎn)云地圖和NDT定位算法。