0 引言

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

1 改進人工勢場

1.1 人工勢場

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

1.2 旋轉速度矢量角

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

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

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

2.1 RRT算法

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

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

2.2 TAS-RRT算法

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

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

3 貪心細分后處理算法

    采用TAS-RRT算法跳出局部極小值陷阱，可能導致路徑繞行，且隨機樹的采樣特性容易產生冗余采樣，因此本文引入細分貪心方法對節點進行后處理，如圖4所示，其中Root為局部極小值節點，Jump為采用旋轉速度矢量分析定位的吸引區跳躍點，實線為初始規劃路徑，由于節點的冗余主要來自于跳出極值吸引區的過程，所以對Jump節點進行兩次細分貪心后處理。

    首先，對節點Jump和Goal之間的路徑進行分段處理，初始化節點behind，即為節點Jump，節點current從Goal開始取點，循環過程為連接Jump和current，并判斷是否與障礙物碰撞，若是front=current，繼續循環直到判斷結果為否，則behind=current，循環結束。其后取behind和front中間節點為current，連接節點Jump和current，判斷節點Jump和current連線是否與障礙物碰撞，若是更新front=current，若否則更新behind=current。其后繼續循環取中間節點直到滿足stopcoditon3，最終取鄰節點next1=behind，文中stopcoditon3為循環次數；圖中從目標點Goal開始取值并與節點Jump連接，最終Jump的鄰節點next1為behind=4。

    其后，對Jump和Start之間的路徑進行第二次細分貪心處理，取behind=Jump，current從Start取點，與第一次細分貪心處理不同之處在于，滿足循環次數后，繼續執行判斷3：鄰節點next2是否為TAS-RRT算法產生的采樣節點，若是，繼續對next2進行細分貪心后處理即初始化behind=next2，從Start取點current并判斷next2與current是否碰撞，不斷循環直到判斷3結果為否，更新相應的鄰節點。圖4中Jump經過第二次細分貪心處理后next2為節點5′，由于5′不是擴展樹采樣節點因此循環結束，圖中后處理路徑采用粗虛線連接。

4 仿真實驗

4.1 旋轉速度矢量角法的適用性和有效性

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

    首先，通過MATLAB建立具有開口凹槽障礙物的地圖環境，如圖5(a)，并人為設定初始坐標[175，410]和目標點坐標[450，130]，其后分別運用3種混合算法進行路徑規劃。

其中算法1、算法2和算法3分別表示稀疏Djkstra、稀疏AS-A*、RRT算法與改進人工勢場混合優化算法。稀疏Djkstra算法和稀疏AS-A*算法相鄰節點間隔均取4，由于RRT算法采樣的隨機性，算法3進行50次路徑規劃實驗。3種算法失敗率為0，結果證明了旋轉速度矢量角法的有效性。

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

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

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

4.2 快速擴展隨機樹的優化改進

    將地圖環境設置為特殊狀態，如圖6(a),即起點附近存在局部極小值且始末點間存在長狹縫通道，運用MATLAB對算法3和算法4進行建模仿真，算法3為改進式人工勢場與RRT混合優化算法，算法4為改進人工勢場與TAS-RRT混合優化算法,兩種算法選用參數相同，最大采樣次數取300，擴張步長取30，狹縫寬度L取10，結果如圖6(b)、圖6(c)、圖6(d)、圖6(e)。從圖6(b)、圖6(c)可看出改進人工勢場與TAS-RRT混合優化算法可快速定位邊界點，而改進人工勢場與RRT混合優化算法在采樣300次后失敗。圖6(d)為改進人工勢場與TAS-RRT混合優化算法的初步路徑，圖6(e)為其經貪心細分后處理的路徑，從兩圖的對比可以，節點數目明顯減少，路徑更為平滑。

    進行50次路徑規劃實驗取參數平均值，如表2，表格中算法4取未經貪心細分后處理的數據，在狹縫環境下改進人工勢場與RRT混合優化算法成功率為60%，改進人工勢場與TAS-RRT混合優化算法成功率為100%。由于算法的差異性主要取決于與人工勢場混合的算法的選擇，因此表格著重分析跳出極小值吸引區的過程，其中：跳躍過程中改進人工勢場與RRT混合優化算法平均采樣112個節點，改進人工勢場與TAS-RRT混合優化算法平均采樣數為14，極大地減少了采樣節點數目，且成功跳躍時采樣節點平均數目n_route、成功跳躍路徑長度L_route、成功跳躍所耗時間t_es的參數性能均優于算法3。

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

5 結論

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

參考文獻

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

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

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

作者信息:

徐曉慧1，張金龍2

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