核島機器人巡檢導航A*算法優化及其應用

陳 姝，馬奔宇，魏文斌

（1.中核武漢核電運行技術股份有限公司，湖北 武漢;2.核動力運行研究所，湖北 武漢）

引言

核島廠房屬特殊的封閉空間，無法使用常規在線導航技術手段，若要實現巡檢機器人點對點快速移動，必須構建核島巡檢機器人專用導航系統。該系統載入的專用大尺寸核島地圖，其路線、障礙物等元素均為不規則形態，無法簡單抽象為節點或線段。通常，距離最短路徑被認為最優，但大尺寸地圖節點多，在所有通路中求解最短路徑耗時過長，難以滿足性能要求。

為解決上述問題，人們提出了Dijkstra 算法，但該算法易受當前近節點影響，從而丟失近線路最優或次優解，為解決這一問題，人們提出了基于“方向優先”的A*算法，該算法考慮當前節點到終點的距離，因此在方向和距離之間進行了折衷，路徑既不過偏也不過遠。參考文獻[1]側重于對A*算法的數學研究；文獻[2]側重A*算法應用于無人機三維路徑規劃；文獻[3]研究A*算法避障路徑規劃。A*算法應用型研究成果較多，公開網絡上[4]可獲取關于A*算法詳細介紹，故不進行贅述。

本文基于核島位置信息點狀分布特性構建地圖矩陣，用0 和1 分別表示未連通和連通狀態。根據核島巡檢機器人在通過彎點時執行“減速- 停止- 轉向- 加速”的運動特征，以及彎點越多耗時越長的客觀情況，從理論上提出了基于A*算法的尋找“距離最短”、“彎點個數最少”的路徑求解方法，使用該方法可讓核島巡檢機器人高效省時地完成路徑規劃和導航運動。

1 基于A*的自主導航算法

作為一種啟發式搜索型快速路徑規劃算法，A*算法相比遍歷式路徑規劃算法如廣度優先算法（BFS）等都更加迅速和節省空間[4]。

1.1 基本思想[4]

引入當前節點j 的估計函數f*，當前節點j 的估計函數f*定義為：

其中，g（j）是從起點到當前節點j 的實際費用量度，h*（j）是從節點j 到終點的最小費用的估計。從起始節點向目的節點搜索時，每次搜索f*（j）最小節點直至發現目的節點，即在和起點距離相等的中間節點集合里，選取與起點和終點構成的直線距離最小節點。顯然，起點和選取的該節點構成的連線與起點和終點構成的連線形成的方向夾角最小，即A*算法的“方向優先”。

1.2 數學函數模型[4]

A* 算法核心為估價函數h*（j）設計。

估價函數1：歐幾里德距離

假設起點S 的坐標（Sx，Sy），中間點坐標為（Nx，Ny），終點G 坐標（Gx，Gy），估價函數h*取歐幾里德距離，表示為：

該估價函數的計算量較大，不適用海量數據計算。

估價函數2：曼哈頓距離

考慮將兩點之間的曼哈頓距離作為估價函數。其中A 點的經緯度為（Ax，Ay），B 點的經緯度是（Bx，By）,則A、B 之間的曼哈頓距離可以表示為:

此估價函數計算量小，非嚴格方向優先，但可保證較短路徑搜索方向接近目標點方向，故采用。

1.3 基于A*算法的設計流程

步驟1）根據核島地圖構建n×n 尺寸0-1 方陣，明確各點可通行性（0 代表有障礙物，1 代表無障礙物），記為F；

步驟2）根據待檢測目標名稱及其所在位置，建立另一n×n 方陣，將焊縫名稱以字符串形式存儲所在位置對應方陣方格中，記為W；

步驟3）確定機器人在F 中當前位置[Ri，Rj]；

步驟4）根據要檢測目標名稱，找到其在矩陣W中行列位置[Oi，Oj]；

步驟5）根據“方向優先”、“距離最短”的原則，運用函數WRouteAStar 實現機器人行進路徑的搜尋。該函數形式為：P=WRouteAStar(F，[Ri，Rj]，[Oi，Oj])，輸入為前面得到的信息矩陣F、機器人在F 中的當前位置[Ri，Rj]和要檢測目標的F 中的位置[Oi，Oj]。

1.4 測試結果分析

核島地圖方陣如下：

機器人在矩陣中的位置為：

RobotPosition=[2，1];

檢測目標在矩陣中的位置為：

WeldPosition=[16，20];

其中兩次運用WRouteAStar 函數得到的路徑結果見圖1。算法運行后反饋不同的可用路徑，單路徑長度（前者48，后者46）相差2 個單位，即單次求解路徑長度不為最短。所有求解路徑均存在冗余拐點，導致路徑距離、時耗和能耗均會增加。

圖1 WRouteAStar 算法求解導航路徑

2 基于最短距離和最少拐彎點的導航提升算法

基于之前測試結果，核島機器人巡檢移動導航使用A*算法求解路徑，因彎點處機器人必須經歷減速- 停止- 轉向- 加速至通常速度，耗時耗能，故還必須考慮拐彎點個數。

2.1 利用路徑矩陣P 求解拐彎點個數

根據前述程序執行后的路徑可知，原算法輸出路徑P 為N×2 矩陣，N 代表距離點數，每一行為一個點，第一列代表矩陣高度(Y 軸)，第二列代表矩陣寬度(X 軸)，起點為矩陣最下行，終點為矩陣最上行。

為與平面圖形坐標表示完全一致，前后行與前后列需互換，語句B=fliplr(flipud(P))可實現互換功能。

通過相鄰兩個離散點可根據坐標求出方向斜率

兩個相鄰點方向角αi與其斜率ki滿足如下反正切關系

無論橫向或縱向，路徑段在無拐彎點情況下，任意相鄰兩點方向角相同，而在拐彎點，前方向角和后方向角不同。要找到拐彎點，必須對方向角向量進行差分處理

然后運用find 函數找到差分處理結果中非零元素位置向量PWanDian，再運用length 確定向量長度，即拐彎點的個數NumWanDian。上述過程的代碼語句表達如下：

2.2 路徑距離和彎點數的特征

基于上述地圖數據，以給定的起始點(2,1)和終點(16,20)為例，運行50 次獲取距離和拐彎點個數，參見圖2。

圖2 路徑距離和拐彎點個數

通過采用哈曼頓距離，80%求解距離可達最短（46個距離單位），其余求解距離與最短距離相差不超過4個距離單位；但在50 次運行的結果中，機器人需要拐彎的次數變化較大，最少為14 次，最多為24 次，相差10 次，故拐彎點個數在路徑質量評價中為重要指標。路徑距離和拐彎點個數的特征可以歸納如下：

基于“方向優先”的A*算法獲取路徑多數情況可滿足最短距離要求；

A*算法獲取的路徑拐彎點個數變化無規律，且變化范圍較大；

A*算法獲取的路徑拐彎點個數與距離長短無關聯。

2.3 基于最短距離和最少拐彎點的導航算法

核島巡檢過程中，機器人導航路徑若能同時實現距離最短、拐彎點數最少，可極大提升機器人工作效率。下面給出這種基于A*的可實現最短距離和最少拐彎點的導航提升算法步驟。

步驟1）輸入核島地圖矩陣F 以及機器人起點S和終點E 在矩陣中的坐標；

步驟2）給定循環次數T，運行A*算法，產生T 條路徑，求出路徑距離向量D 和彎點向量W；

步驟3）根據距離向量D 和彎點向量W 求出最短距離Dmin和最少彎點數Wmin；

步驟4）給新的距離變量DX和彎點變量WX賦初值，如DX=Dmin+1，WX=Wmin+1；

步驟5）執行循環語句：當DX＞Dmin或者WX＞Wmin時，求出其路徑P，并依此求出路徑距離和拐彎點數，分別賦值給DX和WX；當DX≦Dmin且WX≦Wmin時，結束該循環；

步驟6）輸出最后獲得的路徑P。

在該算法中，步驟1）給出算法實現的基本數據；步驟2）至步驟3）確定認可的最短距離和最少拐點數；步驟4）至步驟5）確定循環變量初值、循環變量的每次循環值；步驟6）完成最短距離、最少拐彎點路徑的輸出。用流程圖表示，見圖3。

圖3 基于最短距離和最少拐彎點的導航算法流程圖

基于上述地圖數據，以給定的起始點(2,1)和終點(16,20)為例，在Intel(R) Core(TM)i5-7400 CPU @3.00GHz 3.00GHz 處理器，內存為8.00GB 的計算機平臺運行50 次，得到最小距離46 個距離單位，最少拐彎點個數14 個，循環4 次耗時0.14 秒即找到了最優路徑，參見圖4。具體路徑在地圖上的顯示見圖5。

圖4 最優路徑信息

圖5 最優路徑圖示

上述結果表明，基于最短距離和最少拐彎點的導航提升算法可在較短時間內求解距離最短、拐點最少路徑。該算法可以應用于核島機器人巡航導航以及其它實時導航系統[5]。

3 實用情況總結

核島機器人巡檢移動導航，采用A*算法作為核心算法，兼顧距離最短、方向優先法則，可極大減少尋求最優路徑的時間，快速獲取最優路徑。同時，考慮到機器人行走減速拐彎耗時的特性，本文提出了基于最短路徑和最少彎點的提升算法，其中介紹了彎點數計算方法，多次運用A*算法獲取多條路徑，通過這些路徑計算出從起點到終點的距離向量和彎點數向量，找到從起點到終點可能的最短距離和從起點到終點的最少彎點數的路徑，以最短距離和最少彎點數作為條件，循環搜尋滿足以上兩條件的路徑。

通過本文中的算法代碼運行實驗結果表明，該算法可以在較短時間內找到同時滿足“距離最短”和“彎點數最少”的路徑，具有很強的實用性。