Bezier曲線與A*算法融合的移動機器人路徑規(guī)劃*

郭 江，肖宇峰，劉欣雨，陳 麗

(1.西南科技大學 信息工程學院，四川 綿陽 621010；2.西南科技大學 特殊環(huán)境機器人技術(shù)四川省重點實驗室，四川 綿陽 621010)

Bezier曲線與A*算法融合的移動機器人路徑規(guī)劃*

郭 江1,2，肖宇峰1,2，劉欣雨1，陳 麗1

(1.西南科技大學 信息工程學院，四川 綿陽 621010；2.西南科技大學 特殊環(huán)境機器人技術(shù)四川省重點實驗室，四川 綿陽 621010)

移動機器人路徑規(guī)劃一直是移動機器人領域里的重要技術(shù)問題。A*算法在最優(yōu)路徑搜索上有著比較成功的運用，但在柵格環(huán)境下的A*算法也存在著折線多、轉(zhuǎn)折角度大等問題。在考慮移動機器人的實際工作環(huán)境及相關(guān)運動參數(shù)后，這些問題都將大大地影響移動機器人的工作效率。在對以上問題進行分析后提出了一種基于Bezier曲線與A*算法融合的方法來實現(xiàn)移動機器人的路徑規(guī)劃，再通過MATLAB、V-REP仿真工具來實現(xiàn)Bezier_A*融合算法與平滑A*算法及A*算法的對比。通過Bezier_A*融合算法使得機器人在工作中的尋優(yōu)能力、路徑規(guī)劃效率都得到較大的提高。

移動機器人；路徑規(guī)劃；A*算法；Bezier曲線；融合算法

0 引言

移動機器人路徑規(guī)劃問題一直以來都是移動機器人研究的熱點和難點[1]。在近年的發(fā)展中，越來越多的學者和專家已經(jīng)致力于結(jié)合智能控制算法來解決移動機器人的路徑規(guī)劃問題。例如遺傳算法[2]、蟻群算法[3]、禁忌搜索算法[4]等智能算法及其混合形式都被用來解決路徑規(guī)劃問題。但這些智能算法目前還不太完善，存在著一些缺點，例如遺傳算法編碼長度變化范圍大，求解規(guī)模小；Dijkstra算法[5]直接搜索全局而不考慮目標信息，導致路徑求解時間長，難以滿足快速規(guī)劃路徑的需求；D*算法[6]主要解決局部的動態(tài)路徑規(guī)劃問題。

A*算法[7]作為一種比較成功的全局路徑規(guī)劃算法，已成功應用于機器人的路徑尋優(yōu)和規(guī)劃方面，并且取得良好的路徑規(guī)劃效果。但由于A*算法本身的計算特點，在柵格環(huán)境下A*算法規(guī)劃出的移動機器人路徑一般存在著折線多、累計轉(zhuǎn)角大等問題。在對A*算法進行平滑[8]處理方面，最近幾年也出現(xiàn)了不少的研究成果。但絕大多數(shù)的平滑處理方法都是著眼于障礙物與移動機器人交匯處來進行處理。這種平滑方式有著很大的局限性，平滑算法在對路徑平滑時，都是遍歷所有的節(jié)點，當某一個節(jié)點前后節(jié)點連線上無障礙物時，就將延長線路的這一中間節(jié)點刪除，當遍歷到路徑上從起始點到終止點的所有節(jié)點時，平滑算法終止，路徑平滑規(guī)劃結(jié)束。這樣的平滑方式由于是遍歷所有的節(jié)點，將大大影響算法的運行效率。本文主要處理兩個問題：其一，因A*算法在柵格地圖中生成的路徑折線多、轉(zhuǎn)折多而影響機器人工作效率的問題；其二，現(xiàn)行的路徑平滑算法效率不高的問題。針對以上兩個問題，本文提出一種將Bezier曲線[9]與A*算法融合的規(guī)劃算法，并通過MATLAB、V_REP仿真工具來實現(xiàn)與平滑A*算法、A*算法的性能對比分析。

1 環(huán)境建模

移動機器人的路徑規(guī)劃在實際應用上主要是面對兩個問題：一是環(huán)境建模；二是路徑搜索生成及處理策略[10]。本節(jié)主要討論環(huán)境建模，在下一小節(jié)將對路徑搜索算法及處理策略進行分析。

移動機器人路徑規(guī)劃的環(huán)境建模有很多種，例如柵格法、拓撲圖、幾何信息法等。柵格法在許多機器人系統(tǒng)中得到應用，是比較成功的一種環(huán)境建模方法，且柵格地圖容易創(chuàng)建和維護，因此本文采用柵格法。常用的環(huán)境地圖表示中柵格地圖的特點是容易創(chuàng)建和維護，創(chuàng)建成本和使用成本都比較低。移動機器人所了解的每個柵格信息直接與環(huán)境區(qū)域相對應，且移動機器人可以根據(jù)柵格地圖進行導航與定位。在本文中所創(chuàng)建的柵格環(huán)境模型是根據(jù)實驗室環(huán)境及障礙物映射生成的，最終柵格通過機器人仿真工具V-REP畫出，如圖1所示。V-REP柵格地圖中，黑色區(qū)域表示實驗室中的障礙物區(qū)域，灰白色區(qū)域表示可安全行走區(qū)域。

圖1 V-REP柵格地圖

2 Bezier曲線與A*算法原理分析

2.1 A*算法原理分析

A*算法是建立在Dijkstra和BFS(廣度優(yōu)先搜索)算法基礎上的搜索算法。它的整體框架采用遍歷搜索法，但是它采用了啟發(fā)函數(shù)來估計地圖上任意點到目標點的費用，從而可以很好地選擇搜索方向。

A*算法引入了當前節(jié)點x的啟發(fā)函數(shù)f(x)，當前節(jié)點x的啟發(fā)函數(shù)定義為：

f(x)=g(x)+h(x)

(1)

其中g(shù)(x)是從起點到當前節(jié)點x的實際費用度量，h(x)是從當前節(jié)點x到終點的最小費用估計。h(x)只要滿足相容性條件：不能高于節(jié)點x到終點的實際最小費用，則原問題存在最優(yōu)解，并且A*算法一定能夠求出最優(yōu)路徑。A*算法的優(yōu)點是利用啟發(fā)函數(shù)，使搜索方向更加智能地趨向于終點，所以其搜索深度較小，搜索的節(jié)點數(shù)少，這樣將大大提高算法的運行效率。

A*算法是廣泛使用的移動機器人路徑規(guī)劃上的一類算法，同時它也適用于全局環(huán)境信息已知的路徑規(guī)劃。但是目前A*算法在實際的工程運用中還是存在折線多、轉(zhuǎn)折次數(shù)多、累計轉(zhuǎn)角大等問題，給機器人運行造成較大的麻煩，例如，轉(zhuǎn)角多時，必須準確計算出機器人和障礙物間距，否則就會發(fā)生碰撞；當累計角度大、轉(zhuǎn)角多時，機器人的工作效率也會下降。考慮上述問題，本文首先采用A*算法實現(xiàn)路徑的初步規(guī)劃，再采用Bezier曲線實現(xiàn)融合處理。

2.2 Bezier曲線分析

n次曲線的定義式有多種形式,目前使用最廣泛的是由英國學者Forrest于1972年給出的定義：

(2)

其中，pi(0≤i≤n) 被稱為曲線的第i個控制點，順次連接從p0到pn的折線被稱為Bezier曲線的控制多邊形。Bi,n(u)為n次Bernstein多項式,其表達式為：

(3)

在坐標系xoy中，對于給定已知的四個控制點(x0,y0),(x1,y1),(x2,y2),(x3,y3)可以由公式(4)、(5)確定一個3次Bezier曲線。

=x0(1-u)3+3x1u(1-u)2+3x2u2(1-u)+x3u3

(4)

=y0(1-u)3+3y1u(1-u)2+3y2u2(1-u)+y3u3

(5)

通過分析3次曲線的定義式可知，0次Bezier曲線是其唯一的控制點P0，1次Bezier曲線是連接P0至P1的線段，2次或者2次以上的Bezier曲線則具有以下性質(zhì)：

(1)端點性質(zhì)：Bezier曲線的起點P0和終點Pn與特征多邊形的起點和終點重合。

(2)切矢量性：Bezier曲線的起點和終點處的切線方向和特征多邊形的第一條邊及最后一條邊走向一致。

(3)凸包性：曲線落在特征多邊形各頂點構(gòu)成的凸包之中。

(4)幾何不變性：Bezier曲線的幾何特性不隨坐標變化而變化，Bezier曲線的位置和形狀與特征多邊形頂點的位置有關(guān)，而不依賴坐標的選擇。

通過以上兩個算法的分析，下面將設計Bezier曲線與A*算法的融合方案。

2.3 Bezier曲線與A*算法融合的路徑規(guī)劃方案

移動機器人路徑規(guī)劃的最終目標是：在保證機器人能達到目標點的同時，找到一條平滑可行的最短路徑。移動機器人路徑規(guī)劃的一切設計方案都應該遵循這個宗旨。A*算法能夠保證機器人在充滿障礙物的地圖中找到一條最短路徑。但找到這一條最短的路徑還是遠遠不夠的，A*算法本身存在著許多的不足：在柵格環(huán)境下A*算法規(guī)劃出的移動機器人路徑往往存在著折線多、轉(zhuǎn)折次數(shù)多、累計轉(zhuǎn)折角度大、運行效率低等許多問題。

在分析了A*算法的突出問題后，提出了一種A*算法與Bezier曲線融合的路徑規(guī)劃方法，通過融合算法的實現(xiàn)，將有效地解決A*算法及平滑A*算法所存在的折線多、轉(zhuǎn)折次數(shù)多、轉(zhuǎn)折角度累計大等問題。整個融合算法大體分以下步驟完成：第一步，實現(xiàn)A*算法對移動機器人路徑的初步規(guī)劃；第二步，將所規(guī)劃的路徑進行Bezier曲線再規(guī)劃處理；第三步，對Bezier曲線融合后的分段曲線進行拼接并最終輸出融合路徑。在以上的三個處理步驟中主要的難點問題是如何解決A*算法初次規(guī)劃后，對初步路徑的節(jié)點信息進行再處理。當?shù)玫匠醪降穆窂焦?jié)點信息后，不可能對所有節(jié)點都采取一致的3次Bezier曲線或者2次Bezier曲線優(yōu)化，單調(diào)地使用2次或者3次以及更高次的Bezier曲線處理往往會使移動機器人陷入障礙物死區(qū),如圖2所示，在對4個節(jié)點進行Bezier曲線優(yōu)化時，觸碰到了障礙物，讓機器人陷入了工作死區(qū)。但對于障礙物死區(qū)問題，結(jié)合2次或者3次的處理后，問題將得到很好的解決，能使移動機器人成功地避開障礙物死區(qū)。對于2次的Bezier曲線，由公式(2)可得：

=(1-u)2P0+2u(1-u)P1+u2P2

=(P2-2P1+P0)u2+2(P1-P0)u+P0

(6)

對公式(6)可得到如下矩陣表達式：

(7)

結(jié)合2次曲線和3次曲線的處理，即可避免移動機器人陷入死區(qū)等問題。再對每一段k(k=1,2,3)次曲線按照公式(8)進行拼接:

(8)

圖2 融合算法死區(qū)問題

如圖3所示，通過分段的Bezier處理，有效地避免了障礙物死區(qū)問題。整個融合算法實現(xiàn)偽代碼如下：

OPEN表 = 起始點start；

CLOSED 表 = 空；

BEZIER 表 = 空；

圖3 融合算法避開死區(qū)

While(OPEN != NULL)

{

從OPEN表中取估價函數(shù)f最小節(jié)點n;

If(n==目標節(jié)點){

Break；

}

For(當前節(jié)點n的每個子節(jié)點X){

算X的估價值；

If(Xin OPEN){

If(X的估價值小于OPEN的估價值){

把n設置為X的父節(jié)點；

更新OPEN表中的估價值；

}}

If(Xin CLOSE){

If(X的估價值小于CLOSE表估價值){

把n設置為X的父節(jié)點；

更新CLOSE表中的估價值；

把X節(jié)點放入OPEN；

}}

If(Xnot in both){

把n設置為X的父節(jié)點；

求X的估價值；

并將X插入OPEN表中

}}//end for

將n節(jié)點插入CLOSE表中；

按估價值將OPEN表中的節(jié)點排序；

BEZIER 表 = 初次規(guī)劃的路徑節(jié)點；

}//end while(OPEN != NULL)

While(BEZIER != NULL){

For(對路徑所有節(jié)點進行Bezier處理){

If(4節(jié)點處理無障礙){

3次bezier曲線處理；

更新此4節(jié)點為一段Bezier曲線；

}

If(3節(jié)點處理無障礙){

2次Bezier曲線處理；

更新此3節(jié)點為一段Bezier曲線；

}

If(2節(jié)點處理無障礙){

1次Bezier曲線處理；

更新此2節(jié)點為一段Bezier曲線；

}}//end for

對每段Bezier曲線實現(xiàn)拼接處理；

輸出融合處理后的路徑

}//end while(BEZIER !=NULL)

3 實驗仿真分析

本仿真實驗計算機采用處理器為Intel(R) Core(TM) i5-4595,內(nèi)存為4 GB；算法分析工具為MATLAB；路徑規(guī)劃仿真工具為V-REP。

通過機器人仿真工具V-REP，編寫相關(guān)代碼實現(xiàn)了A*算法及Bezier_A*融合算法的路徑規(guī)劃如圖4、圖5所示。從兩個圖中可以很清晰地看到，A*算法規(guī)劃路徑的折線較多、轉(zhuǎn)折次數(shù)也較多，但在圖5中由Bezier_A*融合算法生成的路徑上折線和轉(zhuǎn)角已經(jīng)大大減少。

圖4 A*算法規(guī)劃路徑

圖5 Bezier_A*融合算法規(guī)劃路徑

在V-REP中將A*算法及融合算法的節(jié)點數(shù)據(jù)導出到數(shù)據(jù)表格，在MATLAB中，得到圖6所示的規(guī)劃路徑。

圖6 路徑規(guī)劃對比效果

為同其他的路徑規(guī)劃算法形成性能分析對比，本文將文獻[7]和[8]中給出的A*算法、平滑A*算法兩種算法的分析結(jié)果與本文算法進行對比分析，對比環(huán)境為：40×40的柵格地圖。表1是Bezier_A*融合算法與A*算法性能對比，表2是Bezier_A*融合算法與平滑A*算法的性能對比。

表1 Bezier_A*融合算法與A*算法

通過仿真實驗可以得出，與A*算法、平滑A*算法相比，使用Bezier_A*融合算法所規(guī)劃的移動機器人路徑各方面性能得到了較大的提升。對比A*算法，Bezier_A*融合算法有效地減少路徑距離6%左右，將累計的轉(zhuǎn)折數(shù)減少了85%左右；對比平滑A*算法，Bezier_A*融合算法能更加有效地減少累計轉(zhuǎn)折角30%左右，并且將算法的運行效率提升了20%左右。

4 結(jié)論

本文主要針對柵格環(huán)境中的移動機器人路徑規(guī)劃實現(xiàn)，分析了A*算法、平滑A*算法在移動機器人路徑規(guī)劃上所存在的缺點，如轉(zhuǎn)折角度大、轉(zhuǎn)折數(shù)多等問題，這些問題都將大大影響移動機器人的實際工作效率。針對以上問題，本文提出A*算法與Bezier曲線融合的路徑規(guī)劃算法，融合算法大大改善了移動機器人的運動性能。最后通過仿真實驗證明，融合算法減少了累計轉(zhuǎn)折角30%左右，減少累計轉(zhuǎn)折數(shù)85%左右，相比于一般的平滑A*算法，融合算法還提升了運行效率。實際運用中Bezier_A*融合算法在障礙物分布廣泛的柵格環(huán)境下具有轉(zhuǎn)折次數(shù)少、累計轉(zhuǎn)折角度小等優(yōu)點，更能滿足移動機器人在工作時的運動需求。

[1] ZAMIRIAN M,KAMYAD A V,FARAHI M H. A novel algorithm for solving optimal path planning problems based on parametrization method and fuzzy aggregation[J].Physics Letter A,2012,373(38)：34-39.

[2] PANDA R K, CHOUDHURY B B. An effective path planning of mobile robot using genetic algorithm[J].IEEE International Conference on Computational Intelligence & Communication Technology,2015,145(15)：287-291.

[3] 張琦，馬家辰，謝偉，等. 基于改進蟻群算法的移動機器人路徑規(guī)劃[J]. 東北大學學報，2013,34(11)：1522-1527.

[4] 劉蓓蕾，江銘炎，張振月. 基于禁忌搜索的人工蜂群算法及其運用[J]. 計算機運用研究，2015,32(7)：2006-2011.

[5] TZAIR R M, AZOUAOUI O, HAZERCHI M, et al. Mobile robot path planning for complex dynamic environments[J].IEEE International Conference on Computational Intelligence & Communication Technology,2015,145(25)：145-150.

[6] Guo Jianming, Liu Liang. An improvement of D* algorithm for mobile robot path planning in partial unknown[J]. International Conference on Intelligent Computation Technology and Automation, 2014,56(10)：46-50.

[7] 張少鵬，王現(xiàn)康，段堅. A*算法在移動機器人路徑規(guī)劃中的運用[J]. 機械工程與自動化，2012,6(14)：147-151.

[8] 王紅衛(wèi)，馬勇，謝勇，等.基于平滑A*算法的移動機器人路徑規(guī)劃[J].同濟大學學報，2010,38(11)：164-169.

[9] 昝杰，蔡宗琰，梁虎，等.基于Bezier曲線的自主移動機器人最優(yōu)路徑規(guī)劃[J].蘭州大學學報，2013,49(2)：250-256.

[10] ZIADI S, NJAH M.PSO-CF2:a new method for the path planning of a mobile robot[J].International Multi-Conference on Systems,Signals&Device,2015,35(2)：6-13.

Mobile robot path planning based on fusion of A* algorithm and Bezier curve

Guo Jiang1,2, Xiao Yufeng1,2, Liu Xinyu1, Chen Li1

(1.School of Information Engineering, Southwest University of Science&Technology, Mianyang 621010, China; 2.Sichuan Province Key Laboratory of Robot Technology Used for Special Environment,Southwest University of Science&Technology,Mianyang 621010,China)

Mobile robot path planning is always a focus issue regarding the mobile robot field. A* algorithm, which is applied successfully as an outstanding algorithm in path optimization and planning, has a lot of problems under grid environment. Due to its particular path planning mode, it offers too many turn angles and will turn overly amount of times with over-sized angles accumulatively. Therefore, in the body of this article, we’ll firstly present a fusion algorithm of both A* algorithm and Bezier curve. Then, we’ll compare the performance between this new path planning method, smoothing A* algorithm as well as A* algorithm separately, based on MATLAB and V-REP simulation tools. In the end, we’ll reach a conclusion of their performances on running stability, path optimization and path planning.

mobile robot; path planning; A* algorithm; Bezier curve; fusion algorithm

TP391

A

10.19358/j.issn.1674- 7720.2017.02.017

郭江，肖宇峰，劉欣雨，等.Bezier曲線與A*算法融合的移動機器人路徑規(guī)劃[J].微型機與應用，2017,36(2)：52-55，59.

國家自然科學基金(61601381);四川省科技支撐計劃項目(2015GZ0035);四川省教育廳重點項目(14ZA0091)

2016-08-28)

郭江(1990-)，男，碩士研究生，主要研究方向：移動機器人技術(shù)、移動機器人路徑規(guī)劃、人機交互技術(shù)。

肖宇峰(1977-)，通信作者，男，副教授，主要研究方向：通信技術(shù)、嵌入式技術(shù)、移動機器人系統(tǒng)技術(shù)。E-mail:1697315739@qq.com。

劉欣雨(1995-)，女，本科，主要研究方向：自動控制系統(tǒng)，機器人。