王 磊，孫力帆

1.河南科技大學 國際教育學院，河南 洛陽 471023

2.河南科技大學 信息工程學院，河南 洛陽 471023

1 引言

機器人技術的發(fā)展是高端科技發(fā)展的前沿，路徑規(guī)劃技術則是機器人研究的一個重要領域，是實現機器人自主定位與導航的關鍵技術之一。路徑規(guī)劃是尋找連接起點位置和終點位置序列點或曲線的策略，其主要任務是使機器人能夠成功避開環(huán)境中的各種障礙，并沿著最低代價的路徑到達終點。路徑規(guī)劃可分為基于先驗完全信息的全局路徑規(guī)劃和基于傳感器信息的局部路徑規(guī)劃，全局路徑規(guī)劃的常用方法主要有遺傳算法[1]、快速隨機搜索樹算法（RRT）[2]和蜂群算法[3]等。局部路徑規(guī)劃常用算法主要包括：人工勢場法[4]、模糊算法[5]、A*（A-Star）算法[6]等。其中A*算法適應環(huán)境能力強，應用十分廣泛。它是Dijkstra算法與廣度優(yōu)先搜索（BFS）算法的組合，在啟發(fā)函數的指引下搜索地圖中的網格點，選取當前代價值最小的網格點作為路徑的擴展，直至找到終點。A*算法對環(huán)境反應迅速，但實時性差，運行效率低。高慶吉等人[7]于2012年提出改進啟發(fā)式函數，利用加權的方法增加其可靠性，同時在確定擴展結點時考察其8鄰域是否可信。Franti?等人[8]于2014年提出了一種基于改進A*算法的移動機器人路徑規(guī)劃方法，分別進行了 Basic Theta*、Phi+*、RSR[9]、JPS[10]方法的改進，通過實驗發(fā)現JPS擴展結點最少、速度最快，而Theta*能實現路徑結果最優(yōu)。李沖等人[11]對單邊矩形擴展A*算法進行了改進，將相鄰結點的兩條重合邊界替換為一條共有邊界，運行速度顯著提高。綜合分析各種算法，本文引入必經點約束對結點擴展方向進行指導，通過降低A*算法搜索過程的盲目性，減少冗余結點擴展數量，提高路徑規(guī)劃效率。

有關必經點最短路徑問題，國內外學者積累了豐富的研究成果。袁紅濤等[12]提出了一種計算K優(yōu)路徑算法，并對Dijkstra 算法的實現進行改進，大大提高了效率。徐慶征等[13]提出了一種求解無環(huán)必經點最短路徑問題的遺傳算法，以自然路徑形式作為染色體，采用單點交叉和引入必經點的變異算子，通過使路徑長度短且包含必經結點多的染色體被優(yōu)先選擇進入下一代，實現三類必經點的最短路徑搜索。馮琳耀等[14]提出了一種幾何代數算法，用于解決必經點最短路徑問題，但該算法主要用于規(guī)劃結點數最少的路徑而非代價最小的路徑，且只適用于無向圖，無法用于有向圖路徑規(guī)劃。黃書力等[15]提出了一種基于Dijkstra的必經點集最短路徑規(guī)劃算法，但該算法規(guī)劃出的路徑有環(huán)，且只適用于無向無權網絡。文獻[16]提出的啟發(fā)式算法可用于解決經過必經點的最短路徑問題，但結果存在誤差。

本文利用A*算法生成起點到必經點、必經點到終點的最短路徑段，再對路徑段進行拼接，得到一條最短路徑。最后，在100×100 網格地圖中采用8 近鄰搜索方案對傳統A*算法與改進算法進行實驗對比，結果表明，改進后的A*算法能有效降低冗余結點的訪問量，提高路徑規(guī)劃效率。

2 A＊算法介紹

A*算法相當靈活，適應性強，是非常受歡迎的路徑規(guī)劃算法。A*定義如下：g(n)表示從起點Start 到達某一結點m的實際路徑代價，h(n)是啟發(fā)式函數，表示從結點m到達終點End 的估計代價，f(n)=g(n)+ h(n)是算法綜合評估值。網格地圖常用的啟發(fā)式函數有曼哈頓距離，歐幾里得距離和對角線距離。A*搜索就是不斷比較移動過程中擴展結點的f(n)值，將每一次的最小f(n)值點存入列表作為備選結點。

A*算法存在兩種極端情況：（1）當啟發(fā)式函數為0時，f(n)=g(n)，則A*演變?yōu)镈ijkstra 算法，規(guī)劃路徑最優(yōu)但算法效率降低；（2）當啟發(fā)式函數遠遠大于g(n)時，f(n)約等于h(n)，此時A*演變成BFS算法，則不一定得到最優(yōu)路徑。如果啟發(fā)式函數值小于實際代價值，則A*能找到最優(yōu)路徑，但是擴展結點更多，運行效率更低。如果啟發(fā)式函數值比實際代價值高，則A*擴展結點較少，運行更快，但路徑規(guī)劃結果不一定是最優(yōu)。

在障礙物較多的地圖中，通過啟發(fā)函數h(n)計算得到的理論代價往往比實際代價小很多，從而導致A*需要在不同方向上擴展更多的冗余結點，使得A*算法運行效率大大降低。因而需要在路徑規(guī)劃過程中適當增加輔助條件對擴展方向進行約束和引導，從而減少冗余結點的數量，提高算法效率。

3 必經點路徑規(guī)劃

要在滿足最短路徑的前提下有效約束搜索過程，減少冗余結點的訪問量，可尋找一個最短路徑必經點，用于路徑規(guī)劃過程中的方向指導。顯然，在某個障礙物的邊緣區(qū)域存在最短路徑必經點。如圖1 所示，從起點S到終點G分別經過A、B、C、D、E五個點可形成五條不同的路徑，顯然S-A-G長度大于S-B-G，S-C-G和S-D-G兩條路徑都需要繞行B點或E點，長度必然大于S-B-G或S-E-G，因而可知B、E 兩點至少有一點是最短路徑的必經點。

圖1 路徑示例圖

依據障礙物在網格地圖中的分布特點，將其劃分成不同的群體，即障礙物塊，再按照各障礙物塊與起點的相對位置關系，由近及遠形成一個障礙物塊層次體系。將“起點-終點”向量作為參照系，計算各障礙物塊中每個網格點到參照系的距離，選取與參照系有交集的障礙物塊兩側最大點作為登陸點，再連接相鄰障礙物塊之間的所有登陸點，形成以各層次登陸點為中間結點的所有模擬路徑。選取代價最小的topk條模擬路徑作為樣本，計算樣本中所有登陸點的綜合評分，得分最高者為最短路徑必經點。

3.1 建立障礙物塊層次體系

A*算法在搜索路徑時如遇到障礙物則自動避開，向其他方向繼續(xù)搜索，不考慮障礙物的分布特點與層次關系，對結點擴展方向無有效控制，因而需要對網格地圖進行預處理，將所有障礙物網格點關聯起來，建立層次體系。首先遍歷地圖中每個網格點，找到所有的障礙物點，將每個障礙物點8鄰域內出現的障礙物歸入當前障礙物點所屬集合中。循環(huán)執(zhí)行，直到將所有障礙物點完成歸類，從而得到所有障礙物集合即障礙物塊。

具體步驟如下：

步驟1建立網格地圖對應的二維數組Grid、存放待檢測結點列表ListObstacles、當前層次障礙物列表ObstacleBlock和障礙物塊層次體系列表ListObstacle-Block。

步驟2依照地圖網格序列，將未檢測到的第一個障礙物網格點存入列表ListObstacles中，并向數組Grid的對應位寫入true。

步驟3檢測列表ListObstacles，如ListObstacles為空，進入步驟4；否則，令Current=ListObstacle.first，刪除ListObstacles第一個結點。檢測Current的8鄰近網格點，如檢測到障礙物且該障礙物在數組Grid中的對應位是false，則加入列表ListObstacles中，并將數組Grid的對應位設置為true，同時將Current結點加入Obstacle-Block中，循環(huán)執(zhí)行步驟3。

步驟4將ObstacleBlock插入列表ListObstacle-Block，如Grid所有值為true，則程序結束，否則進入步驟2。

通過以上步驟對網格地圖進行預處理，形成一個系統可識別的障礙物塊層次體系，如圖2所示。

圖2 障礙物塊層次體系

3.2 計算登陸點

有障礙物遮擋的情況下，最短路徑一定是沿著障礙物邊緣擴展，如進入障礙物范圍則無法通過，偏離障礙物又非最短路徑，本文將最短路徑可能經過的障礙物邊緣點定義為“登陸點”。通過分析各種類型地圖可以發(fā)現，登陸點大多位于障礙物塊中距離“起點-終點”向量最遠的障礙點附近，本文將“起點-終點”向量作為參照系，計算各層次障礙物塊與“起點-終點”向量的相對位置關系，尋找障礙物塊的邊緣點，即登陸點。

使用T表示障礙物塊層次體系，Bi表示第i個障礙物塊，Gi,t表示第i個障礙物塊中的第t個障礙物網格點，則有如下關系：

其中，n表示障礙物塊的總數，p表示第i個障礙物塊中所含障礙物網格點的個數。首先計算每個障礙物網格點與參照系“起點-終點”向量的相對位置關系，即障礙物點位于參照系的左側還是右側，障礙物塊與參照系是否有交集。

利用“二維向量的叉積等于三個點組成的三角形‘面積’的兩倍”這一特性來計算平面中某點與直線的位置關系，這里的“面積”值有正有負，可用于判斷點在線段的左側還是右側。定義平面上的三個點分別為P1(x1,y1)、P2(x2,y2)、P3(x3,y3)，組成的三角形面積S(P1,P2,P3)=(x1-x3)×(y2-y3)-(y1-y3)×(x2-x3)。如果S(P1,P2,P3)＞0，則P3位于向量P1P2的左側；如果S(P1,P2,P3)＜0，則P3位于向量P1P2的右側；S(P1,P2,P3)=0，則P3位于向量P1P2上。

計算與參照系有交集的障礙物塊Bi中每一個網格點Gi,t到直線“起點-終點”的距離，可利用點到直線的距離公式（1）來計算：

其中，A=yEnd-yStart，B=xStart-xEnd，C=xEnd×yStartxStart×yEnd，x0=xGi,t，y0=yGi,t。最后，分別選取障礙物塊Bi位于參照系兩側的最遠點作為登陸點。

3.3 最短路徑必經點

登陸點位于各層障礙物塊的邊緣區(qū)域，包含了最短路徑的必經點。使用D表示登陸點，則Di,l表示第i層的左側登陸點，Di,r表示第i層的右側登陸點。以圖3藍色實線所示，分別連接相鄰層次的每個登陸點，可將障礙物塊關聯起來，形成大量的模擬路徑：

圖3 登陸點連接圖

然后，計算各相鄰結點的歐式距離：

其中，(xDi,yDi) 和(xDi+1,yDi+1) 分別表示兩相鄰登陸點的平面坐標。計算每條模擬路徑patht的路徑代價選取代價最小的topk條模擬路徑作為統計樣本，再依據每個樣本的路徑代價在總體樣本中的綜合占比，計算每個樣本的權重值：weight(u)=統計k條模擬路徑中出現過的所有登陸點并計算其綜合評分其中，pu(Dm,x)表示登陸點Dm,x在模擬路徑pathu中出現的次數（0 或1）。選取F值最大的登陸點，即最短路徑必經點。

最后，利用A*算法分別規(guī)劃出起點到必經點、必經點到終點的最短路徑段，通過拼接得到最短路徑，并進行驗證。

4 實驗分析

在 Intel i7 四核處理器、8 GB 內存、500 GB 固態(tài)硬盤的計算機硬件平臺上，使用C#語言編寫A*算法與改進算法的Windows實驗程序。

4.1 實驗設計

將路徑長度、算法運行時間和訪問結點數量三個指標作為評價算法優(yōu)劣的依據。在4幅100×100的網格地圖中進行實驗，分別運行傳統A*算法與本文提出的改進算法，觀察對比實驗結果。

4.2 實驗結果分析

圖4 是A*算法與改進算法的路徑規(guī)劃效果圖。圖中左側綠色網格點為起點，右側紅色網格點為終點，灰色網格表示障礙物，淡綠色網格表示算法訪問過的結點，黃色線條表示規(guī)劃路徑。由圖4 可見，改進算法的結點訪問量明顯少于原A*算法，具體實驗數據如表1所示。在地圖1中，改進算法比A*算法訪問結點數量減少1 374 個，運行時間減少31.2 ms，效率提高66.7%；在地圖2 中，訪問結點數量減少1 711 個，運行時間減少39.3 ms，效率提高71.3%；在地圖3 中，訪問結點數量減少3 780 個，運行時間減少73.7 ms，效率提高82.5%；在地圖4 中，訪問結點數量減少3 277 個，運行時間減少46.9 ms，效率提高75%。

圖4 兩種算法路徑規(guī)劃效果圖

表1 算法結果數據對比

實驗結果表明，引入必經點約束的改進A*算法能夠有效降低冗余結點的擴展數量，使路徑規(guī)劃效率大幅提高。

5 結束語

本文針對傳統A*算法搜索范圍廣、運行效率低的問題進行研究，提出引入最短路徑必經點的方法對搜索方向進行約束。首先將障礙物分成不同的層次體系，以“起點-終點”向量為參照系，計算出每層障礙物塊的登陸點，再對相鄰層次登陸點進行連接得到大量模擬路徑，依據模擬路徑的長度計算所有登陸點的綜合評分，選取得分最高的登陸點作為最短路徑必經點，利用A*算法分別對起點到必經點、必經點到終點進行路徑規(guī)劃，并將規(guī)劃出的兩條路徑段拼接成一條最短路徑。實驗結果表明，改進算法可有效約束搜索方向，減少冗余結點的擴展，大幅提高路徑規(guī)劃效率。