基于ROS的巡檢機(jī)器人系統(tǒng)設(shè)計(jì)與研究

宮志強(qiáng)，徐世許，王鵬程

（青島大學(xué) 自動(dòng)化學(xué)院，青島 266071）

地下管廊輸電線路安全對(duì)工廠的發(fā)展起著不可替代的作用，任意環(huán)節(jié)發(fā)生事故，都可能帶來(lái)不可估量的損失。為保證對(duì)輸電線路中母線、開(kāi)關(guān)、接觸器等主要設(shè)備運(yùn)行狀態(tài)的實(shí)時(shí)監(jiān)控，需要對(duì)輸電線路進(jìn)行定時(shí)定點(diǎn)的檢查和維護(hù)。目前，越來(lái)越多的工廠采用智能巡檢機(jī)器人來(lái)代替?zhèn)鹘y(tǒng)人工巡檢。智能巡檢機(jī)器人能以自主或遙控的方式，在無(wú)人值守或少人值守的工作環(huán)境中，對(duì)輸電線路進(jìn)行巡檢，能夠及時(shí)發(fā)現(xiàn)輸電線路中的異常情況，自動(dòng)報(bào)警并使用預(yù)先設(shè)置好的故障處理方式進(jìn)行處理[1]，真正起到了減員增效的作用。

本文針對(duì)廠區(qū)地下管廊輸電線路定時(shí)巡檢的需要，設(shè)計(jì)了一種上下位機(jī)結(jié)合的智能巡檢機(jī)器人，智能巡檢機(jī)器人采用PID 控制的全向移動(dòng)底盤，保證其靈活性以及足夠的檢測(cè)范圍。建立了巡檢機(jī)器人工作環(huán)境實(shí)驗(yàn)?zāi)Ｐ停瑧?yīng)用了改進(jìn)后的RBPF-SLAM 算法，實(shí)現(xiàn)了機(jī)器人自主構(gòu)建地圖并規(guī)劃出最優(yōu)路徑。編寫了上位機(jī)軟件，對(duì)云臺(tái)搭載的高清攝像頭獲取的圖像按照特定目的進(jìn)行相應(yīng)的處理[2]，最終得到所需要的數(shù)據(jù)，生成溫度分布圖像進(jìn)行線路異常檢測(cè)，滿足工廠地下管廊輸電線路巡檢的需要。

1 巡檢機(jī)器人總體設(shè)計(jì)

在巡檢機(jī)器人工作中，若要適應(yīng)復(fù)雜的工作環(huán)境，全向移動(dòng)是必備的功能。采用麥克納姆輪（Mecanum wheel）為實(shí)現(xiàn)機(jī)器人的全向移動(dòng)的常用方法。麥克納姆輪的安裝方式為X-長(zhǎng)方形（X-rectangle），輪子分布如圖1所示。

圖1 X-長(zhǎng)方形底盤Fig.1 X-rectangle chassis

巡檢機(jī)器人的核心控制板主要由兩部分組成，一部分是底層驅(qū)動(dòng)板，另一部分是工控機(jī)控制板，兩者分工明確，相輔相成。工控機(jī)選用天啟公司的Firefly-RK3399 型號(hào)開(kāi)發(fā)板，在工控機(jī)上配置ROS開(kāi)發(fā)環(huán)境，通過(guò)工控機(jī)的USB 接口與激光雷達(dá)連接。以STM32F103RC 單片機(jī)作為底層驅(qū)動(dòng)控制板。巡檢機(jī)器人的工控機(jī)開(kāi)發(fā)板如圖2所示。

圖2 工控機(jī)開(kāi)發(fā)板Fig.2 Industrial control computer development board

機(jī)器人啟動(dòng)之后，經(jīng)過(guò)初始化，先根據(jù)是否存在地圖來(lái)選擇調(diào)用地圖還是重新建立地圖再調(diào)用，調(diào)用完地圖后根據(jù)是否設(shè)置目標(biāo)點(diǎn)來(lái)選擇調(diào)用目標(biāo)點(diǎn)還是設(shè)置之后再調(diào)用，之后完成相應(yīng)的巡檢工作，在完成一次巡檢之后，判斷是否結(jié)束巡檢，如若不結(jié)束，則繼續(xù)巡檢。巡檢機(jī)器人系統(tǒng)工作流程如圖3所示。巡檢機(jī)器人實(shí)物如圖4所示。

圖3 系統(tǒng)工作流程Fig.3 System work flow chart

圖4 巡檢機(jī)器人Fig.4 Patrol robot

2 SLAM 地圖構(gòu)建與路徑規(guī)劃

2.1 直流電機(jī)PID 控制

對(duì)于巡檢機(jī)器人巡檢的運(yùn)動(dòng)過(guò)程，根據(jù)位置式和增量式PID 的特點(diǎn)，在直線行駛時(shí)采用增量式離散PID 調(diào)節(jié)來(lái)保證直線行駛時(shí)的穩(wěn)定性。而在轉(zhuǎn)彎過(guò)程中采用位置式離散PID 控制，保證巡檢機(jī)器人在轉(zhuǎn)彎過(guò)程中系統(tǒng)的快速反應(yīng)能力[3]。

位置閉環(huán)控制通過(guò)比較編碼器反饋的位置信息與目標(biāo)值對(duì)比計(jì)算出偏差，隨后進(jìn)行PID 運(yùn)算調(diào)節(jié)，使得偏差趨近于零。速度閉環(huán)控制與位置閉環(huán)控制相似。

位置閉環(huán)控制離散化PID 算式為

速度控制閉環(huán)系統(tǒng)中比較難出現(xiàn)突然擾動(dòng)，因而采用PI 控制，離散化PI 算式為

2.2 巡檢機(jī)器人SLAM 算法

SLAM 即同步定位與地圖構(gòu)建，為激光雷達(dá)的地圖構(gòu)建提供了技術(shù)支持[4]。柵格地圖是激光雷達(dá)SLAM 構(gòu)建地圖比較常用的方法。將激光雷達(dá)掃描到的環(huán)境地圖分解成一個(gè)個(gè)大小一致、相互獨(dú)立的小柵格。柵格地圖具有存儲(chǔ)結(jié)構(gòu)比較緊湊，幾乎不受地形限制的優(yōu)點(diǎn)。因此本文采用柵格地圖來(lái)建立環(huán)境地圖。

機(jī)器人在世界坐標(biāo)系下的位姿參數(shù)位可由機(jī)器人位姿增量算出，位姿參數(shù)為

結(jié)合柵格地圖分辨率參數(shù)，映射出該障礙物在柵格地圖的位置，其映射關(guān)系為

式中：（XGrid，YGrid）為障礙物在柵格地圖的位置；xr，yr為機(jī)器人坐標(biāo)；S 為柵格地圖的分辨率。

根據(jù)上述位置信息，采用基于RBPF 粒子濾波的Gmapping 算法。根據(jù)機(jī)器人自身傳感器的觀測(cè)數(shù)據(jù)和運(yùn)動(dòng)數(shù)據(jù)，推算出機(jī)器人當(dāng)前時(shí)刻的位姿，構(gòu)建環(huán)境地圖。對(duì)于傳統(tǒng)的RBPF-SLAM 算法，在環(huán)境相似度比較高的區(qū)域或是環(huán)境中存在激光雷達(dá)測(cè)量的噪聲干擾時(shí)，會(huì)出現(xiàn)錯(cuò)誤狀態(tài)的粒子比較大的情況。另外，頻繁的重采樣也會(huì)導(dǎo)致粒子多樣性的減少，導(dǎo)致粒子退化。粒子退化現(xiàn)象嚴(yán)重影響巡檢機(jī)器人創(chuàng)建地圖的精度。

針對(duì)上述問(wèn)題，Gmapping 提出減少粒子數(shù)和減少重新采樣次數(shù)的方案，改進(jìn)的提議分布為

式中：Zt為最近一次的觀測(cè)值。

為保證粒子的多樣性，防止粒子退化，在重采樣的過(guò)程中引入了遺傳算法，目的是保留中等權(quán)重的粒子，對(duì)低權(quán)重和高權(quán)重的粒子進(jìn)行自適應(yīng)操作。如式（6）所示，設(shè)定合理的高權(quán)重閾值和低權(quán)重閾值，取兩個(gè)閾值之間的粒子作為中等權(quán)重粒子[5]。

式中：wH為高權(quán)重閾值；wL為低權(quán)重閾值；N 為粒子數(shù)。

Gmapping 算法改進(jìn)了傳統(tǒng)的RBPF-SLAM 算法，使提議分布更加接近目標(biāo)分布。為有效防止算法中粒子退化的問(wèn)題，采取了掃描匹配函數(shù)、計(jì)算粒子權(quán)重以及進(jìn)行重新采樣3 種方法，其中部分代碼如下所示：

OrientedPoint lp=p；

lp.x+=cos（p.theta）*m_laserPose.x-sin（p.theta）*m_laserPose.y；//計(jì)算世界坐標(biāo)系X 坐標(biāo)

lp.y+=sin（p.theta）*m_laserPose.x+cos（p.theta）*m_laserPose.y；//計(jì)算世界坐標(biāo)系Y 坐標(biāo)

lp.theta+=m_laserPose.theta；//計(jì)算世界坐標(biāo)系角度

nlikehood=-0.5 //似然值，激光束沒(méi)有集中時(shí)的似然值為-0.5

double noHit=nlikehood/（m_likehoodSigma）；

2.3 Q-Learning 路徑規(guī)劃算法

Q-Learning 是基于value-based 的強(qiáng)化學(xué)習(xí)算法，是當(dāng)今應(yīng)用比較廣泛的強(qiáng)化學(xué)習(xí)算法。本文采用Q-Learning 算法實(shí)現(xiàn)巡檢機(jī)器人的最優(yōu)路徑規(guī)劃。確保機(jī)器人在完成構(gòu)建環(huán)境柵格地圖后，以最短路徑從起點(diǎn)出發(fā)到達(dá)終點(diǎn)。

設(shè)機(jī)器人每個(gè)狀態(tài)（state）、動(dòng)作（action）都有一個(gè)回報(bào)值Q（s，a）。計(jì)算累積回報(bào)值的公式為

式中：Ut表示累積回報(bào)值；γt表示折扣因子（衰減值）；Rt表示回報(bào)函數(shù)。

累積回報(bào)的期望值用狀態(tài)值函數(shù)Vπ（s）表示出來(lái)，狀態(tài)值函數(shù)為

最優(yōu)的累積回報(bào)期望用V*（s）來(lái)表示，最優(yōu)值函數(shù)為

式中：St表示機(jī)器人某一時(shí)刻的狀態(tài)；At表示機(jī)器人某一時(shí)刻的動(dòng)作；R 為回報(bào)函數(shù)（t 時(shí)刻，機(jī)器人在狀態(tài)s 做出a 動(dòng)作所能獲得的獎(jiǎng)勵(lì)）。

當(dāng)巡檢機(jī)器人執(zhí)行動(dòng)作a 時(shí)，“狀態(tài)-動(dòng)作”Q 值函數(shù)為

根據(jù)實(shí)際需求，期望機(jī)器人做出最有價(jià)值的動(dòng)作，最優(yōu)價(jià)值動(dòng)作函數(shù)Q*（s，a）為

機(jī)器人從起點(diǎn)到達(dá)目標(biāo)點(diǎn)的過(guò)程中，能夠?qū)ふ业揭粭l通過(guò)目標(biāo)點(diǎn)的路徑，從而形成Q 表。Q 表更新的過(guò)程就是路徑優(yōu)化的過(guò)程。

3 系統(tǒng)仿真與實(shí)驗(yàn)

搭建測(cè)試所需環(huán)境，分別對(duì)RBPF-SLAM 構(gòu)建地圖功能、Q-Learning 路徑規(guī)劃、紅外測(cè)溫成像功能進(jìn)行實(shí)驗(yàn)。

在測(cè)溫?cái)z像機(jī)上添加無(wú)線網(wǎng)橋模塊，實(shí)現(xiàn)測(cè)溫?cái)?shù)據(jù)WIFI 發(fā)送功能。

通過(guò)MATLAB 仿真實(shí)驗(yàn)，驗(yàn)證了在激光雷達(dá)構(gòu)建地圖時(shí)RBPF-SLAM 算法的可靠性。巡檢機(jī)器人通過(guò)激光雷達(dá)構(gòu)建出來(lái)的地圖，其精度較好，處理速度較快，與預(yù)先在MATLAB 設(shè)置好的周邊環(huán)境信息基本吻合，如圖5，圖6所示，能夠滿足巡檢機(jī)器人在地下管廊中構(gòu)建地圖的應(yīng)用需求。

圖5 MATLAB 構(gòu)建環(huán)境地圖Fig.5 Building environment map with MATLAB

圖6 構(gòu)建地圖的估計(jì)誤差Fig.6 Estimation error of constructing map

RBPF-SLAM 算法通過(guò)C++語(yǔ)言編程，應(yīng)用于巡檢機(jī)器人。在實(shí)驗(yàn)室環(huán)境中，觀察其構(gòu)建地圖的過(guò)程，驗(yàn)證算法的可靠性和準(zhǔn)確性。在實(shí)驗(yàn)室對(duì)現(xiàn)場(chǎng)環(huán)境進(jìn)行模擬，采集周圍環(huán)境信息，構(gòu)建環(huán)境地圖。實(shí)驗(yàn)室環(huán)境和巡檢機(jī)器人構(gòu)建地圖效果如圖7所示。

圖7 機(jī)器人構(gòu)建環(huán)境地圖效果圖Fig.7 Rendering of robot building environment map

通過(guò)實(shí)驗(yàn)結(jié)果可以看出，當(dāng)實(shí)驗(yàn)環(huán)境中存在多個(gè)障礙物時(shí)，機(jī)器人根據(jù)激光雷達(dá)掃描的數(shù)據(jù)實(shí)時(shí)構(gòu)建環(huán)境地圖。巡檢機(jī)器人可以掃描出障礙物的輪廓和位置信息，并且實(shí)時(shí)生成環(huán)境地圖。環(huán)境地圖精度準(zhǔn)確、效果較好并以BMP 圖片格式存儲(chǔ)到工控機(jī)中。當(dāng)環(huán)境地圖掃描完成后，調(diào)用ROS 開(kāi)發(fā)環(huán)境中的map 存儲(chǔ)包。

在ROS 開(kāi)發(fā)環(huán)境中，使用C++語(yǔ)言編寫QLearning 路徑規(guī)劃程序。當(dāng)巡檢機(jī)器人環(huán)境柵格地圖構(gòu)建完成后，系統(tǒng)調(diào)用工控機(jī)中儲(chǔ)存的柵格地圖，圖中障礙物和起始坐標(biāo)，規(guī)劃出最優(yōu)路徑，顯示到環(huán)境地圖中。在實(shí)驗(yàn)室測(cè)試環(huán)境中，用一個(gè)紙箱作為障礙物，設(shè)定機(jī)器人任務(wù)，從障礙物的左側(cè)，向設(shè)定好的目標(biāo)點(diǎn)移動(dòng)。巡檢機(jī)器人根據(jù)實(shí)際任務(wù)，執(zhí)行Q-Learning 路徑規(guī)劃算法，規(guī)劃出最優(yōu)路徑進(jìn)行移動(dòng)。巡檢機(jī)器人實(shí)際規(guī)劃的路徑如圖8所示。

圖8 巡檢機(jī)器人路徑規(guī)劃Fig.8 Path planning of inspection robot

在圖8中，圓點(diǎn)表示設(shè)定的目的地，帶坐標(biāo)軸的物體表示巡檢機(jī)器人，目標(biāo)點(diǎn)與機(jī)器人間的連線是路徑規(guī)劃算法所生成的機(jī)器人運(yùn)動(dòng)軌跡。在圖中，中間位置為激光雷達(dá)掃描到的障礙物，周圍的線段為激光雷達(dá)實(shí)時(shí)掃描的環(huán)境信息，以便機(jī)器人緊急避障。實(shí)驗(yàn)證明，熱成像巡檢機(jī)器人可以滿足巡檢任務(wù)的實(shí)際要求，為下一步實(shí)際應(yīng)用打下堅(jiān)實(shí)的基礎(chǔ)。

4 結(jié)語(yǔ)

本文以地下管廊巡檢機(jī)器人為對(duì)象，研究了地圖構(gòu)建、路徑規(guī)劃等技術(shù)。巡檢機(jī)器人在移動(dòng)過(guò)程中對(duì)自身進(jìn)行準(zhǔn)確的定位并實(shí)時(shí)構(gòu)建環(huán)境地圖，構(gòu)建完成后，將環(huán)境地圖儲(chǔ)存到工控機(jī)中。巡檢機(jī)器人根據(jù)具體巡檢任務(wù)的起止點(diǎn)要求，規(guī)劃出最優(yōu)的移動(dòng)路徑，使機(jī)器人從起點(diǎn)出發(fā)向目標(biāo)點(diǎn)移動(dòng)，同時(shí)采集管廊線路的溫度數(shù)據(jù)，將其上傳到上位機(jī)計(jì)算機(jī)中。本文在對(duì)機(jī)器人運(yùn)動(dòng)模型和紅外測(cè)溫模型進(jìn)行分析的基礎(chǔ)上，運(yùn)用PID 控制、RBPF-SLAM 算法、Q-Learning 算法，實(shí)現(xiàn)熱成像巡檢機(jī)器人的基本功能，具有較大的實(shí)用價(jià)值。