陳 姝，馬奔宇，魏文斌

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

引言

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

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

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

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

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

1.1 基本思想[4]

引入當前節(jié)點j 的估計函數(shù)f*，當前節(jié)點j 的估計函數(shù)f*定義為：

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

1.2 數(shù)學函數(shù)模型[4]

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

估價函數(shù)1：歐幾里德距離

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

該估價函數(shù)的計算量較大，不適用海量數(shù)據(jù)計算。

估價函數(shù)2：曼哈頓距離

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

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

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

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

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

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

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

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

1.4 測試結(jié)果分析

核島地圖方陣如下：

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

RobotPosition=[2，1];

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

WeldPosition=[16，20];

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

圖1 WRouteAStar 算法求解導航路徑

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

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

2.1 利用路徑矩陣P 求解拐彎點個數(shù)

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

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

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

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

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

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

2.2 路徑距離和彎點數(shù)的特征

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

圖2 路徑距離和拐彎點個數(shù)

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

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

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

A*算法獲取的路徑拐彎點個數(shù)與距離長短無關(guān)聯(lián)。

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

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

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

步驟2）給定循環(huán)次數(shù)T，運行A*算法，產(chǎn)生T 條路徑，求出路徑距離向量D 和彎點向量W；

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

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

步驟5）執(zhí)行循環(huán)語句：當DX＞Dmin或者WX＞W(wǎng)min時，求出其路徑P，并依此求出路徑距離和拐彎點數(shù)，分別賦值給DX和WX；當DX≦Dmin且WX≦Wmin時，結(jié)束該循環(huán)；

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

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

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

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

圖4 最優(yōu)路徑信息

圖5 最優(yōu)路徑圖示

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

3 實用情況總結(jié)

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

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