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