基于自適應目標偏置系數的機械臂路徑規劃算法

吳文迎， 蔡錦達， 高朋帥

(上海理工大學 出版印刷與藝術設計學院， 上海 200093)

機械臂的路徑規劃問題一直是人工智能領域研究的熱點。路徑規劃的目的是使機械臂在一定約束條件下，從起始狀態到目標狀態，在三維空間中搜索無碰撞路徑進行操作[1]。在對機械臂做路徑規劃的同時，要滿足3個原則：①可行性，即路徑規劃必須能夠實際用于真實機械臂，而不是“紙上談兵”；②最優解，避障問題是指在有障礙物的環境中，從起點到目標點規劃一條不發生碰撞的最優路徑[2]；③防碰撞，機械臂投入使用后維修檢測都將會耗費人力財力，因此在做路徑規劃時應最大程度地避免機械臂運動中與障礙物的碰撞。

為了找出能夠同時滿足以上3個原則的路徑規劃方法，LaValle[3]提出了快速搜索隨機樹(rapid-exploration random tree，RRT)算法，該算法可以對高維空間快速搜索，通過在隨機采樣點向空白區域搜索避開模擬障礙物從而高效地解決復雜高維空間的路徑規劃問題。由于RRT算法的實際應用效率較高，越來越多的路徑規劃研究基于該算法，于是涌現出了大量RRT的改進算法。Kuffner等[4]提出了基于雙向擴展平衡的連結型雙樹RRT-connect算法，該算法在RRT算法的基礎上同時發展2棵隨機樹，分別從起始點和目標點生長，并運用貪婪策略將2棵樹連接起來，以減少搜索時間。Sertac Karaman等[5]將基于隨機采樣的路徑規劃算法與隨機幾何理論相結合，提出了一種新的快速尋優隨機圖算法(rapidly exploring random graph，RRG)，并將該算法的樹型版本稱為RRT*算法。

課題組在RRT算法的基礎上，綜合RRT-connect和RRT*算法的優勢，提出了一種基于自適應目標偏置系數的機械臂路徑規劃算法，該算法將自適應偏置系數添加到節點的拓展過程中，從而減少了隨機采樣點的數目，減少了算法運行時間。在路徑樹生成后，加入了剪枝算法來進一步優化路徑樹。與RRT算法及其已有的改進算法相比，該算法可以有效地提高搜索效率和路徑質量。

1 機械臂的運動學分析

課題組將工作末端為無影燈的6自由度機械臂作為研究平臺。建立6自由度機械臂D-H參數連桿坐標系如圖1所示，然后根據坐標系計算各關節的D-H參數。

圖1 D-H參數連桿坐標系Figure 1 D-H parametric linkage coordinate system

由D-H參數坐標系得出各個關節的D-H參數如表1所示。

表1 機械臂的D-H參數表Table 1 D-H coordinate parameters of manipulator

由于課題組對機械臂所做的路徑規劃基于給定的初始點及目標點在坐標系中的關節坐標，因此不對機械臂做逆運動學求解。

2 路徑規劃方法

RRT算法及其改進算法是一種基于隨機采樣的查詢步進式算法，在高維空間和復雜約束下的路徑規劃應用中具備良好的使用性能[6]。

2.1 基礎RRT算法

RRT算法即快速搜索隨機樹算法，原理是隨機發展出一棵路徑樹T。RRT算法主要步驟如流程圖2所示。首先初始化參數，起始點S、目標點G，擴展步長SS、偏差b等。

圖2 RRT算法流程圖Figure 2 RRT algorithm flowchart

路徑規劃問題是對所有τ∈[0,1]，尋找一條從初始配置σ(0)=S開始到達目標區域σ(1)∈G且滿足σ(τ)∈xrand的無碰撞路徑σ:[0，1]→xrand[7]。如圖3所示，這棵路徑樹從起始點S開始生長，在地圖空間中隨機采樣取點，尋找這棵路徑樹中與隨機采樣點Xrand最接近的點Xnear，以Xnear為初始點沿著Xnear-Xrand方向延長步長Δ得到新點Xnew，且此點需要與隨機采樣點不觸碰障礙地連接起來，于是對點Xnew做碰撞檢測，即檢驗Xnew和Xrand2點之間的路徑是否存在障礙。通過碰撞檢測的新點加入路徑樹中。搜索過程中不斷地在路徑樹中加入隨機采樣點，生成隨機路徑樹，直到探索到目標點G，于是生成一條從起始點到目標點的唯一路徑。

圖3 RRT算法路徑規劃的過程Figure 3 RRT algorithm path planning process

RRT算法偽代碼。

T.add(S)

fori=1 toNset

xrand←Sample( )

xnear←Nearest(T,xrand)

xnew←Extend(xnear,xrand,StepSize)

if Collectionfree(xnew,world)

T.add(xnew)

else continue

if ‖xnew-G‖≤bri

returnT, flag=ture

else flag=false

returnT, flag

其中Sample( )函數在六維空間中完成隨機點的采樣。Extend( )函數完成節點的擴展，Collectionfree( )函數完成采樣過程中的碰撞檢測。

圖4所示為RRT算法在二維障礙空間和三維障礙空間中的路徑規劃結果。RRT算法適用于多障礙物情況下6自由度機械臂的運動規劃，但該算法雖然降低了路徑規劃的計算成本，卻因為采樣點的隨機性導致收斂速度慢，路徑生成時間長，且未對生成的路徑做后處理，因此不是路徑規劃的最優解。

圖4 RRT算法路徑規劃結果Figure 4 RRT algorithm path planning results

2.2 RRT-connect算法

RRT算法從起始點發展一棵路徑樹到目標點，由于采樣點的選擇具有隨機性，因此該算法的收斂速度較慢，路徑生成效率不高。RRT-connect算法在RRT算法的基礎上提出了雙樹的構想，即從起始點區域和目標點區域同時搜索狀態空間通過隨機采樣點來各自發展一棵路徑樹。如圖5所示，在每一次迭代中，發展一棵樹的新點后，將此點作為另一棵樹的目標點進行生長，2棵樹不斷地往對方的方向交替擴展，直到2棵樹規劃出的路徑支點相連。不同于RRT算法在生成樣本并插入到路徑樹中時擴展最大擴展長度，在沒有障礙物碰撞的情況下，路徑樹繼續向目標方向擴展，因此可以更快地規劃路徑[8]。

圖5 RRT-connect算法路徑規劃的過程Figure 5 RRT-connect algorithm path planning process

RRT-connect算法偽代碼。

TaTb

fori=1 toNset

xrand←Sample( )

xnear←Nearest(Ta,xrand)

xnew←Extend(xnear,xrand, StepSize)

Ta.add(xnew)

if Getit(xnew)

flag=ture

return(Ta,Tb, flag)

else

swap(Ta,Tb)

flag=false

return(flag)

RRT-connect算法路徑規劃結果如圖6所示。

圖6 應用貪婪策略的RRT-connect算法Figure 6 RRT-Connect algorithm with greedy strategy

RRT-connect算法在處理節點時，只要一棵路徑樹上生成的新節點與另一棵路徑樹的距離比指定的閾值小，則連接2點，這容易導致路徑的連接處轉角過大，甚至出現180°的轉角[9]。較RRT算法而言，RRT-connect算法提高了搜索速度和搜索效率，2棵樹的快速生長帶有啟發性且能夠有效避免陷入局部最優，路徑樹生長的搜索過程更加貪婪和明確。

2.3 RRT*算法

RRT算法中，選取的隨機采樣點總是和距離該點最近的點連接，而不是最優的點。RRT*算法針對這一問題在RRT算法的基礎上做出改進，在隨機采樣點加入路徑樹后，RRT*算法沒有直接將最近鄰節點作為其父節點, 而是以該點周圍半徑為r的區域為目標區域尋找一個使其代價值最小的節點作為父節點[10]，即重置父節點操作。如圖7所示，標準是加入路徑樹后能夠使得該點到起點的距離更短。如果重置后的父節點可以減少路徑代價，則將新的節點加入路徑樹連接起來，摒棄原來的連接線，即重布線操作。重布線操作對每一個新點的父節點選擇增加一個優化，減少每一代的路徑距離[11]。

圖7 重置父節點操作示意圖Figure 7 Resetting parent node operation schematic

RRT*算法偽代碼。

T

fori=1 toNset

xrand←Sample( )

xnear←Nearest(T,xrand)

xnew←Extend(xnear,xrand, StepSize)

T←xnew

xnear←Findnear(xnew,T,r)

Chooseparent(xnew,xnear)

Rewrite(xnew,xnear)

if Getit(xnew)

flag=true

else

flag=false

returnT, flag

RRT*需要大量的節點回溯來判斷約束條件，從而影響收斂速度，難以應用于具有剛性實時性要求的系統[12]，在通道大且障礙物多的地圖環境中效率較低。

2.4 改進RRT算法

綜合RRT-connect算法和RRT*算法的優點，課題組提出了一種基于自適應目標偏置系數的改進RRT算法，改進后的RRT算法基于隨機采樣的方式，應用了雙樹的構想，不應用貪婪算法。在起始點區域和目標點區域各自生長一棵路徑樹，將對方作為交替生長的方向，當雙樹連接后即終止對地圖空間的搜索，將2棵路徑樹整合成為1顆從起始點到目標點的路徑樹，最后對合成的路徑樹做剪枝后處理。

2.4.1 自適應偏置算法

RRT算法具有隨機特性，因此路徑樹的生長缺乏導向性，在路徑樹的生長過程中設置1個偏置概率參數可以使得隨機采樣點更偏向于目標點[13]。

如圖8所示，設置的目標偏置系數P決定了新節點Xnew偏向隨機采樣點Xrand還是目標點G，當P的值越大說明新節點Xnew越偏向目標點G，當P的值越小說明新節點Xnew越偏向隨機采樣點Xrand。且有

圖8 應用目標偏置系數的擴展方法Figure 8 Eextended method of applying target bias coefficient

(1)

式中0

如圖9所示，應用目標偏置系數可以使得對地圖空間的探索更具目的性，但實驗發現當P較小時，路徑樹的搜索更具隨機性，但同時在地圖空間的搜索時間更長；當P較大時，路徑樹的生長更具目的性，但隨機性減小，難以處理遇到較大障礙物時的情況，搜索失敗率提高。因此在路徑樹生長過程中引入自適應偏置系數應用于新節點的選擇，根據Xnear與離它最近的障礙物的直線距離Len來設置偏置系數P。若Len較大，則增大P的值，使得路徑樹能夠更加快速往對方的方向逼近；若Len較小，則先使得路徑樹迅速規劃路線逃離障礙物，再增大P的值，迅速往目標方向逼近。引入自適應偏置系數的目標偏置算法路徑規劃結果如圖10所示。

圖9 目標偏置算法障礙空間中搜索Figure 9 Search in obstacle space of target bias algorithm

圖10 引入自適應偏置系數的目標偏置算法Figure 10 Target bias algorithm with adaptive bias coefficient

自適應偏置函數GoalExtend(xrand,xnear,G)使得路徑樹向另一棵樹的方向生長，得到其偽代碼。

xrand,xnear, G

d←dist(xnear, world)

P←Bias(d)

xnew←f(P)

returnxnew

2.4.2 重置父節點

RRT*算法利用重置父節點和節點重構2個方法來降低路徑樹生長成本，創造最優的路徑收斂效果，但在每一次的迭代中都存在重置父節點和節點重構會較大地影響路徑樹的生長速度。且局部采樣的步驟提前會使得搜索路徑樹的隨機性降低，甚至有可能降低生成的路徑樹質量。改進的RRT算法為路徑樹中生成的新節點擴大重置父節點的范圍并重置父節點，不做節點重構。改進的RRT算法中重置父節點范圍的過程如下[14]：

1) 生成節點xnew，并找出xnear的點集合；

2) 遍歷xnear點集合中的節點，將這些節點的n級父節點加入xnear′的點集合中；

3) 合并xnear與xnear′的點集合，作為xnew重選父節點的范圍。

計算并保存生成的新節點到起始點的代價函數Fmin，將生成的新節點Xnew作為中心，遍歷路徑樹上和Xnew的距離小于r且滿足函數Collectionfree( )的節點，將這些節點作為Xnew的父節點，把它們與Xnew的距離Hn計算保存下來，計算Xnew的代價函數Fn=Gn+Hn，將Fn的最小值與Fmin作比較，若最小值小于Fmin，則重置父節點，重新選用Fn的最小值所對應的節點為Xnew的父節點。

2.4.3 改進的RRT算法偽代碼

得到改進的RRT算法偽代碼。

nTTaTb

fori=1 toNset

xrand←Sample( )

xnear←Nearest(Ta,xrand)

xnew←goalExtend(xnear,xrand,Tb, StepSize)

Ta.add(xnew)

xnear←Findnear(xnew,Ta,r)

Chooseparent(xnew,xnear)

flag=false

if Getit(xnew)

flag=true

else

swap(Ta,Tb)

if flag=true

T=Ta+Tb

nT=Trim(T)

return(nT, flag)

return(flag)

圖11所示為改進后的RRT算法分別在二維障礙空間和三維障礙空間中搜索路徑的結果，可以看出改進RRT算法路徑曲率較小，長度較短，質量較高。

圖11 改進的RRT算法搜索路徑的結果Figure 11 Improved RRT algorithm for searching path results

3 路徑后處理

改進后的RRT算法雖然能夠有效提高地圖空間中的RRT搜索效率，但因其搜索能力更強，也會導致路徑的擴展方向變化較大，優化后路徑仍有許多冗余點。在有障礙物的復雜地圖空間中，冗余點過多會導致無法有效跟蹤機械臂的運動[15]。因此對得到的路徑做剪枝后處理是必要操作。

剪枝處理為路徑樹剔除冗余點，將剩下的關鍵拐點連接起來生成更優的路徑樹。本研究的剪枝處理算法Trim(T)應用了貪婪策略，路徑樹是由位姿點集nT形成的一條連接起始點和目標點的路徑。首先將路徑樹生長過程中生成的節點全部考慮，然后檢查相鄰節點的連接線是否接觸地圖空間中布置的障礙物。只要選取的2端節點的連線和障礙物沒有交叉,則刪除2端節點之間的其他節點,2端節點可以直接連接作為一個新的路徑,直到發生碰撞,將碰撞點的父節點作為新的起點,然后碰撞點再次被持久化，重復以上的步驟直到達到目標點[16]。如圖12所示，經過剪枝處理后的路徑在機械臂的實際應用中更為合理，減少了冗余點和路徑的長度。

圖12 剪枝處理前后得到的路徑Figure 12 Path obtained before and after pruning

4 實驗結果與分析

4.1 對比實驗

基于對6自由度機械臂運動過程中速度和穩定性的要求，利用MATLAB仿真軟件對課題組提出的改進RRT算法進行仿真，并將改進算法中的關鍵參數與不同算法產生的效果進行比較[17]，來判定算法的質量。

在二維的確定障礙空間內進行對比實驗，將搜索時間作為判斷路徑搜索速度的指標；將路徑長度和最大曲率作為判斷路徑質量的指標。考慮到其他因素對實驗的影響，取各算法運行100次的平均值作為結果參數，設置搜索步長SS=0.5。由于RRT*算法具有漸進最優的特性，因此實驗中發現RRT*算法搜索20 000次后生成的路徑基本一致，課題組將RRT*算法搜索20 000次后生成的路徑長度為最短路徑。實驗采用Intel Xeon(至強)W-2255@3.70 GHz的10核處理器，實驗結果如表2所示。

表2 實驗結果對比Table 2 Comparison of experimental results

對比實驗結果顯示，RRT算法搜索速度相對較慢，路徑質量也較差；RRT-connect算法因為應用了貪婪算法，因此每一次迭代都會生成多步搜索，路徑規劃的單次時間最短，搜索速度最快，但RRT-connect算法生成的路徑最大曲率相對另外2種RRT改進算法要大得多，且生成的路徑長度最大，超過RRT*算法生成的路徑長度37.5%，路徑質量較差；RRT*算法產生的路徑長度最小，最大曲率也僅僅略高于本研究中改進的RRT算法，路徑質量較好，但單次搜索時間遠高于其他3種算法，搜索速度太慢。

改進的RRT算法僅在單次運行時間上略長于RRT-connect算法，但該算法生成的路徑最大曲率相對較小，路徑長度略高于RRT*算法，超過RRT*算法規劃的路徑長度的2.6%，實驗結果顯示改進的RRT算法在保證搜索效率的基礎上，大幅提高了生成的路徑質量。

4.2 基于ROS的仿真模擬

在ROS平臺的Rviz仿真軟件中模擬手術室的工作環境[18]，將基于自適應目標偏置系數的RRT算法應用在6自由度機械臂上，通過機械臂避障規劃與控制實驗來驗證算法的有效性。圖13所示為6自由度機械臂在2種不同的環境中做路徑規劃的仿真過程[19]。

圖13 機械臂在變化環境中的路徑規劃Figure 13 Path planning of manipulator in changing environment

圖14所示為6自由度機械臂分別在這2種模擬環境中工作時各個關節的角度隨時間變化的曲線。通過曲線可以看出不同環境中關節的運動存在差異，但各關節變化的曲線都較為平滑，展現了6自由度機械臂模擬工作時穩定性較好，且冗余的路徑長度被有效減少。

圖14 仿真過程中各關節弧度變化曲線Figure 14 Curve of each joint change during simulation

5 結語

課題組提出了一種基于自適應目標偏置系數的機械臂路徑規劃算法，利用MATLAB對該算法進行仿真實驗，將實驗結果與已有的RRT算法及其改進算法的路徑規劃結果作對比分析，并在基于ROS的仿真平臺進行驗證。實驗結果表明：應用自適應目標偏置系數的改進RRT算法兼顧搜索效率和路徑質量，且應用貪婪策略的剪枝后處理算法有效減少了冗余節點和路徑長度，該算法有較好的避障性能和尋優能力，符合在實際應用中對機械臂工作穩定性的要求。課題組改進的RRT算法模擬仿真的地圖環境中只有靜態障礙物，沒有考慮到有動態障礙物的地圖環境。接下來的研究應圍繞如何將應用場景擴大到添加了動態障礙物的地圖環境中對算法作進一步改進，提高算法的實用性。