王 磊，孫力帆

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

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

1 引言

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

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

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

2 A＊算法介紹

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

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

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

3 必經(jīng)點(diǎn)路徑規(guī)劃

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

圖1 路徑示例圖

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

3.1 建立障礙物塊層次體系

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

具體步驟如下：

步驟1建立網(wǎng)格地圖對應(yīng)的二維數(shù)組Grid、存放待檢測結(jié)點(diǎn)列表ListObstacles、當(dāng)前層次障礙物列表ObstacleBlock和障礙物塊層次體系列表ListObstacle-Block。

步驟2依照地圖網(wǎng)格序列，將未檢測到的第一個(gè)障礙物網(wǎng)格點(diǎn)存入列表ListObstacles中，并向數(shù)組Grid的對應(yīng)位寫入true。

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

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

通過以上步驟對網(wǎng)格地圖進(jìn)行預(yù)處理，形成一個(gè)系統(tǒng)可識(shí)別的障礙物塊層次體系，如圖2所示。

圖2 障礙物塊層次體系

3.2 計(jì)算登陸點(diǎn)

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

使用T表示障礙物塊層次體系，Bi表示第i個(gè)障礙物塊，Gi,t表示第i個(gè)障礙物塊中的第t個(gè)障礙物網(wǎng)格點(diǎn)，則有如下關(guān)系：

其中，n表示障礙物塊的總數(shù)，p表示第i個(gè)障礙物塊中所含障礙物網(wǎng)格點(diǎn)的個(gè)數(shù)。首先計(jì)算每個(gè)障礙物網(wǎng)格點(diǎn)與參照系“起點(diǎn)-終點(diǎn)”向量的相對位置關(guān)系，即障礙物點(diǎn)位于參照系的左側(cè)還是右側(cè)，障礙物塊與參照系是否有交集。

利用“二維向量的叉積等于三個(gè)點(diǎn)組成的三角形‘面積’的兩倍”這一特性來計(jì)算平面中某點(diǎn)與直線的位置關(guān)系，這里的“面積”值有正有負(fù)，可用于判斷點(diǎn)在線段的左側(cè)還是右側(cè)。定義平面上的三個(gè)點(diǎn)分別為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的左側(cè)；如果S(P1,P2,P3)＜0，則P3位于向量P1P2的右側(cè)；S(P1,P2,P3)=0，則P3位于向量P1P2上。

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

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

3.3 最短路徑必經(jīng)點(diǎn)

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

圖3 登陸點(diǎn)連接圖

然后，計(jì)算各相鄰結(jié)點(diǎn)的歐式距離：

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

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

4 實(shí)驗(yàn)分析

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

4.1 實(shí)驗(yàn)設(shè)計(jì)

將路徑長度、算法運(yùn)行時(shí)間和訪問結(jié)點(diǎn)數(shù)量三個(gè)指標(biāo)作為評價(jià)算法優(yōu)劣的依據(jù)。在4幅100×100的網(wǎng)格地圖中進(jìn)行實(shí)驗(yàn)，分別運(yùn)行傳統(tǒng)A*算法與本文提出的改進(jìn)算法，觀察對比實(shí)驗(yàn)結(jié)果。

4.2 實(shí)驗(yàn)結(jié)果分析

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

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

表1 算法結(jié)果數(shù)據(jù)對比

實(shí)驗(yàn)結(jié)果表明，引入必經(jīng)點(diǎn)約束的改進(jìn)A*算法能夠有效降低冗余結(jié)點(diǎn)的擴(kuò)展數(shù)量，使路徑規(guī)劃效率大幅提高。

5 結(jié)束語

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