基于自適應軟重采樣粒子濾波網絡的視覺定位

劉艷麗，尹慧君，張 恒+

(1.華東交通大學 信息工程學院，江西 南昌 330013；2.上海電機學院 電子信息學院，上海 201306)

0 引 言

傳統的粒子濾波算法[1-7]的局限性在于它們需要系統的概率模型，都是基于特定模型公式的算法，不能適應比較復雜的環境。隨著深度學習的飛速發展，最近涌現了一系列新興技術將算法結構集成到深度神經網絡中，并以端到端的方式學習模型，逐步解決從數據中學習構建概率系統模型的問題，從而規避了傳統模型學習的困難。Karkus等[8]引入一個粒子濾波網絡(PF-Net)，它將系統模型和粒子濾波算法編碼在一個神經網絡中，利用二維地圖、觀測圖像和里程計信息作為模型的輸入，并且允許端到端進行訓練，解決了學習復雜概率系統模型的關鍵挑戰。當大多數粒子遠離真實狀態時，使用重采樣可以提高準確度，但當接近真實狀態的粒子被消除時，就會產生傷害，通常發生在學習的早期階段。

本文以粒子濾波網絡框架為基礎，對粒子濾波算法進行重新設計，提出自適應軟重采樣粒子濾波網絡(ASRPF-Net)，其網絡結構如圖1所示。為了更好提取地圖信息和觀測信息，先利用空間轉換網絡得到局部地圖，再采用卷積神經網絡學習地圖信息和觀測信息，來提高粒子權重的準確性；通過結合粒子濾波標準技術，提出自適應軟重采樣方法，加入決策，使模型可以自己判斷是否需要進行重采樣，使用更少的粒子，緩解粒子消除現象，解決文獻[8]中在高度不確定性環境下可能需要大量粒子才能準確定位的問題，有利于提高預測的準確性和定位的魯棒性，并且在全局定位任務方面，有著更好的定位性能。

圖1 自適應軟重采樣粒子濾波網絡(ASRPF-Net)

1 粒子濾波器以及循環神經網絡

1.1 粒子濾波

粒子濾波通過在狀態空間中尋找一組隨機樣本(粒子)來逼近概率密度函數，用樣本的期望值代替積分運算獲得系統狀態的最小方差估計[9]。粒子濾波算法使用粒子集來表示概率，在任何能用狀態空間模型描述的非線性系統上都適用。粒子濾波算法的具體步驟如下：

(1)

歸一化

(2)

步驟5 輸出結果，輸出新的粒子集，以及后驗概率估計、狀態估計和方差估計

(3)

(4)

(5)

其中，δ(·) 表示狄拉克函數。

1.2 循環神經網絡

RNN稱為循環神經網絡[10]，RNN神經網絡不僅具有前饋連接，而且具有內部連接的反饋，從而起到“循環”的作用。正因為這種循環機制，讓RNN具備記憶功能，因此RNN非常適用于處理序列數據。RNN被廣泛用于語音識別[11]、機器翻譯[12]、目標跟蹤[13]、室內定位[14]和導航[15]等領域。RNN網絡的圖模型如圖2所示。

圖2 RNN的圖模型

2 網絡結構提出

標準RNN通過維護一個確定性的潛在狀態處理序列預測，該狀態捕獲輸入歷史的足夠統計數據，并在給定新輸入的情況下順序更新。具體來說，RNN使用從數據中學習到的確定性非線性函數來更新hk[16]。

本文的網絡框架類似于文獻[8]中的網絡，我們利用RNN的強大近似能力，在網絡結構中嵌入粒子濾波算法，構建一個概率神經網絡。ASRPF-Net模型以循環神經網絡為基礎框架進行設計，將粒子狀態作為循環神經網絡的隱藏狀態，其網絡結構如圖1所示。結合傳統的貝葉斯算法和神經網絡，在一個循環神經網絡中編碼一個粒子濾波算法，以學習端到端的序列狀態估計的模型，同時將運動模型、觀測模型、空間轉換網絡模型和自適應軟重采樣模型融合到一個RNN單元中。ASRPF-Net模型的具體結構包含3個模塊(運動、觀測和自適應軟重采樣模塊)。

(1)運動模塊：以當前時刻運動信息以及前一時刻粒子狀態作為輸入，獲得在當前時刻粒子的新狀態。

(2)觀測模塊：將當前時刻的粒子狀態、觀測信息和地圖作為輸入，通過利用空間轉換網絡模型獲得局部地圖，再用卷積神經網絡對局部地圖信息和觀測圖像信息進行高效提取，為每一個粒子計算似然度，根據其似然度更新每個粒子權重。

(3)自適應軟重采樣模塊：為了避免粒子退化和規避在RNN單元嵌入粒子濾波過程中梯度消失，加入決策，采用提出的自適應軟重采樣方法，根據上述獲得的當前時刻粒子狀態和粒子權重，對粒子集進行處理，輸出新的粒子集。

該模型是完全可微的，算法和模型都被編碼到統一的網絡中表示，通過結合粒子濾波標準技術，融合一種新的自適應軟重采樣方法，得到自適應軟重采樣粒子濾波網絡。ASRPF-Net模型以循環神經網絡的形式實現粒子濾波算法，將神經網絡作為可微分程序或計算圖模型，允許進行端到端的訓練和學習，如圖3所示。ASRPF-Net中的可微算法先驗是粒子濾波：信念的粒子表示，以及用于運動模型和觀測模型的貝葉斯更新。ASRPF-Net在計算圖中對粒子濾波進行編碼，運動模型和觀測模型是具有特定結構的可訓練神經網絡。其中運動模型主要是采集機器人的動作，觀測模型主要基于地圖信息和機器人的觀測信息，然后利用粒子濾波算法更新狀態空間，達到與真實狀態最一致。

圖3 ASRPF-Net計算

3 ASRPF-Net模型步驟

粒子濾波器在接收到觀測數據后周期性地近似狀態的后驗分布，即使用粒子來表示系統的狀態信念b(h)。 該信念近似由一組粒子組成，即含有加權粒子的概率分布

(6)

(7)

(8)

第一步由式(7)可見，使用躍遷動力學來更新每個粒子。第二步由式(8)可見，將利用觀測信息來更新粒子權重。

我們的信念更新具有與標準粒子濾波器相似的結構，但是我們使用更豐富的函數來替換運動模型和觀測模型。ASRPF-Net模型通過運動變量uk、 觀測變量zk和地圖M更新先前的粒子信念bk-1， 以獲得當前的粒子信念bk。

3.1 粒子狀態更新

3.2 粒子權重更新

圖4 ASRPF-Net觀測模型框架

圖5 CNN1模型結構

圖6 CNN2模型結構

3.3 自適應軟重采樣

為了避免粒子退化，即大多數粒子具有接近零的權重，粒子濾波器通常對粒子進行重采樣。然而重采樣是不可微的，這阻止了使用反向傳播來訓練概率神經網絡，導致模型在訓練過程中不能利用粒子變化的梯度信息。為了規避在RNN單元嵌入粒子濾波過程中梯度消失，我們采用文獻[8]中提出的軟重采樣策略，軟重采樣是一種基于重要性采樣的可微近似，為不可微重采樣步驟提供近似梯度。由于在文獻[8]中發現當大多數粒子遠離真實狀態時，使用重采樣可以提高準確度，但當接近真實狀態的粒子被消除時，就會產生傷害。本文通過結合粒子濾波標準技術，采用自適應重采樣原理，提出自適應軟重采樣方法，加入決策機制，根據當前的有效粒子數目來判斷是否要執行重采樣操作，其中判斷機制的有效粒子數Neff表示粒子的退化程度，Neff值越小，意味著退化現象越嚴重，需要進行重采樣操作。

計算判斷機制的有效采樣尺度Neff， 確定粒子退化程度。Neff如下定義

(9)

由式(9)中可以看出，粒子退化程度越嚴重，粒子權重的方差越大，使得Neff的值越小及樣本中符合條件的高權重粒子越少。因此，為避免出現高權重粒子占的比重過小的情況，算法需要對Neff設立相應的閾值。我們設定有效樣本數Nthreshold=aNparticle作為閾值，其中Nparticle為粒子個數， 0

(10)

若判斷為需要進行重采樣操作，ASRPF-Net模型采用軟重采樣方法(具體公式請參見文獻[8])。在本實驗中取α=0.5。

我們可以有最終粒子信念可以表示為

(11)

自適應軟重采樣方法，通過實時計算有效粒子個數和評判粒子退化程度，保證只在有必要時ASRPF-Net才實施重采樣操作，有效減少重采樣次數，改善頻繁重采樣所導致的樣本枯竭的影響，緩解粒子消除現象，提高算法的魯棒性。

算法1： ASRPF-Net總體算法框架

(1)fori=1 toNdo

(5)ifNeff

(7)end if

(9)end for

(10)returnbk

把該模型運用于機器人視覺定位，機器人在未知的室內環境中進行導航。它需要通過車載攝像頭，里程計信息以及二維地面示意圖來確定自己的位置。通常用最小均方誤差來判定系統的優劣，因此訓練損失函數

(12)

4 實驗研究與分析

為驗證提出算法模型的性能，我們將用基于自適應軟重采樣粒子濾波網絡模型對機器人定位進行實驗仿真，對比其它的網絡結構模型來驗證可微算法先驗和端到端訓練的優勢。我們還研究當機器人的初始信念從圍繞其真實位姿分布到均勻分布整個空間時，設置不同粒子數目和不同初始信念，來對比在不同的不確定性水平下該模型的定位有效性，該模型還同時利用語義信息進行定位，針對語義地圖不同的標簽數據，來比較該模型的定位有效性。為了進一步研究該模型的定位性能，我們在使用視覺信息和里程計信息的模擬三維迷宮中測試全局定位任務。

4.1 實驗數據集

在House3D模擬器中進行仿真實驗，使用的是SUNCG數據集。該數據集是有大量人類設計、真實的住宅建筑數據。平均建筑面積為206平方米，房間的面積為37平方米。實驗所需的訓練集和測試集的部分建筑圖片如圖7所示，它們是來自不同建筑的圖片。對于一個新的、未被看見過的和充滿家具的環境，通過學習復雜環境中的信息，機器人同時利用語義信息將三維世界的豐富視覺特征與示意性二維地圖相匹配，進行自身定位。對每個建筑的語義地圖進行標記，標記的模式如圖8所示，其中標記了門和房間。

圖7 部分訓練集與測試集展示

圖8 標有門和房間類型的語義地圖

在DeepMind Lab的虛擬環境中進行實驗，通過讓模擬機器人在迷宮中行走來收集數據，并將收集的數據作為訓練數據。通過機器人隨機在迷宮中行走獲得的1000條軌跡，機器人每一步都通過相機獲得當前位置的圖像。對于每個迷宮，使用機器人在迷宮中的1000條運動軌跡進行訓練。我們將使用其中的模擬環境Maze1進行實驗，對應的先驗地圖和機器人所看到的圖像如圖9所示。

圖9 模擬環境Maze1

4.2 實驗設置

在對ASRPF-Net網絡進行整體訓練時，訓練集包括45 000組軌跡數據。軌跡是由機器人隨機產生的，通過向前移動(p=0.8)或者轉向(p=0.2)生成軌跡。其中，距離和轉彎角度的范圍分別是[20 cm，80 cm]和[15°，60°]。在訓練過程中，我們設置每條軌跡的步長為24，粒子數目為30，學習率為0.0001，訓練的反向傳播步長為4，生成的初始粒子的平移和旋轉的標準差為σt=0.3 m,σr=30°， 運動的平移和旋轉的標準差為σt=0 m,σr=0°。 在訓練期間沒有進行重采樣操作，因為在訓練過程中不需要短的軌跡和集中的初始信念。實驗中，我們的方法使用TensorFlow框架在Python中實現，并在Nvidia GeForce GTX 1080ti的計算機上進行測試。

為了驗證ASRPF-Net模型在機器人定位的準確性，我們在47個以前未見過的建筑中的820條固定的軌跡上，將該模型與其它模型進行對比實驗，其它模型包括PF-Net[8]、直方圖濾波網絡和LSTM網絡，在實驗中，我們還添加只使用里程計信息更新信念，不使用傳感器作為信息輸入的情況下的Odometry-NF模式。

為進一步分析ASRPF-Net模型的定位性能，在DeepMind Lab的導航環境下進行實驗，對于每一個迷宮，機器人以每秒鐘行走一步的速度被訓練出1000條100步的軌跡。機器人每走一步都能獲取相應的觀測圖像信息，里程計信息包含機器人行走的位置和方向的變化。獲得的機器人軌跡在訓練時每20步使用Adam進行優化并且使用dropout進行規范化，訓練時我們設置學習率為0.0003。

4.3 實驗結果與分析

針對機器人定位跟蹤，對比分析ASRPF-Net模型與其它模型的定位效果。實驗結果使用平均均方根誤差(RMSE)進行評估，RMSE值越小則定位越準確。各種模型方法均在使用RGB傳感器下的跟蹤實驗結果見表1。

表1 各模型的跟蹤RMSE/cm

從表1中，我們可以看出，與PF-Net、HF network和LSTM network模型相對比，ASRPF-Net模型的RMSE值是最小的，說明該模型在相同條件下機器人定位更加準確。從整體上來看，只使用里程計信息作為輸入的Odometry-NF模式定位效果是最差的，PF-Net的定位表現要好于HF network和LSTM network，但是定位效果與ASRPF-Net模型的相比還有一些差距。

為了進一步量化分析，對ASRPF-Net模型和其它模型的定位成功率進行比較。在定位任務中，根據定位成功的軌跡數占全部軌跡數中的比例，可以計算出算法定位結果的成功率。實驗結果使用100步長軌跡的定位成功率，如果軌跡的最后25步的估計誤差小于1 m，則定位成功。各種模型方法均使用σt=0.04 m,σr=5°下的定位的實驗結果如圖10所示。

圖10 各模型的定位成功率

由圖10我們可知，在定位任務中，HF network的定位準確度比較低，是因為該模型本身具有較大的缺陷，不能表示連續的狀態空間。LSTM network雖然可以充分學習數據中包含的相關性，但它缺乏一個確定的概率模型，所以其定位準確度只能達到大約20%，定位的效果較差，很難滿足現實需求，不能用于實際。Odometry-NF由于只將里程計信息作為輸入，該模型的定位準確度最低，定位效果較差。PF-Net的定位成功率都遠遠高于HF network、LSTM network以及Odometry-NF模型，但是ASRPF-Net模型的定位成功率是最高的，可見ASRPF-Net模型的定位效果是最優的，表明基于概率模型的端到端可以提高模型的學習能力和魯棒性。

我們還考慮具有不同程度的不確定性的本地化，在定位任務中，初始信念在一個或者多個房間是一致的。因此，粒子數目和初始信念的不同也會影響模型的定位效果。初始信念設置為一個房間的初始信念一致(N=1)和兩個房間的初始信念一致(N=2)以及在所有房間的初始信念一致(N=ALL)3種參數，粒子數目設置為500、1000、2000和3000，在初始化時設置不同的粒子數目，對比分析各模型的定位成功率，其中成功率同上述定位任務的成功率的計算方法一致，實驗結果如圖11所示。

圖11 不同初始信念和粒子數目的各模型定位成功率

從圖11我們可以看出，當我們把所有房間的初始信念設置為一致的時候定位成功率較低，定位效果最差，因為當所有房間的初始信念設置為一致時，機器人可能需要進行更多的判斷才能精準定位。在不同初始信念和粒子數目情況下，與PF-Net、HF network、LSTM network以及Odometry-NF模型相比，ASRPF-Net模型的定位成功率都是最高的。當粒子數目相同時，實驗結果表明當初始信念在一個房間內一致時，甚至當在整個房間內一致時，不確定性水平增加的ASRPF-Net模型也可以成功定位，并且其定位精度也是最高的。具體來說，當初始信念設置為N=1時，ASRPF-Net模型的定位成功率最高，當初始信念設置為N=ALL時，在初始不確定性較高的情況下，ASRPF-Net模型使用更少的粒子達到更好的定位效果。當初始信念設置相同時，我們看出隨著粒子數目的增加，定位的成功率也在不斷增加，因此粒子數目越多，定位的效果越佳。但是粒子數目增加，計算時間也會增大，可能會導致定位的時效性變差，所以選取適當的粒子數目，有利于達到定位的最好效果。

ASRPF-Net模型使用神經網絡對機器人獲得的圖像進行學習，雖然圖像中包含很多信息，但是因為輸入源信息比較單一，所以不適應很多復雜的環境。因此，ASRPF-Net模型同時利用語義地圖和觀測圖像信息，使模型具有更多的輸入源信息。語義地圖包含建筑的一些基本信息，如房門的位置以及房間的類型。在實驗中按照如圖8所示的標記方式來標記SUNCG數據集中的語義地圖。對數據集中的語義地圖進行標記結果如圖12、圖13所示，圖12(a)和圖13(a)均為數據集中的建筑地圖，圖12(b)和圖13(b)均為只標記房門的地圖，圖12(c)和圖13(c)均為只標記房間的地圖，每個像素對應于唯一的房間id，對于重疊的房間，較高的id會覆蓋較低的id，圖12(d)和圖13(d)均為標記房間id的地圖。

圖12 標記數據集中的語義地圖1

圖13 標記數據集中的語義地圖2

ASRPF-Net模型可以通過端到端訓練，自然地學會使用語義地圖。為驗證這一點，我們用帶有門和房間類別的簡化語義地圖來訓練概率神經網絡，將語義標簽編碼在輸入地圖的不同通道中：1個通道用于門，8個通道用于不同的房間類別。針對語義地圖不同的標簽數據，對提出算法模型的定位有效性進行比較，如表2所示。

表2 不同標簽數據的定位成功率/%

在實驗中，先對語義地圖進行簡單的標記，再對模型進行訓練。語義地圖的標記方式設置4種模式，包含無標記、只對房門做標記、只對房間做標記以及對房門和房間都做標記。從表2的實驗結果，我們可以得出當沒有對地圖進行標記時候，機器人的定位效果最差，當對房門和房間都做標記的地圖，機器人能夠獲取更多的信息，定位的成功率最高，定位的效果最優，表明利用簡單的語義地圖可以提高定位性能。同時為了更深入研究該模型和驗證改進算法的合理性和有效性，進一步做了消融實驗。我們在訓練期間改變網絡的參數設置，增加重采樣，當訓練增加初始不確定性和噪聲里程計，由于結合自適應軟重采樣策略，只有當需要的時候才啟動重采樣，成功率從39%增加至45%。這也表明，使用重采樣可以提高準確度，與PF-Net加入軟重采樣成功率42%相比，ASRPF-Net通過采用自適應軟重采樣方法，加入決策，能夠有效減少接近真實狀態的粒子被消除的現象，提高定位的成功率。

本文進一步使用ASRPF-Net模型對機器人進行全局定位，在DeepMind Lab中的模擬環境Maze1中的全局定位效果如圖14所示，測試軌跡每一個時間步長繪制一張圖。實驗中設置粒子數目為1000，并且使用RGB相機作為輸入。

從圖14實驗結果可以看出，圖14(a)在最初的時候，整個環境中都充滿粒子，其中使用圓形標記機器人的最初的位置，箭頭表示機器人的方向，圖14(a)～圖14(g)中同樣使用圓形標記機器人的位置。在圖14(b)～圖14(d)的定位過程中，隨著訓練的進行，粒子逐漸接近真實狀態，表明模型對機器人運動的預測越來越準確，并且從圖14(e)開始，模型預測的位置與機器人的實際位置非常接近。最后，從圖14(f)～圖14(g)中可以看出所有粒子都基本在同一位置。具體來說，我們在每個環境中使用1000個粒子進行預測，粒子顏色越深，則表明權重越大。開始訓練時，每個粒子的權重都是一樣的，隨著模型的訓練，與真實狀態差異很大的粒子將被去除，而更接近真實狀態的粒子會增大權重，這些粒子參與下一步的訓練，有利于機器人下一步的預測。粒子權重的變化主要通過重采樣過程來實現的，我們采用自適應軟重采樣，在訓練中可以傳播梯度信息，使模型可以在下一次訓練中使用這些信息，加快訓練速度，并在有需要的時候啟動重采樣，使訓練結果可以更快接近真實狀態。在模擬環境Maze1的定位效果如圖14(h)的定位效果，其中淺色表示機器人運動的實際路徑，深色表示模型對機器人運動的預測。可見，在初始時刻，機器人定位的效果并不是很好，但是隨著模型訓練的進行，粒子越來越接近真實狀態，最后所有粒子都在同一處，ASRPF-Net模型的定位精度越來越高，最后的定位效果已經基本接近機器人的實際位置。

圖14 ASRPF-Net模型在Maze1中的定位過程

5 結束語

我們集成算法推理和深度學習進行概率狀態估計，并加入決策，提出自適應軟重采樣粒子濾波網絡，通過計算有效粒子數，使模型只在有需要的時候才啟動重采樣操作。模型采用軟重采樣，重采樣的粒子參與下一步訓練。我們使用ASRPF-Net模型對數據進行訓練，將該模型運用到機器人視覺定位，并且與粒子濾波網絡(PF-Net)模型和其它模型進行對比，實驗結果表明定位的RMSE和成功率都有所提升，在全局定位任務中，模型預測的位置與機器人的實際位置非常接近，說明ASRPF-Net模型在視覺定位中有效提高預測的準確性和定位的魯棒性，在機器人定位方面有著更好的性能。在未來，我們將進一步優化該模型，以提高執行效率和定位精度，從而使該模型能夠更好地應用于機器人定位。