大規(guī)模復雜變電站巡檢機器人分層路徑規(guī)劃

李睿欣，湯明東，李海英，黃 微

（上海理工大學機械工程學院，上海 200093）

0 引言

隨著電網(wǎng)規(guī)模的增大、電壓等級的提高，其對供電安全、可靠性的要求也更加嚴格，變電站正常運行成為保障電力系統(tǒng)供電安全的重要環(huán)節(jié)［1-2］。目前，中國電網(wǎng)主要采用人工巡檢作業(yè)方式，即采用人工巡視、手工記錄的模式對運行中的變電設備進行檢查。人工巡檢存在勞動強度大、工作效率低、巡檢質(zhì)量不穩(wěn)定等缺點，惡劣的氣象條件對巡檢人員的身體也存在危害［3-5］。近年來，采用機器人巡檢代替人工巡檢模式已成為變電站巡檢發(fā)展的熱點方向［6-8］。巡檢機器人可在變電站的復雜環(huán)境中工作，其具有環(huán)境感知、任務規(guī)劃和動作控制等基本功能［9］。在移動機器人技術(shù)研究中，路徑規(guī)劃是有效完成復雜任務的關(guān)鍵。

國內(nèi)山東省電力公司于1999 年最早開始變電站巡檢機器人研究，并于2004 年研制成功第1 臺功能樣機，之后在國家863 項目的支持下研制出變電站巡檢機器人［10］。2012 年，重慶電力公司在500kV 巴南變電站成功應用機器人進行自主巡檢作業(yè)。2014 年，浙江國自機器人公司研制的機器人在瑞安變電站投運。目前，中國在變電站機器人巡檢領(lǐng)域取得了長足進展，但在多傳感器綜合探測、四輪驅(qū)動移動平臺、綜合導航和精確對準、故障精確診斷等技術(shù)方面還存在瓶頸［11-12］。如今，隨著自動駕駛等新理論及應用的提出，具有全自動駕駛能力的移動機器人也得到了迅速發(fā)展，機器人可表現(xiàn)出與人類駕駛員相似的感知和決策能力［13］。雖然機器人在典型的城市交通場景中表現(xiàn)出高性能，但在大規(guī)模復雜場景中仍難以實現(xiàn)類人的靈活性和安全性。因此，在大規(guī)模、未知或部分未知的變電站復雜環(huán)境下，設計一種能夠避開固定障礙物及穿越可變障礙物，同時確保安全性、平滑性、靈活性和效率的路徑規(guī)劃方法仍然具有挑戰(zhàn)性。

如今，研究者提出了許多路徑規(guī)劃算法。這些算法可分為兩類：基于構(gòu)造的方法和基于探索的方法?；跇?gòu)造的方法可以在線生成可行且平滑的局部路徑，而無需復雜的計算。因此，這些方法適用于高速場景中的運動規(guī)劃。例如Piazzi 等［14］和Jian 等［15］使用多項式插值執(zhí)行離線平滑和局部規(guī)劃。為實現(xiàn)類似的過程，Maekawa 等［16］采用基于樣條線的方法，Gu 等［17］使用了回旋線。然而，上述方法無法有效地應用于變電站、停車場、廣場和非結(jié)構(gòu)化場景，這些場景中的結(jié)構(gòu)信息不足以構(gòu)建精確的路徑。此外，不正確的注釋或環(huán)境變化容易導致地圖與現(xiàn)實世界不匹配，可能會導致車輛走錯方向或影響避障性能。

相比之下，基于探索的運動規(guī)劃方法可在不依賴地圖的情況下處理規(guī)劃問題。該方法通過構(gòu)建搜索樹來探索環(huán)境，直到起點與目標連接起來。其中的代表性方法包括A*［18］和快速探索隨機樹（RRT）［19］方法，這兩種方法都可以找到連接兩點的可行路徑，同時避免存在障礙物。

本文提出一種變電站復雜環(huán)境下的巡檢機器人導航方法，以實現(xiàn)巡檢路徑最優(yōu)的路徑規(guī)劃。變電站環(huán)境中存在各種障礙物，如固定障礙物、可變障礙物等。之前的研究只針對固定障礙物或動態(tài)障礙物作出規(guī)劃，本文創(chuàng)新地提出可變障礙物?？勺冋系K物是指從不同方向通過它時，可呈現(xiàn)出障礙物或非障礙物的特點（如斜坡、臺階、洼地等）。按照規(guī)劃的巡檢線路，機器人自動記錄所有設備以及建筑物的地理信息，進而完成整個地圖數(shù)據(jù)的采集，建立變電站巡檢地圖，之后規(guī)劃變電站常規(guī)巡檢路線。本文考慮到可變障礙物的出現(xiàn)，不斷豐富離線地圖。采用全局路徑規(guī)劃—局部避障控制具有層次化結(jié)構(gòu)的機器人導航運動控制器，實現(xiàn)機器人導航行為控制，以確保機器人在與人共處的復雜動態(tài)環(huán)境下可實現(xiàn)安全、高效的導航行為。

為了保持混合A*算法靈活性強的優(yōu)點，并克服其在搜索速度和路徑平滑性方面的缺陷，本文提出一種新的基于層次搜索空間尺度的HHA*運動規(guī)劃方法，在混合A*算法基礎上作進一步優(yōu)化。所提出的HHA*方法包括兩個階段：第一階段在粗標度圖上執(zhí)行二維搜索，其中提取出通往目標的粗糙路徑。通過對此路徑的進一步分析，可找到幾個本地目標作為后續(xù)搜索過程的指導；第二階段在精細尺度上進行優(yōu)化的HHA*搜索，以生成達到目標的可行路徑。首先利用第一階段獲得的全局目標進行引導，提高了啟發(fā)式函數(shù)的精度，從而減少了重復的無用擴展，然后利用立體曲線和解析展開曲線提高路徑的平滑性。在各種復雜的模擬場景中驗證了本文方法的安全性、平滑性、靈活性和高效率。實驗結(jié)果表明，該方法的計算效率相比混合A*算法提高了6 倍。在全局規(guī)劃階段相比混合A*提高了45.48%，在局部階段相比QBC 算法提高了22.87%，相比TEB 算法提高了16.37%。仿真和實驗結(jié)果表明，采用該方法在大規(guī)模變電站復雜場景中進行巡檢，可滿足路徑規(guī)劃平滑度和安全性的要求。

1 基本介紹及框架

變電站機器人巡檢基本框架如圖1 所示。首先機器人按照規(guī)劃的巡檢線路繞整個變電站行走一圈，機器人自動記錄所有設備及建筑物地理信息，結(jié)合變電站建筑圖紙完成整個地圖構(gòu)建數(shù)據(jù)的采集；然后開啟地圖生成程序，自動生成變電站巡檢地圖；地圖構(gòu)建完成后，根據(jù)變電站巡檢設備類型及數(shù)量設定巡檢點，并優(yōu)化巡檢路線。當動態(tài)障礙物突然出現(xiàn)時，會使原來設定的巡檢路線出軌，可利用局部路徑規(guī)劃出新的巡檢路線。若遇到可變障礙物，判斷可變障礙物地形是否可以通過。如果不能通過，則將其視為障礙物并更新離線地圖，繼續(xù)進行局部路徑規(guī)劃。采用全局路徑規(guī)劃—局部避障控制具有層次化結(jié)構(gòu)的機器人導航運動控制器，以確保機器人在與人共處的復雜動態(tài)環(huán)境下實現(xiàn)安全、高效的導航行為。

1.1 變電站離線地圖構(gòu)建

變電站巡檢地圖構(gòu)建步驟如下：開啟機器人激光雷達傳感器傳輸功能，完成激光雷達初始化；遙控機器人按照規(guī)劃的巡檢線路繞變電站行走一圈，機器人自動記錄所有設備及建筑物地理信息，完成地圖構(gòu)建數(shù)據(jù)采集；之后開啟地圖生成程序，自動生成變電站巡檢地圖，見圖2。

地圖構(gòu)建完成后，根據(jù)變電站巡檢設備類型及數(shù)量設定巡檢點，并優(yōu)化巡檢路線，然后進行巡檢測試。按照標定完成的最優(yōu)路線對變電站設備的巡檢點進行測試，查找遺漏并調(diào)整巡檢路線，保證設備巡檢點的全覆蓋。

Fig.1 Basic framework for substation robot inspection圖1 變電站機器人巡檢基本框架

Fig.2 Establishment of offline map of the substation圖2 變電站離線地圖構(gòu)建

1.2 可變障礙物

可變障礙物是指變電站中能夠以某種方向、姿態(tài)通過的障礙物，可呈現(xiàn)障礙物或非障礙物的特點?；谶@一特點，在變電站離線地圖中不可將其視為障礙物，否則將無法得到最優(yōu)巡檢路徑。如圖3（a）所示的可變障礙物，可以上坡但不能下坡；如圖3（b）所示的可變障礙物，只能向下行駛而不能上臺階。變電站巡檢機器人能在平坦、開闊的路面行駛，面對可變障礙物則需要判斷是否能以特定的方向、姿態(tài)穿過。當變電站巡檢機器人檢測到周圍出現(xiàn)人或其他動態(tài)障礙物時，通過局部路徑規(guī)劃生成新軌跡，將新路徑與變電站地圖中的可變障礙物進行比對，判斷生成新的路徑是否能通過可變障礙物，如果不能通過，則將其放入離線地圖中，重新進行局部路徑規(guī)劃?？勺冋系K物路徑規(guī)劃分析過程如圖4所示。

在局部路徑規(guī)劃過程中，隨著離線地圖的不斷豐富，尤其是可變障礙物的出現(xiàn)加大了路徑規(guī)劃難度，所以本文提出面對大規(guī)模復雜變電站的路徑規(guī)劃方法。

1.3 豐富離線地圖

根據(jù)變電站的環(huán)境不斷豐富離線地圖。變電站環(huán)境中地形比較復雜且障礙物較多。一開始根據(jù)傳感器采集到的變電站中電站等固定障礙物信息和可變障礙物信息，將其加入到地圖中建立離線地圖。由于出現(xiàn)動態(tài)障礙物，利用局部路徑規(guī)劃生成新路徑。通過不斷豐富離線地圖，建立動態(tài)地圖。

Fig.3 Variable obstacles圖3 可變障礙物

Fig.4 Variable obstacle path planning analysis process圖4 可變障礙物路徑規(guī)劃分析過程

本文主要將障礙物分為3 種類型：固定障礙物、可變障礙物、動態(tài)障礙物。固定障礙物是指變電站中的大型設備，如變壓器、電站、建筑物等；可變障礙物是指變電站中能夠以某種方式通過的障礙物，不可單純設定為固定障礙物；動態(tài)障礙物是指變電站中的人、突然出現(xiàn)的雜物等。障礙物分類如圖5 所示。隨著離線地圖的不斷豐富以及各種類型障礙物的出現(xiàn)，加大了路徑規(guī)劃難度，因此本文特別提出可變障礙物的路徑規(guī)劃。

Fig.5 Obstacle classification圖5 障礙物分類

2 路徑規(guī)劃算法系統(tǒng)

為了實現(xiàn)變電站巡檢機器人的導航行為控制，使機器人在復雜動態(tài)環(huán)境下實現(xiàn)安全、高效的導航行為，本文提出一種新的基于層次搜索空間尺度的HHA*運動規(guī)劃方法。

2.1 HHA*算法

基于現(xiàn)有工作，HHA*算法包括預處理和搜索階段。在預處理階段，通過粗尺度上的2DA*搜索生成一個引導路徑。通過對生成路徑的分析，可以找到劃分整個搜索空間的局部目標，其中每個部分都相對簡單，然后在精細尺度上執(zhí)行基于A*的混合搜索。在本地目標的指導下，通過使用分層啟發(fā)式函數(shù)，可大大加快搜索速度，從而使整個系統(tǒng)能夠及時響應環(huán)境中的動態(tài)障礙物。此外，利用曲率連續(xù)運動單元和展開曲線，使生成的路徑可直接滿足連續(xù)要求。

2.2 啟發(fā)層次函數(shù)

在子目標的幫助下，對不同子區(qū)域的狀態(tài)進行了不同定義。對于每個狀態(tài)s，將所提出的啟發(fā)式函數(shù)hHHA*定義為：

其中，Sgi表示與s所在位置對應的目標：

hHHA*由兩部分組成：局部啟發(fā)式hLocal，從s到sgi，以及輸入的啟發(fā)式h，從sgi到最終目標。當搜索樹的一個分支通過sgi時，其提供了足夠的梯度，并引導搜索過程更快接近最終目標。hLocal與hHA*略有不同，s可以達到具有橫向偏移量的sgi。當s足夠接近局部目標時，sgi就會退化為h2D，以確保一致性。

2.3 路徑平滑度優(yōu)化

HHA*中的節(jié)點擴展采用直線和圓弧作為運動單元。此外，引入Reeds-Shepp 曲線作為解析展開曲線，求出直接解即這些曲線的組合。雖然用該方法可保證最終路徑方向的連續(xù)性，但是如果直接用于路徑跟蹤，生成路徑在曲率輪廓上的不連續(xù)性會引起抖動。因此，在HHA*中需要一個進一步平穩(wěn)的步驟，以避免這種影響。

本文方法解決了這一問題，即在節(jié)點展開和解析展開曲線生成過程中使曲率曲線保持連續(xù)。對于節(jié)點的展開，引入連接狀態(tài)曲率與線性曲率的方法；對于解析展開曲線的生成，生成一個具有連續(xù)曲率的Reeds-shepp 樣曲線。兩種方法都能保持曲線和端點的曲率連續(xù)，使整個組合路徑連續(xù)。

2.4 運動單元

引入具有線性曲率的運動單元可用參數(shù)｛κ0，σ，L，v｝表示，其中κ0表示初始曲率，σ表示曲率，l表示曲線長度，v=±1 表示驅(qū)動方向。通過積分得到運動單元對狀態(tài)的影響：

通過旋轉(zhuǎn)和平移變換，運動單元可被附加到任何具有相同曲率的狀態(tài)s0=［x0，y0，θ0，κ0，d0］T上。生成的子狀態(tài)如下：

其中，R2× 2(θ)表示二維旋轉(zhuǎn)矩陣：

當σ=0 時，式（17）、式（18）的表達式退化為混合A*的表達式：

否則，根據(jù)初始曲率κ0=0，當σ=0 時，式（17）、式（18）可使用菲涅耳積分計算得到：

其中，F(xiàn)C(t)和FS(t)為菲涅耳積分，分別為：

使用所提出的運動單元的效果如圖6（a）、圖6（b）所示，圖6（a）顯示了環(huán)形曲線（κ0=0，σ=0）平滑地連接直線段（κ0=0，σ=0）與弧線段（κ0=0，σ=0）。圖6（b）的方法與圖6（c）所示的混合A*相比，可在保持生成路徑連續(xù)性的同時，有效地提高整體平滑度。

3 仿真實驗

Fig.6 Comparison of motor units in mixed A*and HHA*圖6 混合A*與HHA*中的運動單元比較

本文提出一種新的基于層次搜索空間尺度的HHA*運動規(guī)劃方法，該方法相比混合A*具有更高的搜索效率，并能得到更平滑的結(jié)果。此外，與RRT*方法相比，HHA*也具備這些優(yōu)勢。在各種虛擬場景下進行實驗比較，驗證該方法的優(yōu)越性。此外，還在變電站巡檢機器人平臺上通過算法實驗作進一步的道路測試。

3.1 層次啟發(fā)式函數(shù)實驗

引入100m×100m 的多障礙物場景來模擬變電站場景，驗證所提出的層次搜索結(jié)構(gòu)，如圖7 所示。其中，黑色柵格為固定障礙物，灰色柵格為可變障礙物，圓圈為動態(tài)障礙物。巡檢機器人從點A 到點B 行駛，黑色虛線表示變電站常規(guī)巡檢路徑，紅色實線表示HHA*算法的實驗結(jié)果，藍色虛線表示混合A*算法的實驗結(jié)果。

Fig.7 Path planning Comparison of HHA*and hybrid A*algorithms圖7 HHA*與混合A*算法路徑規(guī)劃對比

HHA*能夠識別可變障礙物，同時能夠避開動態(tài)障礙物?；旌螦*不能識別可變障礙物，并將可變障礙物當作固定障礙物處理，且路徑較長。

表1 中列出了以上兩種方法的數(shù)值分析結(jié)果。兩種算法都可在場景中生成可行的路徑，但在搜索節(jié)點數(shù)量上存在顯著差異。與預測的一樣，混合A*中采用的兩種啟發(fā)式方法都不能真正代表到目標的距離，其搜索節(jié)點和時間消耗是HHA*的幾十倍。

Table 1 Comparison of HHA*with the other methods表1 HHA*與其他方法比較

兩種方法沿生成路徑移動的f 值如圖8 所示。由于受到被低估的啟發(fā)式函數(shù)的影響，混合A*的f 值在整個過程中不斷增加，如圖8（b）所示。相比之下，由于每個子區(qū)域比較簡單，HHA*中的啟發(fā)式函數(shù)可以很好地進行估計，從而得到圖8（a）中的多個平坦范圍。此外，圖8 中顯示了下降情況，表明當路徑通過子目標時，f 的總體趨勢保持下降，解析展開式對搜索空間的劃分也有較好效果。然而，根據(jù)表1，通過所提出的局部目標，HHA*中的解析擴展式獲得了更高的成功率，從而產(chǎn)生了更好的優(yōu)化效果。與上述相反的是，在混合A*中，雖然嘗試了數(shù)千次的分析展開，但大部分被碰撞實驗拒絕，導致搜索速度無法得到提高。

Fig.8 f values of paths generated by both methods圖8 兩種方法生成的路徑f值

3.2 路徑平滑性分析

對HHA*和混合A*生成的路徑進行曲率分析，比較其平滑性。生成的路徑曲率曲線如圖9 所示，其中圖（a）-（b）由HHA*生成，圖（c）-（d）由混合A*生成。HHA*方法相比混合A*獲得了更平滑的曲率輪廓。在混合A*產(chǎn)生的路徑中有幾十個曲率，其中包含不連續(xù)的點。因此，HHA*中使用的方法滿足連續(xù)要求，而混合A*中使用的Reeds-Shepp曲線則不滿足要求。

4 路徑規(guī)劃實驗

本節(jié)精心設計了具有挑戰(zhàn)性的實驗場景，以驗證該方案的效率、靈活性、平滑性和安全性，并對實驗結(jié)果進行了詳細討論。

Fig.9 Curvature curve of the path圖9 生成的路徑曲率曲線

4.1 實驗設置

本文實驗在配備HokuyoUTM30LX 激光測距儀的先鋒3-DX 差速驅(qū)動機器人上進行，如圖10（a）所示，該機器人的最大線性速度為1.2 m/s?？紤]到變電站導航的安全性，本文將線速度的上界設為0.7 m/s。激光測距儀的掃描范圍為270°，角度分辨率為0.25°，且有效測量范圍為0.1～30m。

Fig.10 Experimental environment圖10 實驗環(huán)境

4.2 全局路徑規(guī)劃

為驗證本文提出的路徑規(guī)劃方法的優(yōu)越性能，方案1的設計如圖10（b）所示。一個盒子被放置在機器人前面作為一個可變障礙物，機器人必須到達門口。

為了對路徑規(guī)劃進行公平的比較，在方案1 的測試中使用了相同場景。在兩組測試中，全局路徑規(guī)劃的線程被觸發(fā)61 次，基于混合A*的路徑規(guī)劃器的平均擴展狀態(tài)數(shù)與圖大小分別為1 794 和4 226，而本文建議的HHA*路徑規(guī)劃的平均擴展狀態(tài)數(shù)分別為978 和1 815。與基于混合A*的路徑規(guī)劃相比，該路徑規(guī)劃的計算效率與內(nèi)存消耗分別提高了45.48%和57.05%，證明了所提出的基于層次搜索空間尺度的HHA*路徑規(guī)劃方法的有效性。

4.3 局部路徑規(guī)劃

為了驗證所提出的局部規(guī)劃方法的靈活性、平滑性和效率，設計了方案2，如圖11（a）所示。在此情況下，機器人需要沿著一條長長的走廊移動，并向右急轉(zhuǎn)彎以通過門。此外，門縫里有兩個盒子作為可變障礙物。該場景需要局部路徑規(guī)劃提供平滑和靈活的運動來引導機器人達到目標。其中，本文HHA*路徑規(guī)劃結(jié)果如圖11（b）、圖11（d）所示，QBC 路徑規(guī)劃結(jié)果如圖11（c）所示，TEB 路徑規(guī)劃結(jié)果如圖11（e）所示。

Fig.11 Comparison of different results for path planning圖11 不同路徑規(guī)劃結(jié)果比較

在方案2 的測試中，本文提出的局部規(guī)劃方法顯示出良好的運動效率和靈活性。特別是當機器人接近門并需要急右轉(zhuǎn)時，通過局部規(guī)劃引導機器人順利通過門，如圖11（b）所示。相比之下，如圖11（c）所示，QBC 會引導機器人緩慢地通過門，離線設計的Bezier 曲線的端點是固定的，不能根據(jù)實時規(guī)劃任務進行調(diào)整，從而限制了局部規(guī)劃運動的靈活性。重復實驗幾次，實驗結(jié)果見表2。QBC大約需要25s 引導機器人達到目標，而本文的HHA*局部規(guī)劃方法只需要19s。與QBC 相比，本文提出的局部規(guī)劃方法的運動效率提高了22.87%。本文還將其與路徑局部規(guī)劃算法TEB 進行了比較，結(jié)果表明，本文方法得到的局部路徑明顯比TEB 更平滑，如圖11（d）、圖11（e）所示。重復幾次實驗，由表2 可知，TEB 大約需要23s 引導機器人到達目標，相比于TEB，本文提出的局部規(guī)劃方法的運動效率提高了16.37%?；谝陨蠈Ρ葘嶒灲Y(jié)果，得出了本文提出的局部規(guī)劃方法在平滑性、運動效率和靈活性方面都具有優(yōu)勢。

Table 2 Comparison of motor efficiency（s）in scheme 2表2 方案2中運動效率比較

5 結(jié)語

本文提出一種HHA*運動規(guī)劃方法來解決變電站復雜場景下機器人的運動規(guī)劃問題，針對變電站中的可變障礙物提出解決方案。本文所提出的HHA*方法包括兩個階段：第一階段在變電站粗標度圖上執(zhí)行二維搜索，其中提取出通往目標的粗糙路徑，通過對此路徑作進一步分析，可將障礙物進行具體分類作為后續(xù)搜索過程的指導；第二階段在精細尺度上進行優(yōu)化的HHA*搜索，克服了混合A*在搜索速度和路徑平滑性方面的缺陷。在該方法中，采用一種新的啟發(fā)式函數(shù)來加速搜索，同時采用連續(xù)運動單元來提高生成路徑的平滑性。實驗結(jié)果表明，本文方法在運行速度和路徑平滑性方面相比其他方法具有更大優(yōu)勢。進一步的道路實驗表明，該方法能更好地響應快速變化。后續(xù)工作還將對該方法進行擴展，以更好地與場景中的動態(tài)障礙物進行交互。