煤礦水泵房巡檢機(jī)器人路徑規(guī)劃與跟蹤算法的研究

金書(shū)奎，寇子明，吳 娟

(1.太原理工大學(xué) 機(jī)械工程學(xué)院,山西 太原 030024；2.山西省礦山流體控制工程實(shí)驗(yàn)室，山西 太原 030024；3.礦山流體控制國(guó)家地方聯(lián)合工程實(shí)驗(yàn)室,山西 太原 030024)

0 引 言

煤礦井下主排水系統(tǒng)是煤炭生產(chǎn)中不可缺少的重要設(shè)備，其運(yùn)行狀況的好壞直接影響著礦山的正常生產(chǎn)和工人的生命安全[1]。為保證排水設(shè)備的正常運(yùn)行，巡檢人員要運(yùn)用感官以及一些配套的檢測(cè)儀器定期對(duì)其進(jìn)行檢查，該方式勞動(dòng)強(qiáng)度大，主觀因素多，巡檢不到位，巡檢結(jié)果數(shù)字化不便[2-3]，不符合數(shù)字礦山的發(fā)展方向。隨著機(jī)器人技術(shù)的發(fā)展，采用機(jī)器人完成水泵房巡檢工作成為了可能，水泵房巡檢機(jī)器人，可以把水泵房巡檢人員從日常簡(jiǎn)單、機(jī)械、重復(fù)性的工作中解放出來(lái)，便于人們集中精力處理更加重要和復(fù)雜的工作，對(duì)強(qiáng)化煤礦生產(chǎn)安全管理，真正實(shí)現(xiàn)減員增效、節(jié)能降耗具有重要意義[3]。巡檢機(jī)器人導(dǎo)航包括定位、路徑規(guī)劃和路徑跟蹤三部分，是巡檢機(jī)器人在水泵房?jī)?nèi)穩(wěn)定安全行進(jìn)的必要條件，沒(méi)有可靠的導(dǎo)航保證，機(jī)器人將無(wú)法完成巡檢任務(wù)。目前常用的導(dǎo)航方式有：電磁導(dǎo)航、視覺(jué)導(dǎo)航、GPS導(dǎo)航等[4]。電磁導(dǎo)航雖然能做到車(chē)體較精確定位，但種導(dǎo)航方式需要在機(jī)器人運(yùn)行路線上鋪設(shè)磁條和設(shè)定RFID標(biāo)簽，施工的工作量較大，不能預(yù)測(cè)前方道路情況，嚴(yán)重制約了移動(dòng)機(jī)器人的行走速度；視覺(jué)導(dǎo)航在黑夜或光線不足的情況下效果較差，并且地面導(dǎo)航標(biāo)志一旦破損或丟失會(huì)極大地影響機(jī)器人的導(dǎo)航定位性能；GPS導(dǎo)航有很好的靈活性，但是在煤礦井下水泵房?jī)?nèi)很難接收到GPS信號(hào)，即使接收到信號(hào)，也無(wú)法解決信號(hào)失真的問(wèn)題[4]。

目前如何在煤礦井下水泵房?jī)?nèi)進(jìn)行精確導(dǎo)航是實(shí)現(xiàn)水泵房巡檢機(jī)器人自主巡檢所面臨的重要問(wèn)題之一。對(duì)此，研究了基于檢測(cè)點(diǎn)、行進(jìn)點(diǎn)的最優(yōu)路徑規(guī)劃算法和點(diǎn)對(duì)點(diǎn)的路徑追蹤算法。首先通過(guò)路徑規(guī)劃算法在建好的二維柵格地圖中規(guī)劃出一條經(jīng)過(guò)所有檢測(cè)點(diǎn)和行進(jìn)點(diǎn)的無(wú)障礙路徑，然后通過(guò)路徑跟蹤算法計(jì)算出巡檢機(jī)器人沿該路徑行走需要的實(shí)時(shí)線速度和角速度，最后將線速度和角速度發(fā)送給巡檢機(jī)器人運(yùn)動(dòng)控制器，巡檢機(jī)器人便可沿著規(guī)劃好的路徑行走，到達(dá)檢測(cè)點(diǎn)后便可采集排水設(shè)備的運(yùn)行數(shù)據(jù)。經(jīng)仿真和實(shí)驗(yàn)結(jié)果表明使用該算法控制機(jī)器人巡檢在導(dǎo)航精度上滿足機(jī)器人巡檢工作的要求。

1 二維柵格地圖的構(gòu)建

柵格地圖是巡檢機(jī)器人自主導(dǎo)航中常用的一種環(huán)境地圖，目前主要通過(guò)激光雷達(dá)傳感器與SLAM算法結(jié)合的方法建立二維柵格地圖，該方法多在室內(nèi)且環(huán)境狀況未知的情況下使用，在實(shí)際建圖過(guò)程中激光雷達(dá)傳感器存在噪音，激光雷達(dá)數(shù)據(jù)存在運(yùn)動(dòng)畸變，使得建立的柵格地圖與真實(shí)環(huán)境有較大的誤差[5]，不利于巡檢機(jī)器人的路徑規(guī)劃，因此采用人工測(cè)量與MATLAB中柵格地圖生成算法結(jié)合建立二維柵格地圖，具體步驟如下：

1)人工測(cè)量可得到水泵房的整體尺寸、排水設(shè)備的具體尺寸和排水設(shè)備在水泵房中的具體位置，根據(jù)這些信息繪制出水泵房二維平面圖,如圖1所示。

圖1 水泵房二維平面圖Fig.1 Two-dimensional plan pump house

2)根據(jù)水泵房二維平面圖將水泵房?jī)?nèi)的障礙物和可通行區(qū)域以數(shù)字化的方式表示成一個(gè)44行34列的矩陣如圖2所示。在矩陣元素中0代表可通行區(qū)域，1代表障礙物區(qū)域。

圖2 水泵房二維平面圖數(shù)字化表示Fig.2 Digital representation of two-dimensional plan of water pump house

3)通過(guò)調(diào)用MATLAB 機(jī)器人工具箱下“robotics.Binary Occupancy Grid”函數(shù)將二維平面圖數(shù)字化矩陣生成二維柵格地圖如圖3所示，該地圖橫向有34個(gè)網(wǎng)格，縱向有44個(gè)網(wǎng)格，地圖的分辨率為每米10個(gè)網(wǎng)格。

圖3 二維占據(jù)柵格地圖Fig.3 2D grid map

由于水泵房?jī)?nèi)各個(gè)設(shè)備的位置已知，且設(shè)備的位置是不經(jīng)常發(fā)生改變的，因此人工測(cè)量與MATLAB中柵格地圖生成算法結(jié)合建立水泵房的二維柵格地圖的方法是可行的。

2 兩輪差速輪式移動(dòng)機(jī)器人里程計(jì)模型

在輪式自主導(dǎo)航系統(tǒng)中，里程計(jì)是重要組成部分，從里程計(jì)的計(jì)算數(shù)據(jù)中可以獲取機(jī)器人運(yùn)動(dòng)的位姿和速度等信息，這些信息對(duì)于實(shí)現(xiàn)機(jī)器人的定位和路徑跟蹤有重要作用[6]。

2.1 機(jī)器人坐標(biāo)系和全局坐標(biāo)系關(guān)系

把機(jī)器人建模成輪子上的一個(gè)剛體，運(yùn)動(dòng)在水平面上，在平面上機(jī)器人底盤(pán)總的自由度是3個(gè)。由于存在輪軸和小腳輪關(guān)節(jié)，還會(huì)有附加的自由度和靈活性。就機(jī)器人的底盤(pán)而言，把它看做剛體，忽略機(jī)器人和它的輪子間內(nèi)在的關(guān)聯(lián)和自由度。為了確定機(jī)器人在平面中的位置，建立全局坐標(biāo)系和機(jī)器人坐標(biāo)系之間的關(guān)系。

如圖4所示，在平面上選擇一點(diǎn)作為原點(diǎn)O，創(chuàng)建相互正交的X軸和Y軸，建立全局坐標(biāo)系。為確定機(jī)器人的位置，在機(jī)器人底盤(pán)上選擇一點(diǎn)C作為位置參考點(diǎn)。以點(diǎn)C為原點(diǎn)，創(chuàng)建相互正交的XR軸和YR軸，建立機(jī)器人坐標(biāo)系。在全局坐標(biāo)系上，機(jī)器人的位置由坐標(biāo)x和y確定，機(jī)器人的姿態(tài)由機(jī)器人坐標(biāo)系XR軸和全局坐標(biāo)系X軸間夾角θ確定。機(jī)器人在全局坐標(biāo)系中的位姿表示ξI=(x,y,θ)，通過(guò)正交旋轉(zhuǎn)矩陣R(θ)，可把機(jī)器人坐標(biāo)系的運(yùn)動(dòng)映射到全局坐標(biāo)系下。

圖4 全局坐標(biāo)系和機(jī)器人坐標(biāo)系Fig.4 World coordinate system and robot coordinate system

2.2 兩輪差速輪式移動(dòng)機(jī)器人里程計(jì)模型

巡檢過(guò)程中地面可能存在渣石、溝坎和凸臺(tái)，造成機(jī)器人的車(chē)輪與地面間出現(xiàn)相對(duì)滑動(dòng)[7]，這使得里程計(jì)計(jì)算得到的位姿和機(jī)器人的真實(shí)位姿之間存在誤差，由于煤礦井下水泵房的地面是平整和清潔的，車(chē)輪與地面間相對(duì)滑動(dòng)可忽略，因此，假設(shè)機(jī)器人車(chē)輪在沒(méi)有打滑的前提下，對(duì)其進(jìn)行運(yùn)動(dòng)學(xué)分析。

機(jī)器人有2個(gè)主動(dòng)輪，輪子直徑為d，兩輪輪間距為l，機(jī)器人左右兩輪的運(yùn)動(dòng)速度分別為VL和VR，其中VL和VR可通過(guò)安裝在電機(jī)上的光電編碼器測(cè)量獲得。巡檢機(jī)器人在機(jī)器人坐標(biāo)系下位置參考點(diǎn)C運(yùn)動(dòng)的線速度為v(t)，角速度為ω(t)為：

(1)

(2)

式中：VL、VR分別為左右車(chē)輪的線速度;v(t)、ω(t)分別為機(jī)器人位置參考點(diǎn)C的線速度和角速度。

則在全局坐標(biāo)系下機(jī)器人在X軸和Y軸方向的線速度和繞Z軸旋轉(zhuǎn)的角速度為：

(3)

在全局坐標(biāo)系下機(jī)器人的位姿為：

(4)

式中：ξI為機(jī)器人在全局坐標(biāo)系下的位姿矩陣。

在給定移動(dòng)機(jī)器人線速度v(t)、角速度ω(t)和機(jī)器人坐標(biāo)系相對(duì)于全局坐標(biāo)系的變換矩陣的情況下，由式(4)可得到任意時(shí)刻機(jī)器人在全局坐標(biāo)系中的位姿。

3 移動(dòng)機(jī)器人路徑規(guī)劃

水泵房巡檢機(jī)器人路徑規(guī)劃就是尋找一條機(jī)器人從起始點(diǎn)，中途經(jīng)過(guò)所有選定的行進(jìn)點(diǎn)和檢測(cè)點(diǎn)，然后到達(dá)最終點(diǎn)的路徑[8]。其中行進(jìn)點(diǎn)是輔助機(jī)器人進(jìn)行路徑規(guī)劃的坐標(biāo)點(diǎn)，檢測(cè)點(diǎn)是機(jī)器人?？坎?duì)水泵房設(shè)備進(jìn)行信息采集的坐標(biāo)點(diǎn)。

3.1 水泵房巡檢任務(wù)中各點(diǎn)坐標(biāo)位置確定

如圖5所示，點(diǎn)1和點(diǎn)6分別為水泵房巡檢任務(wù)中的起始點(diǎn)和最終點(diǎn)，他們處于水泵房入口和出口的中間位置，此處障礙物較少，便于機(jī)器人通行；點(diǎn)2和點(diǎn)4是水泵房巡檢任務(wù)中的檢測(cè)點(diǎn)，檢測(cè)點(diǎn)設(shè)置在排水設(shè)備的中間位置，方便機(jī)器人對(duì)排水設(shè)備中水泵、電機(jī)、聯(lián)軸器、轉(zhuǎn)軸進(jìn)行拍照、紅外測(cè)溫和噪音檢測(cè)；點(diǎn)3和點(diǎn)5是水泵房巡檢任務(wù)中的行進(jìn)點(diǎn)，行進(jìn)點(diǎn)設(shè)置在水泵房走廊的中間位置，其作用是給路徑規(guī)劃算法增加約束，讓該算法規(guī)劃的全局路徑位于障礙物較少的位置。由于水泵房?jī)?nèi)各排水設(shè)備的位置相對(duì)固定，所以點(diǎn)1～點(diǎn)6坐標(biāo)位置可通過(guò)實(shí)地測(cè)量確定下來(lái)，機(jī)器人路徑規(guī)劃算法根據(jù)這些坐標(biāo)點(diǎn)便可規(guī)劃出一條合理的全局路徑。

圖5 帶有行進(jìn)點(diǎn)和巡檢點(diǎn)的二維柵格地圖Fig.5 2D grid maps with travel and patrol points

3.2 PRM路徑規(guī)劃算法

PRM是一種基于圖搜索的算法，它將連續(xù)空間轉(zhuǎn)換成離散空間，再利用A*等搜索算法在路線圖上尋找路徑[9]，在圖6中PRM算法運(yùn)行過(guò)程分為6個(gè)步驟，前5個(gè)步驟為離線學(xué)習(xí)階段，第6個(gè)步驟為查詢階段。離線學(xué)習(xí)階段主要內(nèi)容是在給定地圖的自由空間內(nèi)生成N個(gè)隨機(jī)點(diǎn)，在每個(gè)隨機(jī)點(diǎn)的周?chē)x取鄰域點(diǎn)，把隨機(jī)點(diǎn)和它的所有鄰域點(diǎn)相連，并去掉穿過(guò)障礙物區(qū)域的連線，由此得到一個(gè)無(wú)向路徑網(wǎng)絡(luò)圖G=(V,E)，其中其中V為節(jié)點(diǎn)集，E為所有可能的兩點(diǎn)之間的路徑集。查詢階段主要內(nèi)容是根據(jù)學(xué)習(xí)階段構(gòu)造的無(wú)向網(wǎng)絡(luò)路徑圖，采用某種搜索算法，如A*算法，廣度優(yōu)先搜索算法等，在起點(diǎn)和終點(diǎn)之間搜索出無(wú)碰撞的路徑。

圖6 PRM算法具體流程示意Fig.6 PRM algorithm specific flow

4 移動(dòng)機(jī)器人路徑跟蹤

機(jī)器人路徑跟蹤是在平面坐標(biāo)系下要求機(jī)器人從某一點(diǎn)出發(fā)，按照某種控制規(guī)律到達(dá)路徑規(guī)劃算法生成的最優(yōu)的路徑上，并沿著該路徑運(yùn)動(dòng)到目標(biāo)點(diǎn)[10]。水泵房一定程度上屬于時(shí)變環(huán)境，在已規(guī)劃的路徑上，某段時(shí)間內(nèi)可能存在障礙物，即便是最優(yōu)的路徑，也很難完全考慮到機(jī)器人規(guī)劃路徑中的各種障礙物情況[11]，因此機(jī)器人在進(jìn)行路徑跟蹤時(shí)還需通過(guò)傳感器探測(cè)前方障礙物，并及時(shí)避障。

4.1 Pure Pursuit路徑跟蹤算法

圖7中G(gx,gy)是下一個(gè)要跟蹤的目標(biāo)點(diǎn)，它位于已經(jīng)規(guī)劃好的全局路徑上，現(xiàn)在需要控制巡檢機(jī)器人的位置參考點(diǎn)C經(jīng)過(guò)該目標(biāo)點(diǎn)，參考點(diǎn)C的當(dāng)前位置為(cx,cy)，ld表示巡檢機(jī)器人當(dāng)前位置到目標(biāo)點(diǎn)的距離，α表示目前車(chē)身和目標(biāo)路點(diǎn)的夾角。根據(jù)兩點(diǎn)之間的距離公式可得到：

圖7 Pure Pursuit路徑跟蹤算法簡(jiǎn)化模型Fig.7 Simplified model of Pure Pursuit path tracking algorithm

(5)

式中：gx和gy為目標(biāo)點(diǎn)的橫坐標(biāo)和縱坐標(biāo)；cx和cy為機(jī)器人當(dāng)前位置的橫坐標(biāo)和縱坐標(biāo)；ld為巡檢機(jī)器人當(dāng)前位置到目標(biāo)點(diǎn)的距離。

移動(dòng)過(guò)程中控制機(jī)器人的線速度大小與機(jī)器人距離目標(biāo)的遠(yuǎn)近ld成正比得到：

(6)

式中：VL、VR分別為左右車(chē)輪的線速度;v(t)為機(jī)器人質(zhì)心移動(dòng)的線速度;ld為機(jī)器人位置參考點(diǎn)C距目標(biāo)點(diǎn)的距離;kv為ld與v(t)的比值。

根據(jù)正弦定理可以推導(dǎo)出如下轉(zhuǎn)換式：

(7)

(8)

(9)

式中：ld為機(jī)器人位置參考點(diǎn)C距目標(biāo)點(diǎn)的距離；α為機(jī)器人從當(dāng)前位置運(yùn)動(dòng)到目標(biāo)點(diǎn)需要轉(zhuǎn)過(guò)的角度；R為機(jī)器人運(yùn)動(dòng)軌跡的圓弧半徑。

通過(guò)上式可得到機(jī)器人需要轉(zhuǎn)動(dòng)的角度為：

(10)

式中：α為機(jī)器人從當(dāng)前位置運(yùn)動(dòng)到目標(biāo)點(diǎn)需要轉(zhuǎn)過(guò)的角度；ld為機(jī)器人位置參考點(diǎn)C距目標(biāo)點(diǎn)的距離；R為機(jī)器人運(yùn)動(dòng)軌跡的圓弧半徑。

移動(dòng)過(guò)程中控制機(jī)器人的角速度大小與機(jī)器人需要轉(zhuǎn)動(dòng)的角度α成正比得到：

ω(t)=khα

(11)

式中：ω(t)為機(jī)器人旋轉(zhuǎn)角速度；kh為角速度ω(t)與角度α的比值。

4.2 機(jī)器人避障算法

通過(guò)激光雷達(dá)傳感器實(shí)現(xiàn)機(jī)器人的避障，由激光測(cè)距的原理可知，在激光雷達(dá)旋轉(zhuǎn)一周的過(guò)程中，激光發(fā)射頭會(huì)等時(shí)間間隔發(fā)射激光(每束激光的間隔角度相等)，假定激光雷達(dá)旋轉(zhuǎn)一周后，激光發(fā)射頭總共發(fā)射了360束激光，則每束激光的間隔角度為1°，這樣就可以劃分所有的激光束，使之對(duì)應(yīng)機(jī)器人前后左右的方位，如圖8所示，定義1～3激光束掃描的位置為機(jī)器人右方，4～18激光束掃描的位置為機(jī)器人前方，19～21激光束掃描的位置為機(jī)器人的左方。

圖8 激光束對(duì)應(yīng)的機(jī)器人的方位Fig.8 Orientation of robot corresponding to laser beam

在機(jī)器人右方、前方和左方的每束激光測(cè)得的距離d中選取最小值，分別為y、q和z，避障算法流程如圖9所示。

圖9 避障算法流程Fig.9 Flow of avoidance algorithm

5 仿真試驗(yàn)

5.1 PRM路徑規(guī)劃算法仿真

在3.1節(jié)中已經(jīng)通過(guò)人工測(cè)量的方式得到水泵房巡檢任務(wù)中起始點(diǎn)、行進(jìn)點(diǎn)、檢測(cè)點(diǎn)和最終點(diǎn)總共6個(gè)點(diǎn)的坐標(biāo)位置，此處主要在MATLAB中根據(jù)點(diǎn)1～點(diǎn)6的坐標(biāo)位置結(jié)合PRM路徑規(guī)劃算法進(jìn)行路徑規(guī)劃仿真，由于PRM路徑規(guī)劃算法執(zhí)行一次只能規(guī)劃出兩個(gè)坐標(biāo)點(diǎn)間的路徑，要規(guī)劃出一條經(jīng)過(guò)6個(gè)坐標(biāo)點(diǎn)的路徑，至少需要執(zhí)行5次PRM路徑規(guī)劃算法，執(zhí)行完5次PRM路徑規(guī)劃算法后生成的路徑如圖10所示。

圖10 PRM路徑規(guī)劃算法生成的5段路徑Fig.10 5 paths generated by path planning algorithm

由圖10中可看出，PRM路徑規(guī)劃算法總共生成了5段路徑，這5段路徑連接在一起組成了由起始點(diǎn)(點(diǎn)1)到最終點(diǎn)(點(diǎn)6)的全局路徑，在這條全局路徑上點(diǎn)2和點(diǎn)4是巡檢機(jī)器人對(duì)排水設(shè)備進(jìn)行信息采集的坐標(biāo)點(diǎn)，點(diǎn)3和點(diǎn)5是輔助PRM算法進(jìn)行路徑規(guī)劃的坐標(biāo)點(diǎn)，他們能夠把PRM算法規(guī)劃的路徑約束在道路中間的位置，這樣在滿足路徑較短的前提下，又可讓規(guī)劃的路徑盡量避開(kāi)道路邊緣的障礙物。選取坐標(biāo)點(diǎn)4到坐標(biāo)點(diǎn)5之間的路徑規(guī)劃(路徑4)進(jìn)行重點(diǎn)介紹，在進(jìn)行路徑規(guī)劃前，首先根據(jù)機(jī)器人的尺寸對(duì)水泵房二維柵格地圖進(jìn)行膨脹，以防止機(jī)器人在進(jìn)行路徑跟蹤時(shí)與轉(zhuǎn)角處的障礙物發(fā)生碰撞，然后將膨脹后的地圖給PRM路徑規(guī)劃算法，并設(shè)置規(guī)劃路徑的起點(diǎn)(1.2,2.4)，終點(diǎn)(2.7,3.3)，PRM路徑規(guī)劃算法生成的隨機(jī)點(diǎn)數(shù)目為800個(gè)，PRM路徑規(guī)劃算法的仿真結(jié)果如圖11所示。在圖11中藍(lán)色的點(diǎn)是PRM路徑規(guī)劃算法生成的隨機(jī)點(diǎn)，灰色的區(qū)域?yàn)殡S機(jī)點(diǎn)之間的連線，綠色的線為PRM算法生成的點(diǎn)4～點(diǎn)5的路徑，在圖12中可看到該路徑是由一系列坐標(biāo)點(diǎn)連線組成，這些坐標(biāo)點(diǎn)在接下來(lái)的路徑跟蹤中有重要作用。

圖11 PRM路徑規(guī)劃算法仿真結(jié)果Fig.11 PRM path planning algorithm simulation results

圖12 PRM路徑規(guī)劃算法生成的路徑Fig.12 Path generated by path planning algorithm

5.2 PurePursuit路徑跟蹤算法仿真

在上一節(jié)中，已經(jīng)得到了一條由點(diǎn)1～點(diǎn)6的全局路徑，這條全局路徑上點(diǎn)2和點(diǎn)4是巡檢機(jī)器人對(duì)排水設(shè)備進(jìn)行信息采集的坐標(biāo)點(diǎn)，在這兩點(diǎn)處機(jī)器人要調(diào)整好位姿停止一段時(shí)間對(duì)排水設(shè)備進(jìn)行信息采集，若直接通過(guò)Pure Pursuit算法控制機(jī)器人跟蹤點(diǎn)1～點(diǎn)6這條全局路徑，則在經(jīng)過(guò)點(diǎn)2和點(diǎn)4時(shí)機(jī)器人無(wú)法停留，因此需要先將這條全局路徑分割為3段，分割點(diǎn)即為點(diǎn)2和點(diǎn)4，然后運(yùn)行3次Pure Pursuit算法分別跟蹤路徑1(點(diǎn)1～點(diǎn)2)，路徑2和路徑3(點(diǎn)2～點(diǎn)4)，路徑4和路徑5(點(diǎn)4～點(diǎn)6)，運(yùn)行3次Pure Pursuit算法后機(jī)器人運(yùn)動(dòng)軌跡如圖13所示。

圖13 機(jī)器人的運(yùn)動(dòng)軌跡Fig.13 Trajectory of robot

由圖13看出軌跡1、軌跡2和軌跡3與PRM路徑規(guī)劃算法生成的路徑基本重合。機(jī)器人從起始點(diǎn)(點(diǎn)1)跟蹤路徑1到達(dá)排水設(shè)備1的檢測(cè)點(diǎn)(點(diǎn)2)，機(jī)器人在此處停留一段時(shí)間采集排水設(shè)備1的數(shù)據(jù)，然后跟蹤路徑2和路徑3到達(dá)排水設(shè)備2的檢測(cè)點(diǎn)(點(diǎn)4)，機(jī)器人在此處停留一段時(shí)間采集排水設(shè)備2的數(shù)據(jù)，最后跟蹤路徑4和路徑5到達(dá)最終點(diǎn)(點(diǎn)6)。重點(diǎn)介紹Pure Pursuit算法控制移動(dòng)機(jī)器人跟蹤路徑4和路徑5(點(diǎn)4～點(diǎn)6)。在進(jìn)行路徑跟蹤前要在MATLAB中生成一個(gè)兩輪差速機(jī)器人模型，設(shè)置機(jī)器人的直徑為0.2 m，該機(jī)器人可通過(guò)里程計(jì)得到任意時(shí)刻全局坐標(biāo)系下的位姿，該位姿可以判斷機(jī)器人是否到達(dá)了目標(biāo)點(diǎn)；再把點(diǎn)4～點(diǎn)6的規(guī)劃路徑存儲(chǔ)到Pure Pursuit路徑跟蹤算法中，點(diǎn)4到點(diǎn)6的規(guī)劃路徑由一些坐標(biāo)點(diǎn)連線組成，這些坐標(biāo)點(diǎn)是由PRM算法生成的，他們是Pure Pursuit算法需要跟蹤的目標(biāo)點(diǎn)；設(shè)置機(jī)器人進(jìn)行路徑跟蹤時(shí)的線速度0.1 m/s，最大角速度1 rad/s，距離閾值為0.01 m，執(zhí)行Pure Pursuit算法得到的仿真結(jié)果如圖14所示。

圖14 Pure Pursuit路徑跟蹤算法仿真結(jié)果Fig.14 Simulation results of Pure Pursuit path tracking algorithm

圖15為Pure Pursuit路徑跟蹤算法控制機(jī)器人跟蹤路徑4和路徑5時(shí)各個(gè)時(shí)刻的姿態(tài)角、角速度和線速度的變化，在區(qū)間1內(nèi)機(jī)器人處于起始點(diǎn)，位姿(x,y,θ)=(1.2,2.4,0)，規(guī)劃路徑姿態(tài)角(規(guī)劃路徑與全局坐標(biāo)系X軸間夾角)大于機(jī)器人的姿態(tài)角(機(jī)器人坐標(biāo)系的X軸與全局坐標(biāo)系的X軸之間的夾角)，因此Pure Pursuit路徑跟蹤算法給出的機(jī)器人旋轉(zhuǎn)角速度大于0，機(jī)器人逆時(shí)針?lè)较蛐D(zhuǎn)，機(jī)器人的姿態(tài)角增加，在區(qū)間2內(nèi)機(jī)器人的姿態(tài)角與規(guī)劃路徑姿態(tài)角相等，Pure Pursuit路徑跟蹤算法給出的機(jī)器人旋轉(zhuǎn)角速度等于0，線速度為0.1 m/s保持不變，機(jī)器人將沿著規(guī)劃路徑移動(dòng)。在區(qū)間3內(nèi)由于規(guī)劃路徑的姿態(tài)角進(jìn)一步增大，因此Pure Pursuit路徑跟蹤算法給出的機(jī)器人旋轉(zhuǎn)角速度大于0，機(jī)器人逆時(shí)針?lè)较蛐D(zhuǎn)，機(jī)器人的姿態(tài)角也增大。在區(qū)間4內(nèi)機(jī)器人的姿態(tài)角與規(guī)劃路徑姿態(tài)角相等，Pure Pursuit路徑跟蹤算法給出的機(jī)器人旋轉(zhuǎn)角速度等于0，線速度仍保持不變，機(jī)器人沿著規(guī)劃路徑移動(dòng)。同理在區(qū)間5內(nèi)也發(fā)生了規(guī)劃路徑姿態(tài)角的改變，同時(shí)Pure Pursuit路徑跟蹤算法也給出了機(jī)器人不同的旋轉(zhuǎn)角速度，使機(jī)器人的姿態(tài)角最終與規(guī)劃路徑的姿態(tài)角相等。在區(qū)間6內(nèi)機(jī)器人到達(dá)規(guī)劃路徑的最終點(diǎn)，此時(shí)線速度和角速度都為0，機(jī)器人的最終的姿態(tài)角仍和規(guī)劃路徑姿態(tài)角相等。

圖15 Pure Pursuit算法跟蹤路徑過(guò)程分析Fig.15 Pure Pursuit Algorithm tracking path process analysis

5.3 避障算法仿真

機(jī)器人避障是指機(jī)器人在進(jìn)行路徑跟蹤時(shí)，如果激光雷達(dá)檢測(cè)到機(jī)器人前方存在障礙物，就運(yùn)行避障算法控制機(jī)器人繞開(kāi)該障礙物，然后繼續(xù)跟蹤原始路徑。本節(jié)通過(guò)Pure Pursuit路徑跟蹤算法跟蹤路徑2和路徑3，并在路徑2上設(shè)置一個(gè)障礙物用來(lái)進(jìn)行避障算法的仿真，仿真結(jié)果如圖16所示。

圖16 避障算法仿真結(jié)果Fig.16 Simulation results of obstacle avoidance algorithm

圖17為機(jī)器人在避障過(guò)程中各個(gè)時(shí)刻的機(jī)器人的姿態(tài)角、機(jī)器人前方、右方和左方距障礙物距離的變化，設(shè)置距離閾值為0.1 m，打開(kāi)激光雷達(dá)傳感器，通過(guò)分析激光雷達(dá)傳感器的數(shù)據(jù)可得到機(jī)器各個(gè)方位距障礙物的距離，當(dāng)該距離大于距離閾值時(shí)，機(jī)器人處于路徑跟蹤階段，此時(shí)路徑跟蹤算法控制機(jī)器人沿原始路徑移動(dòng)，當(dāng)機(jī)器人前方或左方距障礙物的距離小于距離閾值時(shí)，機(jī)器人處于避障階段，開(kāi)始執(zhí)行避障算法并停止執(zhí)行路徑跟蹤算法，由于機(jī)器人右方距障礙物的距離明顯大于前方和左方，機(jī)器人右轉(zhuǎn)(順時(shí)針旋轉(zhuǎn))，機(jī)器人的姿態(tài)角減小了，機(jī)器人前進(jìn)一段距離后左轉(zhuǎn)，檢測(cè)到機(jī)器人各個(gè)方位距障礙物的距離已經(jīng)大于距離閾值，說(shuō)明機(jī)器人已經(jīng)繞過(guò)障礙物，機(jī)器人回到路徑跟蹤階段，停止避障算法并繼續(xù)執(zhí)行路徑跟蹤算法。

圖17 避障算法過(guò)程分析Fig.17 Process analysis of avoidance algorithm

5.4 試驗(yàn)驗(yàn)證

對(duì)巡檢機(jī)器人導(dǎo)航至某一坐標(biāo)點(diǎn)時(shí)機(jī)器人的實(shí)際位姿與測(cè)量位姿間的誤差進(jìn)行分析，將MATLAB軟件通過(guò)無(wú)線串口與巡檢機(jī)器人聯(lián)機(jī)，在MATLAB中首先運(yùn)行PRM算法規(guī)劃出一條經(jīng)過(guò)點(diǎn)1～點(diǎn)6的全局路徑，然后運(yùn)行Pure Pursuit路徑跟蹤算法，控制真實(shí)的巡檢機(jī)器人在水泵房環(huán)境中按照PRM算法規(guī)劃的路徑運(yùn)動(dòng)依次經(jīng)過(guò)點(diǎn)1～點(diǎn)6，并避開(kāi)路徑跟蹤過(guò)程中出現(xiàn)的障礙物，如圖18所示，紅色箭頭為巡檢機(jī)器人進(jìn)行路徑跟蹤時(shí)的運(yùn)動(dòng)方向。

圖18 機(jī)器人路徑跟蹤運(yùn)動(dòng)方向Fig.18 Robot path tracking motion direction

測(cè)量機(jī)器人運(yùn)動(dòng)到在每個(gè)坐標(biāo)點(diǎn)處的位姿，測(cè)量結(jié)果見(jiàn)表1和表2。

在實(shí)際導(dǎo)航中，巡檢機(jī)器人始終沿著規(guī)劃的路徑行走，根據(jù)表1和表2的測(cè)量數(shù)據(jù)通過(guò)式(12)和式(13)計(jì)算得到巡檢機(jī)器人在導(dǎo)航過(guò)程中各坐標(biāo)點(diǎn)的位置誤差和角度誤差，見(jiàn)表3。

表1 各個(gè)坐標(biāo)點(diǎn)的實(shí)際位置與測(cè)量位置Table 1 Actual position and measurement position of each coordinate point m

表2 各個(gè)坐標(biāo)點(diǎn)的實(shí)際偏轉(zhuǎn)角與測(cè)量偏轉(zhuǎn)角 Table 2 Actual deflection angle of each coordinate point and measured deflection angle rad

(12)

(13)

由表3可得該巡檢機(jī)器人在導(dǎo)航過(guò)程中位置平均誤差為4 cm，姿態(tài)角度平均誤差為0.095 rad，在機(jī)器人即將離開(kāi)巡檢點(diǎn)并移動(dòng)到下個(gè)巡檢點(diǎn)的時(shí)候，有時(shí)位置誤差超過(guò) 4 cm 且角度誤差大于0.1 rad，這是由于移動(dòng)機(jī)器人在加速或減速過(guò)程中自重引起的慣性頓挫所造成，在所有的測(cè)試中，巡檢移動(dòng)機(jī)器人都沒(méi)有碰到規(guī)劃路徑上的障礙物。結(jié)果表明，該巡檢機(jī)器人能夠水泵房中進(jìn)行巡檢工作且滿足實(shí)際工況需求。

表3 各個(gè)坐標(biāo)點(diǎn)的位置誤差和姿態(tài)角度誤差Table 3 Position error and angle error of each coordinate point

6 結(jié) 論

1)通過(guò)人工測(cè)量的方法在MATLAB中建立了水泵房二維柵格地圖，并在此地圖上根據(jù)巡檢過(guò)程中設(shè)置的檢測(cè)點(diǎn)和行進(jìn)點(diǎn)利用PRM路徑規(guī)劃算法獲得一條經(jīng)過(guò)所有排水設(shè)備的全局路徑。

2)巡檢機(jī)器人通過(guò)Pure Pursuit路徑跟蹤算法使其能夠沿著規(guī)劃的全局路徑移動(dòng)，通過(guò)避障算法使其能夠及時(shí)避開(kāi)周?chē)恼系K物，最終保證了機(jī)器人能夠安全平穩(wěn)地到達(dá)巡檢點(diǎn)。

3)將巡檢機(jī)器人置于水泵房模擬環(huán)境中，對(duì)其路徑跟蹤和避障的能力進(jìn)行多次試驗(yàn)，最終結(jié)果表明：巡檢機(jī)器人在進(jìn)行路徑跟蹤時(shí)沒(méi)有偏離規(guī)劃的路徑，到達(dá)巡檢點(diǎn)后其位置的平均誤差為4 cm，姿態(tài)角度的平均誤差為0.095 rad。