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

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

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

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

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

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

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

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

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

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

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

2.1 SLAM 系統(tǒng)概述

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

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

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

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

3.1 卡爾曼濾波算法研究

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

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

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

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

Kalman 增益的計算公式為

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

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

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

通過以上公式即可預(yù)測出物體的位置坐標(biāo)及速度。

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

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

3.3 Gmapping 建圖算法

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

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

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

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

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

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

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

A*算法的模型為

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

標(biāo)準(zhǔn)的啟發(fā)函數(shù)正是曼哈頓啟發(fā)函數(shù)

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

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

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

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

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

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

5 機器人移動平臺導(dǎo)航實驗研究

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

圖2 實驗環(huán)境

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

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

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

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

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