基于ROS 的目標搜尋系統設計?

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

（中北大學儀器與電子學院 太原 030000）

1 引言

自主機器人是一種智能系統，它利用所安裝的多種傳感器以感知周圍環境［1］。同時處理這些信息，并進行決策，做出相應的動作。機器人系統通常需要使用諸多傳感器，而各種傳感器有著不同的通信方式和數據格式，同時處理各種不同的傳感器數據是一件非常煩瑣的任務，機器人項目的落地常常因為此原因而困難重重。ROS 操作系統為解決這個機器人項目開發的痛點而研發。ROS（Robot Operation System）為機器人開發提供一個種非常便利的消息通信機制，極大地提高了開發中的軟件利用率和復用率［2］。ROS 是一套開源的系統［3］，部署和使用非常的便利。可以看到ROS 系統將成為未來機器人開發的主流。

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

2 系統設計

2.1 系統總體設計

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

圖1 系統框架

整個系統主要由：目標識別，建圖和導航，路徑規劃和機器人運動控制五個部分組成。

2.2 目標識別

目標識別的關鍵在于對識別目標特征的提取。目前主流的提取圖像特征的方法有，卷積神經網絡和特征點提取［5～6］。前者是應用了深度學習的方法，其需要輸入目標樣本訓練識別模型然后建立分類器。這種方法雖然在識別率上取得了顯著的成功，但其在針對特定場景時，部署就需要消耗大量的時間和算力。而后者雖然識別準確率不如前者，但其占用更少的計算資源，可以在通用場景中使用。此次設計要求只輸入目標的圖像信息就可以完成對目標的搜尋。針對此要求，設計采用基于特征點提取點的方式來完成目標的識別。

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

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

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

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

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

圖2 目標檢測流程

2.3 建圖與導航原理

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

圖3 SLAM運行流程

在ROS中，機器人的導航流程如圖4。

圖4 ROS導航流程

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

圖5 ROS中AMCL運行流程

導航框架是整個控制任務的核心，主要是由全局路徑規劃和本地實時規劃這兩個部分組成。全局路徑規劃為找出全局最優路徑，這里采用DIJKSTRA［12］全局規劃算法。本地實時規劃是為了應對地圖的實時變化，既完成動態場景的實時路徑規劃，這也意味著機器人可以完成實時的避障。本地實時規劃采用動態窗口法（DWA）［13］。系統的導航框架如圖6。

圖6 導航框架

代價地圖主要描述環境中的障礙物信息。機器人避障過程如圖7 所示，圖中黑色部分為障礙物。當機器人檢測到障礙物時，如以障礙物區域的幾何中心畫圓，讓半徑最小的圓包裹中障礙物，圖7 中的實線圓，這個圓的半徑稱為膨脹內徑。增加膨脹內徑，以膨脹內徑圓為圓心，畫出膨脹外徑圓，膨脹內徑與膨脹外徑圓之間的區域為膨脹區。機器人絕對不能觸碰膨脹內徑圓，同時避免進入膨脹區。機器人會沿著膨脹外徑圓的外切線規劃路徑。當機器人運行發生錯誤時，機器人會自動重置當前狀態，讓之正常運行。在局部規劃中可以完成機器人的實時動態避障行為。導航節點給出的基本的控制信息經過轉換發送給機器人控制板，完成對機器人的控制。

圖7 避障示意圖

2.4 視覺伺服系統

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

3 系統的ROS框架設計

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

圖8 系統的ROS框架

4 仿真實驗

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

4.1 建圖仿真

圖9 為GAZEBO 機器人的仿真場景，仿真場景為室內封閉的房間。建圖效果如圖10。可以看出其具有比較良好的建圖效果。

圖9 GAZEBO仿真場景

圖10 SLAM建圖效果圖

4.2 導航和避障仿真

導航仿真效果如圖11、圖12。導航中，機器人的位置信息用四元數表示。在圖12 中，箭頭的節點為機器人的到達點，箭頭的指向設定機器人的姿態。

圖11 RVIZ導航顯示效果

圖12 GAZEBO仿真環境運行效果

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

圖13 RVIZ顯示效果

圖14 GAZEBO仿真環境運行效果

4.3 目標搜尋任務仿真

在GAZEBO 仿真打開相機模型，在RVIZ 端可以查看相機發布的圖像信息。輸出目標物體圖15，在GAZEBO仿真環境放置此物體。程序將設置隨機點，使機器人在場景中進行探索，如圖16。

圖15 目標物體

圖16 機器人探索過程

如果隨機設置的目標點在封閉場景外，機器人可以完成整個環境的探索，則一定可以找到目標物體。如何設置的目標點在封閉環境內，且到達目標點后沒發現目標物體，則機器人將前往下一個隨機目標點，此過程持續到找到目標物體。在找到目標物體后，如圖17。程序退出導航程序，視覺伺服節點啟動，機器人接近目標物體，將頭部朝向目標物體。到達物體前0.5m，完成目標搜尋任務，此時機器人返回起始點，如圖18。

圖17 識別到目標物體

圖18 機器人返回起始點

5 實物測試

本次系統設計的實物平臺如圖19，機器人的動力部分由直流減速電機和麥克納姆輪［15～16］（簡稱麥輪）組成。電機帶有光電霍爾編碼器，便于發布里程計信息。機器人的剩余主要硬件如表1所示。

表1 機器人剩余的主要硬件

圖19 機器人實物

使用麥輪的機器人可進行全方位的移動，但它的控制比一般的差速機器人和帶轉向舵的機器人控制復雜。試驗中，麥輪的機器人的結構如圖20，圖中W 為輪距，H 為軸距。對機器人進行運動學分析的時候，不考慮輪子打滑的情況。

圖20 麥輪機器人的結構模型

若機器人中四個輪子對應的轉速分別為VA，VB，VC，VD。Vx為機器人沿X 軸的速度分量，Vy為機器人沿Y 軸的速度分量。ω為機器人沿Z 軸的角速度。令。根據機器人的運動學狀態可以解算出機器人的四個輪子的轉速方程為

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

圖21 機器人建圖與圖傳

在機器人開啟自主導航模式時，會在所建基本地圖信息的周圍生成膨脹區，詳見2.3 節。實時導航效果如圖22。在機器人搜尋的目標物體是，機器人會主動靠近目標物，結果如圖23。可以看到系統在復雜環境中有較好的識別效果。

圖22 機器人導航避障

圖23 目標識別結果

6 結語

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