基于柵格空間的自適應GB_RRT*機械臂路徑規(guī)劃

張立彬,林后凱,譚大鵬

(浙江工業(yè)大學 機械工程學院,浙江 杭州 310014)

0 引言

面向智能制造2025，我國智能機器人技術正處于從低端制造向智能化、精益化制造轉(zhuǎn)型時期。作為智能機器人的一種，移動機械臂可以代替人在救災、采礦、高空裝配、檢測焊接、行星探測等危險環(huán)境中工作[1-4]。由于移動機械臂作業(yè)的工作環(huán)境并不固定，針對環(huán)境變化快速做出反應并尋找通往目標點的無碰撞較優(yōu)路徑成為當前機械臂避障技術攻關的難點，開展不同復雜障礙物環(huán)境下機械臂的快速避障路徑規(guī)劃具有重要的工程意義[5-7]。

避障路徑規(guī)劃指在工作空間存在障礙物的情況下，機器臂在較短時間內(nèi)規(guī)劃一條從起始點到目標點的無碰路徑[8-11]。因為機械臂屬于高維本體結(jié)構，所以路徑規(guī)劃時復雜度較高，目前在國內(nèi)外提出的多種規(guī)劃方法中比較優(yōu)秀的有采樣法和局部規(guī)劃法[12-13]。KALA[14]提出隨機路標圖法(Probabilistic Roadmap, PRM)，其采用完全隨機采樣策略將連續(xù)空間轉(zhuǎn)換為離散空間來構建路徑網(wǎng)絡圖，再采用A*等搜索算法在路線圖上快速規(guī)劃無碰路徑；曹博等[15]提出改進人工勢場法(Artificial Potential Field, APF)，在機械臂關節(jié)空間內(nèi)建立目標角度引力勢場，在笛卡爾空間內(nèi)建立末端引力勢場和障礙物斥力勢場，通過所有勢場共同作用引導機械臂運動；AKGUN等[16]結(jié)合快速搜索隨機樹算法(Rapidly exploring Random Tree star, RRT*)提出漸進最優(yōu)的B-RRT*(bidirectional RRT*)，其用RRT*代替RRT擴展找到初始路徑后，設定采樣約束減少無效節(jié)點，通過局部偏置優(yōu)化路徑上的節(jié)點至局部最優(yōu)；謝碧云等[17]提出雙樹隨機樹搜索(bi-directional Rapidly-exploring Random Tree, bi-RRT)算法，該算法結(jié)合末端姿態(tài)調(diào)整和關節(jié)自運動，利用對機械臂逆解生成目標點的樹根，在給定的障礙物環(huán)境中自動選擇某一合適的位形作為目標節(jié)點來引導搜索樹最有效地生長；李洋等[18]提出引力自適應步長RRT在工作空間對步長進行有效約束，減少了迭代次數(shù)，降低了運行時間并縮短了路徑長度；劉曉倩等[19]提出融合目標偏向優(yōu)點的目標偏向快速搜索隨機樹(Goal Bias Rapidly-exploring Random Tree, GB_RRT)，通過提升RRT的導向性來減少搜索樹葉節(jié)點的數(shù)量，從而提高RRT搜索樹的效率。

通過上述分析可以發(fā)現(xiàn)，避障路徑規(guī)劃技術在機械臂運動領域獲得了廣泛應用，并取得較好的研究結(jié)果，然而上述研究多集中于提升某一特性，尚不能適應不同的復雜障礙物環(huán)境?；谕耆S機采樣的RRT，PRM，RRT*，B-RRT*和引力自適應步長RRT在多障礙物環(huán)境下優(yōu)勢明顯，但在救災、采礦、高空裝配、焊接、行星探測等簡單障礙物工作環(huán)境下，因為機械臂采用隨機采樣策略，所以難以在較短的時間內(nèi)規(guī)劃出較優(yōu)的路徑。GB_RRT*和APF算法雖然在簡單障礙物環(huán)境中的規(guī)劃性能較好，但是APF算法在多障礙物環(huán)境工作時容易陷入局部極值點，使所規(guī)劃的路徑代價過高，而GB_RRT*算法在多障礙物環(huán)境下因存在“徘徊探索”現(xiàn)象而導致搜索時間過大。

針對上述問題，本文采用基于柵格空間的自適應目標偏向快速擴展隨機樹算法(Rapidly exploring Random Tree Star Algorithm for adaptive Goal Bias based on grid space, SAGB_RRT*)在關節(jié)空間對移動機械臂進行避障路徑規(guī)劃：①結(jié)合RRT*漸進最優(yōu)思想對GB_RRT進行優(yōu)化，使搜索路徑朝漸進最優(yōu)解收斂；②通過柵格儲存樹節(jié)點并結(jié)合柵格快速查找相鄰節(jié)點來提升算法效率；③利用open表解決目標偏向采樣策略引起的重復偏向問題；④結(jié)合open表節(jié)點數(shù)量控制偏向性生長值來縮減無效擴展，降低搜索時間和路徑代價；⑤采用可變間隔的貪心算法進一步改善規(guī)劃所得路徑的曲折性，并降低規(guī)劃所得的路徑代價。與其他避障路徑規(guī)劃算法相比，本文算法在不同復雜障礙物環(huán)境下能夠用更短的規(guī)劃搜索時間規(guī)劃出更優(yōu)的避障路徑。本文研究可以為路徑規(guī)劃領域的理論研究提供有益的參考，也可以為智能機器人的系統(tǒng)研發(fā)提供技術支持。

1 GB_RRT*算法原理

GB_RRT*算法是結(jié)合RRT*漸進最優(yōu)思想對目標偏向RRT進行優(yōu)化，使搜索路徑朝漸進最優(yōu)解收斂，其主要通過擴展隨機樹、重選父節(jié)點和重布線對避障路徑進行優(yōu)化[20-21]。

如圖1所示，擴展隨機樹時首先將初始節(jié)點Xinit作為所構建隨機樹T的根節(jié)點，在擴展下一節(jié)點時主要采用目標偏向閾值P控制采樣節(jié)點Xrand的生長，來影響隨機樹擴展的方向，降低了隨機樹搜索過程中的無目的性。閾值P對采樣節(jié)點Xrand的控制是通過比較隨機產(chǎn)生概率Prand和閾值P的大小來實現(xiàn)，主要有兩種情況：

(1)當Prand≥P時，隨機樹傾于向未探索過的空間擴展，即在空間中隨機選擇任意狀態(tài)點作為Xrand。

(2)當Prand

通過上述方式確定Xrand后，遍歷隨機樹找到距離Xrand最近的樹節(jié)點Xnear，在碰撞檢測合格后經(jīng)固定步長擴展得到新節(jié)點Xnew，然后將Xnew添加到隨機樹中并記錄其父節(jié)點和節(jié)點權值。

如圖2所示，重選父節(jié)點的意義在于通過鄰域r內(nèi)的其他節(jié)點降低Xnew節(jié)點的權值。首先計算Xnew連接鄰域r內(nèi)所有節(jié)點的權值，若存在能使Xnew權值更小且不碰撞的Xmin節(jié)點，則刪除Xnew與原父節(jié)點的連線，同時將Xmin節(jié)點作為新的父節(jié)點，更新Xnew的權值。

如圖3所示，重布線過程的意義在于通過Xnew降低其他節(jié)點的權值，在不與障礙物碰撞的前提下，若其他節(jié)點通過將Xnew作為父節(jié)點來降低權值，則將Xnew節(jié)點作為其新父節(jié)點并更新節(jié)點權值。

2 SAGB_RRT*算法

雖然GB_RRT*算法具有漸進最優(yōu)的特點，但是在為移動機械臂規(guī)劃時無法給予CPU足夠的運算時間，而且在不同環(huán)境下無法變換目標偏向閾值，缺乏對環(huán)境的適應性，造成算法無法在不同復雜障礙物環(huán)境下用較短的時間規(guī)劃出較優(yōu)的避障路徑[22]。本文基于GB_RRT*框架，提出適合移動機械臂在救災、采礦、高空裝配、焊接、行星探測等環(huán)境下的SAGB_RRT*(grid space-based adaptive goal bias rapidly exploring random tree star)運動規(guī)劃算法，該算法在原算法概率完備和漸進最優(yōu)性不變的前提下，通過構建全狀態(tài)空間柵格地圖的數(shù)據(jù)存儲結(jié)構優(yōu)化查找相鄰節(jié)點的方法來提升計算速度，采用open表生長策略優(yōu)化隨機樹的生長方式以避免重復偏置并控制偏向性生長，結(jié)合可變間隔貪婪算法以快速優(yōu)化已規(guī)劃路徑，使機械臂在適應各種環(huán)境的同時能用較短的搜索時間規(guī)劃出較優(yōu)的避障路徑。

2.1 節(jié)點查找優(yōu)化

機械臂屬于高維本體結(jié)構，為其規(guī)劃路徑時需要數(shù)量龐大的樹節(jié)點，造成GB_RRT*算法全遍歷查找相鄰節(jié)點的復雜度呈指數(shù)上升，導致算法搜索時間爆炸式增長。為避免原算法在查找相鄰節(jié)點時遍歷所有樹節(jié)點的時間代價過大，本文算法結(jié)合工作空間的大小構建全狀態(tài)空間的柵格地圖來存儲數(shù)據(jù)，通過映射方式分塊整理樹節(jié)點，并通過查找柵格遍歷部分樹節(jié)點，從而降低算法搜索時間。

在二維空間內(nèi)對柵格地圖的原理進行表述，如圖4所示，在確保小柵格邊長s大于步長λ和重布線半徑r的前提下，結(jié)合工作空間大小確定柵格s的值后構建柵格地圖。

構建柵格地圖后，每個節(jié)點存儲時都有對應的柵格，通過各軸向劃分柵格數(shù)和節(jié)點柵格坐標得到節(jié)點的柵格索引值，可以實現(xiàn)節(jié)點和柵格索引值的映射。隨機樹任意節(jié)點的柵格坐標均由其已知工作空間坐標換算得到，具體為：

(1)

(2)

式中：函數(shù)int表示對所求值取整；(x0，y0)為節(jié)點位置坐標，(x，y)為樹節(jié)點柵格坐標；s為每個小柵格的邊長。

通過構建映射關系快速查找相鄰節(jié)點的原理如圖5所示，在擴展生長和重選父節(jié)點查找Xnear時，改進算法只需通過映射關系得到其柵格索引值，即可快速查找Xrand周邊的柵格群，然后在Xrand周邊柵格群遍歷查找樹節(jié)點Xnear。如圖6所示，在重布線階段查找χnear時，改進算法通過映射關系得到的柵格索引值快速查找到Xnew周邊柵格群，然后在Xrand周邊柵格群遍歷查找樹節(jié)點群χnear。

本文通過優(yōu)化節(jié)點查找方式提升規(guī)劃效率。因為構建柵格地圖只是簡單地根據(jù)工作空間確定每個小存儲柵格的大小，相比全遍歷樹節(jié)點查找相鄰節(jié)點的方式，其在時間和空間的復雜度基本忽略不計，所以對算法規(guī)劃性能的分析更多在查找相鄰節(jié)點的時間和空間復雜度上。然而，隨著隨機樹的不斷生長，樹節(jié)點不斷增多，原算法在擴展生長、重選父節(jié)點和重布線時查找相鄰節(jié)點所采用的方式是遍歷所有樹節(jié)點，其時間復雜度與樹節(jié)點個數(shù)為指數(shù)倍關系，改進算法只需從當前節(jié)點所在柵格與其周邊柵格中的樹節(jié)點遍歷查找相鄰節(jié)點，時間復雜度維持在一定范圍內(nèi)，大大降低了時間復雜度。

為了驗證查找節(jié)點優(yōu)化方法的有效性，在障礙物、步長等條件均相同的情況下，分別采用原算法和GB_RRT*算法對6關節(jié)機械臂進行規(guī)劃。比較相同迭代次數(shù)所用的CPU計算時間如圖7所示，可見在相同迭代次數(shù)下，基于柵格化RRT*所需的搜索時間明顯小于RRT*算法，其中當?shù)螖?shù)為30 000時所需的搜索時間從1 695.84 s下降到679.17 s，降低了65%，可知結(jié)合工作空間的大小構建全狀態(tài)空間的柵格地圖進行節(jié)點的相互映射，通過映射關系快速查找相鄰節(jié)點的方法能夠有效降低算法因樹節(jié)點數(shù)量增加而導致的計算量指數(shù)爆炸問題，從而證明本文查找節(jié)點優(yōu)化方法的有效性。

2.2 生長方式優(yōu)化

GB_RRT*算法在規(guī)劃時由于目標偏向會出現(xiàn)重復，造成時間浪費。當選擇的P值較低時，由于隨機樹生長偏向性弱，GB_RRT*算法在簡單環(huán)境中會進行大量無效搜索；當P值較高時，由于隨機樹生長偏向性強，GB_RRT*算法在復雜情況下，尤其是障礙物遮擋目標時，極易陷入局部偏向而重復產(chǎn)生大量失敗的生長，使算法對環(huán)境的適應性較差。為避免重復偏向并保證隨機樹生長時對環(huán)境有較強的適應性，引入自適應學習的思想并構建可存儲目標偏向生長點的open表，將成功擴展所得的Xnew作為目標偏向生長點存入open表，在進行隨機樹擴展時，從open表中查找離Xgoal最近的Xnear節(jié)點生長,不管生長能否成功均將Xnear節(jié)點作為不可偏向生長點從open表中刪除，通過節(jié)點擴展時的搜索方向和反饋的搜索狀態(tài)控制open表中的節(jié)點數(shù)量來控制目標偏向閾值的變化，從而自適應調(diào)整目標偏向閾值P。

通過自適應調(diào)整P值來增強算法對環(huán)境的感知能力，其原理在于，open表中的節(jié)點數(shù)量越大，可偏向生長的樹節(jié)點越多，應該縮減隨機樹探索未知空間的能力，而將主要精力放在目標偏向生長上，即增大P值；open表中的節(jié)點數(shù)量越少，可偏向生長的樹節(jié)點越少，繼續(xù)向目標偏向生長極易造成堆積在障礙物附近的樹節(jié)點過多，因此當節(jié)點數(shù)量較少時，應增強隨機樹探索未知空間的能力，避免隨機樹陷入局部障礙物處難以自拔，即減小P值。圖8所示為自適應調(diào)整目標偏向閾值P的基本框架。

由于SAGB_RRT*算法在規(guī)劃時對open表中節(jié)點的數(shù)量完全可知，可以通過調(diào)節(jié)open表中的節(jié)點數(shù)量在某一范圍內(nèi)自適應調(diào)整目標偏向閾值P，即

(3)

式中：Pmin為目標偏向的最小閾值，Pmax為目標偏向的最大閾值；Knode為open表中當前節(jié)點的數(shù)量；Kmax為open表中所能存放節(jié)點數(shù)量的最大值，

(4)

式中：α為系數(shù)；‖Xgoal-Xstart‖為歐拉范數(shù)定義的目標點到起點的距離；γ為機械臂關節(jié)維度的權值；θ為工作空間的維度；λ為隨機樹擴展的步長。

由式(3)和式(4)可知，引入自適應學習思想生成目標偏向閾值P的表達式為

P=Pmin+(Pmax-Pmin)·

(5)

采用上述方式，結(jié)合open表使隨機樹在未偏向生長的節(jié)點中選擇偏向生長，以避免隨機樹重復偏向生長，從而縮減無效擴展，提升算法效率。通過自適應調(diào)整目標偏向閾值P，使算法在規(guī)劃時能夠用更少的迭代次數(shù)、更強的偏向性令路徑快速收斂。

為進一步證明優(yōu)化SAGB_RRT*算法生長方式的效果，在5種不同環(huán)境下分別采用SAGB_RRT*算法和不同閾值的GB_RRT*算法進行規(guī)劃，如圖9所示。由于兩種算法均基于采樣使規(guī)劃結(jié)果具有不確定性，為使實驗結(jié)果更具說服力，每個算法均仿真分析15次，結(jié)果取平均值。由圖9a和圖9c可見，在5種環(huán)境下，SAGB_RRT*算法更優(yōu)的原因在于引入open表解決了偏向重復，同時采用目標閾值P自適應優(yōu)化了生長方式；由圖9b和圖9d可見，相比GB_RRT*算法，SAGB_RRT*算法所需的樹節(jié)點和失敗生長點均較少，證明了本文SAGB_RRT*算法的有效性。

2.3 路徑優(yōu)化

GB_RRT*是基于隨機采樣的搜索算法，雖然利用目標偏向生長和重布線策略進行優(yōu)化，但是仍然存在曲折路徑，造成最終規(guī)劃的路徑代價較高。貪婪算法能夠?qū)σ阎窂竭M行貪心優(yōu)化，使其不斷接近最優(yōu)解，為了能在較短時間內(nèi)使路徑總代價最低，引入可變間隔m的貪心算法對SAGB_RRT*算法規(guī)劃得到的路徑進行快速優(yōu)化，其中路徑代價目標函數(shù)

(6)

式中：C為機械臂相鄰關節(jié)角的路徑代價；θi為機械臂的第i關節(jié)角。

利用可變間隔m的貪心算法對已知路徑進行優(yōu)化，首先將Xinit作為起始節(jié)點，為保證算法的時效性，依次優(yōu)化從起始節(jié)點到相隔m值的后續(xù)節(jié)點的路徑，將起始節(jié)點和后續(xù)節(jié)點通過正運動學求解后得到機械臂的運動狀態(tài)，并在笛卡爾空間求得機械臂在該運動路徑下的碰撞情況。當與障礙物碰撞時，結(jié)合起始節(jié)點的當前后續(xù)節(jié)點和上個后續(xù)節(jié)點依次逆向查找，若從起始節(jié)點到所查節(jié)點的路徑與障礙物不碰撞則停止搜索。將起始節(jié)點與該節(jié)點相連作為新的路徑，以該節(jié)點為起始節(jié)點繼續(xù)進行間隔m的貪婪搜索，如此反復直到終點，停止搜索。

通過上述方法優(yōu)化已知路徑，當已知路徑中的障礙物較少時采用較大的可變間隔，以減少碰撞檢測次數(shù)，加快規(guī)劃速度，因此應根據(jù)起始目標距離、所得路徑和搜索時的失敗生長點數(shù)選擇m值，具體為

(7)

式中：dist(path)為已規(guī)劃路徑；dist(Xgoal,Xstart)為起點到終點的距離；Qinvalid為失敗生長節(jié)點數(shù)。

3 算法實現(xiàn)

基于上述改進思想，為保證移動機械臂在救災、采礦、高空裝配、檢測焊接、行星探測等危險環(huán)境中能夠用較短的搜索時間快速規(guī)劃較優(yōu)的避障路徑，提出一種避障規(guī)劃SAGB_RRT*算法，其偽代碼如下：

算法1SAGB_RRT*。

1. Wg←Workspace_Grid(W)

2. T←Initialize_Tree()

3. For k=1:k do

4. P←Update_P(Kopen,Kmax)

5. Prand←rand(0～1)

6. If Prand>P

7. Xrand←Random_State()

8. else

9. Xrand←Xgoal

10. χrandgrids←Find_Grids_Near(Xrand,Wg)

11. Xcloset←Find_nodes_Near(χrandgrids,Xrand)

12. Xnew←Steer(Xrand,Xcloset,λ)

13. If Obstacle_Free(Xnew)

14. Xnew←New_State(Xcloset,λ),Open←Add(Xnew)

15. If Xrand=Xgoal

16. Open←Delete(Xcloset)

17. Xmin=Xnear,Cmin=cost(Xnear)+dist(Xnear,Xnew)，Xnewparent←Xcloset

18. χnewgrids←Find_Grids_Near(Xnear,Wg)

19. χnear←Find_nodes_Near(χnewgrids,Xnew,r)

20. For Xnear∈χneardo

21. Cnew=cost(Xnear)+dist(Xnear,Xnew)

22. If Cnew

23. Cmin=Cnew,Xmin=Xnear,Xnewparent←Xnear

24. For Xnear∈χneardo

25. Cold=cost(Xnew),Cnew=cost(Xnear)+dist(Xnear,Xnew)

26. If Cnew

27. Cmin=Cnew,Xmin=Xnew,Xnewparent←Xnew

28. If dist(Xnew,Xgoal)≤r and line(Xgoal,Xnew)∈Xfree

29. path←Getpath()

30. Break

31.Path←GreedyOptimize(path)

SAGB_RRT*算法進行路徑規(guī)劃的主要步驟為：第1步結(jié)合工作空間劃分柵格空間，并利用柵格存儲和映射關系快速查找相鄰節(jié)點；第2步初始化時，定義6關節(jié)移動機械臂的起始關節(jié)數(shù)組Xstart、目標關節(jié)數(shù)組Xgoal和關節(jié)搜索步長λ；第3步～第9步通過反饋open表中的可擴展節(jié)點數(shù)來更新目標偏向閾值P，比較隨機概率Prand和P的大小確定隨機節(jié)點的Xrand，通過Xrand控制隨機樹的生長；第10步～第12步為了快速擴展節(jié)點Xnew，利用Xrand所在柵格的索引值在柵格空間快速查找離Xrand所在柵格最近且含有樹節(jié)點的柵格群，在這些柵格群中迅速找到離Xrand最近的節(jié)點Xcloset，利用Xcloset生長一個步長得到Xnew；第13步～第16步對Xcloset～Xnew的運動進行碰撞檢測，在碰撞檢測合格的前提下按照Xnew生長結(jié)果更新open表，從而避免隨機樹重復偏向生長；第17步～第23步通過Xnew重選父節(jié)點策略來優(yōu)化路徑，其關鍵為在節(jié)點Xnew通過其他節(jié)點獲得更低權值時，更新父節(jié)點并更新權值，主要創(chuàng)新為結(jié)合柵格群χnewgrids快速找到Xnew相鄰節(jié)點群χnear；第24步～第27步通過重布線優(yōu)化路徑，然后結(jié)合相鄰節(jié)點群χnear找到Xnew相鄰半徑r內(nèi)通過Xnew獲得更低權值的節(jié)點，更新父節(jié)點和權值；第28步～第30步為算法循環(huán)停止的條件，當Xnew距離Xgoal小于r且與Xgoal連線和障礙物不碰撞時得到避障路徑，循環(huán)停止；第31步用間隔m貪心算法快速優(yōu)化SAGB_RRT*已規(guī)劃出的路徑，使所規(guī)劃的路徑代價更加接近最優(yōu)解。

4 仿真與實驗

為驗證算法性能，以日本三菱公司RV-2F為研究對象，采用改進D-H(Denavit-Hartenberg)法建立機械臂運動學模型，通過逆運動學求解確立機械臂在關節(jié)空間的起始位置和目標位置后，在關節(jié)空間對機械臂路徑進行采樣，并在笛卡爾空間對采樣結(jié)果進行碰撞檢測。

如圖10所示為RV-2F的三維模型，其有6個自由轉(zhuǎn)動關節(jié)，屬于一種常見的連桿串聯(lián)機器人結(jié)構。RV-2F關節(jié)1的軸線垂直基座朝上；關節(jié)2和關節(jié)3的軸線水平且平行，距離為a2；關節(jié)3和關節(jié)4的軸線相互垂直，距離為a3。圖11所示為采用改進D-H法建立的RV-2F運動學坐標系。

表1所示為采用改進D-H對RV-2F運動學建模后的尺寸參數(shù)，通過測量機械臂可知d1=295 mm，d4=270 mm，d6=70 mm，a2=230 mm，a3=50 mm。

表1 RV-2F機械臂的D-H參數(shù)

表2所示為仿真實驗時的機械臂運動控制參數(shù)，其中起點位置和終點位置是通過對機器臂進行逆運動學求解得到的關節(jié)空間內(nèi)的坐標加權，系數(shù)是多次試驗得到的較優(yōu)值。

表2 機械臂運動控制參數(shù)表

本次仿真實驗采用的計算機處理器為Inter(R) Core(TM) i5-8500 CPU @ 3.00 GHz 3.00 GHz，內(nèi)存為8 G，實驗平臺為MATLAB R2016b。

4.1 仿真分析

4.1.1 SAGB_RRT*算法適應性

為驗證SAGB_RRT*算法在不同環(huán)境下的適應性，選取5種典型環(huán)境進行多次仿真實驗，如圖12所示。環(huán)境1是有一個半徑為70 mm的障礙物，圓心在空間的坐標為(75,54,395)；環(huán)境2有2個半徑為45 mm的障礙物，圓心在空間的坐標為(75,14,395)，(75,104,395)；環(huán)境3有3個半徑為45 mm的障礙物，圓心在空間的坐標為(75,14,395)，(75,104,335)，(75,84,475)；環(huán)境4有4個半徑為45 mm的障礙物，圓心在空間的坐標為(55,14,395)，(75,104,335)，(75,74,475)，(235,134,365)；環(huán)境5有5個半徑為45 mm的障礙物，圓心在空間的坐標為(5,14,395)，(75,104,335)，(75,74,475)，(235,134,365)，(0,74,550)。采用SAGB_RRT*算法在這5種復雜環(huán)境下為機械臂進行路徑規(guī)劃?？梢?，不同復雜度環(huán)境下，SAGB_RRT*均能快速規(guī)劃出符合機械臂運動限制條件的避障路徑。

4.1.2 SAGB_RRT*算法的有效性

為進一步驗證SAGB_RRT*算法的有效性，在4.1.1節(jié)環(huán)境下，分別采用SAGB_RRT*算法和不同閾值的GB_RRT*算法進行15次仿真規(guī)劃，結(jié)果取平均值。如圖13a和圖13b所示，在不同環(huán)境下，SAGB_RRT*算法比不同固定閾值P的GB_RRT*算法規(guī)劃得到的路徑均更優(yōu)，路徑長度降低約30%，而且在多次仿真實驗規(guī)劃出的路徑中，SAGB_RRT*算法比GB_RRT*算法的穩(wěn)定性更好。如圖14c所示，在5種不同的環(huán)境中，由于引入柵格法存儲、open表存儲和自適應調(diào)整目標閾值，SAGB_RRT*算法比GB_RRT*算法規(guī)劃所需的搜索時間更具優(yōu)勢，降低約50%。如圖14d所示，在不同環(huán)境下的15次仿真實驗中，SAGB_RRT*比GB_RRT*所需的搜索時間更穩(wěn)定。綜上所述，相比固定閾值P的GB_RRT*算法，SAGB_RRT*算法對環(huán)境具有較強的感知能力，其能根據(jù)環(huán)境調(diào)整隨機樹擴展的偏向性，保證機械臂在各種復雜的工程環(huán)境下以較短的時間規(guī)劃出最優(yōu)且最穩(wěn)定的避障路徑。

4.1.3 不同算法對比

在4.1.2節(jié)的5種障礙物環(huán)境中，分別比較SAGB_RRT*算法與蟻群優(yōu)化(Ant Colony Optimization, ACO)算法、PRM、APF+RRT混合算法、GB_RRT*算法的規(guī)劃能力。圖14所示為將5種復雜環(huán)境中采用不同算法仿真分析15次后得到的結(jié)果取平均值進行比較，數(shù)據(jù)顯示SAGB_RRT*算法規(guī)劃路徑所需的平均搜索時間和平均代價均優(yōu)于其他算法。

如圖14a所示，SAGB_RRT*算法在5種環(huán)境下均能穩(wěn)定地用較少的搜索時間規(guī)劃出路徑，其優(yōu)勢相比ACO和PRM尤為明顯，主要原因為ACO和PRM在保證采樣次數(shù)的基礎上才能搜索出較優(yōu)路徑，導致其在有限搜索時間內(nèi)的算法性能不佳。如圖14b所示，SAGB_RRT*算法在5種環(huán)境下均能穩(wěn)定地規(guī)劃出較優(yōu)的路徑,其優(yōu)勢相比APF+RRT尤為明顯，主要原因為APF在高維環(huán)境下規(guī)劃時極易陷入局部極值點，RRT將其從局部極值點引出時會“繞路”，導致APF+RRT規(guī)劃的路徑較長。SAGB_RRT*算法也明顯優(yōu)于GB_RRT*，主要原因在于GB_RRT*采用固定的目標偏向閾值，當初始化的目標偏向閾值較高時，雖然可以保證其規(guī)劃的路徑較短，但是卻極易因陷入局部搜索而增大搜索時間；當初始化的目標偏向閾值較低時，會因搜索漫無目的而增大路徑代價和搜索時間。

4.2 實驗驗證

為進一步驗證SAGB_RRT*路徑規(guī)劃方法的有效性和可行性，通過實驗對仿真結(jié)果進行驗證，如圖16所示。圖中采用SAGB_RRT*算法為機械臂規(guī)劃起始位姿到目標位姿的避障路徑，將規(guī)劃所得的機械臂運動路徑導入機器人示教器，并設置機械臂的速度比率為2。重復上述操作并進行20次實驗，機械臂都能避開障礙物且平均運行用時25.8 s。再采用ACO，PRM，APF+RRT，GB_RRT*進行實驗，平均運行時間分別為28.5 s，26.6 s，28.7 s，27.3 s，可見SAGB_RRT*算法所規(guī)劃路徑的平均運行時間小于其他算法。通過實驗可知，采用SAGB_RRT*算法對機械臂進行路徑規(guī)劃不但有效，而且所規(guī)劃出的路徑比其他算法更優(yōu)更短。

5 結(jié)束語

本文通過在救災、采礦、高空裝配、檢測焊接和行星探測等環(huán)境下分析移動機械臂的作業(yè)需求，提出在關節(jié)空間內(nèi)采用SAGB_RRT*算法進行路徑避障規(guī)劃，該算法結(jié)合RRT*漸進最優(yōu)思想對GB_RRT*算法進行優(yōu)化，借助柵格儲存數(shù)據(jù)快速查找相鄰節(jié)點，并引入open表解決目標偏向擴展帶來的重復偏向問題，提出通過open表節(jié)點數(shù)量變化自適應控制偏向性生長，并利用可變間隔的貪心算法對已規(guī)劃的路徑進行優(yōu)化，從而在復雜障礙物環(huán)境下用較短的搜索時間快速規(guī)劃出較優(yōu)的避障路徑。仿真實驗顯示，相比改進前，SAGB_RRT*算法進行規(guī)劃時所需的平均搜索時間降低了50%，規(guī)劃的平均路徑降低了30%，而且大幅提升了規(guī)劃的穩(wěn)定性；相比其他算法，SAGB_RRT*算法能夠很好地兼顧搜索時間和路徑代價，在進行避障的同時保證規(guī)劃所需的平均搜索時間和所規(guī)劃出的平均路徑均為最優(yōu)，因此采用本文所提的SAGB_RRT*算法對移動機械臂進行路徑避障規(guī)劃是有效的?；赟AGB_RRT*算法的路徑避障規(guī)劃方法，能夠在較短的搜索時間內(nèi)規(guī)劃出較優(yōu)的避障路徑，為移動機械臂在危險環(huán)境下進行快速路徑規(guī)劃提供技術支撐。