面向變電站運維的智能機器人路徑規(guī)劃算法研究

吳振躍，章程熙，陸 昱，喬亞興，黃維華，周 琪

(1.國網上海市電力公司市南供電公司，上海 200072；2.上海服澤能源科技有限公司，上海 200025)

近年來，國家對電力系統(tǒng)建設投入很大，變電站工作環(huán)境復雜，穩(wěn)定性要求極高，這給當前變電站運維提出了更高的要求。智能機器人以其穩(wěn)定、可靠、智能的優(yōu)點在國內電力系統(tǒng)內變電站運維中應用應前景廣闊[1-4]。

變電站運維機器人在變電站運維工作中具有廣闊的應用前景。提高智能機器人自主性是發(fā)展的重要趨勢。該領域中，路徑規(guī)劃是一個基本問題[5]。運維機器人執(zhí)行任務時需要規(guī)劃出一條從起始點到指定位置的安全、平穩(wěn)的路線。這條路徑應該盡量使得從起點到目的地的成本最低[6]。路徑規(guī)劃的好壞直接決定了任務是否能夠順利完成。因此，路徑規(guī)劃對于變電站運維機器人的任務完成起到舉足輕重的作用，它是變電站運維機器人實現(xiàn)自主導航的基礎[7]。變電站運維機器人路徑規(guī)劃算法成為變電站運維機器人技術的研究重點。因此，研究基于全局地圖的路徑規(guī)劃技術具有很好的應用前景和使用價值。

本文針對變電站運維機器人尋路問題對標準 A 星算法進行深入的研究，分析A星算法啟發(fā)函數計算的啟發(fā)代價與實際代價之間的關系對于尋路結果的影響，提出基于向量叉積因子的A星算法。最后設計仿真軟件，通過仿真試驗證明基于向量叉積因子的A星算法在變電站運維環(huán)境中的路徑規(guī)劃性能優(yōu)越。

1 路徑規(guī)劃算法

A星算法是在Dijkstra算法基礎上進行改進，使用啟發(fā)信息來引導搜索方向，讓搜索不再具有 “盲目性”，使搜索過程更具智能性，極大提高了搜索的效率，得到了廣泛的應用。

該算法是從起始點開始在它周圍的可能成為路徑的點中選擇出最合適的點，點的合適程度可用估價函數來計算，代價越小就越合適。選擇出最合適的點，然后以該點為擴展點，將它周圍可能組成路徑的點再進行代價計算。這樣循環(huán)下去，直至找到目標點。因此，該算法的重點在于它的估價函數。

1.1 A 星算法的估價函數

在啟發(fā)式搜索中，估價函數占有重要地位。它用來計算地圖中各個頂點的重要程度。通過估價函數計算各個頂點的數值來評估頂點對于組成路徑的重要程度。

依據各個頂點的重要程度來規(guī)劃出一條從起始點到終點的最優(yōu)路徑。

A星算法的估價函數：

F(n)=G(n)+H(n)

(1)

式中n——當前節(jié)點；F(n)是節(jié)點n到目標節(jié)點的總估算代價；G(n)——從起始點開始沿著已經生成的路徑到當前節(jié)點的實際代價；H(n)——從當前節(jié)點到目標節(jié)點的最優(yōu)路徑的估計代價，被稱為啟發(fā)函數。

在變電站運維機器人路徑規(guī)劃問題中，需要對地圖進行柵格化處理然后對環(huán)境進行建模，通過建立網格地圖對環(huán)境進行抽象處理。

在網格地圖中，不同情況下應采用不同的啟發(fā)式函數。啟發(fā)式函數的距離應與所允許的移動方式相匹配。

設當前點坐標為(x1,y1) ，目標點坐標 (x2,y2)。

(1)在正方形網格中，允許向四鄰域移動，可以擴展當前點的上、下、左、右四個點。此時應使用曼哈頓距離作為啟發(fā)函數。曼哈頓距離是兩個點在標準坐標系上的絕對軸距總和。在兩維空間中的計算公式：

d=|x1-x2|+|y1-y2|

(2)

(3)

在正方形網格中，允許任何方向的移動，應采用歐式距離作為啟發(fā)函數，歐式距離為兩個點之間的直線距離。二維空間中歐式距離的計算公式：

(4)

1.2 A 星算法的尋路流程

A星算法在擴展路徑時，為了避免重復遍歷節(jié)點，A星算法將創(chuàng)建兩個空表：OPEN表和CLOSE表。OPEN表用來存放沒有訪問過的節(jié)點，CLOSE用來存放已經訪問過的節(jié)點。A星算法使用估價函數計算考察點的估計代價，將選擇估價函數值最小的點進行路徑擴展，具體流程如下。

(1)定義兩個空表，命名為OPEN表和CLOSED表，其中OPEN用于存放未訪問過的頂點，CLOSED表用于存放已經訪問過的頂點；

(2)把起始點S存儲到OPEN表中；

(3)判斷OPEN表是否有頂點，若沒有頂點，則搜索過程失敗；

(4)若OPEN表中有頂點，選出OPEN表中總估計代價值最小的節(jié)點N，將其移入CLOSED表；

(5)判斷節(jié)點N是否為目標頂點T，若是，則表示路徑搜索成功，算法結束；

(6)若不是目標節(jié)點T，則擴展搜索N的子頂點Vi(i≤n，n為節(jié)點N的子節(jié)點數目)，計算每個子節(jié)點的總估價代價F(Vi)值。判斷Vi是否存在于OPEN表或CLOSED表中：

若Vi同時不存在于OPEN表和CLOSED表中，則將其存放于OPEN表，并給它加個指針指向父頂點N；

若Vi已存在于OPEN表中，則對OPEN表中的總估價代價F(Vi)值進行更新，若新計算的F(Vi)值比OPEN表中F(Vi)舊值更小，則代替之，并將它的指針改為指向父頂點N；

若Vi已存在于CLOSED表中，則忽略此頂點，繼續(xù)訪問節(jié)點N的其他子頂點。

(7)跳轉至步驟(3)，一直循環(huán)下去，直到滿足算法退出條件：找到路徑或者尋找路徑失敗。程序流程圖，如圖1 所示。

2 基于向量叉積因子的A星算法

因為估價函數含有啟發(fā)部分，它包含目標點等有用的信息，使得A 星算法的搜索具有方向性，從而減少了遍歷點的數目，提高了算法的效率。通過分析A 星算法的啟發(fā)代價，針對啟發(fā)代價與實際代價有差異的問題，提出了向量叉積因子優(yōu)化的A 星算法。

2.1 向量叉積的計算

在柵格地圖中，設起點坐標為(start_x,start_y),目標點坐標為(goal_x,goal_y), 當前節(jié)點的坐標為(current_x,current_y)。

設從當前點到終點的橫坐標差為dx1；從當前點到終點的縱坐標差為dy1；從起點到終點的橫坐標差為dx2；從起點到終點的縱坐標差為dy2。因此，從起點到目標點的向量與從當前節(jié)點到目標點的向量叉積：

cross=|dx|·dy2-dx2·dy1

(5)

2.2 向量叉積對啟發(fā)函數權重的優(yōu)化

計算出從起點到目標點的向量與從當前節(jié)點到目標點的向量叉積值后，把向量叉積因子加入到啟發(fā)函數權重中，以此對估價函數進行優(yōu)化。因為向量叉積值比較大，過大的向量叉積值會使計算出來的計算代價遠遠大于實際代價，所以將它乘0.001后再加入到啟發(fā)函數的權重中。

設啟發(fā)函數的權重：

weight=1+0.001cross

(6)

則估價函數：

F(n)=G(n)+(1+0.001cross)

(7)

因為啟發(fā)函數的權重始終是大于1的，向量叉積因子增加了啟發(fā)函數的權重，這將使得A星算法在擴展時候更具有方向性。在擴展的點距離起點到目標點之間的連線相對較遠時，向量叉積值將會變大，所以啟發(fā)函數的權重變大，該點的估計代價也將會變大。

因為選擇open表中的所有點的估計代價最小的節(jié)點進行擴展，所以通過增加函數向量叉積因子的權重將會放大當前點的啟發(fā)代價，使得A星算法忽略掉遠離起點到目標點的連線點，算法擴展時更傾向于擴展與起點到目標點的連線距離相對近的點。這使得A星算法擴展點大大減少，提高了算法的運行效率，可以在很短時間內得到一個路徑較佳的結果，這對于大地圖尋路問題將有很強的適用性。

綜上所述，增加啟發(fā)函數的權重將會使A星算法更具有方向性，在權重中加入向量叉積因子會使得該權重更具有智能性，它將更偏向于遍歷離起點與目標點之間連線較近的節(jié)點。通過這種增加權重的方式，將一些起點與目標點之間連線較遠的點擠出考察區(qū)域，這將使算法運行過程中遍歷節(jié)點的數目將大大減少，從而提高算法運行效率。

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

搭建試驗平臺對改進后的A星算法進行試驗。通過與標準A星算法、向量叉積因子優(yōu)化的A星算法進行對比，證明向量叉積因子優(yōu)化的A星算法在尋路方面具有優(yōu)越性。

3.1 仿真軟件設計

本次試驗平臺為筆記本電腦聯(lián)想-Y430p，處理器為 intel core i5-4210M，內存為12 GB，使用的操作系統(tǒng)為Win10，使用的編程軟件為VC++6.0，編程語言為 C 語言。 基于VC++6.0軟件新建了一個Win32 Application項目。仿真界面設計如圖2所示。

該仿真軟件主要有地圖構建、自建地圖、算法選擇、結果顯示這4個功能。

3.1.1 地圖構建

可以設置障礙密度和柵格大小來新建一個隨機地圖，可以將起點柵格設置為紅色，無障礙的柵格設置為白色，有障礙的柵格設置為黑色，目標點柵格設置為綠色。

單擊清空地圖的按鈕可以清除界面上的地圖信息和尋路結果信息。

單擊清除路徑的按鈕可以清除界面上的路徑。

單擊保存地圖的按鈕可以保存當前的地圖，默認保存地圖的文件路徑在項目當前路徑下。

單擊讀取地圖的按鈕可以讀取最近一張保存的地圖。

單擊擴展點的按鈕可以顯示算法運行的擴展點，顯示的柵格顏色為黃色，再次單擊該按鈕可以取消顯示擴展點。

3.1.2 地圖設置

可以自由設定地圖，先單擊清空地圖按鈕，新建一個空白的柵格地圖，然后單擊起點、終點和障礙點按鈕，分別設置起點、終點和障礙點，可實現(xiàn)自建地圖的功能。

單擊取消點、起點、終點和障礙點的按鈕可以修改地圖，使用取消點的功能

可以取消掉起點，然后使用起點的功能選擇新的起點，從而實現(xiàn)改變起點位置的功能。

3.1.3 算法選擇

啟發(fā)函數的權重可以選擇權重為1的標準值、含有向量叉乘因子的權重。

3.1.4 結果顯示

算法運行結果主要包含兩個部分，路徑和運行結果指標。

路徑通過設置不同顏色顯示不同算法的路徑，在四鄰域A星算法中，設置權重為1的算法運行結果路徑顏色為藍色，含有向量叉乘因子的權重的算法運行結果路徑顏色為紫色、模糊權重的算法結果路徑為棕色。在八鄰域A星算法中，設置權重為1的算法運行結果路徑顏色為黃色，含有向量叉乘因子的權重算法運行結果路徑顏色為灰色、模糊權重的算法結果路徑為青色。

3.2 A星算法仿真

由理論部分可知，啟發(fā)函數的權重有以下兩種：值為1的標準權重、含有向量叉乘因子的權重。通過仿真試驗，比較不同權重下A星算法的運行結果，證明向量叉積因子優(yōu)化的權重對改進A星算法性能上的優(yōu)越性。

建立一個隨機地圖，障礙密度為30% ，柵格大小為8，進行仿真試驗，分別仿真了啟發(fā)函數權重為1的標準A星算法，含有向量叉乘因子的啟發(fā)函數權重的A星算法。

通過進行仿真試驗，得到的試驗結果如圖3所示。

4 結語

作為尋路算法領域可靠性比較高的算法，A星算法有廣泛的應用前景。本文在標準A星算法的基礎上，提出了一種思路：使用向量叉積因子作為智能的啟發(fā)函數權重，提出了基于向量叉積因子的A星算法。該優(yōu)化方法可以大大減少遍歷點數目，提高了算法的運行時間。