移動機器人自主導航系統及上位機軟件設計與實現

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

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

0 引言

移動機器人是一種在復雜環境下工作，具有自行感知、自行規劃、自我決策功能的智能機器人。隨著機器人與傳感器技術的高速發展，移動機器人被廣泛應用于各種各樣的領域，如智能倉儲機器人[1]、清掃服務機器人[2]、自主水下機器人[3]等。

上述各類機器人的功能實現需要機器人具有自主導航的能力，而實現自主導航則需要SLAM與路徑規劃技術。文獻[4]利用改進A*的路徑規劃算法在機器人平臺進行了實驗仿真，結果表明該算法可提高倉儲系統的運行效率。文獻[5]利用高效模板法與動態窗口法進行路徑規劃，可以在動態環境中有效提高清掃服務機器人的安全性和工作效率。文獻[6]利用電子海圖的先驗知識和改進粒子群優化算法對機器人進行全局路徑規劃，在復雜海域航行時，可以縮短航程，提高安全性及工作效率。文獻[7]利用改進RRT-Connect算法刪除冗余節點來簡化路徑，通過添加障礙物周圍的模型和定義安全距離，提高家庭服務機器人工作的安全性。

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

本文針對移動機器人自主導航問題，采用C++語言與圖形界面設計了一款基于Qt的跨平臺實時數據可視化上位機軟件。該軟件界面控件布局合理，可以分頁顯示數據與地圖信息，方便高效地實現遠程自主導航，具備遠程通信、實時顯示機器人數據信息、發布初始位置與目標位置、可視化地圖和機器人運動軌跡等功能，從而達到數據與地圖可視化和自主導航的效果。

1 移動機器人硬件系統與功能簡介

如圖1所示，本文實驗設備是Turtlebot2移動機器人。該移動機器人底盤左右兩邊各有一個差分電機，驅動機器人前進。前后各有一個輔助輪，對機器人起到平衡的作用，保證機器人有較好的穩定性，防滑性。該機器人的主要硬件組成如下所述：

圖1 TurtleBot2移動機器人實物圖

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

2)3軸數字陀螺儀：移動機器人使用的3軸數字陀螺儀測量范圍±250度/秒，偏航軸默認設置為±20度/秒至±100度/秒的范圍內校準。三軸陀螺儀可以測量6個方向數據信息，最主要的作用是測量角速度，判別物體當前運動狀態。

3)角位移編碼器：角位移編碼器采用的是52線/轉，精度為11.7 ticks/mm，能夠將角位移轉換成周期性的電信號，通過該電信號可以計算出機器人里程計信息。

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

2 移動機器人自主導航

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

改進RRT*算法將橢圓狀態子集采樣方法與智能樣本插入相結合，可以減少采樣生成的節點數和得到最優路徑解所需要的迭代次數，從而優化路徑長度和提高算法收斂速度。改進RRT*算法如下所示。

算法1：改進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)

上述改進RRT*算法僅適用于基于環境已知的情況，若機器人在移動的過程中出現未知障礙物，則有可能發生碰撞。因此，需要與動態窗口法相結合，實現局部動態避障。動態窗口法[19]主要是在速度空間內采樣多組角速度和線速度，在這些速度下模擬機器人下一時間間隔內的軌跡。在獲取多組軌跡后，根據相應的評價指標，分別對這些軌跡進行評分，選取評分最高軌跡，再利用該軌跡所對應的速度來控制機器人運動。

在局部路徑規劃中，動態窗口法通過設計合適的評估函數選取最優的軌跡。確保最優軌跡規避未知障礙物后，能夠回到改進RRT*算法規劃的全局參考路徑上，朝著目標位置快速運動，從而完成移動機器人自主導航避障任務。評估函數定義如下：

G(v,w)=

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

(1)

其中:k表示平滑系數；α、β與λ表示加權系數，head(v,w)表示機器人的方位角評價函數，dist(v,w)表示當前速度所對應軌跡與障礙物之間的最近距離，vel(v,w)表示當前軌跡速度大小的評價函數。

通過上述算法，移動機器人可實現自主導航功能，能夠安全、高效地到達指定目標點。同時上位機可以獲取機器人數據信息與當前話題，并在地圖中實時可視化機器人的運動與軌跡狀態。

3 移動機器人上位機軟件設計

3.1 Qt開發環境

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

Qt Creator是由Nokia公司推出的一款新的輕量級集成開發環境，功能強大，操作界面簡潔，配置環境簡單，利用Qt應用程序框架可以快速方便的完成開發任務。Qt Creator包括項目生成向導、高級C++程序語言代碼編輯器、瀏覽文件以及圖形化的GDB調試前端等功能。本文上位機軟件的設計是基于Qt5.9.9版本下開發的，由于該版本較高，能夠滿足項目開發的需要。其操作界面如圖2所示。

3.2 上位機軟件設計過程

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

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

圖4 上位機工作流程圖

該上位機軟件主要由2個主界面組成：主從機的登錄通信模塊以及機器人當前話題列表和運動信息顯示界面、可視化界面以及機器人啟動設置和控制模塊顯示界面。上位機工作流程如圖5所示，利用全局規劃與局部規劃算法完成自主導航功能。上位機軟件所實現的主要功能如下：

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

2)獲取機器人當前話題列表及運動信息：當運行Launch文件時，會產生許多ROS話題。為了方便查閱相關話題，在上位機界面中進行實時顯示。此外，訂閱速度與里程計話題，可以讀取當前機器人的線速度、角速度以及當前位置信息。

3)界面可視化及圖層信息獲取與發布位置信息：創建一個librviz界面類，利用RenderPanel對象向ui界面中添加顯示界面，并且可以實現界面放大、縮小和平移操作。通過createDisplay函數在界面中添加圖層，如柵格圖層、地圖圖層、雷達圖層等。通過可視化圖層信息，可以實時觀察機器人自主導航過程中的運動狀態與軌跡。此外，可以利用上位機發布機器人的初始位置與目標位置，通過rviz控制對象獲取ToolManager工具對象，通過工具對象選取導航初始位置與目標位置。可視化機器人導航界面如圖5所示。

4)啟動設置與實現機器人控制：為了讓機器人自主導航操作更加方便、簡潔，在QGroupBox組件中加入啟動機器人、激光雷達等按鈕。此外，在啟動激光雷達建圖后，通過控制按鈕對周圍環境進行建圖，不需要利用鍵盤對機器人控制。保存好地圖后，就可以利用路徑規劃算法對機器人進行定點導航。

4 實驗驗證與分析

本文所采用的實驗設備為第1節中所提及的Turtlebot2移動機器人，該機器人搭載了一臺主機和Hokuyo激光雷達。另外一臺從機運行上位機軟件，遠程操作移動機器人。具體實驗步驟如下：

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

圖5 可視化機器人導航界面 圖6 登錄通信界面以及機器人信息

2)根據如圖7所示的實驗環境，通過點擊上位機界面中激光雷達建圖按鈕，利用激光雷達和gmapping包對真實環境建圖。通過界面中的控制按鈕移動機器人，完成建圖任務。建立完整的地圖如圖8所示。

圖7 真實環境

圖8 建立地圖

3)首先通過上位機初始位置按鈕在地圖中為移動機器人選擇初始位置，再通過目標位置按鈕選擇導航的全局目標點。此時，利用改進RRT*算法規劃出一條全局最優路徑，并在地圖中實時記錄機器人的運行軌跡，如圖9所示。

圖9 機器人自主導航

4)若移動機器人在運動過程中出現未知障礙物，如圖10所示，灰色箱子是地圖中添加的未知障礙物。移動機器人可以根據當前位置信息，結合激光雷達檢測到未知障礙物，利用動態窗口法進行局部路徑規劃，在規避未知障礙物后重新回到全局參考路徑上，如圖11所示。

圖10 添加未知障礙物 圖11 動態窗口法局部避障

5)判斷機器人是否到達全局目標點。如果機器人到達指定目標點，則完成自主導航任務。否則，如果未到達目標點，則返回步驟4)繼續運動。

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

5 結束語

本文在跨平臺的Qt開發框架下，以C++面向對象編程語言，設計和開發了一款界面簡潔且功能齊全的機器人自主導航系統的上位機軟件。首先介紹了上位機軟件涉及的改進RRT*算法與動態窗口法，并且詳細闡述了上位機軟件工作流程、實現功能以及使用的組件。通過上位機與機器人之間建立通信，遠程控制機器人實現建圖與導航功能，并且上位機界面實時可視化機器人地圖、速度、里程計以及軌跡等信息。最后，通過真實環境進行實驗，在線實時顯示機器人各狀態信息和可視化地圖等，驗證了改進RRT*算法與動態窗口法的可行性與實用性、上位機軟件功能的實時性與有效性。