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

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

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

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

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

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

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

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

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

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

2 機(jī)器人移動(dòng)平臺(tái)的SLAM 系統(tǒng)搭建

2.1 SLAM 系統(tǒng)概述

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

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

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

3 機(jī)器人運(yùn)動(dòng)情況下的定位與對(duì)未知環(huán)境的地圖構(gòu)建

3.1 卡爾曼濾波算法研究

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

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

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

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

Kalman 增益的計(jì)算公式為

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

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

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

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

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

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

3.3 Gmapping 建圖算法

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

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

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

4 機(jī)器人移動(dòng)平臺(tái)的路徑規(guī)劃算法研究

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

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

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

A*算法的模型為

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

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

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

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

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

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

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

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

5 機(jī)器人移動(dòng)平臺(tái)導(dǎo)航實(shí)驗(yàn)研究

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

圖2 實(shí)驗(yàn)環(huán)境

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

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

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

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

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