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

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

（東北林業(yè)大學，哈爾濱 150000）

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

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

1 機器人系統(tǒng)設計框架

1.1 硬件系統(tǒng)設計

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

圖1 機器人的整體結(jié)構(gòu)與硬件組成

1.2 控制系統(tǒng)設計

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

2 機器人移動平臺的SLAM 系統(tǒng)搭建

2.1 SLAM 系統(tǒng)概述

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

2.2 視覺系統(tǒng)模型

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

3 機器人運動情況下的定位與對未知環(huán)境的地圖構(gòu)建

3.1 卡爾曼濾波算法研究

卡爾曼濾波算法（Kalman Filter）通過系統(tǒng)的觀測數(shù)據(jù)及系統(tǒng)狀態(tài)對全局最優(yōu)狀態(tài)進行推算。通過上一狀態(tài)和下一狀態(tài)來預測當前狀態(tài)，公式如下

式中:At指當前時刻的狀態(tài)轉(zhuǎn)移矩陣（就是指從上一狀態(tài)轉(zhuǎn)變?yōu)橄乱粻顟B(tài)的關系矩陣）；Bt為當前時刻的控制矩陣（就是指影響控制量的控制矩陣）；ut為當前時刻的控制量；μt-1為上一時刻最優(yōu)估計值；μˉt為當前時刻估計值。

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

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

Kalman 增益的計算公式為

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

通過Kalman 增益來計算狀態(tài)估計值

預測當前狀態(tài)需要用到上一狀態(tài)的協(xié)方差，所還需要計算當前狀態(tài)的協(xié)方差用于下一次迭代

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

3.2 數(shù)據(jù)融合

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

3.3 Gmapping 建圖算法

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

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

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

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

4.1 全局路徑規(guī)劃——A*算法

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

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

A*算法的模型為

式中：f（n）為總的搜索代價；g（n）為從起點到當前節(jié)點n 的代價和；h（n）為從當前節(jié)點n 到目標節(jié)點的最優(yōu)代價的啟發(fā)函數(shù)。

標準的啟發(fā)函數(shù)正是曼哈頓啟發(fā)函數(shù)

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

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

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

4.2 局部路徑規(guī)劃研究——TEB 局部規(guī)劃控制器實現(xiàn)

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

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

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

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

圖2 實驗環(huán)境

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

圖3 機器人在實際環(huán)境中所找到的障礙物

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

圖4 路徑規(guī)劃時的圖像

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