基于視覺SLAM的履帶傳動式智能拾蛋機器人

王志豪，李銳圳，韓利芬，黃永欣，鄧 君

（東莞理工學院機械工程學院，廣東 東莞 523000）

0 引言

隨著國家的發展，我國即將踏入中等發達國家行列，而我國養殖業在農業中的比重還是比較低。近幾年隨著國家的政策扶持，我國的農村養殖業明顯增多。但是在這些農村養殖業中，養殖的自動化水平普遍較低，甚至還有許多的農戶純人工養殖。蛋類商品家禽養殖和大多數養殖業一樣，同樣是自動化水平低。蛋類商品家禽，以蛋鴨為例，個體戶養殖蛋鴨的過程中一般會選擇一次養殖5000～6000只，而在蛋鴨的產蛋期內，蛋鴨群平均每天的產蛋率能達到70%以上，在這樣的情況下，一天需要拾取的鴨蛋數為3500～4200個，而且還要進行裝箱稱重，再拿出進行裝袋。因為勞動量大，所以一般都會在產蛋期雇傭他人幫忙拾取鴨蛋，這也就減少了養殖的利潤。

SLAM技術有著非常廣闊的前景，無人駕駛技術也有涉及到該種技術，該技術在國內外都很流行。但在工業車間、農業生產中，未能很好地普及該技術，享受該技術所帶來的便利。所以本文設計了一款應用于農村養殖業的智能拾蛋機器人。所設計的智能拾蛋機器人運用目前主流的視覺SLAM技術和機器人操作系統（ROS）來實現機器人在養殖場內進行路徑規劃和導航，相對于其他可供參考的拾蛋機器人[1]，更加注重于智能化實現。參考其他智能導航機器人[2]，設計的智能拾蛋機器人應用在養殖場環境中，其搭載著深度攝像頭，來對周圍環境進行采集，用內置的主控板和驅動板來處理和驅動機器人行駛。其任務是減少勞動量，幫助人們大量拾取蛋類，增加其養殖利潤。

1 機器人整體設計

1.1 組成部分

智能拾蛋機器人在目前市場上并沒有相同或相似的產品。其整體由電腦端和機器人端組成。電腦端主要負責啟動程序運行，監看機器人端的執行情況。機器人端由視覺傳感器、控制部分、拾撿部分、底盤驅動部分等組成。

1.2 系統設計

電腦端遠程啟動機器人端進行建圖。開始時，由人手動控制電腦端來控制機器人行走進行地圖的建立，由視覺傳感器采集的圖像經由樹莓派處理得到養殖場整體的地圖。在建立好地圖后，機器人端進行路徑規劃。機器人端通過建立的地圖進行路徑規劃處理，設計合適的路徑大致走遍養殖場。且機器人在沿著路徑規劃設定的路線行走時，對鴨蛋進行拾撿。機器人的組成框架如圖1所示。機器人整體如圖2所示。

圖1 組成框架 圖2機器人整體

2 機器人硬件結構

2.1 視覺傳感器

視覺傳感器部分采用奧比中光OrbbecAstrapro深度攝像頭。該攝像頭可以采集1080 p的彩色圖像信息，且通過發收紅外光源來采集深度圖像信息，深度范圍為0.3～1 m，深度圖像分辨率為1280×720。該攝像頭延遲低，可以很好地應用在視覺SLAM中。

2.2 控制部分

機器人的控制部分由樹莓派4B（4G）主控板組成。樹莓派主控板相當于小型的電腦，可以實現部分電腦功能，有I/O口和usb口供與外部輸入輸出。樹莓派體積小，算力充足，可以用于在小型機器人上接受和處理圖像信息。

樹莓派主控板中使用ubuntu18.04LTS（Linux）系統，并安裝了ROS（Robot Operating System）機器人操作系統，以控制機器人。樹莓派也內置有WiFi模塊，用來與電腦端通信。樹莓派與底盤單片機通信是用usb接口來實現的。

2.3 底盤驅動

2.3.1 電源的選擇

本機器人采用雙電池供電。機器人整體質量較大，所需的編碼器電機的扭矩較大，所以采用12 V鋰電池來給驅動電機和拾撿裝置的電機供電。由于樹莓派運行視覺SLAM和ROS，并進行路徑規劃，所需的計算量較大，耗電較高，故使用另外的電池對其進行獨立供電。采用12V鋰電池轉接DC-DC降壓模塊（12 V轉5 V）再給樹莓派進行供電。

2.3.2 電機和驅動板的選擇

為了精確行駛，采用了帶霍爾編碼器直流減速電機（減速比1∶30），該電機扭矩大、可靠性強。在電機中使用霍爾編碼器可以反饋得到電機的轉速，從而進行閉環控制電機速度，更好地控制機器人行駛。驅動板選擇L298N電機驅動板來驅動電機轉動。

2.3.3 車輪的確定

車輪采取履帶式設計并加彈簧減震以實現機器人在不規則地面上的正常行走，由電機帶動齒輪帶動履帶前進。履帶的著地面積大，可以減小機器人對養殖場地面的壓強，并能增加牽引能力，能單次承載更多的鴨蛋。也便于在養殖場松軟泥濘或不平的地面上行駛。

2.3.4 驅動控制

樹莓派主控板發出的信息傳輸給底盤的Arduino單片機來控制各電機的運動。Arduino單片機通過單片機板上的I/O口來輸出PWM信號傳輸給L298N電機驅動板來驅動電機。

2.4 拾撿部分

機器人前的拾取裝置為履帶傳動式鴨蛋拾撿機械結構，其上板與底板均使用亞克力板，其中部使用長螺絲進行支撐，支撐點處加有軸承以實現軸的自由轉動。上板處加有直流電機用以驅動拾取裝置的運行，該直流電機可以通過調節輸入的PWM值來實現電機轉速的控制，從而控制鴨蛋拾取裝置的拾取速率。

拾取裝置結構如圖3～5所示。拾取裝置工作時，電機轉動驅動齒輪組的轉動，齒輪組通過連接件與履帶驅動齒輪相結合，經過齒輪組的傳動實現左右履帶驅動齒輪共速，從而實現左右履帶共速相向轉動。左右履帶上每個一定距離均固定一擋板，每塊擋板均為厚度為2 mm的亞克力板，其外形如圖6所示，擋板面上貼有一定厚度的海棉以實現行進拾取過程中對鴨蛋的緩沖作用，另外，擋板設有30°缺角，在實現擋板長度加長的同時使得擋板工作時能夠貼近地面。

圖3 拾撿結構

圖4 拾撿結構正視圖

圖5 拾撿結構俯視圖

圖6 擋板

當履帶運轉時，擋板在履帶的帶動下轉動，左右對稱擋板位置存在一定差異以實現兩板交匯時能有一定長度上的重合，當左右擋板交匯時兩板會共同將位于交匯點的鴨蛋帶入裝置，在左右擋板重合時鴨蛋被帶到拾蛋機器人底盤位置，并在左右擋板結束重合時被帶入機器人的儲蛋空間中，從而達到拾蛋的目的。拾蛋機器人行進過程的同時拾取裝置持續轉動，因此只需確定鴨蛋所在方位便能在機器人行進的同時實現對一個或多個鴨蛋的拾取，縮短了拾取大量鴨蛋所需時長。

3 軟件程序設計

3.1 ORB-SLAM2算法

SLAM（Simultaneous Localization and Mapping）技術用于解決將一個機器人放入未知環境中的未知位置，是否有辦法讓機器人一邊移動一邊逐步描繪出此環境完全的地圖。對于養殖場的未知環境，采用SLAM算法來用于建立地圖、進行導航是有效的。對于采用深度攝像頭（RGB-D）進行視覺SLAM技術，目前主流采用的視覺SLAM技術 有RGB-D_SLAM_V2、ORB_SLAM、RTABMap這3種方法。本文運用的是ORB-SLAM2來進行運動估計和建立地圖，ORB_SLAM2系統功能齊全，可以很好地在CPU上運行，各檢測環節使用同一特征，較其他方法有更高的效率和精確度，對運動估計和建立地圖的效果更好[3]。

ORB-SLAM2采用3個主要線程：跟蹤、局部建圖和回環檢測以及一個額外線程（全局BA）。包括基于ORB（Oriented FAST and Rotated BRIEF）圖像特征的詞袋（Bag of Words，BoW）用于位置識別及回環檢測、信息關聯視圖，用G2o圖優化來作為通用框架[4]。對于大范圍的地圖構建，該算法應用了尺度感知的回環檢測。該算法在全部的處理當中只用到了ORB來作特征點的檢測和描述，這樣一來提高了算法在位置識別和回環檢測中的實現效果。ORB-SLAM2中每個模塊中都采用ORB描述子，而不像之前的其他系統那樣跟蹤和回環檢測使用不同的特征點，這種做法一方面是節省提取時間，另一方面可以使得整個系統更加簡潔。

通過提取圖像特征點，與上幀圖像的特征點匹配，來得出相機的位姿，計算得出視覺里程計[5]。圖7所示相機圖像上的綠色部分為從當前提取的特征點，實時建立稀疏點云地圖。圖中的紅/黑色點云（紅色為局部地圖點，黑色為全局地圖點）為地圖點，藍色框為相機位姿。將ORB-SLAM2文件修改，將其與ROS系統結合，將相機的位姿和里程計生成和發布節點，用于后面的建圖和導航。ORB-SLAM2的系統框架如圖8所示。

圖7 特征點提取

圖8 ORB-SLAM2系統框架

3.2 建立地圖

3.2.1 稀疏點云地圖

在ORB-SLAM2中通過選取的關鍵幀來構建的稀疏點云地圖（圖9）對外部環境的描述較少，所獲取的信息不足以支持用于機器人的路徑規劃和導航，所以需要在ORB-SLAM2中新增加一個線程來對深度攝像頭所探測到的外部環境進行稠密建圖，獲取更多的信息。

圖9 實時建立的稀疏點云地圖

3.2.2 稠密點云地圖

通過ORB-SLAM2發布的節點信息執行稠密點云功能包來實時建立稠密點云地圖?？梢栽赗OS系統中的RVIZ圖像化工具來查看實時生成的稠密點云地圖。稠密點云地圖的建立流程如圖10所示。圖11所示為實時生成的稠密點云地圖[6]。

圖10 稠密點云圖建立流程

圖11 實時建立的稠密點云地圖

稠密點云地圖不能直接用于導航，其本質上是一堆點的集合，數據過于冗余，無法大規模使用，要對其進行去噪和cluster處理（集群技術）才能使用。比較流行的方式是使用3D稠密點云地圖處理生成八叉樹地圖（Octomap）[7]。

3.2.3 八叉樹地圖

八叉樹地圖較其他地圖靈活，壓縮率高，又可以隨時更新。該種地圖是由其最大遞歸深度不斷地進行分割來實現的，如圖12所示[8]。由于其為樹狀結構，子節點是否被占據是通過概率來表達的，但其不確定時，使用父節點來進行表達，這樣可以節約很多的空間。

圖12 八叉樹地圖

在ROS中有基于八叉樹地圖的功能包，可以將接收的稠密點云信息實時轉為八叉樹地圖信息，如圖13所示。八叉樹地圖相對于稠密點云地圖所需的內存大大縮小，更便于用在機器人的導航上[9]。使用八叉樹地圖進行壓縮濾除地面信息，再通過2D投影生成柵格地圖，處理后生成代價地圖（Costmap），最后利用代價地圖進行全局和局部路徑規劃，來進行實時避障和導航。圖中底部白色部分為投影的2D柵格地圖。

圖13 實時建立的八叉樹地圖

3.2.4 代價地圖

代價地圖的生成需要確定劃分柵格的大小，對柵格的標識和對柵格信息的編碼。

2D柵格地圖的柵格根據八叉樹地圖來確定其大小。代價地圖的對柵格的標識方法主要用坐標標識法來進行標識。柵格信息編碼主要是設定代價函數，根據與障礙的距離代入所設定的代價函數中進行編碼。柵格代價分布如圖14所示[10]。

圖14 柵格代價分布

3.3 路徑規劃

3.3.1 全局路徑規劃

全局路徑規劃的算法有多種，例如Dijikstra、A*算法、rrt算法、BFS算法等。而在ROS框架下有Dijkstra和A*算法[11]。Dijikstra可以支持搜索較大面積的地圖空間，該算法開始是以起點向周圍擴散。而A*算法則更專注于目標，選擇的是近似的最優路徑，對比于其他算法更加優化。結合實際環境，該機器人選擇的是A*算法。

A*算法由下列函數計算得到每個節點的優先級：

式中：f（n）為節點n的綜合優先級，當選擇下個需要遍歷的節點時，總選取優先級最高、即其值最小的節點；g（n）為節點n與路徑起點的代價值；h（n）為節點與路徑終點所預測的代價值。這也是A*算法的啟發函數。

3.3.2 局部路徑規劃

機器人在建立好了地圖后，通過全局路徑規劃得到的算法可以得到一條路線，但對于實際環境中的變化，還是需要進行局部路徑規劃來進行調整，避開之前所建立的地圖中所未被檢測到的、或者動態的障礙。在ROS中，是基于DWA算法來進行和實現局部路徑規劃部分的，每當進行移動時，在ROS中內置的導航系統就會自主調用該DWA算法來分析和規劃函數，來計算得出機器人的最優運動指令來控制機器人進行移動[12]。DWA算法實現的流程如圖15所示。

圖15 DWA算法流程

4 驗證和結果分析

4.1 測試環境搭建

在實驗室搭建一個大致的可供拾蛋機器人行走的室內環境，放置了障礙物和鴨蛋。障礙物主要測試機器人的建圖和避障功能，隨機放置的鴨蛋主要測試拾蛋機器人拾蛋結構的拾蛋能力。

4.2 軟件部分

在建圖和避障部分的測試，拾蛋機器人能較好地完成。在建立地圖部分，由于深度攝像頭的檢測范圍有限，并不能像激光雷達可以環繞著四周進行地圖的建立，所以建圖的時間長。避障方面，在大多數情況下都能很好地避開障礙，繼續向規劃好的路徑行走，但在一些特殊的角落，沒能很好地進行處理，拾蛋機器人行走緩慢、有時會卡頓。

4.3 硬件部分

拾蛋結構可以很好地拾撿鴨蛋，在拾蛋機器人前端較大的范圍內遇到的鴨蛋都能很好地進行收集。收集的過程中，鴨蛋并無破損，能夠很好地完成拾蛋作業。

5 結束語

本文系統地介紹了通過視覺SLAM技術結合ROS系統實現地圖構建、路徑規劃、自主導航的具體過程和自主設計的蛋類拾撿機械裝置的結構。通過各種渠道進行相關信息的收集，最后設計出了一種能夠綜合實現拾撿鴨蛋和路徑規劃的實用性強的智能機器人。能夠很好地減少勞動量、節約勞動成本、提高生產效率。