文獻(xiàn)標(biāo)識(shí)碼: A
DOI:10.16157/j.issn.0258-7998.181374
中文引用格式: 徐曉慧,,張金龍. 改進(jìn)人工勢場與TAS-RRT融合優(yōu)化算法[J].電子技術(shù)應(yīng)用,,2018,44(10):88-92.
英文引用格式: Xu Xiaohui,,Zhang Jinlong. Hybrid optimization algorithm of improved artificial potential field and TAS-RRT[J]. Application of Electronic Technique,,2018,44(10):88-92.
0 引言
人工勢場法是一種結(jié)構(gòu)簡單的運(yùn)動(dòng)規(guī)劃算法,,具有魯棒性強(qiáng),、路徑平滑、執(zhí)行效率高的優(yōu)點(diǎn)[1],,但在實(shí)際應(yīng)用中常在障礙物周圍產(chǎn)生局部振蕩,。目前,國內(nèi)外許多學(xué)者對(duì)此進(jìn)行了探索研究,,常見的改進(jìn)思路有增加虛擬障礙物,、建立虛擬目標(biāo)點(diǎn)、改變勢函數(shù)等[2-3],,其適用面窄,、通用性差,,很難實(shí)現(xiàn)振蕩區(qū)域邊界點(diǎn)的精確定位。RRT算法在運(yùn)動(dòng)規(guī)劃中具有良好的擴(kuò)展性和搜索速度,,與人工勢場的結(jié)合可使路徑快速跳出局值吸引區(qū),,但逃離點(diǎn)落在狹縫之中或其附近時(shí)RRT算法易陷入局部死循環(huán)[4-6]。針對(duì)這些不足之處,,本文提出一種基于旋轉(zhuǎn)速度矢量角的人工勢場和TAS-RRT融合優(yōu)化算法,,當(dāng)陷入局部振蕩時(shí),以旋轉(zhuǎn)速度矢量角分析運(yùn)動(dòng)趨勢,、精確定位逃離點(diǎn),,并使用速度矢量角度差引導(dǎo)RRT采樣的分布、調(diào)節(jié)局部規(guī)劃器的連接方式,,使其采樣的方向快速逼近逃離點(diǎn),,當(dāng)采樣點(diǎn)滿足條件時(shí),切換回人工勢場算法進(jìn)行運(yùn)動(dòng)規(guī)劃,。程序運(yùn)行結(jié)果表明,,該算法搜索效率高,復(fù)雜環(huán)境下依然有效,。
1 改進(jìn)人工勢場
1.1 人工勢場
傳統(tǒng)人工勢場是一種虛擬勢場[7],,包括吸引勢場fa(x)和抵制勢場ft(x):
1.2 旋轉(zhuǎn)速度矢量角
針對(duì)人工勢場易陷入局部振蕩的問題,本文提出一種旋轉(zhuǎn)速度矢量角方法,,將其作為跳出極小值吸引區(qū)域的判斷條件,,如圖1所示,其中點(diǎn)Root代表局部極小值點(diǎn),;current為當(dāng)前訪問節(jié)點(diǎn),,V1為位于點(diǎn)current位置時(shí)對(duì)應(yīng)的速度矢量;Next1為下一個(gè)訪問節(jié)點(diǎn),,V2為對(duì)應(yīng)的速度矢量,;Next2為機(jī)器人位于點(diǎn)Next1位置時(shí)下一個(gè)訪問節(jié)點(diǎn),V3為對(duì)應(yīng)的速度矢量,。
說明節(jié)點(diǎn)運(yùn)動(dòng)趨勢為遠(yuǎn)離極值點(diǎn),,且其趨勢不斷增強(qiáng),則當(dāng)前節(jié)點(diǎn)current為局部極小值吸引區(qū)的跳出點(diǎn),,切換回人工勢場算法進(jìn)行運(yùn)動(dòng)規(guī)劃,。
2 基于速度矢量角度差引導(dǎo)的快速隨機(jī)擴(kuò)展樹改進(jìn)算法
2.1 RRT算法
RRT算法以構(gòu)型空間中的初始狀態(tài)xs為起始點(diǎn)構(gòu)建隨機(jī)擴(kuò)展樹,初始化后通過迭代的方式向外擴(kuò)展,,在環(huán)境范圍內(nèi)隨機(jī)采樣自由節(jié)點(diǎn)qrand,,遍歷樹T的所有節(jié)點(diǎn),,選擇其中離qrand最近的節(jié)點(diǎn)qnear,,其后通過Extend(qrand,,qnear)函數(shù)檢驗(yàn)兩節(jié)點(diǎn)的距離是否滿足要求,若滿足qrand即為qnew,,否則沿著qnear至qrand取距離為δ的狀態(tài)節(jié)點(diǎn)作為qnew,,利用局部規(guī)劃器對(duì)qnew和qnear之間的路徑進(jìn)行檢測,若無障礙碰撞,,將其作為新的節(jié)點(diǎn)添加到擴(kuò)展樹上,,并更新父節(jié)點(diǎn)和新樹枝長度。
重復(fù)上述過程,,直到滿足stopcondition:新狀態(tài)節(jié)點(diǎn)與xg構(gòu)建路徑成功或者超過最大迭代次數(shù),,結(jié)束擴(kuò)展。圖形建立完畢后,,利用搜索算法即可得到xs和xg間最短規(guī)劃路徑,。具體搜索過程如圖2所示。
2.2 TAS-RRT算法
RRT算法規(guī)劃的路徑質(zhì)量不高且狹縫通道難以穿越,,針對(duì)以上缺點(diǎn),,本文提出一種基于速度矢量角度差引導(dǎo)的快速隨機(jī)擴(kuò)展樹算法,在采樣的過程中引入速度矢量角度差,,將其作為判斷采樣點(diǎn)有效性的條件,,并根據(jù)潛在節(jié)點(diǎn)的狀態(tài)調(diào)節(jié)樹的規(guī)劃器以改變擴(kuò)張速度。
具體搜索過程如圖3所示,。將局部極小值點(diǎn)作為構(gòu)建擴(kuò)展樹的起點(diǎn),,初始化后以迭代的方式向外擴(kuò)展,在環(huán)境內(nèi)隨機(jī)采樣自由點(diǎn)qrand后選擇樹中最近節(jié)點(diǎn)qnear,,其后Expandcontrol(qrand,,qnear,T)函數(shù)驅(qū)使規(guī)劃器自適應(yīng)地選擇探索速度,,選擇依據(jù)為樹的爬坡狀態(tài),。若是細(xì)化節(jié)點(diǎn)爬坡失敗,即潛在采樣節(jié)點(diǎn)參數(shù)θ1減小,,則轉(zhuǎn)換為擴(kuò)張狀態(tài),,否則全部繼續(xù)細(xì)化樹枝。其中qnear和qrand的距離函數(shù)可以預(yù)估下一步是細(xì)化還是擴(kuò)張,,若距離比擴(kuò)張步delta高,,那么參與樹的擴(kuò)張,相反,,距離小于delta,,認(rèn)為節(jié)點(diǎn)參與樹的細(xì)化。沿qnear至qrand取點(diǎn)qnew使其滿足距離條件,若qnew和qnear之間的路徑無障礙碰撞,,計(jì)算節(jié)點(diǎn)qnew的速度矢量角度差θ1,,并通過函數(shù)Climbtransition(θ1,T)過濾節(jié)點(diǎn),,使其按照速度矢量角度差增長方向爬坡,,以此引導(dǎo)節(jié)點(diǎn)采樣,直到滿足stopcondition2:到達(dá)目標(biāo)狀態(tài)節(jié)點(diǎn)附近或者滿足轉(zhuǎn)換條件,,即式(8),。
3 貪心細(xì)分后處理算法
采用TAS-RRT算法跳出局部極小值陷阱,可能導(dǎo)致路徑繞行,,且隨機(jī)樹的采樣特性容易產(chǎn)生冗余采樣,,因此本文引入細(xì)分貪心方法對(duì)節(jié)點(diǎn)進(jìn)行后處理,如圖4所示,,其中Root為局部極小值節(jié)點(diǎn),,Jump為采用旋轉(zhuǎn)速度矢量分析定位的吸引區(qū)跳躍點(diǎn),實(shí)線為初始規(guī)劃路徑,,由于節(jié)點(diǎn)的冗余主要來自于跳出極值吸引區(qū)的過程,,所以對(duì)Jump節(jié)點(diǎn)進(jìn)行兩次細(xì)分貪心后處理。
首先,,對(duì)節(jié)點(diǎn)Jump和Goal之間的路徑進(jìn)行分段處理,,初始化節(jié)點(diǎn)behind,即為節(jié)點(diǎn)Jump,,節(jié)點(diǎn)current從Goal開始取點(diǎn),,循環(huán)過程為連接Jump和current,并判斷是否與障礙物碰撞,,若是front=current,,繼續(xù)循環(huán)直到判斷結(jié)果為否,則behind=current,,循環(huán)結(jié)束,。其后取behind和front中間節(jié)點(diǎn)為current,連接節(jié)點(diǎn)Jump和current,,判斷節(jié)點(diǎn)Jump和current連線是否與障礙物碰撞,,若是更新front=current,若否則更新behind=current,。其后繼續(xù)循環(huán)取中間節(jié)點(diǎn)直到滿足stopcoditon3,,最終取鄰節(jié)點(diǎn)next1=behind,文中stopcoditon3為循環(huán)次數(shù),;圖中從目標(biāo)點(diǎn)Goal開始取值并與節(jié)點(diǎn)Jump連接,,最終Jump的鄰節(jié)點(diǎn)next1為behind=4,。
其后,對(duì)Jump和Start之間的路徑進(jìn)行第二次細(xì)分貪心處理,,取behind=Jump,,current從Start取點(diǎn),與第一次細(xì)分貪心處理不同之處在于,,滿足循環(huán)次數(shù)后,,繼續(xù)執(zhí)行判斷3:鄰節(jié)點(diǎn)next2是否為TAS-RRT算法產(chǎn)生的采樣節(jié)點(diǎn),,若是,,繼續(xù)對(duì)next2進(jìn)行細(xì)分貪心后處理即初始化behind=next2,從Start取點(diǎn)current并判斷next2與current是否碰撞,,不斷循環(huán)直到判斷3結(jié)果為否,,更新相應(yīng)的鄰節(jié)點(diǎn)。圖4中Jump經(jīng)過第二次細(xì)分貪心處理后next2為節(jié)點(diǎn)5′,,由于5′不是擴(kuò)展樹采樣節(jié)點(diǎn)因此循環(huán)結(jié)束,,圖中后處理路徑采用粗虛線連接。
4 仿真實(shí)驗(yàn)
4.1 旋轉(zhuǎn)速度矢量角法的適用性和有效性
結(jié)合目標(biāo)位置和環(huán)境障礙信息,,通過勢場方程計(jì)算各個(gè)節(jié)點(diǎn)的勢場強(qiáng)度,,并根據(jù)計(jì)算的結(jié)果調(diào)節(jié)仿真圖以驗(yàn)證融合算法的可行性,選擇稀疏Djkstra算法,、基于速度矢量角度差引導(dǎo)的稀疏A*算法,、RRT算法與改進(jìn)式人工勢場進(jìn)行混合,并選取具有有代表性的障礙環(huán)境以驗(yàn)證算法的性能,。
首先,,通過MATLAB建立具有開口凹槽障礙物的地圖環(huán)境,如圖5(a),,并人為設(shè)定初始坐標(biāo)[175,,410]和目標(biāo)點(diǎn)坐標(biāo)[450,130],,其后分別運(yùn)用3種混合算法進(jìn)行路徑規(guī)劃,。
其中算法1、算法2和算法3分別表示稀疏Djkstra,、稀疏AS-A*,、RRT算法與改進(jìn)人工勢場混合優(yōu)化算法。稀疏Djkstra算法和稀疏AS-A*算法相鄰節(jié)點(diǎn)間隔均取4,,由于RRT算法采樣的隨機(jī)性,,算法3進(jìn)行50次路徑規(guī)劃實(shí)驗(yàn)。3種算法失敗率為0,,結(jié)果證明了旋轉(zhuǎn)速度矢量角法的有效性,。
其次,,變換地圖為更加復(fù)雜的多正方形障礙環(huán)境,如圖5(b)所示,,由于障礙物形狀和排布特點(diǎn),,始末節(jié)點(diǎn)間存在多個(gè)局部極小值吸引區(qū)域,通過多次實(shí)驗(yàn)得到的結(jié)果如圖5(c),、圖5(d),、圖5(e),圖中規(guī)劃路徑位于人工勢場線中,,障礙物附近斥力強(qiáng),、場力線密集。從圖5可看出,,雖然障礙物數(shù)量增加,,工作環(huán)境更為復(fù)雜,但采用多種算法與改進(jìn)人工勢場進(jìn)行混合優(yōu)化,,依然能夠順利完成避障,,而且軌跡較為平滑。
兩種地圖環(huán)境下對(duì)混合算法進(jìn)行對(duì)比,,結(jié)果如表1所示,,其中:算法3的參數(shù)取20次實(shí)驗(yàn)的平均值;nto表示生成路徑包含的節(jié)點(diǎn)總數(shù),;t表示算法運(yùn)行所需總時(shí)長,;tes表示跳出局部極小值吸引區(qū)域耗費(fèi)的時(shí)間;nes1表示算法跳出一局部極小值吸引區(qū)時(shí)采樣的節(jié)點(diǎn)總數(shù),,圖5(a)取局部極小值[272,,233],圖5(b)取局部極小值[175,,325],。
對(duì)比表1中參數(shù)可知,運(yùn)行時(shí)間大部分用于跳出局部極小值吸引區(qū)域,,且無論是簡單的地圖環(huán)境還是復(fù)雜的地圖環(huán)境,,改進(jìn)式人工勢場與RRT混合優(yōu)化算法的參數(shù)皆數(shù)倍均優(yōu)于其他算法。再對(duì)地圖環(huán)境,、初始坐標(biāo)和目標(biāo)點(diǎn)坐標(biāo)進(jìn)行多次變換,,路徑規(guī)劃依然成功,說明改進(jìn)人工勢場能與多種算法融合并能適應(yīng)環(huán)境的變化,。
4.2 快速擴(kuò)展隨機(jī)樹的優(yōu)化改進(jìn)
將地圖環(huán)境設(shè)置為特殊狀態(tài),,如圖6(a),即起點(diǎn)附近存在局部極小值且始末點(diǎn)間存在長狹縫通道,運(yùn)用MATLAB對(duì)算法3和算法4進(jìn)行建模仿真,,算法3為改進(jìn)式人工勢場與RRT混合優(yōu)化算法,,算法4為改進(jìn)人工勢場與TAS-RRT混合優(yōu)化算法,兩種算法選用參數(shù)相同,,最大采樣次數(shù)取300,擴(kuò)張步長取30,,狹縫寬度L取10,,結(jié)果如圖6(b)、圖6(c),、圖6(d),、圖6(e)。從圖6(b),、圖6(c)可看出改進(jìn)人工勢場與TAS-RRT混合優(yōu)化算法可快速定位邊界點(diǎn),,而改進(jìn)人工勢場與RRT混合優(yōu)化算法在采樣300次后失敗。圖6(d)為改進(jìn)人工勢場與TAS-RRT混合優(yōu)化算法的初步路徑,,圖6(e)為其經(jīng)貪心細(xì)分后處理的路徑,,從兩圖的對(duì)比可以,,節(jié)點(diǎn)數(shù)目明顯減少,,路徑更為平滑。
進(jìn)行50次路徑規(guī)劃實(shí)驗(yàn)取參數(shù)平均值,,如表2,,表格中算法4取未經(jīng)貪心細(xì)分后處理的數(shù)據(jù),在狹縫環(huán)境下改進(jìn)人工勢場與RRT混合優(yōu)化算法成功率為60%,,改進(jìn)人工勢場與TAS-RRT混合優(yōu)化算法成功率為100%,。由于算法的差異性主要取決于與人工勢場混合的算法的選擇,因此表格著重分析跳出極小值吸引區(qū)的過程,,其中:跳躍過程中改進(jìn)人工勢場與RRT混合優(yōu)化算法平均采樣112個(gè)節(jié)點(diǎn),,改進(jìn)人工勢場與TAS-RRT混合優(yōu)化算法平均采樣數(shù)為14,極大地減少了采樣節(jié)點(diǎn)數(shù)目,,且成功跳躍時(shí)采樣節(jié)點(diǎn)平均數(shù)目nroute,、成功跳躍路徑長度Lroute、成功跳躍所耗時(shí)間tes的參數(shù)性能均優(yōu)于算法3,。
此時(shí)若將狹縫寬度調(diào)整為3,,其他參數(shù)相同,改進(jìn)人工勢場與TAS-RRT混合優(yōu)化算法結(jié)果如圖6(f),、圖6(g),、圖6(h),實(shí)驗(yàn)50次算法4成功率為100%,跳出極小值過程中采樣節(jié)點(diǎn)數(shù)目約為14,,與狹縫寬度為10時(shí)基本相同,,但是采樣時(shí)間增加了約2倍,而改進(jìn)人工勢場與RRT混合優(yōu)化算法成功率為0,。
5 結(jié)論
本文提出一種旋轉(zhuǎn)速度矢量角方法,,改進(jìn)人工勢場以解決局部極小值問題,,并與多種搜索算法優(yōu)化融合,通過分析計(jì)算以自適應(yīng)定位逃離點(diǎn),,精確控制復(fù)雜環(huán)境下的運(yùn)動(dòng)規(guī)劃,,仿真結(jié)果驗(yàn)證了算法的有效性和通用性;其次針對(duì)RRT算法穿越狹縫通道成功幾率小,、運(yùn)行效率低等問題,,提出TAS-RRT算法跳出極小值吸引區(qū)域,以速度矢量角度差引導(dǎo)節(jié)點(diǎn)漸近逃離路徑,,并根據(jù)潛在節(jié)點(diǎn)的狀態(tài)調(diào)節(jié)局部規(guī)劃器從而改變擴(kuò)展速度,,最后對(duì)路徑進(jìn)行細(xì)分貪心后處理以過濾冗余節(jié)點(diǎn)、平滑路徑,。利用MATLAB建立特殊障礙環(huán)境以驗(yàn)證算法的優(yōu)越性,,實(shí)驗(yàn)結(jié)果表明,改進(jìn)人工勢場與TAS-RRT混合優(yōu)化算法不僅成功實(shí)現(xiàn)了復(fù)雜環(huán)境下機(jī)器人運(yùn)動(dòng)規(guī)劃的控制,,而且與RRT混合優(yōu)化算法相比,,大大提高了收斂速度和運(yùn)行效率。
參考文獻(xiàn)
[1] 劉成菊,,韓俊強(qiáng),,安康.基于改進(jìn)RRT算法的RobotCup機(jī)器人動(dòng)態(tài)路徑規(guī)劃[J].機(jī)器人,2017,,39(1):8-15.
[2] 汪首坤,,朱磊,王軍政.基于導(dǎo)航勢函數(shù)法的六自由度機(jī)械臂避障路徑規(guī)劃[J].北京理工大學(xué)學(xué)報(bào),,2015,,35(2):186-191.
[3] 姬偉,程風(fēng)儀,,趙德安,,等.基于改進(jìn)人工勢場的蘋果采摘機(jī)器人機(jī)械手避障方法[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),2013,,44(11):253-259.
[4] QURESHI A H,,AYAZ Y.Potential functions based sampling heuristic for optimal path planning[J].Autonomous Robots,2016,,40(6):1079-1093.
[5] WU D,,SUN Y J,WANG X,,et al.An improved RRT algorithm for crane path planning[J].International Journal of Robotics and Automation,,2016,31(2):84-92.
[6] DU M B,,CHEN J J,,ZHAO P,,et al.An improved RRT-based motion planner for autonomous vehicle in cluttered environments[C].IEEE International Conference on Robotics and Automation. Piscataway,USA:IEEE,,2014:4674-4679.
[7] 何兆楚,,何元烈,曾碧.RRT與人工勢場法結(jié)合的機(jī)械臂避障規(guī)劃[J].工業(yè)工程,,2017,,20(2):56-63.
作者信息:
徐曉慧1,張金龍2
(1.江蘇城市職業(yè)學(xué)院 建筑工程學(xué)院,,江蘇 南京210000,;2.南京師范大學(xué) 電氣與自動(dòng)化工程學(xué)院,江蘇 南京210042)