一種基于障礙物登陸點檢測的全局路徑規(guī)劃算法

王 磊，王 毓，孫力帆

（1. 河南科技大學 國際教育學院，洛陽 471023；2. 河南科技大學 信息工程學院，洛陽 471023）

人工智能時代的機器人已不再充當簡單的指令執(zhí)行角色，而是朝著智能化、高效化的方向發(fā)展，它代表了高科技的發(fā)展前沿，已經在各行各業(yè)中得到廣泛應用。路徑規(guī)劃是移動機器人研究領域的熱點，具有重要的理論與現(xiàn)實意義，其主要任務是使機器人能夠成功避開環(huán)境中的各種障礙，并沿著最低代價的路徑到達終點。依據機器人對地圖信息的掌握情況可將算法分為全局路徑規(guī)劃與局部路徑規(guī)劃。其中，A*（A-Star）[1-3]是一種經典的全局路徑規(guī)劃算法，它是Dijkstra算法[4-6]與寬度優(yōu)先搜索（Breadth First Search,BFS）算法[7]的結合，是一種啟發(fā)式搜索算法。A*算法利用啟發(fā)函數指導尋路過程，通過計算柵格地圖各結點的代價值，選取待擴展的最佳結點，直至找到最終目標點位置，該算法以超強的環(huán)境適應能力獲得了非常廣泛的應用，但缺點是隨著地圖規(guī)模的擴大，待搜索結點數量大幅增加，路徑規(guī)劃時間變得過于漫長。

為了解決A*算法在大場景地圖中的搜索時間問題，前人進行了各種改進。雙向A*（Bidirectional A-Star）算法[8]采用正反向交替機制，分別從起點和終點朝著相對方向進行搜索，最終在起點、終點連線的中點區(qū)域匯合，可找到最短路徑，提升搜索效率。加權A*（The weighted A-Star）算法[9]通過尋找權重和啟發(fā)項之間的關系選擇最佳權重值，后來又提出了基于起點和目標點之間長度與權重集相關聯(lián)的動態(tài)加權方法，還有基于路徑成本與權重集相關聯(lián)的方法，這些方法雖能提升尋路效率，但結果不穩(wěn)定，不能很好地平衡路徑代價與搜索時間之間的關系。Theta*[10]是一種任意角度的尋路算法，可以找到比A*更短的路徑，它在A*基礎上增加了結點之間的可見性檢測，每擴展一個結點都要先計算該結點與相關結點是否可見，因而增加了計算量，比A*耗時更長。松弛A*（Relaxed A-Star）算法[11]用到達結點n的第一條路徑成本來近似從起點到n的所有路徑成本，即g(n)只被計算一次，省略了原A*算法多次比較g(n)、更新g(n)的步驟，因此搜索時間縮短，但不能保證找到最優(yōu)解。Harabor D等人于2011年提出了跳點搜索（Jump Point Search, JPS）算法[12-15]，該算法改進了A*的無差別擴展規(guī)則，只擴展有代表性的特殊柵格點，從而大大降低了結點訪問量，使算法效率得到大幅提升。以上算法僅在A*算法的規(guī)則層面進行了改進，并未考慮到障礙物在地圖中的整體分布情況，仍然處于盲目搜索階段。為了降低搜索過程的盲目性，本文提出了一種障礙物登陸點檢測方法，利用障礙物分布信息尋找登陸點，將結點擴展范圍縮小至障礙物邊緣區(qū)域，從而減少結點訪問數量，再利用改進的Dijkstra算法搜索最短路徑，進一步提高路徑規(guī)劃效率。

1 障礙物登陸點檢測算法實現(xiàn)

1.1 障礙物登陸點檢測算法主要思想

在地圖中規(guī)劃最短路徑，路徑轉折點一般位于障礙物的邊緣區(qū)域，如能夠檢測出起點至終點所有可能經過的障礙物邊緣區(qū)域，不僅覆蓋了最短路徑的拐點，而且極大縮小了路徑規(guī)劃范圍，可減少冗余結點數量，提高算法效率。本文首先引入一個概念，在柵格地圖中相鄰的障礙柵格點組成一個整體區(qū)域，稱為障礙物塊[16]。當起點、終點連線被障礙物塊阻擋時，最短路徑一定經過該障礙物塊的某一邊緣點附近，本文將這一邊緣點定義為登陸點，該點由起點、終點的連線與相交障礙物塊的位置關系共同決定。

定義1：如圖1所示，連接起點S與終點E，直線SE與障礙物塊B1碰撞于A點，以起點S作為定點，將A點向障礙物塊 B1的兩側移動，當角度θ1與θ2最大時，對應柵格點O1L與O1R即為障礙物塊 B1的左、右登陸點。

圖1 登陸點分布圖Fig.1 Distribution diagram of landing nodes

圖1 中，檢測到點O1L與O1R是障礙物塊B1的登陸點，再將O1L作為起點，E作為終點，直線O1LE與障礙物塊 B2發(fā)生碰撞，通過計算θ3與θ4的最大值可得到障礙物塊 B2的登陸點是O2L與O2R，再將O1R作為起點，E作為終點，直線O1RE與障礙物塊 B3發(fā)生碰撞，通過計算θ5與θ6的最大值可檢測到障礙物塊B3的登陸點是O3L與O3R，如此可計算出相關障礙物塊的登陸點O1L、O1R、O2L、O2R、O3L、O3R，使用遞歸算法檢測所有障礙物登陸點，流程圖如圖2所示。

圖2 登陸點遞歸檢測流程圖Fig.2 Flow chart of landing nodes recursive detection

1.2 構建障礙物塊

遍歷地圖中所有柵格點，將上下左右相鄰的障礙物柵格標記到同一個集合中，直到完成所有障礙物柵格的分類，最終形成多個障礙物塊，如圖3所示。具體操作步驟如下：

圖3 障礙物塊示意圖Fig.3 Diagram of obstacle blocks

Step1:建立柵格地圖對應的二維數組G、存放待檢測結點列表 Lobstacle、當前障礙物塊 Bobstacle以及障礙物塊列表LOB；

Step2:檢測當前柵格點，如果是障礙物，則將當前柵格點存入列表 Lobstacle中，并將當前柵格點在數組G的相應位置寫入true；

Step3:檢測列表 Lobstacle，如 Lobstacle為空，進入Step4，否則檢測 Lobstacle中第一個結點的8鄰域柵格點，如檢測到障礙物且該障礙物在數組G中的對應值為false，則將該障礙物插入列表 Lobstacle中，并將數組G的對應位設置為 true，同時將 Lobstacle的第一個結點加入Bobstacle中，然后刪除 Lobstacle中第一個結點，循環(huán)執(zhí)行Step3；

Step4:將 Bobstacle插入列表LOB，如數組G中所有值均為true，則程序結束，否則進入Step2。

1.3 障礙物碰撞檢測

如何快速檢測直線與障礙物的碰撞點是尋找登陸點的第一步。可利用起點與終點坐標，依據兩點式直線方程實現(xiàn)碰撞檢測。首先通過起點與終點坐標建立直線方程，然后從起點開始向終點進行碰撞檢測。設起點為 D1，終點為 D2，移動步長為λ，在直線D1D2上由起點向終點方向移動檢測，檢測直線上的點( xn, yn)是否位于障礙物柵格內，如位于障礙物柵格內部，則發(fā)生碰撞，如移動到終點 D2仍未發(fā)現(xiàn)障礙物柵格，則稱點 D1與 D2可直達。如圖4所示，柵格坐標即為柵格中心點，其中黑色柵格(3,3)為障礙物，從起點 D1(1,0)（黃色柵格）到終點 D2(5,5)（紅色柵格）檢測障礙物，設置步長λ=0.1，利用直線D1D2方程計算對應柵格坐標如式(1)所示：

圖4 障礙物碰撞檢測Fig.4 Obstacle collisiontest

其中，D1x為點 D1的X軸坐標，D1y為點 D1的Y軸坐標，D2x為點 D2的X軸坐標，D2y為點 D2的Y軸坐標。當 xi? [3,3.5]時，可計算出 yi? [2.5,3]，即直線D1D2上的點(xi,yi)位于障礙物柵格(3,3)區(qū)域內，因而檢測到直線D1D2與障礙物柵格(3,3)發(fā)生碰撞，依據此方法可快速判斷出地圖中的任意兩點是否可直達。

1.4 計算障礙物映射角

圖5 中，連接起點S與終點E，則直線SE與灰色障礙物塊發(fā)生碰撞，然后分別連接起點與障礙物塊所有可直達邊緣點。設向量與的夾角為θ，由點積公式可知，且

圖5 障礙物映射角Fig.5 Mapping angles of obstacles

定義2：以SE為基準線，S為支點，分別向障礙物塊的兩側移動，形成夾角∠iOSE即為障礙物映射角。

隨著地圖中障礙物數量的增加，需要計算映射角的障礙柵格點變得越來越多，使得算法計算量增大，效率降低。為了避免出現(xiàn)這種問題，本文依據起點S與障礙物塊的位置關系僅選取某一方向上的邊緣障礙物柵格點，可大幅降低待計算結點數量。以圖5為例，起點位于障礙物塊的X軸負方向以及Y軸正方向，因而只選取障礙物塊X負方向的邊緣點與Y正方向的邊緣點即可，則待計算的障礙柵格點為 O1、O2、 O3、 O4、 O5、 O6、 O7、 O8、 O9、O10，在此方向上計算障礙物映射角，可有效節(jié)約系統(tǒng)資源，保證算法的高效性。具體操作步驟如下：

Step1：檢測起點S與障礙物塊的相對位置，并選取此方向上的障礙物塊邊緣柵格點組成集合G，計算G中所有柵格點的映射角；

Step2：依據障礙物邊緣柵格點與起點、終點連線的左右位置關系，將G中所有柵格點分為兩類，記作集合 GL與GR；

Step3：若 GL中最大的障礙物映射角唯一，則對應柵格為左登陸點，若 GR中最大的障礙物映射角唯一，則對應柵格為右登陸點，程序結束；若不唯一，進入下一步；

Step4：刪除 GL與GR中非最大映射角的柵格點，分別計算 GL與GR中所有柵格點到起點S的直線距離，距離最短者即為登陸點。

1.5 擴展登陸點

無障礙物阻擋時，直線是兩點之間最短路徑；受到障礙物阻擋時，最短路徑轉折點一定位于障礙物邊緣附近，如將登陸點向鄰近的非障礙柵格擴展，則能夠覆蓋最短路徑轉折點。以8鄰域為例，在每一個登陸點8鄰域內進行擴展，找到所有自由柵格點，作為路徑規(guī)劃的搜索空間。

如圖6所示，柵格點O是障礙物塊BLOCK的一個登陸點，在其8鄰域范圍內進行擴展，擴展出7個自由柵格點OE1、OE2、OE3、OE4、OE5、OE6、OE7。依此類推，對地圖中所有登陸點進行擴展，擴展出的柵格點組成了路徑規(guī)劃搜索空間，從而實現(xiàn)地圖規(guī)模的有效縮減，然后利用搜索空間中的柵格點構建賦權圖，以圖7為例，賦權圖由起點、終點以及所有黃色結點與紅色邊組成。

圖6 8鄰域登陸點擴展Fig.6 Landing node expansion in 8 neighborhoods

圖7 非負賦權圖Fig.7 Non-negative graph with weight

2 改進Dijkstra算法

Dijkstra是一種很有代表性的最短路徑算法，它是計算機科學家Edsger W. Dijkstra于1956年提出的，可用于搜索非負賦權圖的單源最短路徑。Dijkstra算法要求每條邊都有一個非負的代價值，算法從起點開始，遍歷圖中每一個結點，它從未遍歷的結點集中選取與當前結點最近的結點進行擴展，當搜索到目標結點時結束，該算法邏輯簡單，搜索速度快，常作為其他圖算法的子模塊使用，圖8是Dijkstra算法流程圖。

圖8 Dijkstra算法流程圖Fig.8 Flow chart of Dijkstra algorithm

其中， pi表示從起點S到結點i的最短路徑中，i點的前一個結點；dt表示從起點S到結點t的最短路徑長度，Count是初始值為1的計數器，VNUM為圖的頂點數量，w(t,m)表示從結點t到結點m的路徑長度。

Dijkstra從起點出發(fā)，搜索最近結點并逐步向外擴展，在機器人路徑規(guī)劃問題中，終點往往是距離起點最遠的結點，當結點規(guī)模較大時，利用Dijkstra算法尋找最短路徑效率太低，于是結合賦權圖結構特點對Dijkstra算法進行改進，進一步提高本文算法的路徑規(guī)劃效率。為更好地展示賦權圖特點，將圖7轉換為圖9，由于圖9中結點為平面坐標點，各結點間代價均為坐標點的直線距離，因而符合“三角形兩邊之和大于第三邊”定理，例如 SN1＜ SN15+ N15N1，則圖中結點N15非最短路徑覆蓋點；同理，N3N10＜ N3N25+ N25N10，N3N10＜ N3N16+ N16N10，則結點N25與N16非最短路徑覆蓋點。以此類推，圖9中只有結點 N1、N2、N3、N4、N5、N6、N7、N8、N9、N10、N11、N12可能位于最短路徑之上，其余結點與最短路徑無關且會增加算法計算量，于是對Dijkstra算法進行改進，詳細描述如下：

圖9 非負賦權圖結構Fig.9 The structure of non-negative graph with weight

Step1：初始化賦權圖結點集合U ={S, N1, N2...Nn-2,G}，集合T=? ，V=? ，W = {q|q ∈ U 且q? V}，結點P=S；

Step2：利用碰撞檢測法檢測結點P與W中每個結點是否可直達，如可直達，則將W中對應結點插入集合T；

Step3：比較T中每個結點的障礙物映射角，如小于對應登陸點映射角，則從T中刪除該結點，同時刪除U中該結點；如大于或等于登陸點映射角，則保留；

Step4：計算結點P到集合T中所有結點之間的代價

St ep5：計算dS,Tk=dP,Tk=|P-Tk|，同時將P結點加入集合V進行標記；，令P=Tk，T=?。如P是終點G，則算法結束；否則進入Step2。

其中：Tk是集合T中第k個結點，dP,Tk是結點P到Tk的直線距離，n為賦權圖中所有結點的個數。

3 實驗分析

通過對比實驗觀察不同算法的路徑規(guī)劃效果。在Windows 10平臺上利用C#語言開發(fā)一套算法實驗平臺，如圖10所示。

圖10 算法實驗平臺Fig.10 Algorithms experiment platform

以路徑長度和運行時間作為評價指標，分別在四組100×100的柵格地圖中進行實驗，實驗環(huán)境選取隨機障礙物分布地圖2組、辦公室走廊區(qū)域地圖1組以及實驗室內部地圖1組，通過與傳統(tǒng)A*算法以及JPS算法進行對比，分析本文算法的優(yōu)劣。各算法實驗效果如圖11所示，其中黑色圓形結點為起點，黑色三角形結點為終點，黑色柵格為障礙物，淺綠色柵格為A*或JPS算法的擴展結點，紅色柵格為本文算法的擴展結點，紅色折線為最優(yōu)路徑。將三種算法的路徑規(guī)劃數據匯總到表1可知，A*算法與JPS算法路徑規(guī)劃距離相同，而本文算法規(guī)劃距離略短，表2對路徑長度進行了對比，顯示本文算法比A*算法的路徑長度縮短了大約2~5個百分點，這是由于本文算法將擴展結點的連線作為最優(yōu)路徑的一部分，打破了傳統(tǒng)N鄰域擴展的角度限制，可規(guī)劃任意角度，因而規(guī)劃路徑的轉折次數更少，路徑更短。表3對三種算法的運行時間進行了比較，可以看到新算法的搜索效率比傳統(tǒng)A*算法有了大幅提升，耗時約為傳統(tǒng)A*算法的5%~25%，效率提升幅度隨著擴展結點數量的變化而變化。同時，本文算法與JPS算法相比，路徑規(guī)劃效率也有提升，在地圖M1、M2和M4中，兩種算法的擴展結點數量相當，耗時比值約為94%~100%，算法效率提升不明顯，而在地圖M3中，辦公室走廊區(qū)域的障礙物分布集中，所有障礙柵格點共同組成了一個障礙物塊，使得本文算法擴展結點數量較少，而JPS算法在障礙物拐點處擴展結點較多，計算冗余結點耗費了更多時間，因而本文算法耗時僅為JPS算法的27.02%，優(yōu)勢明顯。通過以上數據可知，本文算法更適用于障礙柵格點集中或障礙物塊數量較少的大規(guī)模地圖中。

表1 三種算法運行結果Tab.1 Running results of three algorithms

表2 路徑長度對比Tab.2 Comparison of path length

表3 運行時間對比Tab.3 Comparison of running time

圖11 路徑規(guī)劃結果Fig.11 Results of path planning

4 結 論

在移動機器人路徑規(guī)劃過程中，計算時間受到地圖規(guī)模的影響較大，實時性問題突出。為解決這一問題，本文提出了一種基于障礙物登陸點檢測的全局路徑規(guī)劃算法，利用障礙物在地圖中的整體分布信息，通過計算映射角尋找障礙物登陸點，鎖定相關障礙物的邊緣區(qū)域，有效縮小地圖搜索范圍，再通過改進Dijkstra算法優(yōu)化最短路徑搜索過程。最后，以A*算法與JPS算法作為參照進行實驗，實驗結果表明，本文所提算法可有效提高路徑規(guī)劃效率，尤其是在障礙物比較集中或障礙物塊數量較少的大規(guī)模地圖中，能更好地解決實時性問題。但機器人不是一個質點，運行路線還需考慮其實際尺寸，下一步還要對路徑安全性做進一步的優(yōu)化。