面向方形節點拓撲地圖下的移動機器人路徑規劃算法研究

，，

(哈爾濱工業大學機器人技術與系統國家重點實驗室，哈爾濱 150001)

Research on Path Planning Algorithm for Mobile RobotBased on Square Nodes in Topological Map

LI Minglei，ZHAO Jie，LI Ge

(State Key Laboratory of Robotics and System，Harbin Institute of Technology，Harbin 150001，China)

面向方形節點拓撲地圖下的移動機器人路徑規劃算法研究

李明磊，趙杰，李戈

(哈爾濱工業大學機器人技術與系統國家重點實驗室，哈爾濱 150001)

ResearchonPathPlanningAlgorithmforMobileRobotBasedonSquareNodesinTopologicalMap

LIMinglei，ZHAOJie，LIGe

(StateKeyLaboratoryofRoboticsandSystem，HarbinInstituteofTechnology，Harbin150001，China)

摘要：針對方形節點拓撲地圖下的移動機器人的特性，采用了A*算法來實現路徑規劃，并對傳統的A*算法進行改進，一是在啟發函數中引入了位移和角度2個因素，提高了函數的啟發性；二是引入堆的方法優化了數據結構，提高了列表中代價最小節點的搜索速度。仿真實驗結果表明，改進后的A*算法節點的最短路徑節點相對減少，算法效率明顯提高，具有良好的可行性和有效性。

關鍵詞：移動機器人；拓撲地圖；軌跡規劃；A*算法

中圖分類號：TP301.6

文獻標識碼：A

文章編號：1001-2257(2015)10-0067-04

收稿日期：2015-05-28

Abstract：In order to achieve the goal，according to the characteristics of the mobile robot based on square nodes in topological map，the paper improved the A* algorithm as follow. First，the new heuristic function took consideration of two factors：Distance and angle，improved the heuristic ability of A*；Then，the slowest part of the A* pathfinding algorithm is finding the node in the list with the lowest F score，the paper used the binary heaps to make it faster. Simulation results showed that the improved A* decreased the number of result nodes and took less time，demonstrated the feasibility and validity of the algorithm.

作者簡介：李明磊(1990-)，男，山東濰坊人，碩士研究生，研究方向為機器人技術。

Keywords：mobilerobot；topologicalmap；pathplanning；A* algorithm

0引言

在移動機器人技術相關的研究中，路徑規劃算法是一個重要的領域，其核心是按照某一性能指標(如距離、時間等)，快速高效地在有障礙的工作環境中找到兩個或者多個節點之間的最優或次優路徑。現有的路徑規劃算法有很多，例如Dijkstra算法、遺傳算法等。Dijkstra算法不考慮目標信息，需要遍歷計算所有節點，雖然可以找到最短路徑，但求解時間長，效率非常低。遺傳算法也存在求解效率低，難以解決大計算量，容易陷入“早熟”等問題。

A*算法是一種典型的啟發式搜索算法，一般適用于環境信息已知的全局路徑規劃中。該算法的核心在于引入了估價函數，從而避免了大量的無效搜索，提高了算法的效率。針對方形節點拓撲地圖下移動機器人的特性，對A*算法進行了改進，在啟發函數中引入了位移和角度2個參數，讓規劃軌跡更加合理；另外，還采用堆排序的方法優化了算法的數據結構，提高了算法的搜索效率。

1問題描述

方形節點拓撲地圖下移動機器人的工作環境可以看作是二維平面上以正方形或者三角形方式陣列分布的圓形管孔，在環境中存在固定管、堵管等映射成不能通過的障礙區域。記移動機器人c在任意形狀的二維平面區域a上運動，在區域U上隨機的分布著不可達的有限數量的障礙物O{o1，o2，…，on}。為了便于規劃，將任意形狀的a區域補全為最小覆蓋規整長方形區域，記為b，將補全的區域設置為障礙區域。

如圖1所示，將區域b進行柵格化處理，將柵格地圖映射到平面坐標系xOy上，定義原點O位于左上角，左上角的第1個柵格的坐標為(1，1)，記柵格地圖的行列分別為m，n。記p為任意柵格，則柵格序號集{M，N}={(m1，n1)…(mi，nj)} 與P的坐標集{X，Y}={(x1，y1)…(xi，yj)}構成映射關系。

圖1　柵格化的拓撲環境

柵格地圖上的軌跡規劃可以描述為在區域b上移動機器人c從既定起點s找到一條最優或者次優路徑安全無碰撞的到達目標點t。

2基于傳統A*算法的路徑規劃

2.1　算法的基本原理和符號定義

傳統的A*算法的基本原理和Dijkstra算法相似，也是通過試探某個節點到目標節點的所有可能路徑，最后找到最優解，但是由于A*算法引入了估價函數來估價每一步的代價，從而能更好的找到合適的搜索方向。A*算法是一種可采納的最好優先算法，也就是說，如果待解決的問題存在最優解且引入的估價函數滿足相容性條件，那么利用該算法就一定能夠找到最優解。A*算法的估價函數可表示為：

(1)

g′(n)是起始點n到當前節點的路徑代價；h′(n)是節點n到目標最低耗散路徑的估計耗散值。節點之間距離的計算方式有很多種，采用了曼哈頓(Manhattan)距離為：

(2)

此外，還需要注意h′(n)啟發函數的信息性。h′(n)的信息性通俗點說其實就是在估計一個節點的值時的約束條件，如果信息越多或約束條件越多則排除的節點就越多，啟發函數越好或說這個算法越好。但在工程開發中由于實時性的問題，h′(n)的信息越多，它的計算量就越大，耗費的時間就越多，就必須適當的減小h′(n)的信息，即減小約束條件，從而導致算法的準確性就差了，這里就有一個平衡的問題。對A*算法具體符號定義如表1所示。

表1符號定義

符號意義p拓展過程中柵格地圖上的節點n完成拓展的某一節點Ω節點p的八鄰域節點構成的狀態空間g(n)起點s到節點n的實際路徑代價h(n)啟發函數,n到目標節點t的估計代價f(n)估價函數,g(n)+h(n)Exp節點拓展函數Add節點插入函數Openlist待拓展節點集合Closelist已經拓展過的節點集合

2.2　算法流程圖

基于A*算法，給出移動機器人軌跡規劃流程如圖2所示。通過計算可以得到規劃路徑Path。

圖2　A*算法路徑規劃流程

3改進的A*算法

3.1　A*算法的估價函數

A*算法作為一種啟發式搜索算法，其核心就在于設計一個合適的啟發函數。圖3為拓撲地圖下移動機器人的二維示意圖，機器人共包括基座、足部和腳趾。在其工作的過程中，基座和足部可以進行平動和轉動，二者都需要耗費時間，因此在進行軌跡規劃的過程中，不僅要考慮移動，還要考慮轉動因素。

在啟發函數的構造中引入了位移和角度2個要素，即

(3)

L為當前節點和目標點的曼哈頓距離；α是起點到當前節點的向量和當前點到目標點向量的夾角。w1，w2分別是位移和角度的加權值，其取值范圍分別是0.55～0.65和0.35～0.45。具體加權值要根據地圖類型和機器人參數進行調整。

(4)

n為可拓展節點數量。

通過歸一化處理后，啟發函數可以表示為：

(5)

(6)

通過這種方式，可以解決位移容易產生壓倒性作用的問題，二因素具體加權值還要根據具體工程要求進行調整。

3.2　利用二叉堆優化Openlist列表

A*算法中，最耗時的部分就是如何從Openlist中查找到估計函數值最小的節點，其影響程度隨著節點數的增多越來越嚴重。在一般的A*算法中為了優化搜索時間，一般將節點排序存儲，例如冒泡法，在搜索過程中需要遍歷整個Openlist，其時間復雜度是O(n2)，整個搜索速度比較慢。

為了優化Openlist中最小F值的查找速度，采用二叉堆來優化數據結構。A*算法并不需要整個Openlist完全有序，只需要能夠找到整個列表中F值最小的即可，這也為我們使用二叉堆提供了條件。

二叉堆法分為最小二叉堆和最大二叉堆2種。采用的是最小二叉堆，其父節點的值總是小于其子節點，整個數列中最小的值在堆穩定后總是在堆的最頂端。用一維數組來表示二叉堆時，編號為n(n>=1) 的節點，其子節點的編號為2n和2n+1，其父節點的編號為n/2，具體如圖4所示。

圖4　最小二叉堆數組

顯然堆的第1個節點就是F值最小的點，從Openlist中取出堆頂節點后，為了使堆結構保持穩定，需要將最后一個節點移動到頂點，并和其子節點比較，如果父節點比子節點大，就交換二者位置，直到父節點比倆個子節點都小。當向堆中增加節點時，將新節點添加到堆的最后，然后與父節點比較，按照最小堆的特性進行比較，直到新節點大于其父節點或到達堆頂點。

使用最小二叉堆的方式從Openlist中找到F值最小的節點，算法的時間復雜度就可以近似地認為是O(logn)，從理論分析，可以有效地優化搜索效率。

4試驗及分析

實驗環境為：CPU Intel i5 3230，內存8 G，編譯環境為Eclipse 3.2，對不同規模的障礙孔隨機分布的柵格地圖進行軌跡規劃，并給出試驗結果。

為了比較傳統A*算法和改進后A*算法的效率，設計了2種類型的試驗方案。

試驗1。增大搜索空間，柵格地圖采用16×16(如圖5)，32×32……。具體試驗結果如表2所示。

圖5地圖16×16

試驗2。在相同的搜索空間下，改變障礙孔的復雜程度。采用改進后的A*算法，搜索空間為128×128，具體實驗結果如表3所示。

表2不同規模柵格地圖算法性能參數比較

柵格規格16×1632×3264×64128×128256×256拓展節點數A*2680177282695A*改381333465911053搜索時間/sA*0.0480.0640.0920.1770.548A*改0.0060.0160.0320.0820.169最短路徑節點數A*2862135268598A*改2453116241531

表3不同復雜度算法性能比較

障礙孔復雜度拓展節點數搜索時間/s最短路徑節點數10%4190.08416320%4680.09618233%5190.10523150%6110.104310

①改進A*算法極大的提高了搜索的效率，如表2所示，改進后A*算法比傳統A*算法搜索時間大大減少，并且隨著搜索空間的增大，效果越來越明顯，如圖6所示；②改進A*算法由于改進了啟發函數，可以通過更短的節點路徑找到目標點；③改進A*算法在搜索過程中拓展的節點數明顯增多，需要占用更多的系統空間，但在可接受范圍內；④當障礙孔復雜度改變時，算法的拓展節點和最短路徑節點數都有所增加，搜索時間的變化趨勢不太明顯并且在多次試驗過程中都能找到解，說明在當前環境中，當障礙孔隨機分布時對搜索效率影響不大。

圖6　算法效率對比

5結束語

針對拓撲地圖下移動機器人的軌跡規劃，在傳統A*算法的基礎上，啟發函數中引入了位移和角度2個因素，增加了函數的啟發性，并對存儲數據結構利用堆原理進行了改進，提高了算法的搜索效率。仿真試驗結果證明了改進后算法的合理性和有效性，搜索效率和最短路徑都得到了優化。

參考文獻：

蔡自興，謝斌.機器人學.北京：清華大學出版社，2015.

Anthony Stentz. A real-time resolution optimal re-planner for globally constrained problems// The 18th National Conf on Artificial Intelligence. Cambridge，MA：MIT Press，2002：1088-1096.

Latombe J C. Robot motion planning . Kluwer Academic Publishing，Norwell，MA，1991.

Begum M，Mann G K I，Gosine R G. Integrated fuzzy logic and genetic algorithmic approach for simultaneous localization and mapping of mobile robots . Applied Soft Computing，2008，8(1)：150-165.

Stuart Russell，Peter Norvig. Artificial Intelligence：A Modern Approach . Pearson，3 edition，2009.

Hans W Guesgen，Debasis Mitra. A multiple-platform decentralized route finding system .IEA/AIE99，LNAI 1611，2001，707-713.

Andre LaMotte. Windows游戲編程大師技巧.北京：人民郵電出版社，2004.

Taeg-Keun Whangbo.Efficient Bidirectional A*algorithm for optimal route-finding . IEA/AIE2007，LNAI 4570，344-353，2007.

Thomas H Cormen，Charles E Leiserson，Ronald L.算法導論 .北京：機械工業出版社，2013.