吳文迎， 蔡錦達， 高朋帥

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

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

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

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

1 機械臂的運動學分析

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

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

由D-H參數(shù)坐標系得出各個關節(jié)的D-H參數(shù)如表1所示。

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

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

2 路徑規(guī)劃方法

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

2.1 基礎RRT算法

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

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

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

圖3 RRT算法路徑規(guī)劃的過程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( )函數(shù)在六維空間中完成隨機點的采樣。Extend( )函數(shù)完成節(jié)點的擴展，Collectionfree( )函數(shù)完成采樣過程中的碰撞檢測。

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

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

2.2 RRT-connect算法

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

圖5 RRT-connect算法路徑規(guī)劃的過程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算法路徑規(guī)劃結果如圖6所示。

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

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

2.3 RRT*算法

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

圖7 重置父節(jié)點操作示意圖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*需要大量的節(jié)點回溯來判斷約束條件，從而影響收斂速度，難以應用于具有剛性實時性要求的系統(tǒng)[12]，在通道大且障礙物多的地圖環(huán)境中效率較低。

2.4 改進RRT算法

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

2.4.1 自適應偏置算法

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

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

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

(1)

式中0

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

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

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

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

xrand,xnear, G

d←dist(xnear, world)

P←Bias(d)

xnew←f(P)

returnxnew

2.4.2 重置父節(jié)點

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

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

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

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

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

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搜索效率，但因其搜索能力更強，也會導致路徑的擴展方向變化較大，優(yōu)化后路徑仍有許多冗余點。在有障礙物的復雜地圖空間中，冗余點過多會導致無法有效跟蹤機械臂的運動[15]。因此對得到的路徑做剪枝后處理是必要操作。

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

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

4 實驗結果與分析

4.1 對比實驗

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

在二維的確定障礙空間內進行對比實驗，將搜索時間作為判斷路徑搜索速度的指標；將路徑長度和最大曲率作為判斷路徑質量的指標?？紤]到其他因素對實驗的影響，取各算法運行100次的平均值作為結果參數(shù)，設置搜索步長SS=0.5。由于RRT*算法具有漸進最優(yōu)的特性，因此實驗中發(fā)現(xiàn)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算法因為應用了貪婪算法，因此每一次迭代都會生成多步搜索，路徑規(guī)劃的單次時間最短，搜索速度最快，但RRT-connect算法生成的路徑最大曲率相對另外2種RRT改進算法要大得多，且生成的路徑長度最大，超過RRT*算法生成的路徑長度37.5%，路徑質量較差；RRT*算法產生的路徑長度最小，最大曲率也僅僅略高于本研究中改進的RRT算法，路徑質量較好，但單次搜索時間遠高于其他3種算法，搜索速度太慢。

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

4.2 基于ROS的仿真模擬

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

圖13 機械臂在變化環(huán)境中的路徑規(guī)劃Figure 13 Path planning of manipulator in changing environment

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

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

5 結語

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