0 引言

    人工勢場法是一種結(jié)構(gòu)簡單的運動規(guī)劃算法,，具有魯棒性強,、路徑平滑、執(zhí)行效率高的優(yōu)點^[1],，但在實際應(yīng)用中常在障礙物周圍產(chǎn)生局部振蕩,。目前，國內(nèi)外許多學(xué)者對此進行了探索研究,，常見的改進思路有增加虛擬障礙物,、建立虛擬目標點、改變勢函數(shù)等^[2-3],，其適用面窄,、通用性差，很難實現(xiàn)振蕩區(qū)域邊界點的精確定位,。RRT算法在運動規(guī)劃中具有良好的擴展性和搜索速度,，與人工勢場的結(jié)合可使路徑快速跳出局值吸引區(qū)，但逃離點落在狹縫之中或其附近時RRT算法易陷入局部死循環(huán)^[4-6],。針對這些不足之處,，本文提出一種基于旋轉(zhuǎn)速度矢量角的人工勢場和TAS-RRT融合優(yōu)化算法，當陷入局部振蕩時,，以旋轉(zhuǎn)速度矢量角分析運動趨勢、精確定位逃離點，并使用速度矢量角度差引導(dǎo)RRT采樣的分布,、調(diào)節(jié)局部規(guī)劃器的連接方式,，使其采樣的方向快速逼近逃離點，當采樣點滿足條件時,，切換回人工勢場算法進行運動規(guī)劃,。程序運行結(jié)果表明，該算法搜索效率高,，復(fù)雜環(huán)境下依然有效,。

1 改進人工勢場

1.1 人工勢場

    傳統(tǒng)人工勢場是一種虛擬勢場^[7]，包括吸引勢場f_a(x)和抵制勢場f_t(x)： 

1.2 旋轉(zhuǎn)速度矢量角

    針對人工勢場易陷入局部振蕩的問題,，本文提出一種旋轉(zhuǎn)速度矢量角方法,，將其作為跳出極小值吸引區(qū)域的判斷條件，如圖1所示,，其中點Root代表局部極小值點,；current為當前訪問節(jié)點，V₁為位于點current位置時對應(yīng)的速度矢量,；Next1為下一個訪問節(jié)點,，V₂為對應(yīng)的速度矢量；Next2為機器人位于點Next1位置時下一個訪問節(jié)點,，V₃為對應(yīng)的速度矢量,。

說明節(jié)點運動趨勢為遠離極值點，且其趨勢不斷增強,，則當前節(jié)點current為局部極小值吸引區(qū)的跳出點,，切換回人工勢場算法進行運動規(guī)劃。

2 基于速度矢量角度差引導(dǎo)的快速隨機擴展樹改進算法

2.1 RRT算法

    RRT算法以構(gòu)型空間中的初始狀態(tài)xs為起始點構(gòu)建隨機擴展樹,，初始化后通過迭代的方式向外擴展,，在環(huán)境范圍內(nèi)隨機采樣自由節(jié)點q_rand，遍歷樹T的所有節(jié)點,，選擇其中離q_rand最近的節(jié)點q_near,，其后通過Extend(q_rand，q_near)函數(shù)檢驗兩節(jié)點的距離是否滿足要求,，若滿足q_rand即為q_new,，否則沿著q_near至q_rand取距離為δ的狀態(tài)節(jié)點作為q_new，利用局部規(guī)劃器對q_new和q_near之間的路徑進行檢測,，若無障礙碰撞,，將其作為新的節(jié)點添加到擴展樹上，并更新父節(jié)點和新樹枝長度,。

    重復(fù)上述過程,，直到滿足stopcondition：新狀態(tài)節(jié)點與x_g構(gòu)建路徑成功或者超過最大迭代次數(shù),，結(jié)束擴展。圖形建立完畢后,，利用搜索算法即可得到x_s和x_g間最短規(guī)劃路徑,。具體搜索過程如圖2所示。

2.2 TAS-RRT算法

    RRT算法規(guī)劃的路徑質(zhì)量不高且狹縫通道難以穿越,，針對以上缺點,，本文提出一種基于速度矢量角度差引導(dǎo)的快速隨機擴展樹算法，在采樣的過程中引入速度矢量角度差,，將其作為判斷采樣點有效性的條件,，并根據(jù)潛在節(jié)點的狀態(tài)調(diào)節(jié)樹的規(guī)劃器以改變擴張速度。

    具體搜索過程如圖3所示,。將局部極小值點作為構(gòu)建擴展樹的起點,，初始化后以迭代的方式向外擴展，在環(huán)境內(nèi)隨機采樣自由點q_rand后選擇樹中最近節(jié)點q_near,，其后Expandcontrol(q_rand,，q_near，T)函數(shù)驅(qū)使規(guī)劃器自適應(yīng)地選擇探索速度,，選擇依據(jù)為樹的爬坡狀態(tài),。若是細化節(jié)點爬坡失敗，即潛在采樣節(jié)點參數(shù)θ₁減小,，則轉(zhuǎn)換為擴張狀態(tài),，否則全部繼續(xù)細化樹枝。其中q_near和q_rand的距離函數(shù)可以預(yù)估下一步是細化還是擴張,，若距離比擴張步delta高,，那么參與樹的擴張，相反,，距離小于delta,，認為節(jié)點參與樹的細化。沿q_near至q_rand取點q_new使其滿足距離條件,，若q_new和q_near之間的路徑無障礙碰撞,，計算節(jié)點q_new的速度矢量角度差θ₁，并通過函數(shù)Climbtransition(θ₁,，T)過濾節(jié)點,，使其按照速度矢量角度差增長方向爬坡，以此引導(dǎo)節(jié)點采樣,，直到滿足stopcondition2：到達目標狀態(tài)節(jié)點附近或者滿足轉(zhuǎn)換條件,，即式(8)。

3 貪心細分后處理算法

    采用TAS-RRT算法跳出局部極小值陷阱,，可能導(dǎo)致路徑繞行,，且隨機樹的采樣特性容易產(chǎn)生冗余采樣,，因此本文引入細分貪心方法對節(jié)點進行后處理，如圖4所示,，其中Root為局部極小值節(jié)點,，Jump為采用旋轉(zhuǎn)速度矢量分析定位的吸引區(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)直到判斷結(jié)果為否,，則behind=current,，循環(huán)結(jié)束。其后取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算法產(chǎn)生的采樣節(jié)點,，若是，繼續(xù)對next2進行細分貪心后處理即初始化behind=next2,，從Start取點current并判斷next2與current是否碰撞,，不斷循環(huán)直到判斷3結(jié)果為否,，更新相應(yīng)的鄰節(jié)點。圖4中Jump經(jīng)過第二次細分貪心處理后next2為節(jié)點5′,，由于5′不是擴展樹采樣節(jié)點因此循環(huán)結(jié)束,，圖中后處理路徑采用粗虛線連接。

4 仿真實驗

4.1 旋轉(zhuǎn)速度矢量角法的適用性和有效性

    結(jié)合目標位置和環(huán)境障礙信息,，通過勢場方程計算各個節(jié)點的勢場強度,，并根據(jù)計算的結(jié)果調(diào)節(jié)仿真圖以驗證融合算法的可行性，選擇稀疏Djkstra算法,、基于速度矢量角度差引導(dǎo)的稀疏A*算法,、RRT算法與改進式人工勢場進行混合，并選取具有有代表性的障礙環(huán)境以驗證算法的性能,。

    首先,，通過MATLAB建立具有開口凹槽障礙物的地圖環(huán)境，如圖5(a),，并人為設(shè)定初始坐標[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,，結(jié)果證明了旋轉(zhuǎn)速度矢量角法的有效性,。

    其次，變換地圖為更加復(fù)雜的多正方形障礙環(huán)境,，如圖5(b)所示,，由于障礙物形狀和排布特點，始末節(jié)點間存在多個局部極小值吸引區(qū)域,，通過多次實驗得到的結(jié)果如圖5(c),、圖5(d)、圖5(e),，圖中規(guī)劃路徑位于人工勢場線中,，障礙物附近斥力強、場力線密集,。從圖5可看出,，雖然障礙物數(shù)量增加,，工作環(huán)境更為復(fù)雜，但采用多種算法與改進人工勢場進行混合優(yōu)化,，依然能夠順利完成避障,，而且軌跡較為平滑。

    兩種地圖環(huán)境下對混合算法進行對比,，結(jié)果如表1所示,，其中：算法3的參數(shù)取20次實驗的平均值；n_to表示生成路徑包含的節(jié)點總數(shù),；t表示算法運行所需總時長,；t_es表示跳出局部極小值吸引區(qū)域耗費的時間；n_es1表示算法跳出一局部極小值吸引區(qū)時采樣的節(jié)點總數(shù),，圖5(a)取局部極小值[272，233],，圖5(b)取局部極小值[175,，325]。

    對比表1中參數(shù)可知,，運行時間大部分用于跳出局部極小值吸引區(qū)域,，且無論是簡單的地圖環(huán)境還是復(fù)雜的地圖環(huán)境，改進式人工勢場與RRT混合優(yōu)化算法的參數(shù)皆數(shù)倍均優(yōu)于其他算法,。再對地圖環(huán)境,、初始坐標和目標點坐標進行多次變換，路徑規(guī)劃依然成功,，說明改進人工勢場能與多種算法融合并能適應(yīng)環(huán)境的變化,。

4.2 快速擴展隨機樹的優(yōu)化改進

    將地圖環(huán)境設(shè)置為特殊狀態(tài)，如圖6(a),即起點附近存在局部極小值且始末點間存在長狹縫通道,，運用MATLAB對算法3和算法4進行建模仿真,，算法3為改進式人工勢場與RRT混合優(yōu)化算法，算法4為改進人工勢場與TAS-RRT混合優(yōu)化算法,兩種算法選用參數(shù)相同,，最大采樣次數(shù)取300,，擴張步長取30，狹縫寬度L取10,，結(jié)果如圖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)為其經(jīng)貪心細分后處理的路徑，從兩圖的對比可以,，節(jié)點數(shù)目明顯減少,，路徑更為平滑。

    進行50次路徑規(guī)劃實驗取參數(shù)平均值,，如表2,，表格中算法4取未經(jīng)貪心細分后處理的數(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ù)目n_route,、成功跳躍路徑長度L_route、成功跳躍所耗時間t_es的參數(shù)性能均優(yōu)于算法3,。

    此時若將狹縫寬度調(diào)整為3,，其他參數(shù)相同，改進人工勢場與TAS-RRT混合優(yōu)化算法結(jié)果如圖6(f),、圖6(g),、圖6(h),實驗50次算法4成功率為100%，跳出極小值過程中采樣節(jié)點數(shù)目約為14,，與狹縫寬度為10時基本相同,，但是采樣時間增加了約2倍，而改進人工勢場與RRT混合優(yōu)化算法成功率為0,。

5 結(jié)論

    本文提出一種旋轉(zhuǎn)速度矢量角方法,，改進人工勢場以解決局部極小值問題，并與多種搜索算法優(yōu)化融合,，通過分析計算以自適應(yīng)定位逃離點,，精確控制復(fù)雜環(huán)境下的運動規(guī)劃，仿真結(jié)果驗證了算法的有效性和通用性,；其次針對RRT算法穿越狹縫通道成功幾率小,、運行效率低等問題，提出TAS-RRT算法跳出極小值吸引區(qū)域,，以速度矢量角度差引導(dǎo)節(jié)點漸近逃離路徑,，并根據(jù)潛在節(jié)點的狀態(tài)調(diào)節(jié)局部規(guī)劃器從而改變擴展速度，最后對路徑進行細分貪心后處理以過濾冗余節(jié)點、平滑路徑,。利用MATLAB建立特殊障礙環(huán)境以驗證算法的優(yōu)越性,，實驗結(jié)果表明，改進人工勢場與TAS-RRT混合優(yōu)化算法不僅成功實現(xiàn)了復(fù)雜環(huán)境下機器人運動規(guī)劃的控制,，而且與RRT混合優(yōu)化算法相比,，大大提高了收斂速度和運行效率。

參考文獻

[1] 劉成菊,，韓俊強,，安康.基于改進RRT算法的RobotCup機器人動態(tài)路徑規(guī)劃[J].機器人，2017,，39(1)：8-15.

[2] 汪首坤,，朱磊，王軍政.基于導(dǎo)航勢函數(shù)法的六自由度機械臂避障路徑規(guī)劃[J].北京理工大學(xué)學(xué)報,，2015,，35(2)：186-191.

[3] 姬偉，程風(fēng)儀,，趙德安,，等.基于改進人工勢場的蘋果采摘機器人機械手避障方法[J].農(nóng)業(yè)機械學(xué)報，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é)合的機械臂避障規(guī)劃[J].工業(yè)工程，2017,，20(2)：56-63.

作者信息:

徐曉慧1,，張金龍2

(1.江蘇城市職業(yè)學(xué)院 建筑工程學(xué)院，江蘇 南京210000；2.南京師范大學(xué) 電氣與自動化工程學(xué)院,，江蘇 南京210042)