基于ROS 的目標(biāo)搜尋系統(tǒng)設(shè)計(jì)?

薛志凌 孟令軍 王佳軍 李曉宇

（中北大學(xué)儀器與電子學(xué)院 太原 030000）

1 引言

自主機(jī)器人是一種智能系統(tǒng)，它利用所安裝的多種傳感器以感知周圍環(huán)境［1］。同時(shí)處理這些信息，并進(jìn)行決策，做出相應(yīng)的動(dòng)作。機(jī)器人系統(tǒng)通常需要使用諸多傳感器，而各種傳感器有著不同的通信方式和數(shù)據(jù)格式，同時(shí)處理各種不同的傳感器數(shù)據(jù)是一件非常煩瑣的任務(wù)，機(jī)器人項(xiàng)目的落地常常因?yàn)榇嗽蚨щy重重。ROS 操作系統(tǒng)為解決這個(gè)機(jī)器人項(xiàng)目開發(fā)的痛點(diǎn)而研發(fā)。ROS（Robot Operation System）為機(jī)器人開發(fā)提供一個(gè)種非常便利的消息通信機(jī)制，極大地提高了開發(fā)中的軟件利用率和復(fù)用率［2］。ROS 是一套開源的系統(tǒng)［3］，部署和使用非常的便利。可以看到ROS 系統(tǒng)將成為未來機(jī)器人開發(fā)的主流。

利用移動(dòng)機(jī)器人對(duì)環(huán)境進(jìn)行探索和對(duì)特定目標(biāo)的搜尋是當(dāng)前機(jī)器人領(lǐng)域的研究熱點(diǎn)。人類感知周圍環(huán)境的絕大多數(shù)信息來源于視覺［4］，同時(shí)視覺信息的獲取比較容易。這也就表明了視覺相比于其他傳感器，可以獲得相對(duì)準(zhǔn)確豐富的信息，所以視覺傳感已經(jīng)成為大多數(shù)機(jī)器人機(jī)器的標(biāo)配。本文基于ROS 系統(tǒng)，設(shè)計(jì)一套智能移動(dòng)機(jī)器人系統(tǒng)，利用激光雷達(dá)信息完成對(duì)未知環(huán)境的探索和導(dǎo)航，再利用視覺信息對(duì)特定目標(biāo)進(jìn)行搜尋。通過WIFI 在PC 電腦端實(shí)時(shí)監(jiān)控機(jī)器人的運(yùn)行狀態(tài)和查看機(jī)器人采回的數(shù)據(jù)，同時(shí)還可以向機(jī)器人發(fā)送指令來控制機(jī)器人。

2 系統(tǒng)設(shè)計(jì)

2.1 系統(tǒng)總體設(shè)計(jì)

整個(gè)系統(tǒng)的框架如圖1 所示。自主導(dǎo)航算法和視覺伺服算法在樹莓派上完成。STM32 將采集到的IMU（慣性測(cè)量單元）數(shù)據(jù)和輪速計(jì)數(shù)據(jù)，通過串口傳遞給樹莓派，為SLAM（同步定位與建圖）系統(tǒng)提供里程計(jì)信息。顯示屏顯示機(jī)器人的電池電壓數(shù)據(jù)和IMU 傳感器數(shù)據(jù)。藍(lán)牙單元可以使機(jī)器人被移動(dòng)設(shè)備所控制。機(jī)器人通過WIFI（無線寬帶）和PC 端通信。PC 端可以監(jiān)控機(jī)器人的運(yùn)行情況，顯示機(jī)器人采集的視覺信息，機(jī)器人所建地圖和機(jī)器人的位置信息。同時(shí)PC端可以向機(jī)器人發(fā)布控制指令，將機(jī)器人的控制模式從自動(dòng)控制切換到手動(dòng)控制。

圖1 系統(tǒng)框架

整個(gè)系統(tǒng)主要由：目標(biāo)識(shí)別，建圖和導(dǎo)航，路徑規(guī)劃和機(jī)器人運(yùn)動(dòng)控制五個(gè)部分組成。

2.2 目標(biāo)識(shí)別

目標(biāo)識(shí)別的關(guān)鍵在于對(duì)識(shí)別目標(biāo)特征的提取。目前主流的提取圖像特征的方法有，卷積神經(jīng)網(wǎng)絡(luò)和特征點(diǎn)提?。?～6］。前者是應(yīng)用了深度學(xué)習(xí)的方法，其需要輸入目標(biāo)樣本訓(xùn)練識(shí)別模型然后建立分類器。這種方法雖然在識(shí)別率上取得了顯著的成功，但其在針對(duì)特定場(chǎng)景時(shí)，部署就需要消耗大量的時(shí)間和算力。而后者雖然識(shí)別準(zhǔn)確率不如前者，但其占用更少的計(jì)算資源，可以在通用場(chǎng)景中使用。此次設(shè)計(jì)要求只輸入目標(biāo)的圖像信息就可以完成對(duì)目標(biāo)的搜尋。針對(duì)此要求，設(shè)計(jì)采用基于特征點(diǎn)提取點(diǎn)的方式來完成目標(biāo)的識(shí)別。

SIFT（尺度不變特征變換）是一種不需要使用深度學(xué)習(xí)來提取圖像特征的成熟算法［7～8］。其提取圖像特征的大致過程有以下幾步。首先構(gòu)造圖像的尺度空間，這一步的目的是為了獲取圖像相對(duì)于攝像頭在不同距離下圖像的特征。一般的做法是該圖像乘以高斯函數(shù)，如式（1）。

其中，G為高斯函數(shù)，I為圖像的灰度值：

本質(zhì)上，它是給圖像的每一個(gè)像素乘以不同的比例因子，這些比例因子是高斯分布的。σ是高斯函數(shù)的方差，其決定了圖像的平滑程度，σ值越大，對(duì)應(yīng)的圖像就越模糊，也就代表著圖像離我們?cè)竭h(yuǎn)。相機(jī)采集的圖像在遠(yuǎn)處不光變得模糊，同時(shí)會(huì)隨著距離的增加，圖形的像素會(huì)逐漸變少，即圖像逐漸變小。為此我們構(gòu)建圖像金字塔，給經(jīng)過高斯函數(shù)處理的圖像乘以不同的縮放因子，σ值越大，圖像的縮放比例越大。然后對(duì)不同尺度的圖層做差，得到差分高斯金字塔（Difference of Gaussian，DOG）圖層。DOG算子定義為

這樣做的目的是將差值最大的點(diǎn)作為圖像的備選特征點(diǎn)。其次進(jìn)行關(guān)鍵點(diǎn)的提取。完成以上操作后，找到備選特征點(diǎn)中的極值點(diǎn)，作為圖像的特征點(diǎn)。這些點(diǎn)不具備方向信息，為此要確定每個(gè)特征點(diǎn)的方向。對(duì)于每一個(gè)點(diǎn)，只要確定每個(gè)點(diǎn)L(x,y)的梯度的模m(x,y)以及其方向θ(x,y)就可以得到每個(gè)點(diǎn)的位置，尺度和方向信息。

最后，生成其特征描述子，帶有每個(gè)像素點(diǎn)方向信息的特征向量。這樣我們完成了圖像特征的提取。通過RANSACS算法對(duì)目標(biāo)圖像和檢測(cè)圖像匹配［9］，即可找到目標(biāo)圖像。檢測(cè)流程如圖2。

圖2 目標(biāo)檢測(cè)流程

2.3 建圖與導(dǎo)航原理

在陌生環(huán)境中探索，需要機(jī)器人在記錄周圍環(huán)境的同時(shí)知道自己的相對(duì)位置。SLAM［10］技術(shù)可以根據(jù)傳感器采集回的數(shù)據(jù)，生成自身周圍的環(huán)境，即建圖。同時(shí)根據(jù)里程計(jì)信息確定自己的位置，在地圖和位置信息的基礎(chǔ)上完成導(dǎo)航。一般的SLAM 運(yùn)行過程如圖3。傳感器數(shù)據(jù)的輸入一般是激光雷達(dá)和攝像頭。

圖3 SLAM運(yùn)行流程

在ROS中，機(jī)器人的導(dǎo)航流程如圖4。

圖4 ROS導(dǎo)航流程

其中AMCL 是自適應(yīng)蒙特卡洛定位［11］的簡(jiǎn)稱，它是在粒子濾波的基礎(chǔ)上改進(jìn)而來。通過AMCL可以有效減小傳感器測(cè)量中的干擾，得出比較準(zhǔn)確的機(jī)器人位置信息。在ROS 中，AMCL 的運(yùn)行流程如圖5。

圖5 ROS中AMCL運(yùn)行流程

導(dǎo)航框架是整個(gè)控制任務(wù)的核心，主要是由全局路徑規(guī)劃和本地實(shí)時(shí)規(guī)劃這兩個(gè)部分組成。全局路徑規(guī)劃為找出全局最優(yōu)路徑，這里采用DIJKSTRA［12］全局規(guī)劃算法。本地實(shí)時(shí)規(guī)劃是為了應(yīng)對(duì)地圖的實(shí)時(shí)變化，既完成動(dòng)態(tài)場(chǎng)景的實(shí)時(shí)路徑規(guī)劃，這也意味著機(jī)器人可以完成實(shí)時(shí)的避障。本地實(shí)時(shí)規(guī)劃采用動(dòng)態(tài)窗口法（DWA）［13］。系統(tǒng)的導(dǎo)航框架如圖6。

圖6 導(dǎo)航框架

代價(jià)地圖主要描述環(huán)境中的障礙物信息。機(jī)器人避障過程如圖7 所示，圖中黑色部分為障礙物。當(dāng)機(jī)器人檢測(cè)到障礙物時(shí)，如以障礙物區(qū)域的幾何中心畫圓，讓半徑最小的圓包裹中障礙物，圖7 中的實(shí)線圓，這個(gè)圓的半徑稱為膨脹內(nèi)徑。增加膨脹內(nèi)徑，以膨脹內(nèi)徑圓為圓心，畫出膨脹外徑圓，膨脹內(nèi)徑與膨脹外徑圓之間的區(qū)域?yàn)榕蛎泤^(qū)。機(jī)器人絕對(duì)不能觸碰膨脹內(nèi)徑圓，同時(shí)避免進(jìn)入膨脹區(qū)。機(jī)器人會(huì)沿著膨脹外徑圓的外切線規(guī)劃路徑。當(dāng)機(jī)器人運(yùn)行發(fā)生錯(cuò)誤時(shí)，機(jī)器人會(huì)自動(dòng)重置當(dāng)前狀態(tài)，讓之正常運(yùn)行。在局部規(guī)劃中可以完成機(jī)器人的實(shí)時(shí)動(dòng)態(tài)避障行為。導(dǎo)航節(jié)點(diǎn)給出的基本的控制信息經(jīng)過轉(zhuǎn)換發(fā)送給機(jī)器人控制板，完成對(duì)機(jī)器人的控制。

圖7 避障示意圖

2.4 視覺伺服系統(tǒng)

在機(jī)器人沒有檢測(cè)到目標(biāo)物體的時(shí)候，機(jī)器人在自主導(dǎo)航模式下運(yùn)行。當(dāng)機(jī)器人檢測(cè)到目標(biāo)物體時(shí)，機(jī)器人的切換到視覺伺服系統(tǒng)控制，即通過視覺反饋完成對(duì)機(jī)器人的運(yùn)動(dòng)控制。在任務(wù)中，機(jī)器人需要停在被識(shí)別目標(biāo)前，同時(shí)機(jī)器人頭部應(yīng)正對(duì)被識(shí)別目標(biāo)。機(jī)器人與識(shí)別目標(biāo)的距離通過圖像的深度信息判斷。在伺服系統(tǒng)中，用PID 控制器，控制小車接近目標(biāo)物體的線速度，將機(jī)器人與識(shí)別目標(biāo)的距離作為PID 控制器的輸出，機(jī)器人的線速度作為輸出。同時(shí)控制機(jī)器人，使被識(shí)別物體的圖像中心處于相機(jī)呈像的幾何中心附近。

3 系統(tǒng)的ROS框架設(shè)計(jì)

基于ROS系統(tǒng)，可以很容易地搭建起整個(gè)系統(tǒng)的框架。把每一個(gè)功能塊封裝到一個(gè)ROS節(jié)點(diǎn)，通過ROS 的消息收發(fā)機(jī)制，實(shí)現(xiàn)整個(gè)系統(tǒng)的運(yùn)行，本次設(shè)計(jì)的ROS 框架如圖8。建圖導(dǎo)航節(jié)點(diǎn)采用ROS 的GMAPPING 功能包來實(shí)現(xiàn)基本的SIAM 算法，其需要輸入二維點(diǎn)云數(shù)據(jù)和機(jī)器人的里程計(jì)數(shù)據(jù)。使用ROS 的MOVE_BASE 導(dǎo)航框架作為路徑規(guī)劃節(jié)點(diǎn)。視覺伺服節(jié)點(diǎn)有二個(gè)作用，一是為機(jī)器人提供視覺伺服控制。二是當(dāng)機(jī)器人沒有識(shí)別到目標(biāo)物時(shí)，機(jī)器人將會(huì)由路徑規(guī)劃節(jié)點(diǎn)指引，移動(dòng)到指定目標(biāo)點(diǎn)。視覺伺服節(jié)點(diǎn)和路徑規(guī)劃節(jié)點(diǎn)都可以向機(jī)器人發(fā)布速度指令。目標(biāo)點(diǎn)設(shè)置節(jié)點(diǎn)負(fù)責(zé)設(shè)置機(jī)器人移動(dòng)目標(biāo)點(diǎn)和記錄機(jī)器人的初始位置。機(jī)器人在未知環(huán)境下，采用生成隨機(jī)目標(biāo)點(diǎn)的方式探索地圖并搜尋目標(biāo)物。機(jī)器人在已知地圖的條件下，采用固定點(diǎn)搜尋，以此來提高地圖的搜索效率。同時(shí)記錄了機(jī)器人的初始位置，在機(jī)器人完成任務(wù)后，使機(jī)器人可以回到出發(fā)點(diǎn)。

圖8 系統(tǒng)的ROS框架

4 仿真實(shí)驗(yàn)

ROS支持GAZEBO 仿真器和RVIZ可視化顯示工具。GAZEBO 是一種3D 仿真器［14］，其內(nèi)置游戲引擎，可以準(zhǔn)確地仿真機(jī)器人在復(fù)雜環(huán)境中的運(yùn)動(dòng)，同時(shí)其支持多種傳感器的仿真，如相機(jī)，激光雷達(dá)等。所以仿真實(shí)驗(yàn)選擇在GAZEBO 中完成。由于ROS 中發(fā)布的機(jī)器人速度指令格式為線速度加角速度，所以仿真中的機(jī)器人采用圓形差速機(jī)器人，同時(shí)搭載RGBD相機(jī)和激光雷達(dá)。

4.1 建圖仿真

圖9 為GAZEBO 機(jī)器人的仿真場(chǎng)景，仿真場(chǎng)景為室內(nèi)封閉的房間。建圖效果如圖10?？梢钥闯銎渚哂斜容^良好的建圖效果。

圖9 GAZEBO仿真場(chǎng)景

圖10 SLAM建圖效果圖

4.2 導(dǎo)航和避障仿真

導(dǎo)航仿真效果如圖11、圖12。導(dǎo)航中，機(jī)器人的位置信息用四元數(shù)表示。在圖12 中，箭頭的節(jié)點(diǎn)為機(jī)器人的到達(dá)點(diǎn)，箭頭的指向設(shè)定機(jī)器人的姿態(tài)。

圖11 RVIZ導(dǎo)航顯示效果

圖12 GAZEBO仿真環(huán)境運(yùn)行效果

避障效果如圖13、圖14，避障原理參見上文。

圖13 RVIZ顯示效果

圖14 GAZEBO仿真環(huán)境運(yùn)行效果

4.3 目標(biāo)搜尋任務(wù)仿真

在GAZEBO 仿真打開相機(jī)模型，在RVIZ 端可以查看相機(jī)發(fā)布的圖像信息。輸出目標(biāo)物體圖15，在GAZEBO仿真環(huán)境放置此物體。程序?qū)⒃O(shè)置隨機(jī)點(diǎn)，使機(jī)器人在場(chǎng)景中進(jìn)行探索，如圖16。

圖15 目標(biāo)物體

圖16 機(jī)器人探索過程

如果隨機(jī)設(shè)置的目標(biāo)點(diǎn)在封閉場(chǎng)景外，機(jī)器人可以完成整個(gè)環(huán)境的探索，則一定可以找到目標(biāo)物體。如何設(shè)置的目標(biāo)點(diǎn)在封閉環(huán)境內(nèi)，且到達(dá)目標(biāo)點(diǎn)后沒發(fā)現(xiàn)目標(biāo)物體，則機(jī)器人將前往下一個(gè)隨機(jī)目標(biāo)點(diǎn)，此過程持續(xù)到找到目標(biāo)物體。在找到目標(biāo)物體后，如圖17。程序退出導(dǎo)航程序，視覺伺服節(jié)點(diǎn)啟動(dòng)，機(jī)器人接近目標(biāo)物體，將頭部朝向目標(biāo)物體。到達(dá)物體前0.5m，完成目標(biāo)搜尋任務(wù)，此時(shí)機(jī)器人返回起始點(diǎn)，如圖18。

圖17 識(shí)別到目標(biāo)物體

圖18 機(jī)器人返回起始點(diǎn)

5 實(shí)物測(cè)試

本次系統(tǒng)設(shè)計(jì)的實(shí)物平臺(tái)如圖19，機(jī)器人的動(dòng)力部分由直流減速電機(jī)和麥克納姆輪［15～16］（簡(jiǎn)稱麥輪）組成。電機(jī)帶有光電霍爾編碼器，便于發(fā)布里程計(jì)信息。機(jī)器人的剩余主要硬件如表1所示。

表1 機(jī)器人剩余的主要硬件

圖19 機(jī)器人實(shí)物

使用麥輪的機(jī)器人可進(jìn)行全方位的移動(dòng)，但它的控制比一般的差速機(jī)器人和帶轉(zhuǎn)向舵的機(jī)器人控制復(fù)雜。試驗(yàn)中，麥輪的機(jī)器人的結(jié)構(gòu)如圖20，圖中W 為輪距，H 為軸距。對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)分析的時(shí)候，不考慮輪子打滑的情況。

圖20 麥輪機(jī)器人的結(jié)構(gòu)模型

若機(jī)器人中四個(gè)輪子對(duì)應(yīng)的轉(zhuǎn)速分別為VA，VB，VC，VD。Vx為機(jī)器人沿X 軸的速度分量，Vy為機(jī)器人沿Y 軸的速度分量。ω為機(jī)器人沿Z 軸的角速度。令。根據(jù)機(jī)器人的運(yùn)動(dòng)學(xué)狀態(tài)可以解算出機(jī)器人的四個(gè)輪子的轉(zhuǎn)速方程為

ROS中的速度指令cmd_vel包含三自由度的線速度和三個(gè)自由度的角速度。對(duì)于地面機(jī)器人不存在Z 軸的線速度和X，Y 軸的角速度。實(shí)際應(yīng)用中，將cmd_vel 中X 軸的線速度賦給Vx，Y 軸的線速度賦給Vy，Z 軸的角速度賦給ω，即可完成對(duì)機(jī)器人的控制。室內(nèi)運(yùn)行機(jī)器人，建圖效果如圖21。圖中左邊為機(jī)器人所建柵格地圖，右邊為機(jī)器人實(shí)時(shí)傳回的圖像。在圖中我們可以看到實(shí)驗(yàn)中使用的激光雷達(dá)只能檢測(cè)到激光雷達(dá)所在平面的二維障礙物信息。

圖21 機(jī)器人建圖與圖傳

在機(jī)器人開啟自主導(dǎo)航模式時(shí)，會(huì)在所建基本地圖信息的周圍生成膨脹區(qū)，詳見2.3 節(jié)。實(shí)時(shí)導(dǎo)航效果如圖22。在機(jī)器人搜尋的目標(biāo)物體是，機(jī)器人會(huì)主動(dòng)靠近目標(biāo)物，結(jié)果如圖23?？梢钥吹较到y(tǒng)在復(fù)雜環(huán)境中有較好的識(shí)別效果。

圖22 機(jī)器人導(dǎo)航避障

圖23 目標(biāo)識(shí)別結(jié)果

6 結(jié)語

本文設(shè)計(jì)了一套以ROS 為基礎(chǔ)的自主式機(jī)器人搜尋系統(tǒng)。整個(gè)系統(tǒng)框架和相關(guān)算法先在GAZEBO 中仿真實(shí)現(xiàn)，然后應(yīng)用于實(shí)際之中。在仿真和實(shí)際驗(yàn)證中，該系統(tǒng)均擁有良好的性能。系統(tǒng)的核心算法在機(jī)器人的機(jī)載計(jì)算機(jī)中完成，通過WIFI 可在PC 端監(jiān)控機(jī)器人的運(yùn)行狀態(tài)和采回的傳感器數(shù)據(jù)，同時(shí)可向機(jī)器人發(fā)布控制指令。在設(shè)計(jì)中，強(qiáng)調(diào)仿真的重要性，期望實(shí)現(xiàn)真實(shí)世界和虛擬世界機(jī)器人運(yùn)動(dòng)的一致性。相信以后的機(jī)器人系統(tǒng)設(shè)計(jì)會(huì)遵循先仿真后實(shí)物搭建的順序來完成。本研究的下一步研究重點(diǎn)是在機(jī)器人識(shí)別到目標(biāo)后做出實(shí)時(shí)的動(dòng)作，主要應(yīng)用特殊場(chǎng)合的設(shè)備巡視與維護(hù)。