基于視覺的機器人自主定位與障礙物檢測方法

丁斗建 趙曉林 王長根 高關(guān)根 寇磊

摘 要：針對稀疏型同時定位與地圖構(gòu)建（SLAM）算法環(huán)境信息丟失導(dǎo)致無法檢測障礙物問題，提出一種基于視覺的機器人自主定位與障礙物檢測方法。首先，利用雙目相機得到觀測場景的視差圖。然后，在機器人操作系統(tǒng)（ROS）架構(gòu)下，同時運行定位與建圖和障礙物檢測兩個節(jié)點。定位與建圖節(jié)點基于ORB-SLAM2完成位姿估計與環(huán)境建圖。障礙物檢測節(jié)點引入深度閾值，將視差圖二值化;運用輪廓提取算法得到障礙物輪廓信息并計算障礙物凸包面積;再引入面積閾值，剔除誤檢測區(qū)域，從而實時準確地解算出障礙物坐標。最后，將檢測到的障礙物信息插入到環(huán)境的稀疏特征地圖當中。實驗結(jié)果表明，該方法能夠在實現(xiàn)機器人自主定位的同時，快速檢測出環(huán)境中的障礙物，檢測精度能夠保證機器人順利避障。

關(guān)鍵詞：視覺定位;障礙物檢測;視覺同時定位與地圖構(gòu)建;機器人操作系統(tǒng);立體視覺;機器人

中圖分類號： TP242.6智能機器人

文獻標志碼：A

Abstract： Aiming at the obstacle detection problem caused by the loss of environmental information in sparse Simultaneous Localization And Mapping （SLAM） algorithm， an autonomous location and obstacle detection method of robot based on vision was proposed. Firstly， the parallax map of the observed scene was obtained by binocular camera. Secondly， under the framework of Robot Operating System （ROS）， localization and mapping node and obstacle detection node were operated simultaneously. The localization and mapping node completed pose estimation and map building based on ORB-SLAM2. In the obstacle detection node， a depth threshold was introduced to binarize the parallax graph and the contour extraction algorithm was used to obtain the contour information of the obstacle and calculate the convex hull area of the obstacle， then an area threshold was introduced to eliminate the false detection areas， so as to accurately obtain the coordinates of obstacles in real time. Finally， the detected obstacle information was inserted into the sparse feature map of the environment. Experiment results show that this method can quickly detect obstacles in the environment while realizing autonomous localization of the robot， and the detection accuracy can ensure the robot to avoid obstacles smoothly.

Key words： visual localization; obstacle detection; Visual Simultaneous Localization And Mapping （VSLAM）; robot operating system; stereo vision; robot

0 引言

隨著機器人應(yīng)用領(lǐng)域的不斷拓展，對其自主性的要求也在不斷提升[1]。要實現(xiàn)機器人自主移動，自主定位技術(shù)和自主避障技術(shù)是最為關(guān)鍵的兩個方面。在機器人自主定位與自主避障研究中，視覺傳感器由于具有功耗低、價格便宜、可以獲得更加豐富的環(huán)境信息等優(yōu)點[2]，得到了廣泛應(yīng)用[3-4]。

基于視覺的機器人自主定位研究已有近二十年的歷史。文獻[5]中提出視覺里程計以后，基于視覺里程計的視覺同時定位與地圖構(gòu)建（Visual Simultaneous Localization And Mapping， VSLAM）算法受到廣泛的關(guān)注。VSLAM根據(jù)所利用的圖像信息可分為基于特征的稀疏方法和直接的稠密方法。稠密方法可以建立密集的環(huán)境地圖，便于后續(xù)的自主導(dǎo)航，具有代表性的工作是LSD-SLAM[6]。但是，稠密方法計算量很大，難以做到較好的實時性，因此，出于減少計算量、提高系統(tǒng)實時性的目的，稀疏型SLAM受到了許多研究者的青睞。文獻[7]使用尺度不變特征變換（Scale Invariant Feature Transform， SIFT）特征檢測方法，解決了特征匹配中圖像旋轉(zhuǎn)、縮放以及光照變化的影響，但SIFT特征的提取本身就比較耗時。文獻[8]在特征匹配過程中應(yīng)用了極線約束，提高了匹配的速度與精度。文獻[9]提供了一個大場景VSLAM框架，采用關(guān)鍵幀優(yōu)化方法，成為了VSLAM的一個特色，但由于缺少回環(huán)檢測，定位的精度并不高。針對這一問題，文獻[10]提出一種基于ORB（Oriented FAST and Rotated BRIEF）特征的SLAM解決方案，通過加入回環(huán)檢測線程和自動初始化功能，全面提高了系統(tǒng)的性能。但是，以上提到的方法都是通過特征點來建圖，得到的只是稀疏的點圖，只反映了環(huán)境的局部特征信息。因此，只依賴稀疏型SLAM算法，能夠?qū)崿F(xiàn)較好的定位，卻無法辨別出環(huán)境中的障礙物，難以直接用于后續(xù)的避障與路徑規(guī)劃等工作中。

為解決上述問題，文獻[11]提出了一種基于全局觀測地圖模型的SLAM方案，將全局密集信息嵌入到稀疏特征地圖中，并將EKF-SLAM算法與全局地圖觀測模型相結(jié)合，得到了較為可信的密集地圖。文獻[12]基于雙目立體視覺，提出了一種柵格地圖構(gòu)建方法，得到了包含環(huán)境幾何信息的地圖。文獻[13]利用迭代濾波以及高斯分布對數(shù)據(jù)進行稠密化處理，實現(xiàn)了對稀疏性VSLAM地圖的數(shù)據(jù)補插。以上方法基本都是致力于對稀疏特征地圖進行數(shù)據(jù)補充，在得到密集地圖的同時，也會給系統(tǒng)引入較大的計算量。然而，機器人實現(xiàn)一般的自主移動，并不都需要所有的環(huán)境信息，很多情況下只需檢測出環(huán)境中的障礙物即可滿足自主移動要求。

針對上述情況，本文利用立體視覺法檢測障礙物來彌補稀疏型SLAM環(huán)境交互能力弱的缺陷，提出了一種基于視覺的機器人自主定位與障礙物檢測方法。首先，利用雙目相機得到場景視差圖，在視差圖的基礎(chǔ)上，同時運行兩個節(jié)點。定位與建圖節(jié)點依賴成熟的視覺SLAM算法ORB-SLAM2[14]實現(xiàn)相應(yīng)功能。障礙物檢測節(jié)點首先對視差圖進行三維重建與偽彩色處理，得到特征點的三維信息并將疑似障礙物區(qū)域紅色顯示;然后依據(jù)機器人運動特性將視差圖二值化，在二值圖像基礎(chǔ)上提取障礙物輪廓與位置信息并將障礙物信息插入環(huán)境地圖。通過實驗驗證可知，該方法能夠在機器人SLAM的同時，快速檢測出環(huán)境中的障礙物，且檢測效果較好。

1 算法設(shè)計

1.1 算法整體架構(gòu)

算法主要由三個模塊組成，如圖1所示，包括視差圖獲取模塊、障礙物檢測模塊和定位與建圖模塊。

視差圖獲取模塊首先由雙目相機采集場景圖像，并轉(zhuǎn)換為灰度圖像;基于相機標定模型，消除圖片畸變并對圖片進行立體校正;然后，利用ORB特征提取算法提取左右圖像的特征點信息;最后，采用半全局立體匹配（Semi-Global Block Matching， SGBM）算法[15]計算視差，得到較為精確的致密視差圖。得到視差圖之后，定位與建圖模塊依據(jù)ORB-SLAM2算法，實現(xiàn)相機位姿的跟蹤并建立環(huán)境的稀疏特征地圖。與此同時，障礙物檢測模塊依據(jù)立體視覺法檢測出環(huán)境中障礙物，并將障礙物信息插入到定位與建圖模塊所建立的環(huán)境地圖當中。

算法最終得到一幅具有障礙物信息的環(huán)境地圖，這一地圖包含了障礙物坐標信息、特征點三維信息和相機位姿，在ORB-SLAM2基礎(chǔ)上實現(xiàn)了環(huán)境信息的補充，為機器人與環(huán)境的交互提供依據(jù)，同時為后續(xù)的路徑規(guī)劃打下基礎(chǔ)。

1.2 基于ORB-SLAM2的定位與建圖算法

定位與建圖算法基于ORB-SLAM2實現(xiàn)，ORB-SLAM2功能非常全面，并且具有良好的精度和實時性，是當前稀疏型SLAM的代表。因此，本文選擇ORB-SLAM2的雙目部分完成算法的定位與建圖模塊，其整體的結(jié)構(gòu)框圖如圖2所示，主要包含了位姿跟蹤、局部構(gòu)圖和回環(huán)檢測三個線程。

1.3 基于立體視覺的障礙物檢測算法

雙目立體視覺是通過兩個不同位置的攝像機采集場景圖像，根據(jù)兩幅不同角度的場景圖中的對應(yīng)點來計算視差。本文在視差圖的基礎(chǔ)上，設(shè)計了障礙物檢測算法。

首先，在視差圖基礎(chǔ)上，利用三角測量原理將圖像轉(zhuǎn)換成三維點云。給定視差d、重投影矩陣Q和點坐標（x，y），根據(jù)最簡單的相似三角形關(guān)系，就可以得到對應(yīng)的三維坐標（X/W，Y/W，Z/W），其轉(zhuǎn)換公式為：

2 基于ROS的軟件架構(gòu)設(shè)計

本文算法基于機器人操作系統(tǒng)（Robot Operating System， ROS）架構(gòu)實現(xiàn)，系統(tǒng)的整體流程如圖3所示。

系統(tǒng)在開始階段，首先從外部文件中讀取攝像頭參數(shù)并將參數(shù)初始化;然后，由圖像采集節(jié)點不斷地發(fā)布相機采集的場景圖像，得到圖像信息后，ORB-SLAM2節(jié)點與障礙物檢測節(jié)點同時運行并訂閱圖像信息。ORB-SLAM2節(jié)點逐漸完成系統(tǒng)的初始化并針對輸入的圖像不斷執(zhí)行位姿跟蹤、局部構(gòu)圖和回環(huán)檢測三個線程，從而實現(xiàn)相機位姿的跟蹤和環(huán)境特征地圖構(gòu)建。障礙物檢測節(jié)點首先對攝像頭參數(shù)進行檢查，在獲得正確的參數(shù)情況下完成圖像的立體匹配得到視差圖，然后在視差圖的基礎(chǔ)上依次執(zhí)行算法的各個步驟，從而實現(xiàn)環(huán)境中障礙物的檢測并將障礙物信息發(fā)布出去，這一信息將會被ORB-SLAM2節(jié)點訂閱并將其插入到環(huán)境地圖中。當兩個節(jié)點同時判斷系統(tǒng)關(guān)閉時關(guān)閉整個系統(tǒng)。

系統(tǒng)的節(jié)點總體設(shè)計如圖4所示。虛線框外的方框代表的是外部節(jié)點，分別是機器人控制節(jié)點和傳感器之間的坐標轉(zhuǎn)換節(jié)點。虛線框內(nèi)的是本文運行的節(jié)點，圖像采集節(jié)點主要完成圖像的采集和分發(fā);障礙物檢測節(jié)點訂閱來自圖像采集節(jié)點的圖像信息并實現(xiàn)障礙物檢測和發(fā)布障礙物的信息;ORB_SLAM2節(jié)點訂閱來自圖像采集節(jié)點的圖像信息處理后，實時發(fā)布相機的位姿信息，并訂閱來自障礙物檢測節(jié)點的障礙物信息插入環(huán)境地圖。

3 實驗與結(jié)果分析

3.1 數(shù)據(jù)集測試

針對系統(tǒng)的定位性能，本文利用著名的雙目數(shù)據(jù)集KITTI[18]對系統(tǒng)進行測試，實驗設(shè)備為一臺配備Intel core i7處理器的筆記本電腦，并裝有64位的Ubuntu16.04操作系統(tǒng)。各數(shù)據(jù)子集上不同方法的定位均方根誤差結(jié)果對比如表1所示。

從表1中可以看出，在大多數(shù)數(shù)據(jù)子集的測試中，本文算法的定位精度優(yōu)于稠密方法LSD-SLAM，且和ORB-SLAM2的定位精度基本一致，這是由于本文的定位與建圖算法是基于ORB-SLAM2的雙目模塊實現(xiàn)的，因此能夠保證較高的定位精度。

3.2 室內(nèi)實物實驗

3.2.1 實驗硬件選型

實驗硬件系統(tǒng)由兩個同一型號的USB攝像機和圖像處理終端共同組成，并將其安裝在輪式機器人Arduino 4WD上進行實驗。為了實現(xiàn)左右圖像序列的獲取，本文選擇兩個內(nèi)參數(shù)相近的JD-202USB攝像機，如圖5（a）所示，其感光元件為CMOS，最大分辨率為1280×720，幀數(shù)率為30frame/s。為了提高圖像處理能力，本文選擇了香蕉派BPI-M3作為圖像處理終端，如圖5（b）所示。香蕉派BPI-M3是一個擁有8核1.8GHz處理器和2GB LPDDR3內(nèi)存的超級單板電腦，它可以運行Android、Ubuntu等操作系統(tǒng)，具有計算處理速度快、外設(shè)接口豐富、體積小巧輕便易掛載和開源的社區(qū)等特點，滿足本文算法的設(shè)計需求。

3.2.2 實驗方案設(shè)計

實驗數(shù)據(jù)來源于實驗室運動場景，運動場景的平面圖以及設(shè)計的運動軌跡如圖6所示。機器人在室內(nèi)按預(yù)定軌跡運動一周，運動的途中，為在場景圖標注的三個障礙物點設(shè)定三種不同的障礙物環(huán)境，以驗證算法在不同環(huán)境下的障礙物檢測效果。障礙物1為簡單環(huán)境，只設(shè)置一個紋理清晰的靜態(tài)障礙物;障礙物2為復(fù)雜環(huán)境，是在簡單環(huán)境的基礎(chǔ)上添加了多個不同的靜態(tài)障礙物;障礙物3為動態(tài)環(huán)境，是指將靜態(tài)障礙物換成運動的履帶小車。

簡單環(huán)境和復(fù)雜環(huán)境實驗過程中，分別在機器人距離障礙物0.3m、0.8m、1.5m、2.0m、2.5m、3.0m的地方，對相應(yīng)場景進行障礙物檢測，分別記錄系統(tǒng)的檢測距離、單幀處理時間和誤檢率。動態(tài)障礙物環(huán)境下，履帶車在機器人前以不同的速度進行運動，以此來檢驗系統(tǒng)對于運動目標的檢測能力。深度閾值取對應(yīng)的場景距離分別為1.8m、2.5m、3.2m的三個值，在不同閾值下進行多次實驗;面積閾值取為100個像素點，將凸包面積小于100個像素點的區(qū)域視為誤檢測區(qū)域，予以剔除。

3.2.3 結(jié)果分析

對于實物實驗的自主定位性能，本文只針對實驗結(jié)果作簡要分析。精度方面，形成閉環(huán)路徑前，系統(tǒng)生成的路徑會出現(xiàn)局部的偏移，閉環(huán)路徑形成后，系統(tǒng)開始進行全局優(yōu)化，對軌跡圖和位姿進行調(diào)整。將生成的路徑與設(shè)計的路徑比較，可以發(fā)現(xiàn)整體生成圖與設(shè)計的路線擬合得比較好，沒有出現(xiàn)較大的偏差。實時性方面，系統(tǒng)啟動以后，每秒處理幀數(shù)在14～18，可以滿足機器人一般運動狀態(tài)下的定位需要。穩(wěn)定性方面，實驗過程中，有時會出現(xiàn)跟蹤丟失的情況，但是由于采用了關(guān)鍵幀存取，實現(xiàn)了系統(tǒng)快速的重定位能力，可以保證穩(wěn)定的定位輸出。

障礙物檢測方面，三種環(huán)境下不同時刻的障礙物檢測結(jié)果如圖8～10所示，圖中子圖（a）、（b）、（c）分別為機器人在距離障礙物2.0m、1.5m和0.8m處的實驗結(jié)果。在每幅子圖中，從上到下依次為左圖像，右圖像，偽彩色圖像和障礙物提取圖像。簡單環(huán)境和復(fù)雜環(huán)境的測試距離分別由遠到近進行，動態(tài)障礙物的運動分為沿相機坐標系的X軸和Z軸方向運動。

4 結(jié)語

本文主要研究了基于視覺的機器人自主定位和障礙物檢測問題，利用典型稀疏性SLAM算法ORB-SLAM2實現(xiàn)了算法的自主定位功能。針對ORB-SLAM2存在的環(huán)境信息丟失的問題，在ROS架構(gòu)下，引入了基于深度的障礙物檢測算法，解決了其不能檢測障礙物的問題。在基于立體視覺的障礙物檢測算法中，進一步引入深度與面積閾值，提高算法的檢測精度。將所提算法在搭建的實驗平臺上進行驗證，實驗結(jié)果表明本文系統(tǒng)是有效可行的。如何在具有障礙物信息的環(huán)境地圖中進行路徑規(guī)劃是未來需要進一步研究的方向。

參考文獻 （References）

[1] 楊洋，童東兵，陳巧玉.面向未知地圖的六足機器人路徑規(guī)劃算法[J].計算機應(yīng)用，2018，38（6）：1809-1813.（YANG Y， TONG D B， CHEN Q Y. Six-legged robot path planning algorithm for unknown map [J]. Journal of Computer Applications， 2018， 38（6）： 1809-1813.）

[2] NAYA B， SABRINA H， STEFAN W， et al. Navigation and vision system of a mobile robot [C]// Proceedings of the 2018 19th International Conference on Research and Education in Mechatronics. Piscataway， NJ： IEEE， 2018： 99-104.

[3] 胡章芳，鮑合章，陳旭，等.基于改進閉環(huán)檢測算法的視覺同時定位與地圖構(gòu)建[J].計算機應(yīng)用，2018，38（3）：873-878.（HU Z F， BAO H Z， CHEN X， et al. Visual simultaneous location and mapping based on improved closed-loop detection algorithm [J]. Journal of Computer Applications， 2018， 38（3）： 873-878.）

[4] 張永，楊浩.基于優(yōu)化視覺詞袋模型的圖像分類方法[J].計算機應(yīng)用，2017，37（8）：2244-2247，2252.（ZHANG Y， YANG H. Image classification method based on optimized bag-of-visual words model [J]. Journal of Computer Applications， 2017， 37（8）： 2244-2247， 2252.）

[5] NISTER D， NARODITSKY O， BERGEN J R. Visual odometry [C]// Proceedings of the 2004 IEEE Computer Society Conference on Computer Vision and Pattern Recognition. Piscataway， NJ：IEEE， 2004： 652-659.

[6] ENGEL J， SCHOPS T， CREMERS D. LSD-SLAM： large-scale direct monocular SLAM [C] // Proceedings of the 2014 13th European Conference on Computer Vision， LNCS 8690. Berlin： Springer， 2014： 834-849.

[7] SIM R， ELINAS P， GRIFFIN M， et al. Design and analysis of a framework for real-time vision-based SLAM using Rao-Blackwellised particle filters [C]// CRV 2006： Proceedings of the 3rd Canadian Conference on Computer and Robot Vision. Washington， DC： IEEE Computer Society， 2006： 21-29.

[8] YANG M L， LIU Y G， YOU Z S. The Euclidean embedding learning based on convolutional neural network for stereo matching [J]. Neurocomputing， 2017， 267： 195-200.

[9] KLEIN G， MURRAY D. Parallel tracking and mapping for small AR workspaces [C]// ISMAR 2007： Proceedings of the 2007 6th IEEE and ACM International Symposium on Mixed and Augmented Reality. Washington， DC： IEEE Computer Society， 2007： 225-234.

[10] MUR-ARTAL R， MONTIEL J M M， TARDOS J D. ORB-SLAM： a versatile and accurate monocular SLAM system [J]. IEEE Transactions on Robotics， 2015， 31（5）： 1147-1163.

[11] 周武，趙春霞，沈亞強，等.基于全局觀測地圖模型的SLAM研究[J].機器人，2010，32（5）：647-654.（ZHOU W， ZHAO C X， SHEN Y Q， et al. SLAM research based on global observation map model [J]. Robot， 2010， 32（5）： 647-654.）

[12] 王軒，葉平，賈慶軒.雙目立體視覺柵格地圖構(gòu)建方法[J].軟件，2012，33（11）：233-236.（WANG X， YE P， JIA Q X. Stereo vision location and grid map building method [J]. Computer Engineering & Software， 2012， 33（11）： 233-236.）

[13] 陳煒楠，朱蕾，張宏，等.稀疏視覺SLAM對平面激光雷達傳感的稠密化模擬[J].機器人，2018，40（3）：273-281.（CHEN W N， ZHU L， ZHANG H， et al. Planar LiDAR densified simulation from sparse visual SLAM [J]. Robot， 2018， 40（3）： 273-281.）

[14] MUR-ARTAL R， TARDOS J D. ORB-SLAM2： an open-source SLAM system for monocular， stereo and RGB-D cameras [J]. IEEE Transactions on Robotics， 2016， 33（5）： 1255-1262.

[15] HIRSCHMULLER H. Accurate and efficient stereo processing by semi-global matching and mutual information [C]// CVPR 2005： Proceedings of the 2005 IEEE Computer Society Conference on Computer Vision and Pattern Recognition. Washington， DC： IEEE Computer Society， 2005： 807-814.

[16] GALVEZ-LOPEZ D， TARDOS J D. Bags of binary words for fast place recognition in image sequences [J]. IEEE Transactions on Robotics， 2012， 28（5）： 1188-1197.

[17] SUZUKI S， ABE K. Topological structural analysis of digitized binary images by border following [J]. Computer Vision， Graphics， and Image Processing， 1985， 30（1）： 32-46.

[18] GEIGER A， LENZ P， STILLER C， et al. Vision meets robotics： the KITTI dataset [J]. International Journal of Robotics Research， 2013， 32（11）： 1231-1237.