基于端到端模型的機器人室內單目視覺定位算法

謝 非，吳 俊，黃 磊，趙 靜，劉錫祥，錢偉行

（1.南京師范大學 電氣與自動化工程學院，南京 210023；2.南京林業大學 機械電子工程學院，南京 210037；3.南京郵電大學 自動化學院&人工智能學院 南京 210023；4.東南大學 儀器科學與工程學院，南京 210096；5.南京智能高端裝備產業研究院有限公司，南京 210042）

機器人在工作環境中實現自主導航的前提是要實現對自身的定位。定位對于移動機器人實現路徑規劃、避障等功能尤為重要。移動機器人的定位功能在室外可以通過GPS（Global Positioning System）實現，但是在室內環境中機器人難以獲得GPS 信號，需要采用其他傳感器完成定位，例如WIFI、NFC、藍牙、激光傳感器、視覺傳感器等[1-3]。移動機器人使用WIFI、NFC、藍牙等設備進行定位時，需要提前在環境中安裝輔助設備，會導致環境的改變；激光傳感器可以獲得較高的定位精度，但是設備價格昂貴，成本較高[4]；視覺傳感器成本相對低廉對環境要求較低，隨著視覺SLAM（Simultaneous Localization And Mapping）算法和深度學習的發展，基于視覺的移動機器人定位也有了新的發展[5,6]。

視覺SLAM算法定位是將當前圖像與系統保存的關鍵幀進行匹配，或者將當前圖像中的關鍵點與場景中的點建立2D 到3D 對應關系以此來估計相機的位姿，以上方法主要是基于特征點方法，例如SIFT 和ORB[7,8]。視覺SLAM 算法面臨兩個問題，首先是需要存儲密集間隔的關鍵幀，建立幀到幀的特征對應會帶來較大的計算量和較慢的計算速度[9]。其次，使用相機傳感器的移動機器人在運動過程中，不能獲得全局上下文特征，遇到光線變化強烈、運動過快或物體遮擋等情況時，容易無法采集特征以至于丟失位姿，無法繼續感知環境，導致定位失敗[10]。

本文提出一種基于端到端模型的機器人室內單目視覺定位算法，以端到端的方式實現從圖像恢復出機器人空間位置，即從單幅圖像中回歸出機器人在全局坐標系下的位置。本文算法具有以下優點：

(1) 針對傳統相機定位方法存在的特征提取、圖優化等計算量大的問題，通過神經網絡訓練生成的模型進行室內定位算法，極大地減少了計算量，提高了定位速度。

(2) 本文定位算法不依賴于前后幀的運動及地圖構建等步驟，從查詢圖像輸入網絡模型到預測輸出機器人當前所處空間位置，對于光照和運動更加魯棒。在TUM 公開數據集及真實環境中的實驗結果表明，室內場景下僅使用單目圖像的神經網絡算法，定位效果優于使用圖像的傳統SLAM 算法ORB-SLAM2。

1 基于端到端模型的機器人室內單目視覺定位算法框架和原理

1.1 算法流程

本文移動機器人室內單目視覺定位算法基于深度神經網絡，實現如圖1所示的移動機器人室內單目定位算法。預先將機器人在運動場景進行數據采集，對采集的圖像數據進行標注，而后輸入深度神經網絡進行訓練，得到一個訓練好的場景模型，機器人在進行定位時將需要查詢的圖像輸入模型，模型輸出機器人此時的空間位置，完成機器人單目視覺定位。

數據采集階段，對采集的圖像數據進行逐一標注。數據集分為公開數據集和實測環境數據集。對于采用TUM 給出的公共數據集，通過數據集給出的標準軌跡位姿，對每張照片標注其在機器人運動過程中當時對應的空間位置[11]。在實際的測試環境中，采用運動恢復結構技術，通過分析圖像序列進行三維重建，最后采用三維環境點云模型對實驗場景的圖像數據逐一標注其空間位置，完成對數據集的標注。

測試階段，在運動環境中使用單目相機拍攝圖像，將拍攝的圖像輸入訓練好的模型中查詢，通過神經網絡模型直接輸出相機空間位置，實現端到端的單目視覺定位系統。

圖1 基于神經網絡的移動機器人室內單目定位流程Fig.1 Neural network-based indoor monocular positioning algorithm flow of mobile robot

1.2 端到端神經網絡結構

本文機器人單目視覺定位算法的神經網絡結構以GoogleLenet 網絡為基礎，在網絡中使用Inception 網中網結構，Inception 模塊如圖2包含了不同大小的卷積核。在不增加計算負載的情況下，Inception 結構減少網絡參數，增加網絡寬度和深度。Inception 結構有四條通路，包括三條卷積通路和一條池化通路，具有不同的卷積核大小，不同卷積核大小可以提取不同尺度的特征。最后將不同通路提取的特征合并起來，不同通路得到的特征圖大小是一致的。當輸入和輸出的通道數很大時，乘起來也會使得卷積核參數變的很大，而加入1×1 卷積后可以降低輸入的通道數，卷積核參數、運算復雜度也就跟著降下來。Inception 模塊綜合考慮多個卷積核的運算結果，獲得輸入圖像的不同信息，獲得更好的圖像表征。通過加入1×1 卷積，網絡可以降低輸入的通道數，卷積核參數、運算復雜度也就跟著降下來。通過這種微型神經網絡代替傳統卷積的過程，方便對網絡進行修改從而適應定位算法網絡。

圖2 Inception 結構Fig.2 Inception structure

圖3 單目定位算法神經網絡結構Fig.3 Neural network structure of monocular positioning algorithm

在網絡構建及訓練階段，網絡基于GoogleLenet深度神經網絡，GoogleLenet 結構如圖3，深度有22層，包括9 個Inception 模塊[11]。通過利用Inception結構的1×1 卷積降維后，得到了更為緊湊的網絡結構，雖然網絡結構總共有22 層，但是參數數量卻只是8層的AlexNet 網絡的十二分之一。

在網絡不同深處添加兩個loss僅在訓練時使用，在測試時不使用，以此來解決梯度消失問題，也是一種正則化手段。兩個輔助分類器在此時對其中兩個Inception 模塊的輸出執行softmax 操作，然后在同樣的標簽上計算輔助損失，總損失即輔助損失和真實損失的加權和。將 GoogleLenet 三個分類分支移除Softmax 層，并新增全連接回歸層，回歸部分由EuclideanLossLayer 中比較最后一層的輸出和標簽中位置的歐氏距離作為目標函數。通過修改GoogleLenet網絡結構，使它適用于本文算法空間位置回歸問題。在全連接回歸層前插入神經元數為2048 的特征向量層；移除Softmax 層，新增具有3 個神經元的全連接回歸層，用來回歸相機位置[12-14]。

在測試時，將網絡最后一層輸出作為網絡預測機器人空間位置值輸出，從而完成整個單目視覺定位算法流程。

1.3 模型訓練

損失函數采用L2 范數，如式(1)所示

X和分別表示真實的相機空間位置(x,y,z)和預測的相機空間位置，計算兩個向量間的歐氏距離。采用隨機梯度下降（stochastic gradient descent，SGD）優化方法進行訓練；采用學習率衰減策略，初始學習率設為10-5，然后每迭代80 輪衰減90%，動量因子設為0.9。訓練時間如圖4所示，迭代速度相對較快。

圖4 訓練時間隨次數變化Fig.4 Training time varies with the number of times

本文使用神經網絡訓練生成的模型大小取決于模型網絡結構，與訓練圖像數量無關，本文算法生成的模型大小如圖5，使用15579 張訓練圖像，僅需占用49 MB 內存空間。

圖5 模型大小與圖像數量關系Fig.5 Model size and number of pictures

迭代10000 次以后，訓練集loss隨迭代次數的變化如圖6，測試集loss隨迭代次數的變化如圖7，可以看出網絡收斂速度較快，損失值收斂平滑且穩定。

圖6 訓練集loss 隨迭代次數的變化Fig.6 Change of training set loss with number of iterations

圖7 測試集loss 隨迭代次數的變化Fig.7 Change of test set loss with number of iterations

2 實驗與分析

2.1 數據集制作

本次實驗中使用了TUM 的Computer Vision Lab公布的數據集。數據集下有8 位三通道彩色圖像，16位比例系數為5000 的單通道深度圖像，外部運動捕捉系統采集到的真實軌跡groundtruth，groundtruth 在實驗中作為真實軌跡參與誤差計算。數據集一共有8 列，結構如式(2)所示：

該數據集場景主要是室內場景，幀數大小不同，運動有快有慢。同時該數據集還具有不同結構和紋理的數據、運動物體等，常被用來測試視覺SLAM 系統的性能，在本文中用來測試算法的定位性能。如表1，本次實驗中所用到的數據集以TUM 數據集中的handle SLAM 系列來制作，共使用6 個動態場景如圖8，15579 張RGB 圖像及其對應的相機空間三維坐標數據，平移的平均速度為0.272 m/s，平均角速度19.378 °/s，測試期間，相機的運動包括勻速運動和變速運動，期間某些時刻用手對相機進行遮擋。

表1 TUM Handheld SLAM 系列數據Tab.1 TUM Handheld SLAM series data

圖8 數據集場景圖Fig.8 Scene graph of datasets

數據集按時間序列給出圖像及其采集時間和每個時刻的標準位置和姿態，其中給出的位姿信息頻率遠高于圖像采集頻率。首先，將圖像時間和groundtruth中的相機標準位置姿態進行對應，選取一張圖像并讀取時間，遍歷roundtruth 查找是否存在時間誤差在0.005 s 之內的位姿，若存在則將此圖像和此時刻的位置進行匹配；否則將此圖像舍棄，繼續下一張圖像與位姿的匹配，直到遍歷所有的圖像，得到的圖像標簽如式(3)。

圖9 數據集制作流程Fig.9 Data set production process

如圖9，將匹配好的圖像及其空間位置坐標隨機劃分為訓練子集和測試子集，返回劃分好的訓練集測試集樣本和訓練集測試集標簽。為了加速網絡收斂，對數據集圖像進行調整(resize)、減均值、亂序等操作，在后續的神經網絡訓練中將處理好的數據作為輸入。

2.2 對比實驗與結果分析

2.2.1 數據集實驗結果

為了驗證本文算法的有效性，分別將ORB-SLAM2 和本文算法在公共數據集和真實環境中進行了測試。計算機硬件為Intel i9-9900 K，主頻最高為3.6 GHZ，64 G 內存，顯卡為NVIDIA TITAN RTX，顯存24 G。軟件平臺為Ubuntu16.04。

測試數據集使用了6 個TUM 公開數據集，主要是室內場景包括典型的辦公環境、空曠的大廳、具有很多紋理和結構的家庭場景。真實軌跡groundtruth 是從具有八個高速跟蹤攝像機的高精度運動捕捉系統獲得。在TUM 數據集fr1_desk 上進行測試，本文算法定位軌跡結果和ORB-SLAM2 的定位結果如圖10 和圖11 所示。

圖10 本文算法輸出空間位置與實際空間位置對比Fig.10 Comparison of this paper output space position and actual space position

圖11 ORB-SLAM2 輸出空間位置與實際空間位置對比Fig.11 Comparison of ORB-SLAM2 output space position and actual space position

本文算法在空間x,y,z三個方向的定位精度和穩定性均優于ORB-SLAM2，輸出預測定位結果更貼近真實空間位置。測試結果如表2，在單目視覺情況下，相比傳統的視覺SLAM 算法，使用基于神經網絡的視覺定位算法能夠將誤差降低38%以上。在定位過程中，本文算法相比于ORB-SLAM2 的定位模塊定位精度更高也更加穩定。

表2 定位平均誤差對比Tab.2 Position average error comparison

2.2.2 實測環境實驗結果

為了驗證在實際環境中本文算法的準確性以及穩定性，通過手持相機在辦公室環境中采集視頻數據，通過運動結構恢復方法,采用Visual SFM(Structure from Motion) 及 LS-ACTS(Large-Scale Automatic Camera Tracking System)軟件在如圖12 的實驗環境中，構造出如圖13 的實驗環境點云模型，從而獲得相機在實驗過程中的運動及定位。

使用實驗環境點云模型對實驗場景的圖像數據逐一標注其空間位置[15-17]。如圖14 所示，隨著訓練圖像數量的增加，幾百張圖像使得運動恢復結構所生成的模型占用的內存高達數GB。本文使用神經網絡訓練生成的模型大小取決于模型網絡結構，與訓練圖像數量無關。因而在沒有網絡結構的情況下，隨著訓練圖像數量的增加，本文算法生成的模型大小僅需占用49.12 MB 內存空間。

在對全部圖像數據進行標注以后，進行數據集制作以及模型的訓練，并將測試圖像數據輸入模型，得到對應空間位置坐標，如圖15 所示。同時，在此視頻數據上測試ORB-SLAM2 算法的定位性能，如圖16所示。本文基于端到端模型的移動機器人室內定位算法可以進行全局定位，并且誤差在運動條件復雜的情況下依然保持相對穩定，優于傳統視覺SLAM 算法中的定位模塊。

圖12 室內實驗場景Fig.12 Indoor experiment scene

圖13 構建環境模型Fig.13 Build environment model

圖14 模型占用內存和訓練圖像數量關系Fig.14 The relationship between the model's memory usage and the number of training images

圖15 本文算法定位軌跡Fig.15 The algorithm in this paper positioning track

圖16 ORB-SLAM2 定位軌跡Fig.16 ORB-SLAM2 positioning track

3 結 論

基于深度神經網絡的移動機器人室內定位算法，直接將圖像輸入網絡，讓網絡自動學習有用的特征，避免提取人工設計的特征點，極大地降低硬件成本。對于活動范圍固定在室內的移動機器人，需要采集的環境圖像信息有限，使用基于神經網絡的移動機器人重定位算法，通過預訓練網絡模型能夠獲取相對視覺SLAM 更好的定位效果，抵抗運動過程中遮擋或視野范圍內的運動物體的干擾，且在運動速度快或場景紋理單一的環境中，不受機器人前一時刻運動的影響更具魯棒性。

對數據集標注時，受限于相機運動采集的精度以及使用運動恢復結構所構造模型的精度，將導致照片標注精度降低，從而訓練出的網絡模型在預測位置時難以取得較高的精度。在后續的研究中，將考慮聯合激光設備構造點云對圖像進行標注，進一步提高訓練數據的標注精度，從而提高室內移動機器人單目視覺定位精度。