崔 奇,夏 浩,滕 游,劉安東

(浙江工業(yè)大學(xué) 信息工程學(xué)院，杭州 310023)

0 引言

移動(dòng)機(jī)器人是一種在復(fù)雜環(huán)境下工作，具有自行感知、自行規(guī)劃、自我決策功能的智能機(jī)器人。隨著機(jī)器人與傳感器技術(shù)的高速發(fā)展，移動(dòng)機(jī)器人被廣泛應(yīng)用于各種各樣的領(lǐng)域，如智能倉(cāng)儲(chǔ)機(jī)器人[1]、清掃服務(wù)機(jī)器人[2]、自主水下機(jī)器人[3]等。

上述各類機(jī)器人的功能實(shí)現(xiàn)需要機(jī)器人具有自主導(dǎo)航的能力，而實(shí)現(xiàn)自主導(dǎo)航則需要SLAM與路徑規(guī)劃技術(shù)。文獻(xiàn)[4]利用改進(jìn)A*的路徑規(guī)劃算法在機(jī)器人平臺(tái)進(jìn)行了實(shí)驗(yàn)仿真，結(jié)果表明該算法可提高倉(cāng)儲(chǔ)系統(tǒng)的運(yùn)行效率。文獻(xiàn)[5]利用高效模板法與動(dòng)態(tài)窗口法進(jìn)行路徑規(guī)劃，可以在動(dòng)態(tài)環(huán)境中有效提高清掃服務(wù)機(jī)器人的安全性和工作效率。文獻(xiàn)[6]利用電子海圖的先驗(yàn)知識(shí)和改進(jìn)粒子群優(yōu)化算法對(duì)機(jī)器人進(jìn)行全局路徑規(guī)劃，在復(fù)雜海域航行時(shí)，可以縮短航程，提高安全性及工作效率。文獻(xiàn)[7]利用改進(jìn)RRT-Connect算法刪除冗余節(jié)點(diǎn)來(lái)簡(jiǎn)化路徑，通過添加障礙物周圍的模型和定義安全距離，提高家庭服務(wù)機(jī)器人工作的安全性。

在自主導(dǎo)航系統(tǒng)中，SLAM技術(shù)[8-9]是通過激光雷達(dá)傳感器，獲取周圍環(huán)境的信息，實(shí)現(xiàn)即時(shí)定位與地圖構(gòu)建的過程。相比于視覺傳感器，激光雷達(dá)傳感器不受環(huán)境光照的影響，對(duì)障礙物識(shí)別準(zhǔn)確度與可靠性高。路徑規(guī)劃技術(shù)[10-11]則是在SLAM建立完整地圖基礎(chǔ)上，按照某種評(píng)價(jià)指標(biāo)尋找一條從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)無(wú)碰撞安全路徑。然而由于自主導(dǎo)航系統(tǒng)的定位、建圖與導(dǎo)航等一系列操作較復(fù)雜，且移動(dòng)機(jī)器人地圖、數(shù)據(jù)信息和運(yùn)行軌跡無(wú)法直觀化，為此需要設(shè)計(jì)上位機(jī)將定位、建圖與導(dǎo)航等功能集成到上位機(jī)操作界面，同時(shí)實(shí)時(shí)顯示移動(dòng)機(jī)器人地圖、數(shù)據(jù)和運(yùn)行軌跡。文獻(xiàn)[12]使用目標(biāo)跟蹤算法提取特征點(diǎn)完成視覺跟蹤任務(wù)，并設(shè)計(jì)了上位機(jī)軟件實(shí)時(shí)繪制機(jī)器人運(yùn)動(dòng)軌跡。文獻(xiàn)[13]設(shè)計(jì)了車輛勘查機(jī)器人上位機(jī)軟件，通過無(wú)線網(wǎng)絡(luò)實(shí)現(xiàn)機(jī)器人遠(yuǎn)程操作和車輛圖像信息實(shí)時(shí)顯示。

本文針對(duì)移動(dòng)機(jī)器人自主導(dǎo)航問題，采用C++語(yǔ)言與圖形界面設(shè)計(jì)了一款基于Qt的跨平臺(tái)實(shí)時(shí)數(shù)據(jù)可視化上位機(jī)軟件。該軟件界面控件布局合理，可以分頁(yè)顯示數(shù)據(jù)與地圖信息，方便高效地實(shí)現(xiàn)遠(yuǎn)程自主導(dǎo)航，具備遠(yuǎn)程通信、實(shí)時(shí)顯示機(jī)器人數(shù)據(jù)信息、發(fā)布初始位置與目標(biāo)位置、可視化地圖和機(jī)器人運(yùn)動(dòng)軌跡等功能，從而達(dá)到數(shù)據(jù)與地圖可視化和自主導(dǎo)航的效果。

1 移動(dòng)機(jī)器人硬件系統(tǒng)與功能簡(jiǎn)介

如圖1所示，本文實(shí)驗(yàn)設(shè)備是Turtlebot2移動(dòng)機(jī)器人。該移動(dòng)機(jī)器人底盤左右兩邊各有一個(gè)差分電機(jī)，驅(qū)動(dòng)機(jī)器人前進(jìn)。前后各有一個(gè)輔助輪，對(duì)機(jī)器人起到平衡的作用，保證機(jī)器人有較好的穩(wěn)定性，防滑性。該機(jī)器人的主要硬件組成如下所述：

圖1 TurtleBot2移動(dòng)機(jī)器人實(shí)物圖

1)有刷直流電機(jī)：移動(dòng)機(jī)器人使用型號(hào)為RP385-ST-2060的有刷直流電機(jī)，額定電壓為12 V，額定負(fù)載為5 mN·m，額定負(fù)載電流為750 mA，電樞電感為1.51 mH，有電壓源和脈寬調(diào)制兩種控制方法。有刷直流電機(jī)作為移動(dòng)機(jī)器人的動(dòng)力系統(tǒng)重要組成部分，控制機(jī)器人移動(dòng)。

2)3軸數(shù)字陀螺儀：移動(dòng)機(jī)器人使用的3軸數(shù)字陀螺儀測(cè)量范圍±250度/秒，偏航軸默認(rèn)設(shè)置為±20度/秒至±100度/秒的范圍內(nèi)校準(zhǔn)。三軸陀螺儀可以測(cè)量6個(gè)方向數(shù)據(jù)信息，最主要的作用是測(cè)量角速度，判別物體當(dāng)前運(yùn)動(dòng)狀態(tài)。

3)角位移編碼器：角位移編碼器采用的是52線/轉(zhuǎn)，精度為11.7 ticks/mm，能夠?qū)⒔俏灰妻D(zhuǎn)換成周期性的電信號(hào)，通過該電信號(hào)可以計(jì)算出機(jī)器人里程計(jì)信息。

4)激光雷達(dá)傳感器：移動(dòng)機(jī)器人使用由HOKUYO公司推出的URG-04LX-UG01激光雷達(dá)。測(cè)量范圍240°，DC5V輸入，用于機(jī)器人避障和位置識(shí)別，精度與分辨率較高，低功耗且供電方便。

2 移動(dòng)機(jī)器人自主導(dǎo)航

本文利用移動(dòng)機(jī)器人執(zhí)行SLAM技術(shù)和路徑規(guī)劃算法，進(jìn)行數(shù)據(jù)顯示與地圖可視化來(lái)驗(yàn)證上位機(jī)功能效果。首先通過SLAM技術(shù)建立真實(shí)環(huán)境地圖，再利用改進(jìn)RRT*算法[14-16]進(jìn)行全局路徑規(guī)劃以及動(dòng)態(tài)窗口法[17-18]進(jìn)行局部路徑規(guī)劃。最后通過上位機(jī)界面完成一系列遠(yuǎn)程操作，實(shí)時(shí)讀取機(jī)器人的線速度、角速度和當(dāng)前位置信息等，可以高效、便捷地實(shí)現(xiàn)移動(dòng)機(jī)器人自主導(dǎo)航功能。

改進(jìn)RRT*算法將橢圓狀態(tài)子集采樣方法與智能樣本插入相結(jié)合，可以減少采樣生成的節(jié)點(diǎn)數(shù)和得到最優(yōu)路徑解所需要的迭代次數(shù)，從而優(yōu)化路徑長(zhǎng)度和提高算法收斂速度。改進(jìn)RRT*算法如下所示。

算法1：改進(jìn)RRT*算法

1Va←{xs};Ea←?;Ta←(Va,Ea)

2Vb←{xg};Eb←?;Tb←(Vb,Eb)

3cbest←∞

4 fori←0 toNdo

5 ConnFlag←True

6xrand←Sample(xs,xg,cbest)

10 ConnFlag←False

13 ifFlagthen

14Ta←InsertNodes(xrand,xmin,Ta)

16else

17Tb←InsertNodes(xrand,xmin,Tb)

19 return {Ta,Tb}=(Va∪Vb,Ea∪Eb)

上述改進(jìn)RRT*算法僅適用于基于環(huán)境已知的情況，若機(jī)器人在移動(dòng)的過程中出現(xiàn)未知障礙物，則有可能發(fā)生碰撞。因此，需要與動(dòng)態(tài)窗口法相結(jié)合，實(shí)現(xiàn)局部動(dòng)態(tài)避障。動(dòng)態(tài)窗口法[19]主要是在速度空間內(nèi)采樣多組角速度和線速度，在這些速度下模擬機(jī)器人下一時(shí)間間隔內(nèi)的軌跡。在獲取多組軌跡后，根據(jù)相應(yīng)的評(píng)價(jià)指標(biāo)，分別對(duì)這些軌跡進(jìn)行評(píng)分，選取評(píng)分最高軌跡，再利用該軌跡所對(duì)應(yīng)的速度來(lái)控制機(jī)器人運(yùn)動(dòng)。

在局部路徑規(guī)劃中，動(dòng)態(tài)窗口法通過設(shè)計(jì)合適的評(píng)估函數(shù)選取最優(yōu)的軌跡。確保最優(yōu)軌跡規(guī)避未知障礙物后，能夠回到改進(jìn)RRT*算法規(guī)劃的全局參考路徑上，朝著目標(biāo)位置快速運(yùn)動(dòng)，從而完成移動(dòng)機(jī)器人自主導(dǎo)航避障任務(wù)。評(píng)估函數(shù)定義如下：

G(v,w)=

k[αhead(v,w)+βdist(v,w)+λvel(v,w)]

(1)

其中:k表示平滑系數(shù)；α、β與λ表示加權(quán)系數(shù)，head(v,w)表示機(jī)器人的方位角評(píng)價(jià)函數(shù)，dist(v,w)表示當(dāng)前速度所對(duì)應(yīng)軌跡與障礙物之間的最近距離，vel(v,w)表示當(dāng)前軌跡速度大小的評(píng)價(jià)函數(shù)。

通過上述算法，移動(dòng)機(jī)器人可實(shí)現(xiàn)自主導(dǎo)航功能，能夠安全、高效地到達(dá)指定目標(biāo)點(diǎn)。同時(shí)上位機(jī)可以獲取機(jī)器人數(shù)據(jù)信息與當(dāng)前話題，并在地圖中實(shí)時(shí)可視化機(jī)器人的運(yùn)動(dòng)與軌跡狀態(tài)。

3 移動(dòng)機(jī)器人上位機(jī)軟件設(shè)計(jì)

3.1 Qt開發(fā)環(huán)境

Qt是一款基于C++的跨平臺(tái)圖形用戶界面應(yīng)用程序開發(fā)類軟件，Qt不僅支持PC和服務(wù)器平臺(tái)，包括Windows、Linux、macOS等，還支持移動(dòng)和嵌入式操作系統(tǒng)，包括iOS、Embedded Linux、Android等?？缙脚_(tái)意味著只需要編寫一次程序，在無(wú)需改動(dòng)源碼情況下進(jìn)行編譯，就可以形成在不同平臺(tái)上運(yùn)行的版本。這種跨平臺(tái)功能為程序開發(fā)者提供了極大的便利[20]。Qt使用獨(dú)特的信號(hào)與槽機(jī)制進(jìn)行對(duì)象間通信替代傳統(tǒng)的回調(diào)函數(shù)機(jī)制進(jìn)行對(duì)象間信息傳遞，降低了Qt對(duì)象之間的耦合度，確保參數(shù)傳遞的準(zhǔn)確性與唯一性。Qt有著豐富而又全面的C++圖形庫(kù)，基礎(chǔ)的封裝模塊主要有Core模塊、OpenGL模塊、SQL模塊、XML模塊、Network模塊與Test模塊等，這些封裝模塊都支持多線程操作，極大地提高了Qt軟件開發(fā)大規(guī)模復(fù)雜工程項(xiàng)目的能力[21]。

Qt Creator是由Nokia公司推出的一款新的輕量級(jí)集成開發(fā)環(huán)境，功能強(qiáng)大，操作界面簡(jiǎn)潔，配置環(huán)境簡(jiǎn)單，利用Qt應(yīng)用程序框架可以快速方便的完成開發(fā)任務(wù)。Qt Creator包括項(xiàng)目生成向?qū)?、高?jí)C++程序語(yǔ)言代碼編輯器、瀏覽文件以及圖形化的GDB調(diào)試前端等功能。本文上位機(jī)軟件的設(shè)計(jì)是基于Qt5.9.9版本下開發(fā)的，由于該版本較高，能夠滿足項(xiàng)目開發(fā)的需要。其操作界面如圖2所示。

3.2 上位機(jī)軟件設(shè)計(jì)過程

本文基于QMianWindow作為主窗口類設(shè)計(jì)的上位機(jī)。主窗口包括主菜單欄、工具欄和狀態(tài)欄，操作方便、可擴(kuò)展性好。在主窗口中加入QWidget類，QWidget是所有用戶界面對(duì)象的基類，可以在其中加入各種界面組件，如QLabel、QPushButton、QLineEdit等，這些界面組件的具體功能都是通過QWidget類得以實(shí)現(xiàn)。同時(shí)，使用了QTabWidget組件進(jìn)行分頁(yè)處理，實(shí)現(xiàn)不同功能界面的切換，上位機(jī)軟件的主界面如圖3所示。

圖2 Qt Creator操作界面示意圖 圖3 上位機(jī)軟件主界面

圖4 上位機(jī)工作流程圖

該上位機(jī)軟件主要由2個(gè)主界面組成：主從機(jī)的登錄通信模塊以及機(jī)器人當(dāng)前話題列表和運(yùn)動(dòng)信息顯示界面、可視化界面以及機(jī)器人啟動(dòng)設(shè)置和控制模塊顯示界面。上位機(jī)工作流程如圖5所示，利用全局規(guī)劃與局部規(guī)劃算法完成自主導(dǎo)航功能。上位機(jī)軟件所實(shí)現(xiàn)的主要功能如下：

1)建立主從機(jī)之間的通信：將兩臺(tái)電腦連接在相同的局域網(wǎng)下，通過上位機(jī)分別獲取主從機(jī)的IP地址以及URL地址，再通過SSH協(xié)議讓從機(jī)登錄主機(jī)并且控制主機(jī)，從而完成登錄通信過程。

2)獲取機(jī)器人當(dāng)前話題列表及運(yùn)動(dòng)信息：當(dāng)運(yùn)行Launch文件時(shí)，會(huì)產(chǎn)生許多ROS話題。為了方便查閱相關(guān)話題，在上位機(jī)界面中進(jìn)行實(shí)時(shí)顯示。此外，訂閱速度與里程計(jì)話題，可以讀取當(dāng)前機(jī)器人的線速度、角速度以及當(dāng)前位置信息。

3)界面可視化及圖層信息獲取與發(fā)布位置信息：創(chuàng)建一個(gè)librviz界面類，利用RenderPanel對(duì)象向ui界面中添加顯示界面，并且可以實(shí)現(xiàn)界面放大、縮小和平移操作。通過createDisplay函數(shù)在界面中添加圖層，如柵格圖層、地圖圖層、雷達(dá)圖層等。通過可視化圖層信息，可以實(shí)時(shí)觀察機(jī)器人自主導(dǎo)航過程中的運(yùn)動(dòng)狀態(tài)與軌跡。此外，可以利用上位機(jī)發(fā)布機(jī)器人的初始位置與目標(biāo)位置，通過rviz控制對(duì)象獲取ToolManager工具對(duì)象，通過工具對(duì)象選取導(dǎo)航初始位置與目標(biāo)位置?？梢暬瘷C(jī)器人導(dǎo)航界面如圖5所示。

4)啟動(dòng)設(shè)置與實(shí)現(xiàn)機(jī)器人控制：為了讓機(jī)器人自主導(dǎo)航操作更加方便、簡(jiǎn)潔，在QGroupBox組件中加入啟動(dòng)機(jī)器人、激光雷達(dá)等按鈕。此外，在啟動(dòng)激光雷達(dá)建圖后，通過控制按鈕對(duì)周圍環(huán)境進(jìn)行建圖，不需要利用鍵盤對(duì)機(jī)器人控制。保存好地圖后，就可以利用路徑規(guī)劃算法對(duì)機(jī)器人進(jìn)行定點(diǎn)導(dǎo)航。

4 實(shí)驗(yàn)驗(yàn)證與分析

本文所采用的實(shí)驗(yàn)設(shè)備為第1節(jié)中所提及的Turtlebot2移動(dòng)機(jī)器人，該機(jī)器人搭載了一臺(tái)主機(jī)和Hokuyo激光雷達(dá)。另外一臺(tái)從機(jī)運(yùn)行上位機(jī)軟件，遠(yuǎn)程操作移動(dòng)機(jī)器人。具體實(shí)驗(yàn)步驟如下：

1)機(jī)器人硬件系統(tǒng)與主機(jī)通過USB接口進(jìn)行串口通信，從機(jī)的上位機(jī)軟件系統(tǒng)獲取主機(jī)與本機(jī)的IP地址及URL地址，進(jìn)一步與主機(jī)建立通信并且獲得主機(jī)的控制權(quán)。同時(shí)，不需要在終端進(jìn)行輸入指令，就可以查看當(dāng)前機(jī)器人的話題列表信息，并且可以實(shí)時(shí)獲取當(dāng)前機(jī)器人運(yùn)行的線速度、角速度以及當(dāng)前位置信息，如圖6所示。

圖5 可視化機(jī)器人導(dǎo)航界面 圖6 登錄通信界面以及機(jī)器人信息

2)根據(jù)如圖7所示的實(shí)驗(yàn)環(huán)境，通過點(diǎn)擊上位機(jī)界面中激光雷達(dá)建圖按鈕，利用激光雷達(dá)和gmapping包對(duì)真實(shí)環(huán)境建圖。通過界面中的控制按鈕移動(dòng)機(jī)器人，完成建圖任務(wù)。建立完整的地圖如圖8所示。

圖7 真實(shí)環(huán)境

圖8 建立地圖

3)首先通過上位機(jī)初始位置按鈕在地圖中為移動(dòng)機(jī)器人選擇初始位置，再通過目標(biāo)位置按鈕選擇導(dǎo)航的全局目標(biāo)點(diǎn)。此時(shí)，利用改進(jìn)RRT*算法規(guī)劃出一條全局最優(yōu)路徑，并在地圖中實(shí)時(shí)記錄機(jī)器人的運(yùn)行軌跡，如圖9所示。

圖9 機(jī)器人自主導(dǎo)航

4)若移動(dòng)機(jī)器人在運(yùn)動(dòng)過程中出現(xiàn)未知障礙物，如圖10所示，灰色箱子是地圖中添加的未知障礙物。移動(dòng)機(jī)器人可以根據(jù)當(dāng)前位置信息，結(jié)合激光雷達(dá)檢測(cè)到未知障礙物，利用動(dòng)態(tài)窗口法進(jìn)行局部路徑規(guī)劃，在規(guī)避未知障礙物后重新回到全局參考路徑上，如圖11所示。

圖10 添加未知障礙物 圖11 動(dòng)態(tài)窗口法局部避障

5)判斷機(jī)器人是否到達(dá)全局目標(biāo)點(diǎn)。如果機(jī)器人到達(dá)指定目標(biāo)點(diǎn)，則完成自主導(dǎo)航任務(wù)。否則，如果未到達(dá)目標(biāo)點(diǎn)，則返回步驟4)繼續(xù)運(yùn)動(dòng)。

實(shí)驗(yàn)過程中，移動(dòng)機(jī)器人的實(shí)驗(yàn)參數(shù)設(shè)置如下：最大線速度為0.5 m/s，最大角速度為1 rad/s，障礙物的膨脹半徑0.3 m，機(jī)器人半徑為0.2 m，傳感器上傳信息頻率為5.0 Hz，地圖更新頻率設(shè)置為0.5 Hz。圖9中柵格地圖的分辨率為20 cm×20 cm，設(shè)坐標(biāo)原點(diǎn)為左下角弧形頂點(diǎn)，水平方向?yàn)閤軸，豎直方向?yàn)閥軸，起始位置設(shè)為(1.6 m，3.2 m)，目標(biāo)位置設(shè)為(1.5 m，12 m)，機(jī)器人實(shí)際所走的路線如圖中的線所示，最終到達(dá)目標(biāo)點(diǎn)，與期望目標(biāo)點(diǎn)誤差0.12 m。由于地圖定位不準(zhǔn)確，里程計(jì)計(jì)算出的機(jī)器人當(dāng)前位置存在偏差和機(jī)器人與地面會(huì)存在打滑的可能性，導(dǎo)致了機(jī)器人實(shí)際到達(dá)目標(biāo)點(diǎn)與期望的目標(biāo)點(diǎn)存在誤差，但誤差不超過0.2 m，可以滿足實(shí)際需求與應(yīng)用。

5 結(jié)束語(yǔ)

本文在跨平臺(tái)的Qt開發(fā)框架下，以C++面向?qū)ο缶幊陶Z(yǔ)言，設(shè)計(jì)和開發(fā)了一款界面簡(jiǎn)潔且功能齊全的機(jī)器人自主導(dǎo)航系統(tǒng)的上位機(jī)軟件。首先介紹了上位機(jī)軟件涉及的改進(jìn)RRT*算法與動(dòng)態(tài)窗口法，并且詳細(xì)闡述了上位機(jī)軟件工作流程、實(shí)現(xiàn)功能以及使用的組件。通過上位機(jī)與機(jī)器人之間建立通信，遠(yuǎn)程控制機(jī)器人實(shí)現(xiàn)建圖與導(dǎo)航功能，并且上位機(jī)界面實(shí)時(shí)可視化機(jī)器人地圖、速度、里程計(jì)以及軌跡等信息。最后，通過真實(shí)環(huán)境進(jìn)行實(shí)驗(yàn)，在線實(shí)時(shí)顯示機(jī)器人各狀態(tài)信息和可視化地圖等，驗(yàn)證了改進(jìn)RRT*算法與動(dòng)態(tài)窗口法的可行性與實(shí)用性、上位機(jī)軟件功能的實(shí)時(shí)性與有效性。