基于可視圖與A*算法的路徑規(guī)劃

黎 萍，朱軍燕，彭 芳，楊 亮

?

基于可視圖與A*算法的路徑規(guī)劃

黎 萍1，朱軍燕2，彭 芳1，楊 亮1

(1. 電子科技大學中山學院，廣東 中山 528402；2. 中山出入境檢驗檢疫局技術中心，廣東 中山 528403)

結合可視圖的骨架構造方法和A*圖搜索方法，采用矩形包絡障礙物，在障礙物頂點外延生成路徑點。在此基礎上，提出一種新的路徑規(guī)劃算法Lambda*，與A*算法類似，搜索過程需要2張表，但CLOSED表保存從起始節(jié)點開始的路徑節(jié)點，OPEN表保存CLOSED表中擴展節(jié)點的后續(xù)節(jié)點，可減少在OPEN表中保存的節(jié)點數(shù)量，減少計算量和耗時，并通過增加SMOOTH過程以提高路徑的平滑度。將算法應用于二維空間環(huán)境進行機器人路徑規(guī)劃仿真實驗，結果表明，與A*算法相比，Lambda*算法能夠以增加較少路徑長度為前提，大幅降低路徑規(guī)劃的耗時。

路徑規(guī)劃；可視圖；A*算法；Lambda*算法；路徑平滑

1 概述

路徑規(guī)劃作為很多領域的關鍵技術已經(jīng)成為當前的研究熱點之一[1]，它是一個帶約束的復雜優(yōu)化問題，其任務是在具有障礙物的環(huán)境內按照一定的評價標準尋找一條從起始狀態(tài)到達目標狀態(tài)的無碰路徑。

基于自由空間構造的規(guī)劃方法是一類重要的機器人路徑規(guī)劃方法，它們的工作原理基本上都是先通過幾何方法構造自由空間的骨架圖，然后利用圖搜索算法搜索一條可行的最短路徑[2]。常用的適合低維狀態(tài)空間的骨架圖有可視圖[3]、Voronoi圖[4]、柵格圖分解[5]以及切線圖[6]等，這些骨架圖不適用于高姿態(tài)空間，但具有較好的完備性[1]。常用的圖搜索方法包括深度優(yōu)先和廣度優(yōu)先、Dijkstra算法[7]、A*算法[7]和D*算法[8]。

經(jīng)典啟發(fā)式搜索式算法——A*算法是20世紀60年代提出的，在相關領域得到了廣泛的應用。但該算法只能用在格子環(huán)境中，在一定程度上限制了其進一步的推廣，算法復雜度和發(fā)現(xiàn)路徑的能力受柵格細化程度的影響，且環(huán)境信息存儲量大。文獻[9]提出了一種A*算法的變種—— Theta*算法，突破了格子的限制，允許以任意角度改變路徑的方向，但依然采用柵格地圖描述環(huán)境。

本文結合可視圖的骨架構造方法和A*圖搜索方法，采用矩形包絡障礙物的碰撞檢測方法，為簡化路徑優(yōu)化，在障礙物頂點外延生成路徑點。在此基礎上，提出一種新的路徑規(guī)劃算法Lambda*，并將算法應用于二維環(huán)境空間進行機器人路徑優(yōu)化。

2 算法描述

2.1 碰撞檢測

為了解決機器人與障礙物之間的碰撞檢測問題，本文采用一種矩形包絡碰撞檢測方法。障礙物一般具有不規(guī)則幾何形狀[10]，采用矩形對二維空間的障礙物包絡進行簡化，如圖1所示。這種建模方法雖然在一定程度上擴大了障礙域，但使得障礙域大大簡化，提高了規(guī)劃的效率，使得避碰路徑更具安全性[11]。

圖1 障礙物的矩形包絡幾何模型

在簡化障礙物的基礎上，障礙物與機器人之間的碰撞檢測就轉化為機器人路徑(表現(xiàn)為線)與障礙物(表現(xiàn)為矩形)的位置關系問題，當線與矩形的任意一條邊線段相交則發(fā)生碰撞，反之不發(fā)生碰撞。在檢測碰撞時，僅需分別計算路徑點之間連線與障礙物矩形4條邊之間的位置關系，就可以判斷機器人是否與障礙物發(fā)生碰撞。

2.2 路徑點的生成

可視圖法視移動機器人為一點，將機器人、目標點和障礙物的各頂點進行組合連接，并保證這些直線均不與障礙物相交，這就形成了一張圖，稱為可視圖[1-3]。由于任意兩直線的頂點都是可見的，從起點沿著這些直線到達目標點的所有路徑均是運動物體的無碰路徑。搜索最優(yōu)路徑的問題就轉化為從起點經(jīng)過這些可視直線到目標點的最短距離問題。

在可視圖中，機器人以所有障礙物的頂點為中間路徑點。本文采用矩形代替任意形狀的多邊形包絡障礙物，為保證路徑的安全性，在矩形包絡的對角線上外延一定的距離產(chǎn)生路徑點，如圖2所示，若將外延距離設為0，則退化為一般可視圖。

圖2 路徑點

2.3 Lambda*算法

Lambda*算法與A*算法類似，在搜索過程中需設置 2張表：OPEN表和CLOSED表。在A*算法中，OPEN表保存所有已生成而未考察的節(jié)點，CLOSED表中記錄已訪問過的節(jié)點；而在Lambda*算法中，CLOSED表保存獲取的從起始節(jié)點開始的路徑節(jié)點，OPEN表保存CLOSED表中擴展節(jié)點的后續(xù)節(jié)點。假設是起點、終點和所有插入的路徑點的集合，從幾何路徑點構造雙向鏈表的節(jié)點單元，節(jié)點的數(shù)據(jù)結構為：

classdef Node

point //幾何路徑點

value //節(jié)點的值

Prev //該節(jié)點的前驅節(jié)點

end

end

其中，表示從起點到節(jié)點的路徑長度；start表示起始節(jié)點；goal表示終點；所有節(jié)點創(chuàng)建時僅有point特性，其value特性值設為0。Lambda*算法的估價函數(shù)為：(P)=(P)+(P)，與A*算法估價函數(shù)的選取一致，(P)表示從起始節(jié)點start到節(jié)點P的路徑長度；(P)為啟發(fā)函數(shù)，Lambda*算法的(P)采用從節(jié)點P到終點goal之間的直線距離，節(jié)點P距離goal越近，(P)越小，以此引導搜索的方向，使得機器人逐步靠近目標。Lambda*算法如圖3所示。

圖3 Lambda*算法流程

首先創(chuàng)建2張空的雙向鏈表OPEN和CLOSED，將起始節(jié)點start插入CLOSED鏈表，若終點不在CLOSED鏈表中，則清空OPEN鏈表，從CLOSED中選取節(jié)點進行擴展，將擴展節(jié)點的后續(xù)節(jié)點加入OPEN鏈表。計算OPEN表中所有節(jié)點的估價值，搜索具有最小估價值的節(jié)點，將其插入CLOSED鏈表，并修改插入節(jié)點的value值；若終點在OPEN表中，則直接將其插入CLOSED，省去計算估價值和比較估價值的步驟，節(jié)省路徑規(guī)劃時間。擴展OPEN鏈表時，遍歷中幾何點構造的所有節(jié)點，若節(jié)點P不在CLOSED表中且與CLOSED表的擴展節(jié)點close可視(之間沒有障礙物)，則將其作為一個后續(xù)節(jié)點。

Lambda*算法與A*算法的主要區(qū)別是在Lambda*算法中，OPEN表僅保存當前擴展節(jié)點的后續(xù)節(jié)點，不保存已擴展節(jié)點的其他后續(xù)節(jié)點，這大大減少了OPEN表保存的節(jié)點數(shù)量，減少了計算量和時耗。

CLOSED中保存的是一條從起點到終點的無碰路徑，受啟發(fā)函數(shù)的限制，獲取的路徑不保證為最短路徑，如圖4所示。對圖4(a)采用SMOOTH過程后，能進一步優(yōu)化路 徑，獲得如圖4(b)所示的最優(yōu)路徑，增加路徑的光滑度，從而提高達到目標點的效率。因此，在獲得這樣一條路徑后，Lambda*算法通過SMOOTH過程對CLOSED鏈表表示的路徑進行平滑[12]，SMOOTH過程的流程如圖5所示。

圖4 路徑平滑前后效果對比

圖5 SMOOTH過程流程

3 仿真實驗及結果分析

為了測試Lambda*的性能，將Lambda*和A*算法[13]應用于有障礙物的2D環(huán)境中進行路徑規(guī)劃實驗，在100×100的二維環(huán)境下，起點為(0,0)，終點為(100,100)，隨機生成障礙物，障礙物比例分別設為0%、5%、10%、20%、30%、40%，2種算法均在Matlab 2012a上實現(xiàn)，實驗用計算機CPU型號為Intel(R)Core(TM)2 Duo T6500，主頻均為2.1 GHz，RAM為1.86 GB。本文并未對算法的實現(xiàn)進行優(yōu)化，因此，實驗結果可能還能進一步改進。

統(tǒng)計每種情況下用2種算法分別進行100次路徑規(guī)劃的平均路徑長度、耗時、總節(jié)點數(shù)、擴展節(jié)點數(shù)和OPEN中容納的節(jié)點總數(shù)，結果如表1所示。隨機障礙物比例越大，總節(jié)點數(shù)增多，路徑規(guī)劃也越來越復雜，所得路徑長度和耗時、擴展節(jié)點數(shù)以及OPEN表中的節(jié)點數(shù)都呈增加趨勢，若改變環(huán)境大小，則相同比例下障礙物數(shù)量不同，總節(jié)點數(shù)也隨之改變，本文未對2D環(huán)境不同環(huán)境大小時的路徑規(guī)劃進行實驗，但隨著節(jié)點數(shù)增多，路徑規(guī)劃越復雜。在有障礙物的2D環(huán)境下，障礙物比例為0%~5%的情況下，路徑規(guī)劃較簡單，總節(jié)點比較少，A*和Lambda*所獲得路徑質量相當，90%以上的情況2種算法所獲路徑相同。隨著障礙物比例增大，Lambda*算法所獲路徑質量略低于A*算法，同時Lambda*算法耗時大幅減少。

表1 2D有障礙物環(huán)境下的實驗結果

此外，還統(tǒng)計了采用2種方法所獲路徑的方向變化次數(shù)，采用Lambda*算法獲得的路徑方向平均變化2次，而A*算法獲得的路徑方向平均變化3.143次。實驗結果表明，障礙物越多，路徑越長，算法耗時越多。在相同情況下，Lambda*需擴展的節(jié)點數(shù)和OPEN表中節(jié)點總數(shù)都顯著小于A*的對應值，路徑長度略有增長，所獲路徑質量稍遜于A*算法所得但耗時大幅減少。

4 結束語

本文結合可視圖和A*算法，提出了一種新的路徑規(guī)劃算法Lambda*，采用矩形包絡二維空間的障礙物，為保證路徑的安全性，在障礙物頂點外延生成路徑點。Lambda*算法采用鏈表的數(shù)據(jù)結構來描述路徑，在路徑搜索時，CLOSED表保存獲取的從起始節(jié)點開始的路徑節(jié)點，OPEN表保存CLOSED表中擴展節(jié)點的后續(xù)節(jié)點，對獲取的路徑進行平滑處理，并進一步優(yōu)化，提高路徑的平滑度。

仿真實驗表明，Lambda*算法能夠在增加較少路徑長度的前提下，大大降低路徑規(guī)劃的耗時。然而驗證實驗在二維空間進行，下一步研究需要將算法拓展到三維空間，且進一步探究如何能快速地獲得高質量的路徑。

[1] 朱大奇, 顏明重. 移動機器人路徑規(guī)劃技術綜述[J]. 控制與決策, 2010, 25(7): 961-967.

[2] 成偉明, 唐振民, 趙春霞, 等. 移動機器人路徑規(guī)劃中的圖方法應用綜述[J]. 工程圖學學報, 2008, 29(4): 12-20.

[3] Oommen B, Iyengar S, Rao N S V, et al. Robot Navigation in Unknown Terrains Using Learned Visibility Graphs[J]. IEEE Journal of Robotics and Automation, 1987, 3(6): 672-681.

[4] Choset H, Burdick J. Sensor Based Planning, Part I: The Generalized Voronoi Graph[C]//Proc. of IEEE International Conference on Robotics and Automation. [S. l.]: IEEE Press, 1995: 1643-1648.

[5] Parsons D, Canny J F. A Motion Planner for Multiple Mobile Robots[C]//Proc. of IEEE International Conference on Robotics and Automation. [S. l.]: IEEE Press, 1990: 8-13.

[6] Liu Yunhui, Arimoto S. Computation of the Tangent Graph of Polygonal Obstacles by Moving-line Processing[J]. IEEE Transactions on Robotics and Automation, 1994, 10(6): 823-830.

[7] Papadatos A. Research on Motion Planning of Autonomous Mobile Robot[D]. Monterey, USA: Naval Postgraduate School, 1996.

[8] Stentz A. The Focussed D* Algorithm for Real-time Re- planning[C]//Proc. of the 14th International Joint Conference on Artificial Intelligence. San Francisco, USA: ACM Press, 1995: 1652-1659.

[9] Daniel K, Nash A, Koenig S, et al. Theta*: Any-angle Path Planning on Grids[J]. Journal of Artificial Intelligence Research, 2010, 39(1): 533-579.

[10] 汪首坤, 邸 智, 王軍政, 等. 基于A*改進算法的機械臂避障路徑規(guī)劃[J]. 北京理工大學學報, 2011, 31(11): 1302- 1306.

[11] 賈慶軒, 陳 鋼, 孫漢旭, 等. 基于A*算法的空間機械臂避障路徑規(guī)劃[J]. 機械工程學報, 2010, 46(13): 109-115.

[12] Botea A, Müller M, Schaeffer J. Near Optimal Hierarchical Path-finding[J]. Journal of Game Development, 2004, 1(1): 7-28.

[13]Lozano P T, Wesley M A. An Algorithm for Planning Collision-free Paths Among Polyhedral Obstacles[J]. Communications of the ACM, 1979, 22(10): 560-570.

編輯 顧逸斐

Path Planning Based on Visibility Graph and A*Algorithm

LI Ping1, ZHU Jun-yan2, PENG Fang1, YANG Liang1

(1. Zhongshan Institute, University of Electronic Science and Technology of China, Zhongshan 528402, China; 2. Technical Center, Zhongshan Entry-Exit Inspection and Quarantine, Zhongshan 528403, China)

Inspired by skeleton construction method visibility graph and graph search methods A*, obstacles are enveloped by rectangles, path points are generated as obstacle vertices’ extension, and a new path planning algorithm Lambda* is presented, which needs two lists. But differently, CLOSED list in Lambda* is used to store the path nodes from starting node sequentially, and OPEN list is used to save subsequence nodes for the extending node in CLOSED list. What’s more, SMOOTH process is added to smooth the path saved in CLOSED. For validation verification, Lambda* is used in 2D simulation environment. The simulation results show that Lambda* algorithm’s time consuming is decreased substantially with little increase of path length compared with A* algorithm.

path planning; visibility graph; A* algorithm; Lambda* algorithm; path smoothing

1000-3428(2014)03-0193-03

A

TP24

國家科技型中小企業(yè)技術創(chuàng)新基金資助項目(12C26214405188)；廣東省教育部產(chǎn)學研基金資助項目(2011B090400371)； 電子科技大學中山學院博士啟動基金資助項目(410YKQ01)。

黎 萍(1981－)，女，講師、博士，主研方向：人工智能；朱軍燕，工程師、碩士；彭 芳，副教授、碩士；楊 亮， 講師、碩士。

2013-08-29

2013-10-29 E-mail：lipingjxau@163.com

10.3969/j.issn.1000-3428.2014.03.040