儲(chǔ)罐檢測爬壁機(jī)器人全遍歷路徑規(guī)劃

唐東林，龍?jiān)儆?，湯炎錦，潘 峰，游傳坤

（西南石油大學(xué)機(jī)電工程學(xué)院，四川成都610500）

路徑規(guī)劃是實(shí)現(xiàn)移動(dòng)機(jī)器人精確運(yùn)動(dòng)和控制的核心問題之一，大量學(xué)者進(jìn)行了深入研究，并取得了一定的成就［1-4］。移動(dòng)機(jī)器人的路徑規(guī)劃主要分為2類：一類是點(diǎn)到點(diǎn)的路徑規(guī)劃，另一類是全遍歷路徑規(guī)劃。點(diǎn)到點(diǎn)的路徑規(guī)劃是指機(jī)器人在工作環(huán)境中尋找一條從起點(diǎn)到終點(diǎn)的安全避障路徑［5-7］。全遍歷路徑規(guī)劃是指機(jī)器人規(guī)劃出一條能夠完全遍歷環(huán)境中未被障礙物占據(jù)區(qū)域的路徑，并要求所規(guī)劃路徑滿足安全性和合理性要求［8-10］。全遍歷路徑規(guī)劃在草坪修剪機(jī)［11］、收割機(jī)［12］、清掃機(jī)器人［13］以及軍用掃雷機(jī)器人［14］中有廣泛應(yīng)用。

根據(jù)是否掌握工作空間的環(huán)境信息，可將全遍歷路徑規(guī)劃分為已知環(huán)境規(guī)劃和未知環(huán)境規(guī)劃［15-17］。針對(duì)已知環(huán)境的路徑規(guī)劃，已有相對(duì)成熟的方法，且容易實(shí)現(xiàn)對(duì)環(huán)境的全遍歷。針對(duì)未知環(huán)境的路徑規(guī)劃，一般采用隨機(jī)遍歷的方式，這種方式效率很低且無法實(shí)現(xiàn)全遍歷。近年來未知環(huán)境遍歷路徑規(guī)劃算法成為了研究熱點(diǎn)。文獻(xiàn)［18］將生物激勵(lì)神經(jīng)網(wǎng)絡(luò)與啟發(fā)式規(guī)則相結(jié)合，用于遍歷路徑規(guī)劃，實(shí)現(xiàn)了簡單環(huán)境的遍歷，但不能遍歷含有U型障礙物的環(huán)境和使機(jī)器人快速逃離死區(qū)。文獻(xiàn)［19］將基于柵格地圖的單元分解法用于遍歷路徑規(guī)劃，實(shí)現(xiàn)了對(duì)簡單結(jié)構(gòu)化環(huán)境的遍歷和死區(qū)逃離，但對(duì)含有各向U型障礙物的復(fù)雜環(huán)境無法實(shí)現(xiàn)全遍歷。

本文根據(jù)儲(chǔ)罐檢測爬壁機(jī)器人工作環(huán)境的特點(diǎn)，提出了一種基于滾動(dòng)窗口的優(yōu)先級(jí)啟發(fā)式算法。基于柵格地圖建立儲(chǔ)罐檢測爬壁機(jī)器人工作環(huán)境模型，利用設(shè)計(jì)的優(yōu)先級(jí)啟發(fā)式算法在滾動(dòng)窗口對(duì)路徑進(jìn)行搜索。引入U(xiǎn)型障礙物區(qū)域填充算法和死區(qū)逃離算法，使機(jī)器人能有效地遍歷U型障礙物區(qū)域并且快速逃離死區(qū)，降低遍歷重復(fù)率，使機(jī)器人在未知儲(chǔ)罐環(huán)境下能有效地遍歷整個(gè)工作區(qū)域。

1 儲(chǔ)罐檢測爬壁機(jī)器人工作環(huán)境建模

1.1 儲(chǔ)罐壁面簡化

立式圓柱體儲(chǔ)罐外壁是圓弧面，為三維工作環(huán)境。圖1所示為儲(chǔ)罐檢測爬壁機(jī)器人壁面工作圖。

圖1 儲(chǔ)罐檢測爬壁機(jī)器人壁面工作圖Fig.1 Wall working diagram of oil tank detection wall climbing robot

圖1（a）中：O為儲(chǔ)罐橫截面圓心；R為儲(chǔ)罐半徑；n為機(jī)器人左端點(diǎn)；m為機(jī)器人質(zhì)心；A為機(jī)器人底板距儲(chǔ)罐外壁距離；B為機(jī)器人寬度；α為機(jī)器人爬行后的曲率角。

常用的圓柱體儲(chǔ)罐直徑為24.0～40.0 m，高為9.6～16.3m。以直徑為24.0 m、高為9.6 m 的儲(chǔ)罐，寬度B=0.3 m的機(jī)器人（機(jī)器人底板距儲(chǔ)罐外壁的距離A很小，可以忽略）為例，進(jìn)行計(jì)算：

α代表機(jī)器人在儲(chǔ)罐壁面爬行時(shí)的變形量。從式（1）可得：α的值很小，可忽略不計(jì)；隨著儲(chǔ)罐容積增大，α值呈降低趨勢(shì)。因此機(jī)器人在儲(chǔ)罐壁面爬行時(shí)幾乎不發(fā)生變形，可將機(jī)器人看作在豎直二維平面上爬行。

1.2 基于柵格地圖的環(huán)境建模

基于柵格地圖對(duì)儲(chǔ)罐外壁建模，根據(jù)機(jī)器人檢測探頭的寬度確定柵格大小。如式（2）所示，每個(gè)柵格有一個(gè)表示柵格狀態(tài)的值xi，并隨著機(jī)器人運(yùn)動(dòng)實(shí)時(shí)更新。

定義1：只要柵格被障礙物占據(jù)，則將該柵格定義為障礙物柵格。

根據(jù)儲(chǔ)罐外壁的情況建立環(huán)境模型，如圖2 所示，有針對(duì)性地在環(huán)境中設(shè)置了儲(chǔ)罐外壁可能存在的障礙物。該環(huán)境的柵格地圖模型如圖3所示，其中黑色部分為障礙物柵格，空白部分為未遍歷柵格。通過對(duì)比圖2和圖3可得，基于柵格地圖進(jìn)行環(huán)境建?？梢允弓h(huán)境中的不規(guī)則障礙物變成規(guī)則的方形障礙物，使機(jī)器人在遍歷時(shí)運(yùn)動(dòng)方式變得簡單。

圖2 儲(chǔ)罐檢測爬壁機(jī)器人工作環(huán)境模型Fig.2 Working environment model of oil tank detection wall climbing robot

圖3 儲(chǔ)罐檢測爬壁機(jī)器人工作環(huán)境模型的柵格地圖Fig.3 Grid map of working environment model of oil tank detection wall climbing robot

若根據(jù)柵格的賦值情況建立三維柵格地圖，則未遍歷柵格位于地圖的頂部，已遍歷柵格位于地圖的中部，而障礙物柵格位于地圖的底部，如圖4所示?？蓪⒃摲椒ㄓ糜诨趥鞲衅鞯奈粗h(huán)境實(shí)時(shí)局部建模。

圖4 儲(chǔ)罐檢測爬壁機(jī)器人工作環(huán)境模型的三維柵格地圖Fig.4 3D grid map of working environment model of oil tank detection wall climbing robot

2 基于滾動(dòng)窗口的遍歷路徑規(guī)劃

2.1 路徑規(guī)劃描述

本文討論的是儲(chǔ)罐全局環(huán)境未知情況下爬壁機(jī)器人全遍歷路徑規(guī)劃問題。規(guī)劃的目的是機(jī)器人能遍歷工作環(huán)境中所有可達(dá)區(qū)域，并滿足遍歷重復(fù)率最小的要求［20］。

儲(chǔ)罐外壁分布著有限個(gè)大小、形狀不受限制的障礙物，機(jī)器人事先未存儲(chǔ)任何障礙物信息，只能利用搭載的傳感器探測有限范圍內(nèi)的環(huán)境信息。為了安全起見，在機(jī)器人在線探測時(shí)把障礙物作一定的膨化處理［21］，同時(shí)機(jī)器人具有存儲(chǔ)運(yùn)動(dòng)路徑信息和自定位功能。將機(jī)器人視為一個(gè)質(zhì)點(diǎn)，當(dāng)它運(yùn)動(dòng)到地圖中的某個(gè)柵格時(shí)，認(rèn)為該柵格已遍歷。

2.2 基于滾動(dòng)窗口的優(yōu)先級(jí)啟發(fā)式路徑規(guī)劃

圖5所示為基于滾動(dòng)窗口的優(yōu)先級(jí)啟發(fā)式路徑規(guī)劃算法的構(gòu)建流程。

圖5 基于滾動(dòng)窗口的優(yōu)先級(jí)啟發(fā)式路徑規(guī)劃算法的構(gòu)建流程Fig.5 Construction flowchart of priority heuristic path planning algorithm based on rolling window

2.2.1 滾動(dòng)窗口

基于滾動(dòng)窗口的路徑規(guī)劃（簡稱滾動(dòng)規(guī)劃）是機(jī)器人在工作空間運(yùn)動(dòng)過程中實(shí)時(shí)探測周圍的局部環(huán)境信息，并以逐步滾動(dòng)的方式進(jìn)行在線路徑規(guī)劃，可實(shí)現(xiàn)路徑的優(yōu)化與反饋，確保規(guī)劃算法的合理性和安全性。

在滾動(dòng)規(guī)劃中定義的滾動(dòng)窗口包括可視窗口和規(guī)劃窗口。滾動(dòng)窗口的生成包括以下4個(gè)步驟：

1）設(shè)置滾動(dòng)窗口的參考坐標(biāo)系。

如圖6所示，XOY表示世界坐標(biāo)系，xoy表示機(jī)器人坐標(biāo)系。機(jī)器人利用其攜帶的傳感器探測障礙物的位置信息，在xoy坐標(biāo)系中建立局部環(huán)境模型，用該坐標(biāo)系中的坐標(biāo)來表示障礙物的具體位置。機(jī)器人在全局環(huán)境中的位置則需要用XOY坐標(biāo)系的坐標(biāo)來表示，通過XOY與xoy的坐標(biāo)變換則可以確定障礙物在XOY坐標(biāo)系中的具體位置，從而建立全局環(huán)境模型。

圖6 滾動(dòng)窗口參考坐標(biāo)系Fig.6 Rolling window reference coordinate system

2）確定滾動(dòng)窗口的范圍。

設(shè)W表示機(jī)器人所在二維平面所有點(diǎn)的集合，P表示環(huán)境中的任意一點(diǎn)，PXY(t)表示機(jī)器人當(dāng)前時(shí)刻位置，d表示機(jī)器人當(dāng)前時(shí)刻位置與環(huán)境中任意一點(diǎn)的距離。機(jī)器人傳感器能夠探測以機(jī)器人為中心、半徑為r的圓形區(qū)域環(huán)境信息。

定義2：Win(PXY(t))={P|P∈W，d(P，PXY(t))≤r}為機(jī)器人的滾動(dòng)窗口。

3）確定可視窗口。

可視窗口是在滾動(dòng)窗口的基礎(chǔ)上設(shè)定的，在不影響局部路徑規(guī)劃的前提下，考慮局部環(huán)境信息的存儲(chǔ)及計(jì)算量，選取合適的滾動(dòng)窗口大小。

定義3：以機(jī)器人為中心的5×5 個(gè)柵格為可視窗口。

4）確定規(guī)劃窗口。

規(guī)劃窗口是機(jī)器人進(jìn)行局部路徑規(guī)劃的區(qū)域，其大小不能超過可視窗口，且需根據(jù)局部路徑規(guī)劃算法確定。根據(jù)優(yōu)先級(jí)啟發(fā)式路徑規(guī)劃算法，以機(jī)器人前進(jìn)1個(gè)單位長度來定義規(guī)劃窗口。

定義4：以機(jī)器人為中心的3×3 個(gè)柵格為規(guī)劃窗口。

如圖7 所示，可視窗口和規(guī)劃窗口分別包含24個(gè)和8個(gè)柵格的信息。機(jī)器人在規(guī)劃窗口內(nèi)確定局部子目標(biāo)并規(guī)劃路徑向子目標(biāo)行進(jìn)。機(jī)器人每行進(jìn)1 步，窗口相應(yīng)向前滾動(dòng)，搭載的傳感器將實(shí)時(shí)獲得最新的環(huán)境信息并對(duì)滾動(dòng)窗口進(jìn)行更新。如圖8所示，Rob1、Rob2、Rob3 分別表示機(jī)器人移動(dòng)時(shí)的位置，r1、r2、r3為相對(duì)應(yīng)的滾動(dòng)窗口的半徑。

圖7 滾動(dòng)窗口Fig.7 Rolling window

圖8 窗口滾動(dòng)示意圖Fig.8 Schematic diagram of window rolling

2.2.2 優(yōu)先級(jí)啟發(fā)式算法設(shè)計(jì)

啟發(fā)式算法是與具體問題求解及搜索過程相關(guān)的，將求解問題的具體信息融入算法中，從而提高算法的搜索效率，具有簡單、直觀且有效的優(yōu)點(diǎn)。因此本文提出基于滾動(dòng)窗口的優(yōu)先級(jí)啟發(fā)式算法，將啟發(fā)式算法運(yùn)用在路徑規(guī)劃以提高路徑搜索的時(shí)空效率。

本文提出的基于滾動(dòng)窗口的優(yōu)先級(jí)啟發(fā)式算法，是根據(jù)建立的環(huán)境模型在以機(jī)器人為中心的規(guī)劃窗口內(nèi)設(shè)計(jì)優(yōu)先級(jí)順序，確定鄰域柵格搜索的優(yōu)先級(jí)，從而規(guī)劃出合理的局部遍歷路徑。

如何結(jié)合可視窗口的環(huán)境信息和路徑規(guī)劃的具體要求設(shè)計(jì)合理的優(yōu)先級(jí)順序是整個(gè)路徑規(guī)劃的關(guān)鍵。本文設(shè)計(jì)的優(yōu)先級(jí)順序?yàn)椋篜X-1，Y（左柵格）、PX，Y-1（下柵格）、PX，Y+1（上柵格）、PX+1，Y（右柵格）、PX+1，Y-1（右下柵格）、PX+1，Y+1（右上柵格）、PX-1，Y-1（左下柵格）、PX-1，Y+1（左上柵格）。如果PX-1，Y的柵格屬性值xi=1，則優(yōu)先遍歷PX-1，Y；如果PX-1，Y的柵格屬性值xi≠1（即xi=0 或xi=-1），則 考 慮PX，Y-1。以 此 類 推，再 考 慮PX，Y+1、PX+1，Y、PX+1，Y-1、PX+1，Y+1、PX-1，Y-1，PX-1，Y+1。

若以柵格形式表示優(yōu)先級(jí)順序，則形成了如圖9所示由9個(gè)柵格組成的優(yōu)先級(jí)啟發(fā)式算法的優(yōu)先級(jí)順序。中心柵格為機(jī)器人所在位置，其相鄰柵格搜索時(shí)的優(yōu)先級(jí)由所在柵格的羅馬數(shù)字表示，數(shù)字I表示最先搜索，Ⅱ次之，其他依此類推。

圖9 以柵格形式表示的優(yōu)先級(jí)啟發(fā)式算法優(yōu)先級(jí)順序Fig.9 Priority order of priority heuristic algorithm represented by grid

圖10為優(yōu)先級(jí)啟發(fā)式算法遍歷規(guī)劃示意圖，圖中的1，2，3表示柵格遍歷的順序。當(dāng)機(jī)器人在位置1時(shí)，在規(guī)劃窗口按照上述算法建立優(yōu)先級(jí)順序，如圖中規(guī)劃窗口1所示。此時(shí)，機(jī)器人右側(cè)的柵格優(yōu)先級(jí)最大，因此機(jī)器人向右側(cè)柵格行進(jìn)1步，移動(dòng)到圖中的位置2。同理當(dāng)機(jī)器人在位置2時(shí)，建立優(yōu)先級(jí)順序，如圖中規(guī)劃窗口2所示，機(jī)器人正下方的柵格優(yōu)先級(jí)最大，機(jī)器人移動(dòng)到圖中的位置3，以此類推。

圖10 優(yōu)先級(jí)啟發(fā)式算法遍歷規(guī)劃示意圖Fig.10 Diagram of priority heuristic algorithm coverage planning

基于滾動(dòng)窗口的優(yōu)先級(jí)啟發(fā)式路徑規(guī)劃算法的流程如圖11所示。優(yōu)先級(jí)啟發(fā)式算法實(shí)際上是一個(gè)先左后右、先下后上的迂回式遍歷規(guī)劃。如圖12所示，該算法不僅能完成對(duì)環(huán)境的迂回遍歷，還能實(shí)現(xiàn)機(jī)器人的障礙物邊緣行為策略的功能［22］，完成對(duì)障礙物邊緣的處理，避免機(jī)器人頻繁陷入死區(qū)，有效地提高路徑搜索的時(shí)空效率。

圖11 基于滾動(dòng)窗口的優(yōu)先級(jí)啟發(fā)式算法流程Fig.11 Flowchart of priority heuristic algorithm based on rolling window

圖12 優(yōu)先級(jí)啟發(fā)式算法遍歷規(guī)劃實(shí)例Fig.12 Example of priority heuristic algorithm coverage planning

2.3 U型障礙物區(qū)域填充算法

儲(chǔ)罐外壁的真實(shí)環(huán)境是比較復(fù)雜的，可能存在向內(nèi)凹的U 型障礙物，如圖13 所示為開口向左的U型障礙物。直接采用優(yōu)先級(jí)啟發(fā)式算法對(duì)含有該類型障礙物的環(huán)境進(jìn)行遍歷，規(guī)劃的路徑將從障礙物的左側(cè)柵格經(jīng)過，U型障礙物內(nèi)部將無法覆蓋，造成遍歷不完全。因此針對(duì)該問題，本文設(shè)計(jì)了U型障礙物區(qū)域填充算法。實(shí)現(xiàn)U型障礙物區(qū)域的填充包括U型障礙物的識(shí)別和U 型障礙物區(qū)域直接填充兩個(gè)方面。

2.3.1 U型障礙物的識(shí)別

圖13 開口向左的U型障礙物Fig.13 U-shaped obstacles opening to left

機(jī)器人只能通過傳感器探測周圍的環(huán)境，其最大探測范圍為可視化窗口的大小。U型障礙物分為小型U型障礙物和大型U型障礙物兩種。設(shè)H表示U型障礙物的開口寬度（用柵格數(shù)表示大?。梢暣翱诘拇笮?個(gè)。

定義5：H≤5 個(gè)時(shí)，為小型U 型障礙物；H＞5 個(gè)時(shí)，為大型U型障礙物。

1）小型U型障礙物的識(shí)別。

如圖14所示，當(dāng)機(jī)器人在G點(diǎn)時(shí)，在可視窗口內(nèi)可觀察到障礙物點(diǎn)L、I，記錄兩障礙物點(diǎn)的位置信息。當(dāng)機(jī)器人在C點(diǎn)時(shí)，如果在可視窗口的右上方存在連續(xù)障礙物，則可確定該障礙物為U型障礙物。

圖14 小型U型障礙物的識(shí)別Fig.14 Identification of small U-shaped obstacles

2）大型U型障礙物的識(shí)別。

如圖15所示，當(dāng)機(jī)器人運(yùn)動(dòng)到D、E兩點(diǎn)所在柵格時(shí)，在可視窗口內(nèi)可觀察到障礙物點(diǎn)J、K，記錄兩障礙物點(diǎn)的位置信息。當(dāng)機(jī)器人在F點(diǎn)時(shí)，如果在可視窗口右上方有連續(xù)障礙物，則可確定該障礙物為U型障礙物。

圖15 大型U型障礙物的識(shí)別Fig.15 Identification of large U-shaped obstacles

2.3.2 U型障礙物區(qū)域直接填充

為了對(duì)圖13所示的U型障礙物區(qū)域進(jìn)行直接填充，機(jī)器人的運(yùn)動(dòng)路徑需向右延伸，因此在確定規(guī)劃窗口柵格的遍歷順序時(shí)，應(yīng)使機(jī)器人右側(cè)相鄰柵格的優(yōu)先級(jí)最高。以柵格形式表示遍歷的優(yōu)先級(jí)順序，本文設(shè)計(jì)了如圖16所示的優(yōu)先級(jí)啟發(fā)式算法優(yōu)先級(jí)順序，可實(shí)現(xiàn)對(duì)U 型障礙物區(qū)域直接填充。圖17 為U型障礙物區(qū)域直接填充的實(shí)例。

圖16 U 型障礙物區(qū)域直接填充的優(yōu)先級(jí)啟發(fā)式算法優(yōu)先級(jí)順序Fig.16 Priority order of priority heuristic algorithm for direct filling of U-shaped obstacle area

圖17 U型障礙物區(qū)域直接填充實(shí)例Fig.17 Example of direct filling of U-shaped obstacles area

根據(jù)上述的方法識(shí)別U 型障礙物可能會(huì)出現(xiàn)2種判斷結(jié)果：U型障礙物和2條平行的障礙物帶。如果是2條平行的障礙物帶，如圖18所示，采用U型障礙物區(qū)域直接填充算法會(huì)造成規(guī)劃路徑的不合理，因此引入虛擬U 型障礙物的概念，當(dāng)機(jī)器人向右填充時(shí)，若規(guī)劃窗口的右側(cè)和右下（上）側(cè)均為未遍歷柵格時(shí)（如圖19（a）所示），便把右側(cè)柵格當(dāng)作虛擬障礙物，如圖19（b）所示。

圖18 2條平行的障礙物帶Fig.18 Two parallel obstacles

圖19 虛擬障礙物的識(shí)別Fig.19 Identification of virtual obstacles

另外，采用優(yōu)先級(jí)啟發(fā)式算法可以對(duì)如圖20所示環(huán)境中其他開口方向的U型障礙物區(qū)域進(jìn)行填充，因此不需要單獨(dú)設(shè)計(jì)直接填充算法。

圖20 其他開口方向U型障礙物區(qū)域的填充Fig.20 Filling of U-shaped obstacle area opening to other directions

2.4 死區(qū)逃離算法

如圖21所示，當(dāng)儲(chǔ)罐檢測爬壁機(jī)器人周圍柵格均為障礙物、邊界或已遍歷柵格時(shí)，機(jī)器人陷入死區(qū)。

圖21 儲(chǔ)罐檢測爬壁機(jī)器人陷入死區(qū)Fig.21 Oil tank detection wall climbing robot falling into the dead zone

機(jī)器人陷入死區(qū)后，無法再采用優(yōu)先級(jí)啟發(fā)式算法進(jìn)行局部路徑規(guī)劃，則應(yīng)讓機(jī)器人盡快以最優(yōu)路徑逃離死區(qū)。因此引入了方向函數(shù)yi，如圖22所示，引導(dǎo)機(jī)器人快速逃離死區(qū)，其定義為：

式（4）中：(Xp，Yp)為機(jī)器人當(dāng)前位置坐標(biāo)；（Xv，Yv）為機(jī)器人下一步移動(dòng)位置坐標(biāo)；（Xu，Yu）為距機(jī)器人當(dāng)前柵格最近的未遍歷柵格坐標(biāo)；φi為機(jī)器人當(dāng)前柵格和最近未遍歷柵格連線與機(jī)器人當(dāng)前柵格和下一步移動(dòng)?xùn)鸥襁B線的夾角；θi為機(jī)器人當(dāng)前柵格和最近未遍歷柵格連線與X軸的夾角；αi為機(jī)器人當(dāng)前柵格和下一步移動(dòng)?xùn)鸥襁B線與X軸的夾角。

為了使機(jī)器人以合理的路徑逃離死區(qū)，僅僅依靠方向函數(shù)是無法實(shí)現(xiàn)的，引入了一個(gè)路徑選擇函數(shù)fi，其定義為：

路徑選擇函數(shù)不僅僅包含了方向函數(shù)，還加入了柵格的屬性值xi，c是一個(gè)權(quán)值常數(shù)，0＜c＜1。

圖22 儲(chǔ)罐檢測爬壁機(jī)器人逃離死區(qū)方向函數(shù)示意圖Fig.22 Schematic diagram of direction function of oil tank detection wall climbing robot escaping from dead zone

式中：F表示路徑選擇函數(shù)最大值；k表示與機(jī)器人相鄰柵格的個(gè)數(shù)。

機(jī)器人根據(jù)相鄰柵格的路徑選擇函數(shù)值，逃離死區(qū)時(shí)始終往路徑函數(shù)值最大的方向移動(dòng)。因此可得圖21中機(jī)器人陷入死區(qū)時(shí)的具體逃離路徑，如圖23所示。

綜上所述，在儲(chǔ)罐檢測爬壁機(jī)器人未知儲(chǔ)罐工作環(huán)境下基于滾動(dòng)窗口的全遍歷路徑規(guī)劃算法的流程如圖24所示。

圖23 儲(chǔ)罐檢測爬壁機(jī)器人逃離死區(qū)的路徑Fig.23 Path of oil tank detection wall climbing robot escaping from dead zone

3 全遍歷路徑規(guī)劃仿真試驗(yàn)和分析

為了檢驗(yàn)基于滾動(dòng)窗口的全遍歷路徑規(guī)劃算法的正確性，本文進(jìn)行了仿真試驗(yàn)，如圖25所示。

仿真環(huán)境為由25×20個(gè)柵格組成的柵格地圖，環(huán)境中含有儲(chǔ)罐外壁可能存在的障礙物，自由柵格數(shù)為457 個(gè)。儲(chǔ)罐檢測爬壁機(jī)器人最初不具備任何環(huán)境的信息，只能利用其自身攜帶的傳感器實(shí)時(shí)探測周圍的局部環(huán)境，進(jìn)行在線局部環(huán)境建模和滾動(dòng)路徑規(guī)劃。

圖24 基于滾動(dòng)窗口的全遍歷路徑規(guī)劃算法流程Fig.24 Flowchart of complete coverage path planning algorithm based on rolling window

圖25中空白柵格為未遍歷柵格，黑色柵格為障礙物。以柵格地圖的左下角為起點(diǎn)，機(jī)器人利用滾動(dòng)的可視窗口環(huán)境信息和優(yōu)先級(jí)啟發(fā)式算法進(jìn)行在線規(guī)劃。通過對(duì)（6，6）、（7，6）、（6，11）三點(diǎn)的判斷，識(shí)別U 型障礙物，采用U 型障礙物區(qū)域填充算法進(jìn)行遍歷，避免了柵格的漏檢。當(dāng)運(yùn)動(dòng)到（7，20）時(shí)，周圍區(qū)域均為障礙物、邊界和已遍歷柵格，機(jī)器人陷入死區(qū)。此時(shí)采用死區(qū)逃離算法，機(jī)器人運(yùn)動(dòng)到最近未遍歷的柵格（8，17），逃離死區(qū)。當(dāng)運(yùn)動(dòng)到（17，3）時(shí)，遇到2條平行的障礙物帶，啟用虛擬障礙物和U型障礙物區(qū)域填充算法進(jìn)行填充。填充完畢后繼續(xù)采用優(yōu)先級(jí)啟發(fā)式算法進(jìn)行遍歷，直到完成區(qū)域全遍歷。

圖25 基于滾動(dòng)窗口的全遍歷路徑規(guī)劃仿真試驗(yàn)Fig.25 Simulation test of complete coverage path planning based on rolling window

定義已遍歷柵格數(shù)與可達(dá)柵格數(shù)之比為遍歷率，重復(fù)遍歷柵格數(shù)與可達(dá)柵格數(shù)之比為重復(fù)遍歷率。在同樣的環(huán)境下采用基于柵格地圖的單元分解遍歷路徑規(guī)劃算法進(jìn)行仿真，結(jié)果如圖26所示，表明該方法針對(duì)復(fù)雜環(huán)境無法實(shí)現(xiàn)全遍歷且遍歷重復(fù)率較高。

圖26 基于柵格地圖的單元分解遍歷路徑規(guī)劃仿真結(jié)果Fig.26 Simulation result of unit decomposition coverage path planning based on grid map

從圖25、圖26中可以得出不同算法下機(jī)器人遍歷性能評(píng)價(jià)指標(biāo)，如表1所示。

由表1可知：基于柵格地圖的單元分解遍歷路徑規(guī)劃方法不能實(shí)現(xiàn)對(duì)環(huán)境的全遍歷，遍歷率為95%，重復(fù)率為4%，轉(zhuǎn)彎數(shù)為148次；本文提出的基于滾動(dòng)窗口的全遍歷路徑規(guī)劃方法能很好地實(shí)現(xiàn)對(duì)工作空間的遍歷，遍歷率達(dá)100%，重復(fù)率為0.88%，轉(zhuǎn)彎次數(shù)為136次。

表1 不同算法下儲(chǔ)罐檢測爬壁機(jī)器人遍歷性能評(píng)價(jià)指標(biāo)Table 1 Coverage performance evaluation index of oil tank detection wall climbing robot under different algorithms

同時(shí)，為驗(yàn)證基于滾動(dòng)窗口的全遍歷路徑規(guī)劃算法在不同環(huán)境下的魯棒性，選擇不同環(huán)境對(duì)算法進(jìn)行測試。在3種大小及內(nèi)部障礙物均不同的環(huán)境下的路徑規(guī)劃仿真結(jié)果如圖27所示。從圖中可以看出，隨著環(huán)境變大、變復(fù)雜，路徑也越復(fù)雜，機(jī)器人陷入死區(qū)的次數(shù)增多：在15×15個(gè)和20×20個(gè)柵格環(huán)境下，機(jī)器人陷入死區(qū)的次數(shù)均為2次；在30×30個(gè)柵格環(huán)境中，機(jī)器人陷入死區(qū)的次數(shù)達(dá)到了5次。機(jī)器人陷入死區(qū)的次數(shù)越多，重復(fù)遍歷的柵格越多。但在3種環(huán)境下，本文算法都能對(duì)U型障礙物、平行障礙物帶和陷入死區(qū)進(jìn)行處理，最終都能實(shí)現(xiàn)機(jī)器人工作區(qū)域的全遍歷。不同環(huán)境下機(jī)器人遍歷性能評(píng)價(jià)指標(biāo)如表2所示。

圖27 不同環(huán)境下基于滾動(dòng)窗口的全遍歷路徑規(guī)劃仿真結(jié)果Fig.27 Simulation results of complete coverage path planning based on rolling window in different environments

表2 不同環(huán)境下儲(chǔ)罐檢測爬壁機(jī)器人遍歷性能評(píng)價(jià)指標(biāo)Table 2 Coverage performance evaluation index of oil tank detection wall climbing robot in different environments

4 結(jié) 論

本文采用基于滾動(dòng)窗口的優(yōu)先級(jí)啟發(fā)式路徑規(guī)劃算法，實(shí)現(xiàn)了儲(chǔ)罐檢測爬壁機(jī)器人在未知儲(chǔ)罐外壁環(huán)境情況下對(duì)工作區(qū)域的全遍歷。利用滾動(dòng)的可視窗口環(huán)境信息和設(shè)計(jì)的優(yōu)先級(jí)啟發(fā)式遍歷算法，實(shí)現(xiàn)機(jī)器人對(duì)工作空間的迂回遍歷；提出U型障礙物區(qū)域填充算法和虛擬障礙物概念，實(shí)現(xiàn)U型障礙區(qū)域的連續(xù)遍歷；當(dāng)機(jī)器人陷入死區(qū)時(shí)，快速啟用死區(qū)逃離算法以使機(jī)器人逃離死區(qū)。仿真結(jié)果表明該方法能夠?qū)崿F(xiàn)機(jī)器人對(duì)儲(chǔ)罐外壁的全遍歷，降低了遍歷重復(fù)率，整體上提高了遍歷效率。