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

，，

(哈爾濱工業(yè)大學機器人技術與系統(tǒng)國家重點實驗室，哈爾濱 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)

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

李明磊，趙杰，李戈

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

ResearchonPathPlanningAlgorithmforMobileRobotBasedonSquareNodesinTopologicalMap

LIMinglei，ZHAOJie，LIGe

(StateKeyLaboratoryofRoboticsandSystem，HarbinInstituteofTechnology，Harbin150001，China)

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

關鍵詞：移動機器人；拓撲地圖；軌跡規(guī)劃；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引言

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

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

1問題描述

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

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

圖1　柵格化的拓撲環(huán)境

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

2基于傳統(tǒng)A*算法的路徑規(guī)劃

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

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

(1)

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

(2)

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

表1符號定義

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

2.2　算法流程圖

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

圖2　A*算法路徑規(guī)劃流程

3改進的A*算法

3.1　A*算法的估價函數(shù)

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

在啟發(fā)函數(shù)的構造中引入了位移和角度2個要素，即

(3)

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

(4)

n為可拓展節(jié)點數(shù)量。

通過歸一化處理后，啟發(fā)函數(shù)可以表示為：

(5)

(6)

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

3.2　利用二叉堆優(yōu)化Openlist列表

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

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

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

圖4　最小二叉堆數(shù)組

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

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

4試驗及分析

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

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

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

圖5地圖16×16

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

表2不同規(guī)模柵格地圖算法性能參數(shù)比較

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

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

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

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

圖6　算法效率對比

5結束語

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

參考文獻：

蔡自興，謝斌.機器人學.北京：清華大學出版社，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.算法導論 .北京：機械工業(yè)出版社，2013.