，，

(哈爾濱工業(yè)大學(xué)機(jī)器人技術(shù)與系統(tǒng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，哈爾濱 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é)點(diǎn)拓?fù)涞貓D下的移動(dòng)機(jī)器人路徑規(guī)劃算法研究

李明磊，趙杰，李戈

(哈爾濱工業(yè)大學(xué)機(jī)器人技術(shù)與系統(tǒng)國(guó)家重點(diǎn)實(shí)驗(yàn)室，哈爾濱 150001)

ResearchonPathPlanningAlgorithmforMobileRobotBasedonSquareNodesinTopologicalMap

LIMinglei，ZHAOJie，LIGe

(StateKeyLaboratoryofRoboticsandSystem，HarbinInstituteofTechnology，Harbin150001，China)

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

關(guān)鍵詞：移動(dòng)機(jī)器人；拓?fù)涞貓D；軌跡規(guī)劃；A*算法

中圖分類號(hào)：TP301.6

文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：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.

作者簡(jiǎn)介：李明磊(1990-)，男，山東濰坊人，碩士研究生，研究方向?yàn)闄C(jī)器人技術(shù)。

Keywords：mobilerobot；topologicalmap；pathplanning；A* algorithm

0引言

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

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

1問題描述

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

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

圖1　柵格化的拓?fù)洵h(huán)境

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

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

2.1　算法的基本原理和符號(hào)定義

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

(1)

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

(2)

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

表1符號(hào)定義

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

2.2　算法流程圖

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

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

3改進(jìn)的A*算法

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

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

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

(3)

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

(4)

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

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

(5)

(6)

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

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

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

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

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

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

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

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

4試驗(yàn)及分析

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

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

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

圖5地圖16×16

試驗(yàn)2。在相同的搜索空間下，改變障礙孔的復(fù)雜程度。采用改進(jìn)后的A*算法，搜索空間為128×128，具體實(shí)驗(yàn)結(jié)果如表3所示。

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

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

表3不同復(fù)雜度算法性能比較

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

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

圖6　算法效率對(duì)比

5結(jié)束語(yǔ)

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

參考文獻(xiàn)：

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