《電子技術(shù)應(yīng)用》
您所在的位置:首頁 > 測試測量 > 設(shè)計(jì)應(yīng)用 > 改進(jìn)人工勢場與TAS-RRT融合優(yōu)化算法
改進(jìn)人工勢場與TAS-RRT融合優(yōu)化算法
2018年電子技術(shù)應(yīng)用第10期
徐曉慧1,張金龍2
1.江蘇城市職業(yè)學(xué)院 建筑工程學(xué)院,江蘇 南京210000,;2.南京師范大學(xué) 電氣與自動(dòng)化工程學(xué)院,江蘇 南京210042
摘要: 針對(duì)人工勢場法易陷入局部極小值的缺陷,,提出旋轉(zhuǎn)速度矢量角以精確定位逃離點(diǎn),,并將TAS-RRT算法與人工勢場算法結(jié)合進(jìn)行動(dòng)態(tài)路徑規(guī)劃。采用人工勢場法進(jìn)行避障規(guī)劃,,當(dāng)陷入局部最小值時(shí),,使用基于速度矢量角度差引導(dǎo)的快速隨機(jī)擴(kuò)展樹算法調(diào)節(jié)擴(kuò)張速度,,自適應(yīng)地尋找逃離點(diǎn),對(duì)RRT算法的采樣策略和局部規(guī)劃器進(jìn)行改進(jìn),,使搜索過程快速跳出局部極小值,,當(dāng)采樣點(diǎn)的旋轉(zhuǎn)速度矢量角滿足條件時(shí),切換人工勢場進(jìn)行規(guī)劃,。仿真實(shí)驗(yàn)表明,,TAS-RRT算法引導(dǎo)路徑快速漸進(jìn)逃離點(diǎn),與人工勢場結(jié)合進(jìn)行運(yùn)動(dòng)規(guī)劃,,能適應(yīng)環(huán)境的變化,,控制精度和處理速度得到大大提高。
中圖分類號(hào): TP242
文獻(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.
Hybrid optimization algorithm of improved artificial potential field and TAS-RRT
Xu Xiaohui1,,Zhang Jinlong2
1.School of Architectural Engineering,,The City Vocational College of Jiangsu,Nanjing 210000,,China,; 2.School of Electrical and Automation Engineering,Nanjing Normal University,,Nanjing 210042,,China
Abstract: To solve the problem of artificial potential field algorithm(APF) being liable to fall into local minimum,this paper presents a strategy based on angle of rotating speed vectors to accurately locate the position of jump point.Combined with improved APF,the RRT based on transition of angle different of rotating speed vectors(TAS-RRT)can be used to path planning dynamicly. Firstly, artificial potential field algorithm is used in obstacle avoidance motion planning. When falling into local minimum,the sampling strategy of the basic RRT is improved to adaptively seek the target point and the connection mode of the local planner is adjusted to change exploration rate of the tree,so that the search process can be made to jump out of the attractive areas of local minimum point quickly.In addition, APF will be applied again when the the sampling point meets the stop condition of angle of rotating speed vectors. The simulation experiments show that the control precision and velocity of the motion planning are enhanced with TAS-RRT guiding the sampling nodes to the target one gradually and quickly.Besides that,the method can be applied to motion planning of different obstacle environment.
Key words : motion planning of robot,;artificial potential field,;angle of rotating speed vectors;TAS-RRT

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): 

ck3-gs1-4.gif

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)的速度矢量,。

ck3-t1.gif

ck3-gs5.gif

ck3-gs6-8.gif

說明節(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所示。

ck3-t2.gif

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),。

ck3-t3.gif

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ì)分貪心后處理。

ck3-t4.gif

    首先,,對(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)化,,依然能夠順利完成避障,,而且軌跡較為平滑。

ck3-t5.gif

    兩種地圖環(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],。

ck3-b1.gif

    對(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ù)目明顯減少,,路徑更為平滑。

ck3-t6.gif

    進(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,。

ck3-b2.gif

    此時(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)

此內(nèi)容為AET網(wǎng)站原創(chuàng),,未經(jīng)授權(quán)禁止轉(zhuǎn)載,。