基于ROS的垃圾督導(dǎo)機(jī)器人導(dǎo)航系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

何海濤　陳勁杰

關(guān)鍵詞：機(jī)器人；導(dǎo)航；ROS

中圖分類號(hào)：TP311 文獻(xiàn)標(biāo)識(shí)碼：A

1 引言（Introduction）

2019年7月1日起《上海市生活垃圾管理?xiàng)l例》正式實(shí)施，上海成為全國(guó)第一個(gè)立法推進(jìn)垃圾分類的省級(jí)行政區(qū)[1]，同時(shí)上海各高校也紛紛實(shí)行垃圾分類措施，定時(shí)定點(diǎn)進(jìn)行垃圾投放，但是部分同學(xué)仍然在規(guī)定時(shí)間外隨意投放垃圾，該行為為學(xué)校后勤工作帶來(lái)了不便?；趯W(xué)校后勤需求，有必要設(shè)計(jì)一款垃圾督導(dǎo)機(jī)器人對(duì)同學(xué)隨意丟棄垃圾的行為進(jìn)行文明、友好的監(jiān)督。然而，機(jī)器人系統(tǒng)的復(fù)雜性為垃圾督導(dǎo)機(jī)器人的開發(fā)帶來(lái)很大的困難。機(jī)器人操作系統(tǒng)（ROS）的出現(xiàn)，大大降低機(jī)器人的開發(fā)門檻[2]，功能也越來(lái)越成熟[3]。ROS使用分布式架構(gòu)，有大量開源用于實(shí)現(xiàn)特定功能的機(jī)器人應(yīng)用程序，稱為功能包，功能包單獨(dú)設(shè)計(jì)，在運(yùn)行時(shí)松散耦合，提高了代碼復(fù)用率[4]。因此，開發(fā)者不需要重復(fù)造輪子，可以像搭建積木一樣快速搭建各種功能模塊，專注于某一領(lǐng)域的研究，快速完成開發(fā)[5]。

導(dǎo)航功能是該機(jī)器人核心和基礎(chǔ)功能，其功能的好壞和開發(fā)周期關(guān)系到機(jī)器人的魯棒性和生產(chǎn)成本。因此，本文基于ROS提供的功能包設(shè)計(jì)并開發(fā)了垃圾督導(dǎo)機(jī)器人導(dǎo)航系統(tǒng)。

2 系統(tǒng)整體設(shè)計(jì)（Overall system design）

垃圾督導(dǎo)機(jī)器人主要由底層驅(qū)動(dòng)模塊和頂層控制模塊構(gòu)成，其整體系統(tǒng)設(shè)計(jì)如圖1所示。

底層驅(qū)動(dòng)模塊是垃圾督導(dǎo)機(jī)器人控制機(jī)器人運(yùn)動(dòng)的核心。從控制器使用的是ST公司基于Co t e x-M3內(nèi)核的STM32RCT6，其板載資源能夠滿足整個(gè)控制系統(tǒng)的使用。此外，在該開發(fā)板上移植了FreeRTOS（Free Real-TimeOperating System， 自由實(shí)時(shí)操作系統(tǒng)），使用默認(rèn)的搶占式調(diào)度，能夠保證系統(tǒng)的可靠性和實(shí)時(shí)性[6]，使得不同任務(wù)解耦，方便對(duì)后續(xù)的任務(wù)進(jìn)行擴(kuò)展。

從控制器通過獲取編碼器信息，計(jì)算得出機(jī)器人的線速度與角速度，并通過PWM（Pulse Width Modulation，脈沖寬度調(diào)制）驅(qū)動(dòng)電機(jī)控制板，進(jìn)而控制電機(jī)的速度；通過模擬I2C（Inter-Integrated Circuit，串行雙向總線）獲取MPU6050（整合性6軸運(yùn)動(dòng)處理組件）的陀螺儀和加速度信息，然后將機(jī)器人的速度、陀螺儀和加速度等信息通過串口上傳到主控制器，此外將MPU6050放置于機(jī)器人的中心，以便獲取準(zhǔn)確的信息。

頂層控制模塊是垃圾督導(dǎo)機(jī)器人導(dǎo)航系統(tǒng)的主控制中心，主要對(duì)整個(gè)系統(tǒng)進(jìn)行決策。主控制器使用的是JetsonNano，是一款搭載低配GPU（Graphics Processing Unit，顯卡）運(yùn)算平臺(tái)的小型計(jì)算機(jī)[7]。該主控制器控制2個(gè)激光雷達(dá)獲取二維平面點(diǎn)云信息，并且通過串口向從控制器以比特流的方式發(fā)送線速度和角速度來(lái)控制電機(jī)的速度，從而控制機(jī)器人的移動(dòng)。此外，主控制器通過獲取從控制器發(fā)過來(lái)的速度信息計(jì)算得出機(jī)器人的里程計(jì)，并獲取從控制器發(fā)送的陀螺儀和加速度信息，結(jié)合從雷達(dá)獲取的二維點(diǎn)云數(shù)據(jù)，完成機(jī)器人的定位建圖和導(dǎo)航功能，并基于上述功能，使用狀態(tài)機(jī)完成巡邏任務(wù)。

3 系統(tǒng)硬件設(shè)計(jì)（System hardware design）

該機(jī)器人以四輪差速移動(dòng)機(jī)器人為原型，在進(jìn)行結(jié)構(gòu)設(shè)計(jì)時(shí)，對(duì)部分空間進(jìn)行分層，放置不同功能的硬件設(shè)備，其硬件結(jié)構(gòu)如圖2所示。

激光雷達(dá)為“思嵐”A1激光雷達(dá)，可以獲取雷達(dá)平面的二維點(diǎn)云數(shù)據(jù)，由于該機(jī)器人是在平地上運(yùn)行，因此使用二維雷達(dá)可以滿足機(jī)器人的運(yùn)行要求。為了讓機(jī)器人能夠識(shí)別低處的障礙物，不宜將雷達(dá)放置在高點(diǎn)，應(yīng)放置于底盤位置，并且在前后位置各放1個(gè)，以加快機(jī)器人對(duì)地圖的建立速度并提升定位的準(zhǔn)確性。

4 系統(tǒng)軟件設(shè)計(jì)（System software design）

軟件部分主要由底層驅(qū)動(dòng)模塊和頂層控制模塊構(gòu)成。

4.1 底層驅(qū)動(dòng)模塊軟件設(shè)計(jì)

從控制器使用STM32F103RCT6，其系統(tǒng)軟件結(jié)構(gòu)如圖3所示，移植了FreeRTOS，并且總共設(shè)置了4個(gè)任務(wù)，并且使用中斷接收主控制器發(fā)送過來(lái)的速度控制信息，控制機(jī)器人運(yùn)動(dòng)，通過查看LED是否閃爍，判斷系統(tǒng)是否正常運(yùn)行。

4.1.1 當(dāng)前運(yùn)動(dòng)速度獲取

在每個(gè)電機(jī)的尾部都裝有增量式光電編碼器，通過捕獲對(duì)應(yīng)的脈沖，可以計(jì)算出電機(jī)的轉(zhuǎn)速。假設(shè)編碼器的分辨率為M，設(shè)在一個(gè)時(shí)間周期t（s）的范圍內(nèi)，通過輸入捕獲得到的脈沖數(shù)為N，那么可以計(jì)算得出電機(jī)的實(shí)際轉(zhuǎn)速n（r/min）。

4.1.3 MPU6050信息讀取

MPU6050是一款常用的IMU（Inertial MeasurementUnit，慣性測(cè)量單元），使用模擬I2C周期性讀取MPU6050的陀螺儀和加速度信息，在MPU6050上電之后，需要等待一段時(shí)間穩(wěn)定后再對(duì)其進(jìn)行讀數(shù)，此外水平面要平穩(wěn)且放置于機(jī)器人的中間，提高其測(cè)量穩(wěn)定性。

4.1.4 串口通信發(fā)送數(shù)據(jù)信息

使用串口發(fā)送的數(shù)據(jù)，進(jìn)行數(shù)據(jù)處理的時(shí)候，均沒有使用浮點(diǎn)類型，而是使用16位短整型，將參數(shù)擴(kuò)大了1 000倍，STM32F103系列沒有浮點(diǎn)運(yùn)算單元，如果計(jì)算浮點(diǎn)運(yùn)算，速度比較慢，會(huì)影響其運(yùn)行速度；并且，由于使用串口發(fā)送，沒有校驗(yàn)，因此在數(shù)據(jù)幀中添加了簡(jiǎn)易的異或校驗(yàn)進(jìn)行數(shù)據(jù)幀格式的校驗(yàn)，保證數(shù)據(jù)傳輸?shù)恼_性。

4.2 頂層控制模塊軟件設(shè)計(jì)

主控制器使用Jetson Nano，環(huán)境為Ubuntu18.04，安裝ROS版本為Melodic。ROS提供了Gazebo（開源機(jī)器人仿真器）進(jìn)行機(jī)器人仿真，使用Rviz（ROS Visualization，ROS可視化工具）可視化界面，實(shí)時(shí)查看機(jī)器人的行為動(dòng)作。

垃圾督導(dǎo)機(jī)器人的巡邏任務(wù)主要基于定位建圖和導(dǎo)航功能完成。定位建圖與導(dǎo)航框架如圖6所示。

4.2.1 定位建圖

由于垃圾督導(dǎo)機(jī)器人只是在一小片區(qū)域活動(dòng)，因此建圖方法選擇Gmapping（Grid-based FastSLAM mapping，基于柵格的快速建圖算法） 建圖， 是一種基于濾波的SLAM（Simultaneous Localization and Mapping， 同時(shí)定位與建圖）框架[8-9]，其在使用構(gòu)建小區(qū)域地圖時(shí)所需的計(jì)算量較小且在室外測(cè)試具有良好的效果。由于有2個(gè)激光雷達(dá)，需要對(duì)兩個(gè)激光雷達(dá)的信息進(jìn)行融合，因此使用ROS提供的laser_tools功能包對(duì)2個(gè)激光雷達(dá)的點(diǎn)云數(shù)據(jù)進(jìn)行融合。由于安裝在前面的雷達(dá)后半部分會(huì)被機(jī)器人自身?yè)踝?，后面雷達(dá)的前面部分也會(huì)被機(jī)器人自身遮擋，在實(shí)際測(cè)試中如果不進(jìn)行處理，機(jī)器人會(huì)認(rèn)為其位置存在障礙，導(dǎo)致其導(dǎo)航功能失效，機(jī)器人會(huì)不停地在原地?cái)[動(dòng)，不斷進(jìn)行軟件復(fù)位，因此可以使用ROS中的功能包laser_filters設(shè)置需要過濾的區(qū)域，避免對(duì)建圖導(dǎo)航等產(chǎn)生影響。

此外，機(jī)器人在地圖中的定位也十分重要，只有讓機(jī)器人知道身處的確定位置，才能根據(jù)路徑規(guī)劃到達(dá)下一個(gè)目的地，這就需要較高的定位精確度。使用Gmapping建圖算法與AMCL（Adaptive Monte Carlo Localization，自適應(yīng)蒙特卡羅定位）進(jìn)行定位，非常依賴?yán)锍逃?jì)數(shù)據(jù)?，F(xiàn)實(shí)生活中，機(jī)器人在運(yùn)動(dòng)過程中不可避免地會(huì)發(fā)生打滑，導(dǎo)致建圖與重定位的效果不理想。為改善這種情況，使用EKF（ExtendedKalman Filter， 擴(kuò)展卡爾曼濾波）算法對(duì)輪式里程計(jì)與IMU數(shù)據(jù)進(jìn)行融合，從而進(jìn)一步提升里程計(jì)數(shù)據(jù)的準(zhǔn)確度[10]，進(jìn)而提高定位精確度。

4.2.2 導(dǎo)航

全局路徑規(guī)劃使用global_planner功能包，其實(shí)際在調(diào)用A*算法或者Dijkstra最短路徑算法進(jìn)行全局規(guī)劃，通過更改參數(shù)即可在兩者之間進(jìn)行選擇，該系統(tǒng)使用了A*全局路徑規(guī)劃算法，A*算法相比Djkstra算法，在尋找最短路徑的問題上效率更高。使用局部路徑規(guī)劃DWA算法（Dynamic WindowApproach， 動(dòng)態(tài)窗口法）進(jìn)行導(dǎo)航，通過ROS進(jìn)行參數(shù)的配置和調(diào)節(jié)，即可進(jìn)行機(jī)器人的導(dǎo)航操作。在實(shí)際測(cè)試中，DWA算法在進(jìn)行局部路徑規(guī)劃時(shí)，不同于TEB（Timed ElasticBand， 時(shí)域彈性帶）算法，如果使用該方式進(jìn)行導(dǎo)航，可以使機(jī)器人進(jìn)行原地旋轉(zhuǎn)的操作，滿足該任務(wù)需求。只需在Rviz界面中指定到達(dá)的位置，通過上述算法就可以規(guī)劃出一條全局路徑，然后將速度信息發(fā)送給從控制器進(jìn)行底盤的控制，完成機(jī)器人的移動(dòng)，同時(shí)根據(jù)周圍的環(huán)境調(diào)整局部路徑，使機(jī)器人順利到達(dá)指定地點(diǎn)。

4.2.3 巡邏任務(wù)

需要完成的任務(wù)如下：首先使用機(jī)器人在固定的垃圾堆旁邊建立地圖并保存。然后設(shè)置需要進(jìn)行導(dǎo)航的位置，為了能夠觀察四周的情況，假設(shè)安裝相機(jī)的可視范圍為90°，那么每次需要旋轉(zhuǎn)4次，并且每旋轉(zhuǎn)一次后，需要暫停模擬攝像頭捕捉行為，在每個(gè)指定地點(diǎn)巡視一周后，到達(dá)下一個(gè)指定地點(diǎn)，如此反復(fù)，可完成巡邏任務(wù)。

協(xié)調(diào)各個(gè)任務(wù)，也是軟件設(shè)計(jì)中的一個(gè)重點(diǎn)，在ROS中，不同的任務(wù)運(yùn)行在不同的節(jié)點(diǎn)，那么需要協(xié)調(diào)各個(gè)節(jié)點(diǎn)，使其能夠有序地進(jìn)行。為了完成相對(duì)復(fù)雜的任務(wù)，該機(jī)器人使用SMACH（State Machine for Advanced Robots，高級(jí)機(jī)器人狀態(tài)機(jī)）有限狀態(tài)機(jī)，有限狀態(tài)機(jī)又稱有限狀態(tài)自動(dòng)機(jī)，是表示系統(tǒng)中有限個(gè)狀態(tài)以及在這些狀態(tài)之間的轉(zhuǎn)移和動(dòng)作等行為的數(shù)學(xué)模型[11]，該狀態(tài)機(jī)可以明確地描述可能的狀態(tài)和狀態(tài)轉(zhuǎn)換，將不同的模塊組合成為一個(gè)系統(tǒng)，對(duì)于復(fù)雜的機(jī)器人系統(tǒng)可以有效地分解成不同的任務(wù)進(jìn)行組合。其整個(gè)工作流程如圖7所示。假設(shè)進(jìn)行導(dǎo)航巡邏任務(wù)，地圖建立完畢后，在地圖上找到需要進(jìn)行多點(diǎn)導(dǎo)航巡邏的點(diǎn)，進(jìn)行標(biāo)記，記錄需要到達(dá)的位置，將需要導(dǎo)航到地點(diǎn)的位置參數(shù)按照數(shù)組的格式寫入文本文檔中。當(dāng)啟動(dòng)導(dǎo)航巡邏任務(wù)時(shí)，讀取文檔中的位置地點(diǎn)，并創(chuàng)建相應(yīng)的任務(wù)，在到達(dá)每個(gè)位置之后，都需要進(jìn)行四周的巡檢，循環(huán)進(jìn)行任務(wù)，直到完成所有的任務(wù)。

雖然該巡邏任務(wù)并不復(fù)雜，但是可以基于該狀態(tài)機(jī)對(duì)任務(wù)進(jìn)行擴(kuò)展，比如在安裝攝像頭后檢測(cè)到有行人路過，那么狀態(tài)就可以更新為停止，等待行人經(jīng)過，當(dāng)行人走過之后，再返回之前的狀態(tài)繼續(xù)執(zhí)行任務(wù)。也就是說(shuō)，使用狀態(tài)機(jī)具有良好的擴(kuò)展性。

5 仿真與實(shí)地測(cè)試（Simulation and experimental test）

5.1 功能仿真

ROS提供了仿真功能，可以使用Gazebo進(jìn)行仿真，仿真平臺(tái)模擬機(jī)器人運(yùn)行的真實(shí)環(huán)境，為機(jī)器人有關(guān)實(shí)物實(shí)驗(yàn)提供安全可靠的依據(jù)，并大大縮短機(jī)器人的開發(fā)以及實(shí)驗(yàn)周期，具有安全可靠、高效靈活的優(yōu)點(diǎn)[12]。

為了使仿真環(huán)境更接近真實(shí)環(huán)境，使用SolidWorks（CAD設(shè)計(jì)繪畫軟件）進(jìn)行模型建立，在建模時(shí)對(duì)仿真模型進(jìn)行了簡(jiǎn)化，設(shè)置機(jī)器人在長(zhǎng)寬方向的尺寸一致，高度略有差距，但是不影響對(duì)功能的仿真。由于計(jì)算機(jī)使用虛擬機(jī)，資源有限，因此選取比較簡(jiǎn)單的場(chǎng)景進(jìn)行仿真，如圖8所示。

首先操作機(jī)器人進(jìn)行建圖，可以手動(dòng)控制機(jī)器人進(jìn)行地圖的建立，或者使用導(dǎo)航算法進(jìn)行建圖。本次測(cè)試通過導(dǎo)航進(jìn)行建圖操作，如圖9所示。

使用機(jī)器人行走路徑形成閉環(huán)之后，可以生成最終的地圖，進(jìn)行保存，如圖10所示。

最后通過在地圖上采集巡邏點(diǎn)，將巡邏位置寫入記事本，使用狀態(tài)機(jī)進(jìn)行巡邏測(cè)試，可以達(dá)到預(yù)期的要求。

5.2 實(shí)地測(cè)試

由于在室外條件下，沒有Wi-Fi連接，因此使用JetsonNano的網(wǎng)卡，開啟熱點(diǎn)，使用電腦與機(jī)器人實(shí)現(xiàn)連接，在仿真滿足功能需求的前提下，將垃圾督導(dǎo)機(jī)器人拿到扔垃圾的地點(diǎn)進(jìn)行實(shí)地的定位建圖、導(dǎo)航及巡邏功能的測(cè)試，如圖11所示。經(jīng)過測(cè)試，垃圾督導(dǎo)機(jī)器人定位準(zhǔn)確且導(dǎo)航功能可以完成既定的目標(biāo)，也可以完成在多個(gè)地點(diǎn)之間的巡邏操作，符合任務(wù)的需要。

6 結(jié)論（Conclusion）

本文設(shè)計(jì)了基于ROS的垃圾督導(dǎo)機(jī)器人導(dǎo)航巡邏系統(tǒng)。該機(jī)器人能夠快速響應(yīng)控制信號(hào)，及時(shí)移動(dòng)或者停止。主控制器使用ROS，實(shí)現(xiàn)了建圖、定位導(dǎo)航等功能，并且通過其提供的仿真功能，節(jié)省了在實(shí)體機(jī)上調(diào)試的時(shí)間，使用狀態(tài)機(jī)對(duì)復(fù)雜的移動(dòng)巡邏任務(wù)進(jìn)行規(guī)劃。通過實(shí)地實(shí)驗(yàn)，證明機(jī)器人能夠完成巡邏任務(wù)，其導(dǎo)航功能能夠滿足現(xiàn)實(shí)需要。同時(shí)，該機(jī)器人移動(dòng)導(dǎo)航系統(tǒng)的軟、硬件設(shè)計(jì)為移動(dòng)機(jī)器人的開發(fā)具有較強(qiáng)的指導(dǎo)意義和應(yīng)用價(jià)值，對(duì)于不同的移動(dòng)機(jī)器人模型，只需要修改對(duì)應(yīng)的運(yùn)動(dòng)學(xué)模型，即可實(shí)現(xiàn)導(dǎo)航功能。

由于該機(jī)器人必須通過手動(dòng)建圖、手動(dòng)設(shè)置目標(biāo)點(diǎn)，才能完成相應(yīng)的功能，因此自動(dòng)探索和生成導(dǎo)航路徑也是該導(dǎo)航系統(tǒng)需要進(jìn)一步研究的方向，并且可以逐步融合視覺、語(yǔ)音等方面以增加并完善該機(jī)器人的功能，做到真正的商業(yè)化落地。

作者簡(jiǎn)介：

何海濤（1998-），男，碩士生.研究領(lǐng)域：嵌入式，智能控制.

陳勁杰（1965-），男，碩士，副教授.研究領(lǐng)域：人工智能，智能機(jī)器人.