儲罐檢測爬壁機器人全遍歷路徑規劃

唐東林，龍再勇，湯炎錦，潘 峰，游傳坤

（西南石油大學機電工程學院，四川成都610500）

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

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

本文根據儲罐檢測爬壁機器人工作環境的特點，提出了一種基于滾動窗口的優先級啟發式算法。基于柵格地圖建立儲罐檢測爬壁機器人工作環境模型，利用設計的優先級啟發式算法在滾動窗口對路徑進行搜索。引入U型障礙物區域填充算法和死區逃離算法，使機器人能有效地遍歷U型障礙物區域并且快速逃離死區，降低遍歷重復率，使機器人在未知儲罐環境下能有效地遍歷整個工作區域。

1 儲罐檢測爬壁機器人工作環境建模

1.1 儲罐壁面簡化

立式圓柱體儲罐外壁是圓弧面，為三維工作環境。圖1所示為儲罐檢測爬壁機器人壁面工作圖。

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

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

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

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

1.2 基于柵格地圖的環境建模

基于柵格地圖對儲罐外壁建模，根據機器人檢測探頭的寬度確定柵格大小。如式（2）所示，每個柵格有一個表示柵格狀態的值xi，并隨著機器人運動實時更新。

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

根據儲罐外壁的情況建立環境模型，如圖2 所示，有針對性地在環境中設置了儲罐外壁可能存在的障礙物。該環境的柵格地圖模型如圖3所示，其中黑色部分為障礙物柵格，空白部分為未遍歷柵格。通過對比圖2和圖3可得，基于柵格地圖進行環境建模可以使環境中的不規則障礙物變成規則的方形障礙物，使機器人在遍歷時運動方式變得簡單。

圖2 儲罐檢測爬壁機器人工作環境模型Fig.2 Working environment model of oil tank detection wall climbing robot

圖3 儲罐檢測爬壁機器人工作環境模型的柵格地圖Fig.3 Grid map of working environment model of oil tank detection wall climbing robot

若根據柵格的賦值情況建立三維柵格地圖，則未遍歷柵格位于地圖的頂部，已遍歷柵格位于地圖的中部，而障礙物柵格位于地圖的底部，如圖4所示。可將該方法用于基于傳感器的未知環境實時局部建模。

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

2 基于滾動窗口的遍歷路徑規劃

2.1 路徑規劃描述

本文討論的是儲罐全局環境未知情況下爬壁機器人全遍歷路徑規劃問題。規劃的目的是機器人能遍歷工作環境中所有可達區域，并滿足遍歷重復率最小的要求［20］。

儲罐外壁分布著有限個大小、形狀不受限制的障礙物，機器人事先未存儲任何障礙物信息，只能利用搭載的傳感器探測有限范圍內的環境信息。為了安全起見，在機器人在線探測時把障礙物作一定的膨化處理［21］，同時機器人具有存儲運動路徑信息和自定位功能。將機器人視為一個質點，當它運動到地圖中的某個柵格時，認為該柵格已遍歷。

2.2 基于滾動窗口的優先級啟發式路徑規劃

圖5所示為基于滾動窗口的優先級啟發式路徑規劃算法的構建流程。

圖5 基于滾動窗口的優先級啟發式路徑規劃算法的構建流程Fig.5 Construction flowchart of priority heuristic path planning algorithm based on rolling window

2.2.1 滾動窗口

基于滾動窗口的路徑規劃（簡稱滾動規劃）是機器人在工作空間運動過程中實時探測周圍的局部環境信息，并以逐步滾動的方式進行在線路徑規劃，可實現路徑的優化與反饋，確保規劃算法的合理性和安全性。

在滾動規劃中定義的滾動窗口包括可視窗口和規劃窗口。滾動窗口的生成包括以下4個步驟：

1）設置滾動窗口的參考坐標系。

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

圖6 滾動窗口參考坐標系Fig.6 Rolling window reference coordinate system

2）確定滾動窗口的范圍。

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

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

3）確定可視窗口。

可視窗口是在滾動窗口的基礎上設定的，在不影響局部路徑規劃的前提下，考慮局部環境信息的存儲及計算量，選取合適的滾動窗口大小。

定義3：以機器人為中心的5×5 個柵格為可視窗口。

4）確定規劃窗口。

規劃窗口是機器人進行局部路徑規劃的區域，其大小不能超過可視窗口，且需根據局部路徑規劃算法確定。根據優先級啟發式路徑規劃算法，以機器人前進1個單位長度來定義規劃窗口。

定義4：以機器人為中心的3×3 個柵格為規劃窗口。

如圖7 所示，可視窗口和規劃窗口分別包含24個和8個柵格的信息。機器人在規劃窗口內確定局部子目標并規劃路徑向子目標行進。機器人每行進1 步，窗口相應向前滾動，搭載的傳感器將實時獲得最新的環境信息并對滾動窗口進行更新。如圖8所示，Rob1、Rob2、Rob3 分別表示機器人移動時的位置，r1、r2、r3為相對應的滾動窗口的半徑。

圖7 滾動窗口Fig.7 Rolling window

圖8 窗口滾動示意圖Fig.8 Schematic diagram of window rolling

2.2.2 優先級啟發式算法設計

啟發式算法是與具體問題求解及搜索過程相關的，將求解問題的具體信息融入算法中，從而提高算法的搜索效率，具有簡單、直觀且有效的優點。因此本文提出基于滾動窗口的優先級啟發式算法，將啟發式算法運用在路徑規劃以提高路徑搜索的時空效率。

本文提出的基于滾動窗口的優先級啟發式算法，是根據建立的環境模型在以機器人為中心的規劃窗口內設計優先級順序，確定鄰域柵格搜索的優先級，從而規劃出合理的局部遍歷路徑。

如何結合可視窗口的環境信息和路徑規劃的具體要求設計合理的優先級順序是整個路徑規劃的關鍵。本文設計的優先級順序為：PX-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，則優先遍歷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。

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

圖9 以柵格形式表示的優先級啟發式算法優先級順序Fig.9 Priority order of priority heuristic algorithm represented by grid

圖10為優先級啟發式算法遍歷規劃示意圖，圖中的1，2，3表示柵格遍歷的順序。當機器人在位置1時，在規劃窗口按照上述算法建立優先級順序，如圖中規劃窗口1所示。此時，機器人右側的柵格優先級最大，因此機器人向右側柵格行進1步，移動到圖中的位置2。同理當機器人在位置2時，建立優先級順序，如圖中規劃窗口2所示，機器人正下方的柵格優先級最大，機器人移動到圖中的位置3，以此類推。

圖10 優先級啟發式算法遍歷規劃示意圖Fig.10 Diagram of priority heuristic algorithm coverage planning

基于滾動窗口的優先級啟發式路徑規劃算法的流程如圖11所示。優先級啟發式算法實際上是一個先左后右、先下后上的迂回式遍歷規劃。如圖12所示，該算法不僅能完成對環境的迂回遍歷，還能實現機器人的障礙物邊緣行為策略的功能［22］，完成對障礙物邊緣的處理，避免機器人頻繁陷入死區，有效地提高路徑搜索的時空效率。

圖11 基于滾動窗口的優先級啟發式算法流程Fig.11 Flowchart of priority heuristic algorithm based on rolling window

圖12 優先級啟發式算法遍歷規劃實例Fig.12 Example of priority heuristic algorithm coverage planning

2.3 U型障礙物區域填充算法

儲罐外壁的真實環境是比較復雜的，可能存在向內凹的U 型障礙物，如圖13 所示為開口向左的U型障礙物。直接采用優先級啟發式算法對含有該類型障礙物的環境進行遍歷，規劃的路徑將從障礙物的左側柵格經過，U型障礙物內部將無法覆蓋，造成遍歷不完全。因此針對該問題，本文設計了U型障礙物區域填充算法。實現U型障礙物區域的填充包括U型障礙物的識別和U 型障礙物區域直接填充兩個方面。

2.3.1 U型障礙物的識別

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

機器人只能通過傳感器探測周圍的環境，其最大探測范圍為可視化窗口的大小。U型障礙物分為小型U型障礙物和大型U型障礙物兩種。設H表示U型障礙物的開口寬度（用柵格數表示大小），可視窗口的大小為5個。

定義5：H≤5 個時，為小型U 型障礙物；H＞5 個時，為大型U型障礙物。

1）小型U型障礙物的識別。

如圖14所示，當機器人在G點時，在可視窗口內可觀察到障礙物點L、I，記錄兩障礙物點的位置信息。當機器人在C點時，如果在可視窗口的右上方存在連續障礙物，則可確定該障礙物為U型障礙物。

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

2）大型U型障礙物的識別。

如圖15所示，當機器人運動到D、E兩點所在柵格時，在可視窗口內可觀察到障礙物點J、K，記錄兩障礙物點的位置信息。當機器人在F點時，如果在可視窗口右上方有連續障礙物，則可確定該障礙物為U型障礙物。

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

2.3.2 U型障礙物區域直接填充

為了對圖13所示的U型障礙物區域進行直接填充，機器人的運動路徑需向右延伸，因此在確定規劃窗口柵格的遍歷順序時，應使機器人右側相鄰柵格的優先級最高。以柵格形式表示遍歷的優先級順序，本文設計了如圖16所示的優先級啟發式算法優先級順序，可實現對U 型障礙物區域直接填充。圖17 為U型障礙物區域直接填充的實例。

圖16 U 型障礙物區域直接填充的優先級啟發式算法優先級順序Fig.16 Priority order of priority heuristic algorithm for direct filling of U-shaped obstacle area

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

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

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

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

另外，采用優先級啟發式算法可以對如圖20所示環境中其他開口方向的U型障礙物區域進行填充，因此不需要單獨設計直接填充算法。

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

2.4 死區逃離算法

如圖21所示，當儲罐檢測爬壁機器人周圍柵格均為障礙物、邊界或已遍歷柵格時，機器人陷入死區。

圖21 儲罐檢測爬壁機器人陷入死區Fig.21 Oil tank detection wall climbing robot falling into the dead zone

機器人陷入死區后，無法再采用優先級啟發式算法進行局部路徑規劃，則應讓機器人盡快以最優路徑逃離死區。因此引入了方向函數yi，如圖22所示，引導機器人快速逃離死區，其定義為：

式（4）中：(Xp，Yp)為機器人當前位置坐標；（Xv，Yv）為機器人下一步移動位置坐標；（Xu，Yu）為距機器人當前柵格最近的未遍歷柵格坐標；φi為機器人當前柵格和最近未遍歷柵格連線與機器人當前柵格和下一步移動柵格連線的夾角；θi為機器人當前柵格和最近未遍歷柵格連線與X軸的夾角；αi為機器人當前柵格和下一步移動柵格連線與X軸的夾角。

為了使機器人以合理的路徑逃離死區，僅僅依靠方向函數是無法實現的，引入了一個路徑選擇函數fi，其定義為：

路徑選擇函數不僅僅包含了方向函數，還加入了柵格的屬性值xi，c是一個權值常數，0＜c＜1。

圖22 儲罐檢測爬壁機器人逃離死區方向函數示意圖Fig.22 Schematic diagram of direction function of oil tank detection wall climbing robot escaping from dead zone

式中：F表示路徑選擇函數最大值；k表示與機器人相鄰柵格的個數。

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

綜上所述，在儲罐檢測爬壁機器人未知儲罐工作環境下基于滾動窗口的全遍歷路徑規劃算法的流程如圖24所示。

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

3 全遍歷路徑規劃仿真試驗和分析

為了檢驗基于滾動窗口的全遍歷路徑規劃算法的正確性，本文進行了仿真試驗，如圖25所示。

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

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

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

圖25 基于滾動窗口的全遍歷路徑規劃仿真試驗Fig.25 Simulation test of complete coverage path planning based on rolling window

定義已遍歷柵格數與可達柵格數之比為遍歷率，重復遍歷柵格數與可達柵格數之比為重復遍歷率。在同樣的環境下采用基于柵格地圖的單元分解遍歷路徑規劃算法進行仿真，結果如圖26所示，表明該方法針對復雜環境無法實現全遍歷且遍歷重復率較高。

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

從圖25、圖26中可以得出不同算法下機器人遍歷性能評價指標，如表1所示。

由表1可知：基于柵格地圖的單元分解遍歷路徑規劃方法不能實現對環境的全遍歷，遍歷率為95%，重復率為4%，轉彎數為148次；本文提出的基于滾動窗口的全遍歷路徑規劃方法能很好地實現對工作空間的遍歷，遍歷率達100%，重復率為0.88%，轉彎次數為136次。

表1 不同算法下儲罐檢測爬壁機器人遍歷性能評價指標Table 1 Coverage performance evaluation index of oil tank detection wall climbing robot under different algorithms

同時，為驗證基于滾動窗口的全遍歷路徑規劃算法在不同環境下的魯棒性，選擇不同環境對算法進行測試。在3種大小及內部障礙物均不同的環境下的路徑規劃仿真結果如圖27所示。從圖中可以看出，隨著環境變大、變復雜，路徑也越復雜，機器人陷入死區的次數增多：在15×15個和20×20個柵格環境下，機器人陷入死區的次數均為2次；在30×30個柵格環境中，機器人陷入死區的次數達到了5次。機器人陷入死區的次數越多，重復遍歷的柵格越多。但在3種環境下，本文算法都能對U型障礙物、平行障礙物帶和陷入死區進行處理，最終都能實現機器人工作區域的全遍歷。不同環境下機器人遍歷性能評價指標如表2所示。

圖27 不同環境下基于滾動窗口的全遍歷路徑規劃仿真結果Fig.27 Simulation results of complete coverage path planning based on rolling window in different environments

表2 不同環境下儲罐檢測爬壁機器人遍歷性能評價指標Table 2 Coverage performance evaluation index of oil tank detection wall climbing robot in different environments

4 結 論

本文采用基于滾動窗口的優先級啟發式路徑規劃算法，實現了儲罐檢測爬壁機器人在未知儲罐外壁環境情況下對工作區域的全遍歷。利用滾動的可視窗口環境信息和設計的優先級啟發式遍歷算法，實現機器人對工作空間的迂回遍歷；提出U型障礙物區域填充算法和虛擬障礙物概念，實現U型障礙區域的連續遍歷；當機器人陷入死區時，快速啟用死區逃離算法以使機器人逃離死區。仿真結果表明該方法能夠實現機器人對儲罐外壁的全遍歷，降低了遍歷重復率，整體上提高了遍歷效率。