基于ROS 的機器人自主探索導航與地圖構建研究

邵連奇，張媛媛，袁 田，淡睿哲，馬帥輝，顧偉宏

（東北林業大學，哈爾濱 150000）

ROS（Robot Operating System，下文簡稱“ROS”）全稱為機器人操作系統，是適用于機器人開源的一種元操作系統，是管理機器人硬件的一個操作系統架構。其為操作系統提供了應有的服務，包括硬件抽象、底層設備控制、常用函數的實現和進程間消息傳遞，以及包管理。ROS 也提供了用于獲取、編譯、編寫和跨計算機運行代碼所常用的機器人開發通用工具和開源庫函數。

ROS 系統的起源是2007 年斯坦福大學人工智能實驗室與Wilow Garage 公司的一個機器人合作項目——斯坦福人工智能機器人項目（the STAIR project）。2008 年之后由Willow Garagre 來進行推動。ROS現己成為國內外大學進行機器人實驗研發的首選平臺。據調查可知，實現無人駕駛技術的最關鍵問題之一就是地圖構建。地圖構建的精準度，又受制于傳感器所獲取信息的時效性及準確性等。在ROS 的基礎上，利用攝像頭和激光雷達對周圍的環境的進行探測。相比于攝像頭傳感器，雷達的檢測范圍更大、檢測角度更廣。利用雷達檢測這一優點可以快速對障礙物進行避障，進而提高了機器人運行速度上限。但隨著運動速度的提高，單一傳感器檢測易產生誤判，因此加入攝像頭進行識別輔助判斷，通過對多傳感器融合使用以及對數據的融合處理，降低誤判的程度。

1 機器人系統設計框架

1.1 硬件系統設計

機器人的整體結構與硬件組成如圖1 所示，該機器人主要由激光雷達、視覺模塊、控制模塊和移動平臺4 個部分組成。其中移動平臺由常規智能車底盤進行改造，選用航模電池進行供電。激光雷達采用三角測距原理，通過脈沖激光獲取目標物的標點數據，該機器人采用的LS01X 型激光雷達測距半徑可達16 m。視覺模塊選取工業攝像頭，利用CCD 感光芯片，對外界環境進行探測與感知。控制系統的核心為MiniPC，承載Ubuntu 20.04。移動平臺在無刷電機、舵機、輪式編碼器及散熱模塊與減震模塊的協同下，可以在復雜的環境中進行快速行駛。

圖1 機器人的整體結構與硬件組成

1.2 控制系統設計

機器人的自主導航以及地圖構建需要利用激光雷達、攝像頭、IMU（慣性傳感器）和編碼器4 種傳感器所采集的信息，通過MiniPC 將這些數據進行融合處理；并且借助ROS 豐富的開源社區資源與rviz 軟件平臺實現自主探索導航與二維可視化建圖功能。

2 機器人移動平臺的SLAM 系統搭建

2.1 SLAM 系統概述

SLAM 是一種解決移動機器人同步定位與導航的技術，機器人通過其所搭載的傳感器對外界環境信息進行獲取，從而進行增量式地圖構建，進而利用所獲取的環境信息對自身位姿進行獲取與調整。SLAM 主要分為4 個方面：激光SLAM、視覺SLAM、多傳感器融合SLAM 和深度學習SLAM，其中多傳感器融合SLAM相較于其他SLAM 定位精度更高，擁有更好的魯棒性。

2.2 視覺系統模型

OpenCV 是一個開源的BSD 許可庫，由一系列的C 函數和少量C++類構成，可以實現計算機視覺和圖像處理方面的很多算法，具有強大的圖像和矩陣計算能力。圖像處理與分析函數、輪廓、幾何學處理函數、目標跟蹤與追蹤函數及顏色識別函數是OpenCV 常用的幾種函數，合理的應用OPenCV 可以有效提高機器人的自主導航與避障能力。

3 機器人運動情況下的定位與對未知環境的地圖構建

3.1 卡爾曼濾波算法研究

卡爾曼濾波算法（Kalman Filter）通過系統的觀測數據及系統狀態對全局最優狀態進行推算。通過上一狀態和下一狀態來預測當前狀態，公式如下

式中:At指當前時刻的狀態轉移矩陣（就是指從上一狀態轉變為下一狀態的關系矩陣）；Bt為當前時刻的控制矩陣（就是指影響控制量的控制矩陣）；ut為當前時刻的控制量；μt-1為上一時刻最優估計值；μˉt為當前時刻估計值。

預測均值的協方差來計算Kalman 增益為，其中協方差為

式中：Σt-1為上一時刻預測值方差矩陣；Rt指當前時刻測量值噪聲矩陣。

Kalman 增益的計算公式為

式中：KT為卡爾曼增益（指的就是權值）；CT為測量方程；QT為觀察量的協方差矩陣；

通過Kalman 增益來計算狀態估計值

預測當前狀態需要用到上一狀態的協方差，所還需要計算當前狀態的協方差用于下一次迭代

通過以上公式即可預測出物體的位置坐標及速度。

3.2 數據融合

robot_pose_ekf 是 ROS Navigation stack 中的一個package，通過擴展卡爾曼濾波器對IMU、里程計odom、視覺里程計vo 的數據進行融合，從而計算出機器人在平面上的真實位姿，并輸出odom_combined 消息。robot_pose_ekf 只適用于平面上的輪式移動機器人，因此odom 信息中可以忽略z，pitch 與roll 分量。IMU 可以提供機器人坐標系相對于世界坐標系的姿態（RPY 角），其中Roll 和Pitch 是絕對角度，而偏航角Yaw 在IMU 中沒有集成電子羅盤測量地球磁場角作為參考則是一個相對角度。IMU 姿態的協方差矩陣代表了姿態測量的不確定度。robot_pose_ekf 節點默認會從odom、IMU_data、vo 這三個topic 上訂閱消息，通過remap 將其映射到新名稱的topic 上。每當節點使用重映射中的原始名時，ROS 客戶端庫就會將其默默地替換成其對應的新名稱。IMU 信息的協方差矩陣中代表機器人航向角的分量方差為10-6，而里程計信息的協方差矩陣中機器人姿態分量的協方差為103，2 個值相差很大。而在進行EKF 融合時，會更“相信”IMU 提供的姿態信息，因其方差更小。

3.3 Gmapping 建圖算法

Gmapping 是一種基于Rao-Blackwellized 粒子濾波的二維激光SLAM 方法，gmapping 的運行步驟一般為：采樣—計算權重—重采樣—地圖估計。gmapping 基于RBpf 粒子濾波算法，即將定位和建圖過程分離，先進行定位再進行建圖。東北林業大學為了解決RBpf 頻繁重采樣的缺點，gmapping 提出了選擇性重采樣，即減少重采樣的次數，設定閾值；當粒子權重大于所設閾值才執行重采樣，這樣便減少了采樣的次數，即減緩了粒子的退化。

利用gmapping 功能包訂閱機器人的深度信息、通過利用IMU 信息和里程計信息以及卡爾曼濾波融合，進行小車的精確定位，同時進行一些必要的參數配置，例如對gmapping 功能包中的雷達最大檢測距離及地圖刷新頻率的調試，進而更好地實現同步定位與地圖構建。

編寫gmapping 節點相關launch 文件，修改關鍵參數map_update_interval（地圖刷新頻率）為3，maxUrange（雷達最大檢測范圍）40 cm，其余參數保持不變，修改好～base_frame（string, default:"base_link"）機器人基坐標。～map_frame（string, default:"map"）地圖坐標系。～odom_frame（string, default:"odom"）里程計坐標系，運行launch 文件，即可創建并輸出基于概率的二維柵格地圖，因為其方差更小。

4 機器人移動平臺的路徑規劃算法研究

4.1 全局路徑規劃——A*算法

全局路徑規劃是指輪式機器人找到起點與目標點間可通行路徑的過程，此過程僅包含路徑的信息，同時構建一系列規劃的目標，包括路徑最短躲避障礙物等。本文研究是基于ROS 的研究，故選取A*算法作為全局路徑規劃算法。

A*算法的關鍵在于如何利用啟發式信息決定哪個點是下一步要擴展的節點，其搜索過程實際上是被選節點擴展的過程，使用最少的資源獲取最優解。機器從起始點出發，不斷地尋找以最小代價通向目標點的節點并優先擴展那些可使目標函數較小的路徑點，進而形成一個點集。該點集中的路徑點有序連接即為所求的最優路徑，因此啟發函數是A*算法核心內容，啟發函數越精確，找到最優解的速度就越快、越準確。

A*算法的模型為

式中：f（n）為總的搜索代價；g（n）為從起點到當前節點n 的代價和；h（n）為從當前節點n 到目標節點的最優代價的啟發函數。

標準的啟發函數正是曼哈頓啟發函數

如果單元在地圖上允許對角線移動，對角線函數便可以對其進行補充

如果單元在地圖上允許向任意方向移動，這時利用歐幾里得算法可以得到最短距離，但A*算法運行時間將更長

式中：xi（i=n，goal），yi（i=n，goal）和zi（i=n，goal）分別是節點n 與終點節點在三維空間中的坐標。

4.2 局部路徑規劃研究——TEB 局部規劃控制器實現

在局部路徑規劃問題中，受控對象的動力學模型會被更多地考慮進來并建立局部動態地圖，將自身空間參數與障礙物信息進行融合，并引入時間信息。算法不斷在適量空間中，實時得出各軸的速度、角速度及加速度的信息，并將所篩選的信息發布給控制器進行使用，從而實現局部路徑規劃。

實現TEB 局部規劃控制需要先下載安裝TEB，和TEB 例程。軟件包包括一個簡單的測試節點（test_optim_node），其可以優化固定初始姿態與目標姿態之間的軌跡。首先在初始與目標之間配置單個軌跡（Timed-Elastic-Band）的規劃，然后在獨特的拓撲中激活和設置規劃。使用ROS 參數服務器停用并行計劃（確保運行roscore）啟動test_optim_node 并結合預配置的rviz 節點進行可視化。通過運行rqt_reconfigure 來定制優化。解決本地優化方案的問題，并在獨特的拓撲中實現并行規劃。此擴展計劃程序默認啟用，需要更多計算資源。重新啟動roscore 或重新激活擴展計劃程序，啟動test_optim_node 并結合預配置的rviz 節點進行可視化。

5 機器人移動平臺導航實驗研究

本設計在一定的實驗環境下，采用合理的模擬方法，建立了以錐桶為障礙物圍繞而成的環形通道，如圖2所示。實現了在節省地方的基礎上，最大化地滿足研究需求。本實驗是讓機器人繞環形通道行駛2 圈，在第一圈時，自動識別障礙物并進行建圖。第二圈時，在已構建的地圖基礎上，進行路徑規劃進而實現自動導航的功能。

圖2 實驗環境

首先，讓機器人能夠自主進行場地信息的處理。本設計采用視覺系統以及雷達系統信息相結合的方法，來完成環形通道的行駛。通過雷達對周圍信息的反饋，根據雷達返回的每個點對應的距離以及角度，判斷出障礙物的大致范圍，進而通過視覺實現精準的定位，如圖3 所示。最終，可以使機器人自主通過此環形區域。

圖3 機器人在實際環境中所找到的障礙物

與此同時，本設計也利用采集到的雷達信息，結合輪式編碼器及IMU 的信息進行gmapping 建圖操作，從而實現機器人邊行駛邊進行二維柵格地圖建立的過程，當機器人完成全部的建圖之后，便得到一個環形的通道圖形，如圖4 所示。

圖4 路徑規劃時的圖像

此外，本設計利用上一步所建立的圖形，進行局部規劃以及利用A*算法對圖形進行第二圈的導航。在實驗過程中，發現本步驟十分依賴于第一步所建立的地圖。然而，作者在實驗過程中發現，若想建立好穩定的圖形，就要保證激光雷達始終保持在一個基準平面，這樣所建立的圖形就能夠精準地還原實際狀態。我們通過對雷達等周邊零件的固定，以及改造車體，使其更加不易晃動。最終，通過這些步驟，在完善設計所建立的地圖后，通過局部規劃以及A*算法成功地實現了第二圈閉環跑操作。