基于Netvlad神經網絡的室內機器人全局重定位方法

陳承隆，邱志成，杜啟亮，田聯房，林 斌，李 淼

1.華南理工大學 機械與汽車工程學院，廣州510640

2.華南理工大學 自主系統與網絡控制教育部重點實驗室，廣州510640

3.華南理工大學 珠海現代產業創新研究院，廣東 珠海519170

4.廣州地鐵設計研究院股份有限公司，廣州510010

5.日立電梯（廣州）自動扶梯有限公司，廣州510660

1 引言

近年來，隨著人工智能技術飛速發展，作為人工智能技術結晶的智能機器人行業也迎來新的契機，越來越智能的機器人被應用于社會生活的方方面面，如超市導購、快遞分揀、酒店前臺引導、智能送餐、變電站巡檢[1]等。而這些機器人功能的基礎都需要精確的位置信息才能夠完成，因此機器人全局定位和定位失效恢復功能就顯得極其重要。

目前室內機器人的全局定位恢復功能的實現主要分為四種方案：基于激光雷達二維點云的匹配定位[2-4]、基于WLAN 和位置指紋的定位[1]、基于文本的圖像檢索定位[5]和基于圖像內容檢索[6-15]的定位。其中基于激光雷達的匹配定位使用貝葉斯濾波框架[3]將定位問題轉換為了概率分布問題，在小型室內地圖中比基于WLAN和位置指紋的定位效果更加穩健，但是在區域有動態目標和地圖中有對稱房間的情況下定位容易失敗。基于WLAN 和位置指紋的定位則是通過信號衰減理論模型將接收到的信號強度和能量損耗轉化為傳播距離的方法，在無噪聲干擾的實驗室環境下表現較好，但在實際環境中容易受到環境噪聲干擾，精度下降嚴重。基于文本的圖像檢索需要人工構建圖像的文本描述，對于小型數據集比較適用，但是因為人們難以用文字全面描述整個圖像的內容而導致查詢精度并不高。對于基于圖像內容檢索的定位，又分為全局特征向量匹配和局部特征向量匹配[16]，其中局部特征向量的生成需要人工構建感興趣區域，容易受到人為認知局限性的影響，而全局特征向量更好地利用了卷積神經網絡能夠提取深層特征的特性，能夠提取到人工難以找到的細節特征，從而在高相似數據集中具有較高的魯棒性。提取特征向量后通過特征匹配方法在圖像數據庫中找到與查詢圖像最相似圖像的位置標簽作為查詢圖像的位置，此方法因其較高的準確性被廣泛應用于定位問題中，但其在弱光以及缺少特征情況下仍表現不佳。

室內環境中，不僅是相近地點的全景圖像具有很高的相似性，而且對于空間隔離的全景圖像有時候也表現為極高的相似性，所以在室內僅使用攝像頭一種傳感器進行重定位是非常困難的。充分利用機器人上多種傳感器的信息是解決機器人全局定位[17]的主流方向。

針對上述定位方法存在的查詢準確率低的問題，本文提出了多階段多傳感器自適應調整的全局重定位方法，包括重定位引導階段、粗定位階段和精定位階段三個子階段。下面分別介紹各個階段及具體流程。

2 框架流程及引導階段

整體流程分為在線定位和離線訓練兩個部分，如圖1所示，離線階段通過改進的Netvlad網絡訓練獲取模型參數并保存模型，以供在線定位階段使用。在線定位階段首先獲取激光雷達的實時信息，根據障礙物信息以一定規則引導機器人到達空曠區域，然后旋轉拍攝離散的全景圖像，將圖像輸入Netvlad[18]網絡進行圖像查詢，計算相似性排名，如果相似性得分都小于閾值則判定查詢失敗，返回引導階段，否則將得到的粗略的定位信息作為蒙特卡洛定位的初值繼續迭代定位，經過一段時間粒子不收斂則判定為定位失敗，返回引導階段，否則認為定位成功。

圖1 全局重定位流程圖

在整個流程中引導過程十分重要，它是整個方法形成閉環的關鍵過程，一定程度上提高了重定位系統的穩定性。

首先將激光雷達障礙物信息按照角度均分為8 個區域，每個區域中心軸為區域主方向，統計每個區域內障礙物平均距離，以機器人當前位置為圓心，1 m 為半徑，與平均距離最遠的區域的主方向相交點為目標點，如果目標點在障礙物上（不可達）或者接近障礙物，則將半徑縮小為0.5 m。最后將機器人引導至目標點，在定位失敗后重復此過程直至定位成功。

3 原始Netvlad網絡粗定位

Netvlad是Relja等人提出的一種快速高效的針對室外的大規模地點識別方法，其使用了Vlad（Vector of Locally Aggregated Descriptors）編碼技術，即使用聚合后的局部特征表示全局特征的編碼方法。Netvlad 考慮了離散全景圖像之間的空間信息，可以從任意一張或多張圖片中快速準確地進行地點識別，并使用了軟分配和更多的可訓練參數，得到的特征向量描述對于視角和光照變化具有很高的魯棒性，原始論文中的實驗也證明了這一點。

3.1 原始Netvlad網絡結構及原理

Netvlad 卷積神經網絡的訓練過程是求解和優化特征提取網絡輸出的N×D(N=W×H)維特征圖描述符中K×D 個聚類中心的位置和每個描述符對所屬類中心貢獻的權重，統計特征圖描述符與聚類中心之間的帶權重殘差和作為圖像描述向量，使得其與查詢圖像之間歐幾里德距離最小的圖像之間的描述向量的相似度最高，而與遠離查詢圖像的圖像之間的描述向量相似度最小。其中使用圖像的拍攝地點來作為訓練過程中的弱監督標簽，判定正負樣本規則使用歐幾里德距離函數。網絡結構[18]如圖2 所示，輸入圖像尺寸為640×480，基礎網絡為VGG-16[19]，使用了VGG-16的前13個卷積層，最終輸出特征圖尺寸為W×H×D（20×15×512），在VGG-16特征圖輸出后接入Netvlad層。在Netvlad中輸入N(W×H)個D 維局部描述符，將聚類算法獲得的K×D 個聚類中心作為參數，計算N 個特征點與聚類中心的帶權重殘差和作為圖像描述矩陣V 的元素，最終將V 轉換成矢量形式并歸一化作為圖像的特征描述向量輸出。矩陣V 元素計算公式為：

式中，Xi是特征圖輸入，xi(j )第j 維的第i 個描述符，ck(j )為第j 維的第k 個聚類中心，是每個xi(j )描述符對第k 個聚類中心的貢獻權重。軟分配權重計算公式為：

式中，wk=2αck，bk=-α‖ ck‖2，α是一個正常數，用來控制權重對距離的響應程度，ck是第j維第k個聚類中心。軟分配函數是Netvlad 卷積神經網絡與原始的Vlad 方法的核心區別，因為原始Vlad 方法使用的是硬分配函數，權重分配非1 即0，即應分配函數不可微，無法參與誤差反向傳播，一般使用提取到的特征向量送入SVM、Random Forest等分類器訓練。而Netvlad將權重分配函數換成了可微分函數，又將聚類中心ck解耦成wk、bk、ck三個可訓練參數，比傳統方法更具有靈活性。

3.2 損失函數

Netvlad 網絡中損失函數使用了弱監督的三元組排序損失，即，對于每個查詢圖像q都從數據庫中找到一組正樣本圖像和一組負樣本圖像，如果查詢圖像q 與最匹配的一個正樣本的地點標簽之間的歐幾里德距離小于其與所有負樣本之間的歐幾里德距離，則損失為0，反之，則按照距離差值進行損失懲罰。匹配度最高的唯一正樣本圖像則根據歐幾里德距離最小原則在一組正樣本圖像中進行挑選，選取后三元組為，其中為：

總損失函數為：

圖2 原始Netvlad網絡結構圖

4 改進的Netvlad網絡粗定位

4.1 柵格地圖連通域距離

歐幾里德距離和連通域距離對比如圖3 所示，圖中圓點為數據集圖像采集點，實線長度為歐幾里德距離，虛線長度為連通域距離，兩點之間只有實線則兩種距離相等。如果使用歐幾里德距離判定1 號位置的正樣本圖像，則3 號地點圖像會被判定為正樣本，然而3 號地點由于和1 號地點隔著墻體，圖像相似度必然會很低，在訓練過程中會產生錯誤懲罰，影響模型收斂速度和穩定性，同理2 號點的正樣本中會包含4 號點。如果使用連通域距離，1 號點的正樣本則不會選擇3 號點，而是2 號點，2 號點的最匹配圖像則會選擇6 號點。在負樣本選擇中，3 號和4 號點會被分配到1 號和2號點的負樣本中。本文將正樣本選擇標準中計算量較大的歐幾里德距離計算改為矩陣元素值查詢，但稍微增大了存儲空間，在室內環境下的數據集中能夠提高算法的穩定性。

圖3 歐幾里德距離和連通域距離對比圖

4.2 引入Resnet殘差網絡

VGG-16 網絡是由16 個卷積層和3 個全連接層組成，整個網絡全部統一使用3×3 的卷積核，共有16 個權重層，包含參數1.38 億個。VGG-16 網絡具有訓練參數眾多，計算量大，網絡深度較淺，提取特征層級較淺，收斂速度較慢等缺點，而Resnet-50 由于引入了殘差結構使得深層網絡不容易發生退化問題，與具有相同參數量和計算量的線性網絡結構相比能夠提取到更加深層的特征，減小了模型的復雜度，提高了模型的檢測識別率和收斂速度。鑒于上述優點，本文將Netvlad 中提取特征的主干網絡替換為Resnet-50，改進后的Netvlad 主干網絡的網絡結構如圖4所示，網絡輸入仍是640×480，新的特征圖提取層為Resnet-50 中的Conv2_x、Conv3_x、Conv4_x 和Conv5_x，最終輸出特征圖尺寸為20×15×512，去掉Resnet-50 最后的均值池化層和全連接層，添加1×1卷積層conv6，步長為1，輸出通道為512。

5 自適應蒙特卡洛精定位

傳統的蒙特卡洛定位在一定程度上可以解決全局定位和機器人綁架問題，一般通過添加隨機粒子嘗試恢復位姿，這個方法會大量增加無貢獻粒子的數目，容易導致算法收斂性變慢或者耗盡計算機內存，且只能小概率恢復定位。本文結合庫爾貝克-萊布勒散度[4]（Kullback-Leibler Divergence，KLD）采樣，以及自適應蒙特卡洛算法。由圖像查詢得到的粗定位地點位置作為自適應蒙特卡羅定位的初始隨機抽樣種子。將按照高斯分布產生的粒子，作為初始位置，可以明顯提高位姿恢復的概率。

KLD 是一個描述兩個概率分布差的測量值，其大小能夠反映粒子群當前分布和真實分布之間的偏差。根據偏差變化大小可以判定機器人是否定位丟失和被綁架，并以此為標準動態的增加或減少粒子數，時刻保證使用最少的粒子獲得當前分布對真實分布的最優近似。最小的KLD粒子數為：

圖4 改進后的Resnet-50結構

其中，Nb表示樣本占用的子空間數；ε 是目標分布誤差的最大值；1-δ 表示誤差小于ε 的概率。當生成的粒子數大于Nkld時，可以停止采樣過程。

6 實驗

6.1 實驗設備及軟件環境

本文使用輪式機器人作為移動平臺，如圖5 所示，在機器人頂部中心位置安裝有舵機和攝像機，舵機只有一個俯仰自由度，偏航自由度由移動平臺提供。攝像機分辨率設置為640×480，在距地面37 cm 處安裝了一個單線激光雷達，掃描角度360°，角分辨率0.5°，幀率為10 幀/s。具體設備如表1所示。

圖5 實驗硬件平臺

表1 移動平臺設備組成

為了對比原始模型和改進Netvlad 網絡模型的區別，基于圖5 所介紹的實驗平臺，本文在Ubuntu 操作系統下結合ROS軟件框架實現了以上所述基于改進Netv‐lad 卷積神經網絡的室內機器人全局重定位方法。算法運行環境如表2所示。

6.2 數據集

全局定位范圍大小為30 m×60 m 的室內區域，包含6 個房間和1 條走廊，為保證定位精度，數據采集點間隔為2 m，拍攝起始角度為全局地圖y 軸方向，多數選擇室內過道中間區域為采集點，訓練集共采集108 個地點圖像，共864 張圖像，測試集共采集42 個地點。數據集的格式制作為標準的Pittsburgh 數據集格式，每個地點圖像包括以機器人為圓心的圓上均勻的8個方向的8張圖像，作為離散化的全景圖像，由于特征向量的提取考慮了同一個地點離散圖像之間的空間信息，所以不需要人工標注和拼接成全景圖像。圖6 顯示了華南理工大學3號樓2 層和部分實驗室室內區域的柵格地圖，分辨率為5 cm×5 cm。圖中選擇性的顯示了測試集中4 個地點的圖像，同一個地點僅顯示第一張圖像，黑色箭頭為圖像拍攝方向。

表2 軟件環境

圖6 華南理工大學3號樓2層柵格地圖

6.3 模型訓練及驗證

在6.2 節所述的數據集上分別使用原始的Netvlad模型和改進的Netvlad 模型進行訓練，訓練采用SGD 優化方法，動量因子設置為0.9，圖像批處理大小設置為12，聚類算法中心數k 為64，初始學習率為0.001，如果模型訓練10 個周期內性能不再提升，則將學習率減半，20個周期內性能不再提升，則停止訓練。如果根據圖像相似度查詢得到的前N 個預測地點中任意一個在以查詢圖像真實地點為圓心1 m 為半徑的圓內，角度誤差在22.5°以內，則認為查詢結果正確。文中定義TOP@N為根據相似度查詢得到的前N 個圖像。對于圖像查詢一般使用精確率對模型進行評價，精確率定義為查詢正確地點數目和查詢地點總數的比值。對比實驗分別訓練4個模型：原始Netvlad（VGG-16）、單獨添加連通域距離改進的Netvlad 模型（VGG-16+AStar）、單獨引入殘差網絡改進的Netvlad 模型（Resnet-50）和同時加入兩種改進的Netvlad 模型（Resnet-50+AStar），模型訓練損失及測試集精確率如圖7（a）～（d）所示，損失曲線x軸已做歸一化，曲線平滑因子0.744。

圖7 （a） VGG-16+AStar和Resnet-50損失曲線

圖7 （b） VGG-16和Resnet-50+AStar損失曲線

圖7 （c） VGG-16+AStar和Resnet-50精確率

圖7 （d） VGG-16和Resnet-50+AStar精確率

由訓練過程中的損失曲線可知，未加入連通域距離的模型損失震蕩幅值范圍較大，未引入殘差網絡的模型的最終損失值較高，由精確率曲線可知，兩種改進均可提高模型收斂速度和精確率，且當TOP@N 為1 時引入連通域距離后模型精確率提升了1.323%，引入殘差網絡后模型精確率提升了3.287%，當TOP@N 為4 時引入連通域距離后模型精確率提升了3.529%，引入殘差網絡后模型精確率提升了8.541%，故加入連通域距離的改進可以抑制室內場景模型的錯誤懲罰，加快模型收斂，引入殘差網絡可以提高模型的判別能力。

6.4 基于圖像查詢的粗定位

本文模型可以兼容每個地點的查詢圖像小于8 張的情況，以簡化重定位復雜度，但會損失部分精確率。為了全面評價4個模型定位精確率在1 m范圍內和角度誤差范圍內的精確率，故對測試集42 個地點進行精確率測試。在測試集中選取10 個地點分別使用4 個模型對每個地點進行50 次查詢，為了獲得最佳查詢性能每個地點查詢圖像固定為8 張圖像，TOP@N 為4，測試結果如表3所示。

表3 平均精確率結果

根據表3 可知，加入連通域距離后4 個模型精確率在TOP@N 為1 時平均提升了1.7%，為4 時平均提升了3.2%，引入殘差網絡后模型精確率在TOP@N 為1 時平均提升了2.9%，為4 時平均提升了8.0%。同時加入兩種改進后模型在TOP@N 為1 時精確率提升了4.6%，為4 時提升了11.2%。基于圖像查詢的重定位雖然可以幫助機器人定位，但定位準確率會隨著定位精度的提高呈指數型下降，隨著TOP@N 的減小而下降。故為了提高機器人定位精度，本文選擇按照圖像相似度排名給予查詢出的地點標簽相應權重，權重系數為4∶3∶2∶1[17]，按照圖像標簽和權重系數分配自適應蒙特卡洛定位算法中初始粒子的分布位置和數量。

6.5 自適應蒙特卡洛精定位

根據上一節測試結果，本節實驗選擇原始Netvlad模型（VGG-16）和改進的Netvlad 模型（Resnet-50+AStar）做對比，本文采用TOP@N 為4作為本節實驗設定，在實驗過程中機器人每旋轉45°拍攝一張環境圖像。本文在實現了整個全局重定位系統的基礎上，對全局重定位和綁架問題進行了測試。測試流程如圖8 所示，其中A 至B 段為人工操控機器人行走階段，B 至C 段為機器人綁架路線，在C 點由機器人自行判定定位丟失，這種情況下需要系統進行機器人全局定位，由改進的圖像粗定位模塊提供初始位姿，將排名前5 的地點按照上一節中的權重向自適應蒙特卡洛系統注入1 000 個初始粒子，經過引導階段和圖像查詢定位以及自適應蒙特卡洛精定位過程，在D點成功定位。

圖8 機器人重定位流程圖

圖9 原始模型和改進模型全局定位誤差圖

圖9 顯示了兩種模型在TOP@N 為4 情況下的全局重定位誤差，在a 段機器人從B 點被綁架至10 m 外的C點，3 s 后啟動全局重定位，b 段從12 s 開始引導機器人前進1 m，原地旋轉并拍攝8張查詢圖像，c段從第21.5 s開始圖像查詢，原始Netvlad 模型經過e 段在第49.8 s 完成圖像查詢粗定位過程，粗定位誤差為1.63 m，由于粗定位誤差較大，經過f 段在第66.7 s 定位收斂，而改進的Netvlad 模型經過a、b 段在第21.5 s 完成查詢圖像拍攝，然后經過c 段在第38.5 s 完成粗定位過程，粗定位平均誤差為0.9 m，最后經過d 段在第48.8 s 全局定位收斂。原始Netvlad 模型提取特征時比改進的Netvlad 模型的參數量大，故粗定位過程要慢11.3 s，其因為誤差較大，精定位過程收斂要比改進的Netvlad 模型慢6.6 s，粗定位誤差過大容易引起二次全局重定位，定位時間將會翻倍。故從整體上看，改進的Netvlad 模型要優于原始Netvlad模型。

7 結論

針對室內移動機器人的全局定位和綁架問題，本文提出一種利用激光雷達信息預先引導，再利用改進的Netvlad 卷積神經網絡圖像查詢進行粗定位，最后利用粗定位得到的位置作為初始位置進行自適應蒙特卡洛定位的閉環定位方法。本文方案使用聚合的局部特征作為全局特征表示，比人工標注感興趣區域方法的特征表征能力更強，對于環境高度相似的數據集具有更高的魯棒性，并省去了繁雜的圖像標注工作，實驗效果滿足要求，故本方案能夠有效解決室內移動機器人的全局定位和和綁架問題。但系統對于動態場景的適應性還有待進一步研究，對于多個場景下機器人自適應性問題，可以使用由大型室內數據集訓練的有一定泛化能力的通用模型，省去離線訓練階段，但是有針對性的訓練則精確率會更高。