郭江1,2，肖宇峰1,2，劉欣雨1，陳麗1
(1.西南科技大學(xué) 信息工程學(xué)院，四川 綿陽 621010；2.西南科技大學(xué) 特殊環(huán)境機(jī)器人技術(shù)四川省重點(diǎn)實(shí)驗(yàn)室，四川 綿陽 621010)
摘要：移動(dòng)機(jī)器人路徑規(guī)劃一直是移動(dòng)機(jī)器人領(lǐng)域里的重要技術(shù)問題。A*算法在最優(yōu)路徑搜索上有著比較成功的運(yùn)用，但在柵格環(huán)境下的A*算法也存在著折線多、轉(zhuǎn)折角度大等問題。在考慮移動(dòng)機(jī)器人的實(shí)際工作環(huán)境及相關(guān)運(yùn)動(dòng)參數(shù)后，這些問題都將大大地影響移動(dòng)機(jī)器人的工作效率。在對(duì)以上問題進(jìn)行分析后提出了一種基于Bezier曲線與A*算法融合的方法來實(shí)現(xiàn)移動(dòng)機(jī)器人的路徑規(guī)劃，再通過MATLAB、VREP仿真工具來實(shí)現(xiàn)Bezier_A*融合算法與平滑A*算法及A*算法的對(duì)比。通過Bezier_A*融合算法使得機(jī)器人在工作中的尋優(yōu)能力、路徑規(guī)劃效率都得到較大的提高。
關(guān)鍵詞：移動(dòng)機(jī)器人；路徑規(guī)劃；A*算法；Bezier曲線；融合算法
中圖分類號(hào)：TP391文獻(xiàn)標(biāo)識(shí)碼：ADOI： 10.19358/j.issn.1674-7720.2017.02.017
引用格式：郭江，肖宇峰，劉欣雨，等.Bezier曲線與A*算法融合的移動(dòng)機(jī)器人路徑規(guī)劃［J］.微型機(jī)與應(yīng)用，2017,36（2）：52-55，59.
0引言
*基金項(xiàng)目：國(guó)家自然科學(xué)基金(61601381);四川省科技支撐計(jì)劃項(xiàng)目(2015GZ0035);四川省教育廳重點(diǎn)項(xiàng)目(14ZA0091)移動(dòng)機(jī)器人路徑規(guī)劃問題一直以來都是移動(dòng)機(jī)器人研究的熱點(diǎn)和難點(diǎn)［1］。在近年的發(fā)展中，越來越多的學(xué)者和專家已經(jīng)致力于結(jié)合智能控制算法來解決移動(dòng)機(jī)器人的路徑規(guī)劃問題。例如遺傳算法［2］、蟻群算法［3］、禁忌搜索算法［4］等智能算法及其混合形式都被用來解決路徑規(guī)劃問題。但這些智能算法目前還不太完善，存在著一些缺點(diǎn)，例如遺傳算法編碼長(zhǎng)度變化范圍大，求解規(guī)模小；Dijkstra算法［5］直接搜索全局而不考慮目標(biāo)信息，導(dǎo)致路徑求解時(shí)間長(zhǎng)，難以滿足快速規(guī)劃路徑的需求；D*算法［6］主要解決局部的動(dòng)態(tài)路徑規(guī)劃問題。
A*算法［7］作為一種比較成功的全局路徑規(guī)劃算法，已成功應(yīng)用于機(jī)器人的路徑尋優(yōu)和規(guī)劃方面，并且取得良好的路徑規(guī)劃效果。但由于A*算法本身的計(jì)算特點(diǎn)，在柵格環(huán)境下A*算法規(guī)劃出的移動(dòng)機(jī)器人路徑一般存在著折線多、累計(jì)轉(zhuǎn)角大等問題。在對(duì)A*算法進(jìn)行平滑［8］處理方面，最近幾年也出現(xiàn)了不少的研究成果。但絕大多數(shù)的平滑處理方法都是著眼于障礙物與移動(dòng)機(jī)器人交匯處來進(jìn)行處理。這種平滑方式有著很大的局限性，平滑算法在對(duì)路徑平滑時(shí)，都是遍歷所有的節(jié)點(diǎn)，當(dāng)某一個(gè)節(jié)點(diǎn)前后節(jié)點(diǎn)連線上無障礙物時(shí)，就將延長(zhǎng)線路的這一中間節(jié)點(diǎn)刪除，當(dāng)遍歷到路徑上從起始點(diǎn)到終止點(diǎn)的所有節(jié)點(diǎn)時(shí)，平滑算法終止，路徑平滑規(guī)劃結(jié)束。這樣的平滑方式由于是遍歷所有的節(jié)點(diǎn)，將大大影響算法的運(yùn)行效率。本文主要處理兩個(gè)問題：其一，因A*算法在柵格地圖中生成的路徑折線多、轉(zhuǎn)折多而影響機(jī)器人工作效率的問題；其二，現(xiàn)行的路徑平滑算法效率不高的問題。針對(duì)以上兩個(gè)問題，本文提出一種將Bezier曲線［9］與A*算法融合的規(guī)劃算法，并通過MATLAB、V_REP仿真工具來實(shí)現(xiàn)與平滑A*算法、A*算法的性能對(duì)比分析。
1環(huán)境建模
移動(dòng)機(jī)器人的路徑規(guī)劃在實(shí)際應(yīng)用上主要是面對(duì)兩個(gè)問題：一是環(huán)境建模；二是路徑搜索生成及處理策略［10］。本節(jié)主要討論環(huán)境建模，在下一小節(jié)將對(duì)路徑搜索算法及處理策略進(jìn)行分析。
移動(dòng)機(jī)器人路徑規(guī)劃的環(huán)境建模有很多種，例如柵格法、拓?fù)鋱D、幾何信息法等。柵格法在許多機(jī)器人系統(tǒng)中得到應(yīng)用，是比較成功的一種環(huán)境建模方法，且柵格地圖容易創(chuàng)建和維護(hù)，因此本文采用柵格法。常用的環(huán)境地圖表示中柵格地圖的特點(diǎn)是容易創(chuàng)建和維護(hù)，創(chuàng)建成本和使用成本都比較低。移動(dòng)機(jī)器人所了解的每個(gè)柵格信息直接與環(huán)境區(qū)域相對(duì)應(yīng)，且移動(dòng)機(jī)器人可以根據(jù)柵格地圖進(jìn)行導(dǎo)航與定位。在本文中所創(chuàng)建的柵格環(huán)境模型是根據(jù)實(shí)驗(yàn)室環(huán)境及障礙物映射生成的，最終柵格通過機(jī)器人仿真工具VREP畫出，如圖1所示。VREP柵格地圖中，黑色區(qū)域表示實(shí)驗(yàn)室中的障礙物區(qū)域，灰白色區(qū)域表示可安全行走區(qū)域。
2Bezier曲線與A*算法原理分析
2.1A*算法原理分析
A*算法是建立在Dijkstra和BFS(廣度優(yōu)先搜索)算法基礎(chǔ)上的搜索算法。它的整體框架采用遍歷搜索法，但是它采用了啟發(fā)函數(shù)來估計(jì)地圖上任意點(diǎn)到目標(biāo)點(diǎn)的費(fèi)用，從而可以很好地選擇搜索方向。
A*算法引入了當(dāng)前節(jié)點(diǎn)x的啟發(fā)函數(shù)f(x)，當(dāng)前節(jié)點(diǎn)x的啟發(fā)函數(shù)定義為：
f(x)=g(x)+h(x)(1)
其中g(shù)(x)是從起點(diǎn)到當(dāng)前節(jié)點(diǎn)x的實(shí)際費(fèi)用度量，h(x)是從當(dāng)前節(jié)點(diǎn)x到終點(diǎn)的最小費(fèi)用估計(jì)。h(x)只要滿足相容性條件：不能高于節(jié)點(diǎn)x到終點(diǎn)的實(shí)際最小費(fèi)用，則原問題存在最優(yōu)解，并且A*算法一定能夠求出最優(yōu)路徑。A*算法的優(yōu)點(diǎn)是利用啟發(fā)函數(shù)，使搜索方向更加智能地趨向于終點(diǎn)，所以其搜索深度較小，搜索的節(jié)點(diǎn)數(shù)少，這樣將大大提高算法的運(yùn)行效率。
A*算法是廣泛使用的移動(dòng)機(jī)器人路徑規(guī)劃上的一類算法，同時(shí)它也適用于全局環(huán)境信息已知的路徑規(guī)劃。但是目前A*算法在實(shí)際的工程運(yùn)用中還是存在折線多、轉(zhuǎn)折次數(shù)多、累計(jì)轉(zhuǎn)角大等問題，給機(jī)器人運(yùn)行造成較大的麻煩，例如，轉(zhuǎn)角多時(shí)，必須準(zhǔn)確計(jì)算出機(jī)器人和障礙物間距，否則就會(huì)發(fā)生碰撞；當(dāng)累計(jì)角度大、轉(zhuǎn)角多時(shí)，機(jī)器人的工作效率也會(huì)下降。考慮上述問題，本文首先采用A*算法實(shí)現(xiàn)路徑的初步規(guī)劃，再采用Bezier曲線實(shí)現(xiàn)融合處理。
2.2Bezier曲線分析
n次曲線的定義式有多種形式,目前使用最廣泛的是由英國(guó)學(xué)者Forrest于1972年給出的定義：
P(u)=∑niPiBi,n(u),u［0,1］(2)
其中，pi(0≤i≤n) 被稱為曲線的第i個(gè)控制點(diǎn)，順次連接從p0到pn的折線被稱為Bezier曲線的控制多邊形。Bi,n(u)為n次Bernstein多項(xiàng)式,其表達(dá)式為：
在坐標(biāo)系xoy中，對(duì)于給定已知的四個(gè)控制點(diǎn)(x0,y0),(x1,y1),(x2,y2),(x3,y3)可以由公式（4）、（5）確定一個(gè)3次Bezier曲線。
通過分析3次曲線的定義式可知，0次Bezier曲線是其唯一的控制點(diǎn)P0，1次Bezier曲線是連接P0至P1的線段，2次或者2次以上的Bezier曲線則具有以下性質(zhì)：
?。?）端點(diǎn)性質(zhì)：Bezier曲線的起點(diǎn)P0和終點(diǎn)Pn與特征多邊形的起點(diǎn)和終點(diǎn)重合。
?。?）切矢量性：Bezier曲線的起點(diǎn)和終點(diǎn)處的切線方向和特征多邊形的第一條邊及最后一條邊走向一致。
?。?）凸包性：曲線落在特征多邊形各頂點(diǎn)構(gòu)成的凸包之中。
?。?）幾何不變性：Bezier曲線的幾何特性不隨坐標(biāo)變化而變化，Bezier曲線的位置和形狀與特征多邊形頂點(diǎn)的位置有關(guān)，而不依賴坐標(biāo)的選擇。
通過以上兩個(gè)算法的分析，下面將設(shè)計(jì)Bezier曲線與A*算法的融合方案。
2.3Bezier曲線與A*算法融合的路徑規(guī)劃方案
移動(dòng)機(jī)器人路徑規(guī)劃的最終目標(biāo)是：在保證機(jī)器人能達(dá)到目標(biāo)點(diǎn)的同時(shí)，找到一條平滑可行的最短路徑。移動(dòng)機(jī)器人路徑規(guī)劃的一切設(shè)計(jì)方案都應(yīng)該遵循這個(gè)宗旨。A*算法能夠保證機(jī)器人在充滿障礙物的地圖中找到一條最短路徑。但找到這一條最短的路徑還是遠(yuǎn)遠(yuǎn)不夠的，A*算法本身存在著許多的不足：在柵格環(huán)境下A*算法規(guī)劃出的移動(dòng)機(jī)器人路徑往往存在著折線多、轉(zhuǎn)折次數(shù)多、累計(jì)轉(zhuǎn)折角度大、運(yùn)行效率低等許多問題。
在分析了A*算法的突出問題后，提出了一種A*算法與Bezier曲線融合的路徑規(guī)劃方法，通過融合算法的實(shí)現(xiàn)，將有效地解決A*算法及平滑A*算法所存在的折線多、轉(zhuǎn)折次數(shù)多、轉(zhuǎn)折角度累計(jì)大等問題。整個(gè)融合算法大體分以下步驟完成：第一步，實(shí)現(xiàn)A*算法對(duì)移動(dòng)機(jī)器人路徑的初步規(guī)劃；第二步，將所規(guī)劃的路徑進(jìn)行Bezier曲線再規(guī)劃處理；第三步，對(duì)Bezier曲線融合后的分段曲線進(jìn)行拼接并最終輸出融合路徑。在以上的三個(gè)處理步驟中主要的難點(diǎn)問題是如何解決A*算法初次規(guī)劃后，對(duì)初步路徑的節(jié)點(diǎn)信息進(jìn)行再處理。當(dāng)?shù)玫匠醪降穆窂焦?jié)點(diǎn)信息后，不可能對(duì)所有節(jié)點(diǎn)都采取一致的3次Bezier曲線或者2次Bezier曲線優(yōu)化，單調(diào)地使用2次或者3次以及更高次的Bezier曲線處理往往會(huì)使移動(dòng)機(jī)器人陷入障礙物死區(qū),如圖2所示，在對(duì)4個(gè)節(jié)點(diǎn)進(jìn)行Bezier曲線優(yōu)化時(shí)，觸碰到了障礙物，讓機(jī)器人陷入了工作死區(qū)。但對(duì)于障礙物死區(qū)問題，結(jié)合2次或者3次的處理后，問題將得到很好的解決，能使移動(dòng)機(jī)器人成功地避開障礙物死區(qū)。對(duì)于2次的Bezier曲線，由公式（2）可得：
結(jié)合2次曲線和3次曲線的處理，即可避免移動(dòng)機(jī)器人陷入死區(qū)等問題。再對(duì)每一段k(k=1,2,3)次曲線按照公式（8）進(jìn)行拼接:
S(u)=∑ki=0(Pi－Bi(ui))2(8)
如圖3所示，通過分段的Bezier處理，有效地避免了障礙物死區(qū)問題。整個(gè)融合算法實(shí)現(xiàn)偽代碼如下：
OPEN表 = 起始點(diǎn)start；
CLOSED 表 = 空；
BEZIER 表 = 空；
圖3融合算法避開死區(qū)
While(OPEN != NULL)
{
從OPEN表中取估價(jià)函數(shù)f最小節(jié)點(diǎn)n;
If(n==目標(biāo)節(jié)點(diǎn)){
Break；
}
For(當(dāng)前節(jié)點(diǎn)n的每個(gè)子節(jié)點(diǎn)X){
算X的估價(jià)值；
If(X in OPEN){
If(X的估價(jià)值小于OPEN的估價(jià)值){
把n設(shè)置為X的父節(jié)點(diǎn)；
更新OPEN表中的估價(jià)值；
}}
If(X in CLOSE){
If(X的估價(jià)值小于CLOSE表估價(jià)值){
把n設(shè)置為X的父節(jié)點(diǎn)；
更新CLOSE表中的估價(jià)值；
把X節(jié)點(diǎn)放入OPEN；
}}
If(X not in both){
把n設(shè)置為X的父節(jié)點(diǎn)；
求X的估價(jià)值；
并將X插入OPEN表中
}}//end for
將n節(jié)點(diǎn)插入CLOSE表中；
按估價(jià)值將OPEN表中的節(jié)點(diǎn)排序；
BEZIER 表 = 初次規(guī)劃的路徑節(jié)點(diǎn)；
}//end while(OPEN != NULL)
While(BEZIER != NULL){
For(對(duì)路徑所有節(jié)點(diǎn)進(jìn)行Bezier處理){
If(4節(jié)點(diǎn)處理無障礙){
3次bezier曲線處理；
更新此4節(jié)點(diǎn)為一段Bezier曲線；
}
If(3節(jié)點(diǎn)處理無障礙){
2次Bezier曲線處理；
更新此3節(jié)點(diǎn)為一段Bezier曲線；
}
If(2節(jié)點(diǎn)處理無障礙){
1次Bezier曲線處理；
更新此2節(jié)點(diǎn)為一段Bezier曲線；
}}//end for
對(duì)每段Bezier曲線實(shí)現(xiàn)拼接處理；
輸出融合處理后的路徑
}//end while(BEZIER !=NULL)
3實(shí)驗(yàn)仿真分析
本仿真實(shí)驗(yàn)計(jì)算機(jī)采用處理器為Intel(R) Core(TM) i54595,內(nèi)存為4 GB；算法分析工具為MATLAB；路徑規(guī)劃仿真工具為VREP。
通過機(jī)器人仿真工具VREP，編寫相關(guān)代碼實(shí)現(xiàn)了A*算法及Bezier_A*融合算法的路徑規(guī)劃如圖4、圖5所示。從兩個(gè)圖中可以很清晰地看到，A*算法規(guī)劃路徑的折線較多、轉(zhuǎn)折次數(shù)也較多，但在圖5中由Bezier_A*融合算法生成的路徑上折線和轉(zhuǎn)角已經(jīng)大大減少。
在V-REP中將A*算法及融合算法的節(jié)點(diǎn)數(shù)據(jù)導(dǎo)出到數(shù)據(jù)表格，在MATLAB中，得到圖6所示的規(guī)劃路徑。
圖6路徑規(guī)劃對(duì)比效果為同其他的路徑規(guī)劃算法形成性能分析對(duì)比，本文將文獻(xiàn)［7］和［8］中給出的A*算法、平滑A*算法兩種算法的分析結(jié)果與本文算法進(jìn)行對(duì)比分析，對(duì)比環(huán)境為：40×40的柵格地圖。表1是Bezier_A*融合算法與A*算法性能對(duì)比，表2是Bezier_A*融合算法與平滑A*算法的性能對(duì)比。
降低率/%累計(jì)
轉(zhuǎn)折數(shù)折線
減少率/%A*45.873 636Bezier_A*43.234 65.75683.33
表2Bezier_A*融合算法與平滑A*算法算法累計(jì)
轉(zhuǎn)折角轉(zhuǎn)折
降低率/%運(yùn)行
時(shí)間/s時(shí)間
減少率/%平滑A*456.873 60.362 1Bezier_A*326.811 528.460.291 619.47
通過仿真實(shí)驗(yàn)可以得出，與A*算法、平滑A*算法相比，使用Bezier_A*融合算法所規(guī)劃的移動(dòng)機(jī)器人路徑各方面性能得到了較大的提升。對(duì)比A*算法，Bezier_A*融合算法有效地減少路徑距離6%左右，將累計(jì)的轉(zhuǎn)折數(shù)減少了85%左右；對(duì)比平滑A*算法，Bezier_A*融合算法能更加有效地減少累計(jì)轉(zhuǎn)折角30%左右，并且將算法的運(yùn)行效率提升了20%左右。
4結(jié)論
本文主要針對(duì)柵格環(huán)境中的移動(dòng)機(jī)器人路徑規(guī)劃實(shí)現(xiàn)，分析了A*算法、平滑A*算法在移動(dòng)機(jī)器人路徑規(guī)劃上所存在的缺點(diǎn)，如轉(zhuǎn)折角度大、轉(zhuǎn)折數(shù)多等問題，這些問題都將大大影響移動(dòng)機(jī)器人的實(shí)際工作效率。針對(duì)以上問題，本文提出A*算法與Bezier曲線融合的路徑規(guī)劃算法，融合算法大大改善了移動(dòng)機(jī)器人的運(yùn)動(dòng)性能。最后通過仿真實(shí)驗(yàn)證明，融合算法減少了累計(jì)轉(zhuǎn)折角30%左右，減少累計(jì)轉(zhuǎn)折數(shù)85%左右，相比于一般的平滑A*算法，融合算法還提升了運(yùn)行效率。實(shí)際運(yùn)用中Bezier_A*融合算法在障礙物分布廣泛的柵格環(huán)境下具有轉(zhuǎn)折次數(shù)少、累計(jì)轉(zhuǎn)折角度小等優(yōu)點(diǎn)，更能滿足移動(dòng)機(jī)器人在工作時(shí)的運(yùn)動(dòng)需求。
參考文獻(xiàn)
?。?］ 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.
?。?］ 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.
?。?］ 張琦，馬家辰，謝偉，等. 基于改進(jìn)蟻群算法的移動(dòng)機(jī)器人路徑規(guī)劃［J］. 東北大學(xué)學(xué)報(bào)，2013,34（11）：1522-1527.