張婷婷，柳林燕，汪惠芬

(南京理工大學(xué)機械工程學(xué)院，南京 210094)

0 引言

關(guān)節(jié)機械臂是目前工業(yè)場景中應(yīng)用最廣泛的工業(yè)機械臂種類之一，其中6自由度機械臂的應(yīng)用率最高，例如噴漆，裝配等工作。因為6自由度的機械臂具有：第一，基本可以到達滿足工作空間的所有點；第二，可以通過控制連續(xù)實現(xiàn)復(fù)雜的運動。因此需要對關(guān)節(jié)機械臂進行路徑規(guī)劃，使其更好地完成指定任務(wù)。

機械臂的路徑規(guī)劃即從起始點開始朝著目標(biāo)點運動，躲避整個空間中出現(xiàn)的障礙物，并且最終找到一個最優(yōu)或者相對優(yōu)化的路徑。目前已有A*，RRT，隨機路標(biāo)圖(probabilistic roadmap method,PRM)等成熟的算法。

傳統(tǒng)的RRT算法，雖然概率完備但是每次試驗得出的路徑并不是最優(yōu)的，并且有很多的冗余點?；诖祟悊栴}，孫豐財?shù)萚1]加入了偏向策略，實現(xiàn)了時間優(yōu)化，但是其在三維空間中的路徑優(yōu)化并不明顯。在此之后，提出一種漸進最優(yōu)性(距離最短)的RRT*算法[2-3]，該算法在RRT的算法上比對多個父節(jié)點，找到實現(xiàn)每個隨機點路徑的最短距離，最后實現(xiàn)最終路徑最短并且可以實現(xiàn)更高的搜索成功率，但RRT*有每次搜索時相似度不高，路徑結(jié)果差異較大等缺點。Informed-RRT*算法采用的是先驗知識的改進操作，即先利用RRT*算法找到第一條路徑的前提下，對該路徑進行優(yōu)化[4-5]，但是因為基于的RRT*采樣就是隨機的，無法保證首次的路徑就是最優(yōu)的，會導(dǎo)致對此條路徑做優(yōu)化，無法得到最優(yōu)解。上述算法在二維空間中已有詳細的驗證，但在三維空間中會出現(xiàn)并不理想的結(jié)果。

本文基于三維空間的復(fù)雜條件下，針對傳統(tǒng)的RRT算法，進行目標(biāo)導(dǎo)向處理使得隨機樹更趨向于目標(biāo)點，再進行冗余檢測剔除多余的節(jié)點，對處理后的隨機樹進行平滑處理，更加符合六自由度機械臂的空間運動。

1 算法基礎(chǔ)

1.1 機械臂運動學(xué)建模

本文采用的是某品牌型號為ER10-900的機械臂，工作空間是900 mm。建立機械臂的數(shù)學(xué)模型采用的是D-H參數(shù)，此方法需要在機器人的每個連桿的關(guān)節(jié)軸處建立一個笛卡爾坐標(biāo)系，機械臂的DH連桿坐標(biāo)系如圖1所示。建立運動學(xué)模型，是為了在得出路徑后，通過運動學(xué)逆解法，讓機械臂沿著該路徑進行運動，在本文的后續(xù)會有展示。

圖1 機械臂的DH連桿坐標(biāo) 圖2 機械臂的模型

所用的4個參數(shù)代表的意義分別是：θi是關(guān)節(jié)角，di是連桿偏移量，ai是連桿長度，αi是連桿的扭轉(zhuǎn)角。根據(jù)以上的4個參數(shù)可以表述相鄰倆連桿之間的空間狀態(tài)，然后依據(jù)機械臂的逆運動學(xué)，可以將路徑規(guī)劃中所得到的每個點在笛卡爾坐標(biāo)系中的表示形式，轉(zhuǎn)化為在關(guān)節(jié)空間中每個連桿需要的變化[6]。依據(jù)上述理論，在MATLAB中模擬出本文所用機械臂的模型如圖2所示，此狀態(tài)為機械臂最靈活的空間狀態(tài)，所以最終運動完都回到此狀態(tài)。

1.2 機械臂碰撞檢測

機械臂碰撞檢測是進行路徑規(guī)劃的前提條件，主要分為機械臂各連桿與工作空間障礙物的碰撞檢測和機械臂自身桿件的碰撞檢測。本文所基于的背景條件，只需進行障礙物和機械臂桿件的碰撞檢測。因為實際環(huán)境中的障礙物形狀各異，為了避免不必要的計算時間，文獻[7]以長方體包絡(luò)檢測法，而本文使用的是球體包絡(luò)檢測[8]。

機械臂以不同半徑的圓柱體表示不同連桿，通過檢測圓柱體的軸線和障礙物球體之間的距離，來判斷兩者是否發(fā)生碰撞。

2 RRT算法原理

RRT算法與PRM算法較為類似，都是一種概率完備型算法，是通過在已知的地圖上產(chǎn)生抽樣點并建立無向圖，進而通過搜索的方法尋找到相對最優(yōu)路徑。不同點在于，PRM算法在一開始就通過抽樣在地圖上構(gòu)建出完整的無向圖，再進行圖搜索；而RRT算法則是從某個點出發(fā)一邊搜索，一邊抽樣建圖[9]。但是如果規(guī)劃器的參數(shù)設(shè)置不合理(如搜索次數(shù)太少、采樣點過少等)，都可能使RRT算法最終無法找到解，因此參數(shù)設(shè)置對于RRT算法來說，是至關(guān)重要的。

以xstart為起始點，xgoal為目標(biāo)點，建立搜索樹T，其運動過程如下：

(1)首先產(chǎn)生第一個節(jié)點xstart屬于xfree，在每一次循環(huán)中，產(chǎn)生一個隨機點xrand，隨機點的生成是任意的，即可以在整個狀態(tài)空間X內(nèi)；

(2)在產(chǎn)生隨機點后，遍歷隨機樹中的每一個節(jié)點，計算每一個節(jié)點與該循環(huán)生成的隨機點之間的距離，找出距離此隨機點最近的節(jié)點，記為xnearest；

(3)定義一個固定步長stepsize，當(dāng)找到xnearest時，向xnearest與xrand連線方向擴展stepsize長度，擴展后產(chǎn)生新的節(jié)點xnew；

(4)在插入新節(jié)點xnew的過程中，如果xnearest、xnew和xnearest到xnew之間的邊任意一個位于障礙物空間xobs中或者與xobs相交，則此次循環(huán)不添加任何節(jié)點，在下一次循環(huán)中重新生成新的隨機點xnew，然后再進行判斷，如果屬于xfree，則保留新節(jié)點；

(5)重復(fù)上述步驟直至xnew和xgoal小于一個固定的閾值，則代表到達目標(biāo)點，算法終止。

根據(jù)上面的基本原理，得出傳統(tǒng)的RRT算法存在隨機性大，具有盲目性，因此導(dǎo)致從起始點到目標(biāo)點擴展的時間較長，并且在本文設(shè)定的迭代次數(shù)為10 000內(nèi)尋找的成功率不高；冗余點較多，增加了路徑成本；路徑抖動較大，對于本文所述的6自由度機械臂運動而言，并不友好，為了改善以上缺點，需要對RRT算法進行改進。

3 改進的RRT算法

3.1 目標(biāo)導(dǎo)向處理(M-RRT)

本文采用的是向目標(biāo)點進行擴展，將原來隨機擴展的節(jié)點，再處理后使其更有方向性的運動[1,7,10-12]。在隨機函數(shù)的前提下引入系數(shù)，沿著xnearest到xrand和xnearest到xgoal兩個方向分別取u1、u2兩個系數(shù)，在這兩個系數(shù)的共同作用下產(chǎn)生全新的節(jié)點，新節(jié)點的產(chǎn)生如式(1)所示。

(1)

主要步驟如下：

(1)在三維位形空間中生成xstart、xgoal和障礙物模型；

(2)起始點在位形空間中每次隨機采樣2個點，按照距離目標(biāo)點的遠近排序，將最近的節(jié)點設(shè)定為xnearest，與xgoal進行連線，然后對該線段進行障礙物碰撞檢測，當(dāng)發(fā)生碰撞時u2=0，隨機樹朝著產(chǎn)生新隨機點的方向擴展；不發(fā)生碰撞時u1=0，隨機樹朝著目標(biāo)點的方向擴展；

(3)當(dāng)xgoal與新生成的xnew的距離小于或等于設(shè)置的固定步長時，則xnew=xgoal。

目標(biāo)偏向采用的是兩種不同的步長，即引入的參數(shù)u1=10和u2=15，當(dāng)隨機樹偏向目標(biāo)點擴展時，所用的步長較大；當(dāng)隨機樹偏向新產(chǎn)生的隨機點擴展時，所用的步長較小。因為朝著目標(biāo)點擴展是已經(jīng)確定該新節(jié)點是正確的方向，所以可以給予一個大的步長加快運算；朝著新的隨機點的運動，因為無法確定下一次檢測是否是正確的方向，所以要用小步長來進行擴展。

3.2 冗余檢測處理(R-RRT)

改進的算法引入系數(shù)后，明顯的降低了隨機性，隨機樹大概率朝著目標(biāo)點進行運動。雖然在上述的改進下時間縮短了很多，但是路徑長度的變化并不大，本文想要的是在時間變化較小的前提下，路徑相對較短[13-16]。

基于上述的改進，對產(chǎn)生隨機樹的每個節(jié)點進行冗余檢測，冗余檢測示意圖如圖3所示。

圖3 冗余檢測示意圖

具體的操作步驟如下：

(1)以目標(biāo)點為冗余檢測的開始點，下一個節(jié)點為新的目標(biāo)點；

(2)用線段連接這兩個點，將線段進行n等分劃分，再依次判斷等分點是否在障礙物內(nèi)；

(3)若有等分點在障礙物內(nèi)，則將該新的目標(biāo)點的子節(jié)點放入優(yōu)化后的隨機樹的節(jié)點中；若等分點不在障礙內(nèi)，則對該節(jié)點的父節(jié)點進行冗余檢測；

(4)直到將冗余檢測起始點到新的目標(biāo)點之間的所有冗余點刪除后，再以此時優(yōu)化后的隨機樹中的目標(biāo)點為開始點，進行后續(xù)的冗余檢測；

(5)將所有節(jié)點冗余檢測完后，生成一個節(jié)點更少的隨機樹。

由圖3可知基于目標(biāo)導(dǎo)向后的路徑是8-7-6-5-4-3-2-1，再通過冗余檢測后的路徑為8-6-4-1，理論上大大地縮短了路徑長度。

3.3 路徑平滑處理(P-RRT)

經(jīng)過優(yōu)化后的路徑，雖然非必要的轉(zhuǎn)折點變少，但是相鄰節(jié)點之間的拐角變大，機械臂在依據(jù)此路徑運動時，會引起機械臂的沖擊和振動[12]。為了減少機械臂后續(xù)可能發(fā)生的磨損和破壞，本文采用的是基于B樣條曲線的平滑處理。

貝塞爾曲線是采用遞歸的方法來繪制曲線，從而逼向特征多邊形，這意味著若修改一個控制節(jié)點，則整個曲線都會發(fā)生變化，并且冪次過高則會更難修改等問題。因此基于這些問題，RYBUS等[16-17]提出了B樣條曲線，即用B樣條曲線的基函數(shù)來代替貝塞爾函數(shù)的基函數(shù)。B樣條曲線可以指定階數(shù)，還可以在移動控制點的前提下，只改變部分曲線的形狀，而不會對整體發(fā)生大改變。即B樣條采用的是將一條曲線變成多段貝塞爾曲線的拼接。設(shè)現(xiàn)有n+1個控制點Pi，則k階(k-1)次B樣條曲線的表達式為：

(2)

式中，Ni,k(t)是B樣條曲線函數(shù)的基函數(shù)。

B樣條曲線基函數(shù)是以一個非遞減的參數(shù)為t的序列所決定的k階分段多項式，則基函數(shù)Ni,k(t)可用deBoor-Cox的遞推公式表示為：

k=1時，

(3)

k>1時，

(4)

Ni,k(t)是由k-1階B樣條基函數(shù)遞推得出，t下標(biāo)的范圍是i到i+k。即要定義第i個k階B樣條Ni,k(t)，需要用到k+1個節(jié)點ti...tk，即[ti,ti+k]這個區(qū)間是Ni,k(t)的支撐區(qū)間。(支撐區(qū)間是指函數(shù)值不為0的區(qū)間)。

為了保證生成的曲線分別與第一個和最后一個控制點的第一條邊和最后一條邊相切，則第一個節(jié)點和最后一個節(jié)點的重復(fù)度必須為k。針對冗余檢測后的路徑，進行平滑處理，得出的路徑為8-6′-4′-1，二維路徑B樣條曲線示例如圖4所示。

圖4 二維路徑B樣條曲線示例圖

4 仿真實驗

根據(jù)上述改進的理論，在MATLAB中進行實驗仿真，為了更好的驗證上述改進算法，障礙物選取較為簡單的球體包絡(luò)，并且相對集中的分布在起始點與目標(biāo)點的對角連線周圍，起始點為(10,10,10)，目標(biāo)點為(150,150,150)，有效隨機樹用紅色線條表示。最終仿真出來的算法路徑對比圖如圖5所示，未經(jīng)任何修改的傳統(tǒng)RRT如圖5a所示，增加系數(shù)進行目標(biāo)導(dǎo)向處理后M-RRT如圖5b所示，基于目標(biāo)導(dǎo)向后進行冗余檢測和平滑處理后的R-RRT和P-RRT如圖5c所示。

(a) 傳統(tǒng)RRT (b) M-RRT

(c) R-RRT和P-RRT圖5 算法路徑對比圖

可以看出，未經(jīng)過處理的傳統(tǒng)RRT在產(chǎn)生隨機樹的情況下，出現(xiàn)了較多的分叉樹，這不僅會增加迭代的次數(shù)，導(dǎo)致時間變長，同時也會使得產(chǎn)生的節(jié)點數(shù)變多，最終路徑變長。

在通過目標(biāo)導(dǎo)向處理引入?yún)?shù)u1和u2后，隨機樹中出現(xiàn)的分支條少了很多，產(chǎn)生的隨機點更多的是朝向目標(biāo)點及進行隨機樹的擴展。基于目標(biāo)導(dǎo)向后的路徑進行冗余檢測，從圖中可以明顯看出路徑的節(jié)點變的更少，路徑變得更適宜6自由度機械臂的運動。以第1節(jié)為基礎(chǔ)建立的機械臂運動學(xué)模型，經(jīng)過相關(guān)的正逆運動學(xué)計算[18]，運動路徑示意圖如圖6所示。

圖6 機械臂模型的運動路徑示意圖

本文所給的搜索次數(shù)是10 000，進行30組仿真模擬，RRT算法出現(xiàn)一次無法找到路徑的情況，而M-RRT和R-RRT則是每次都成功找到路徑。根據(jù)數(shù)據(jù)繪出時間和距離對比如圖7所示。

(a) 時間對比圖 (b) 距離對比圖圖7 時間和距離對比圖

可以看出，M-RRT的時間整體優(yōu)于傳統(tǒng)RRT，但是在路徑長度方面并沒有發(fā)生較大的變化，因此在此基礎(chǔ)上進行冗余檢測，得出R-RRT縮短路徑長度。

本文所研究的機械臂路徑規(guī)劃對于路徑長度更為側(cè)重，因此R-RRT算法雖然平均時間相較于傳統(tǒng)RRT多出0.6 s，但是平均路徑長度縮短了58 mm。R-RRT的時間增加的原因是在進行冗余點檢測時，三維空間各等分點的檢測迭代次數(shù)較多。后續(xù)相關(guān)研究中，如果側(cè)重于時間可以僅用目標(biāo)導(dǎo)向處理的M-RRT，如果側(cè)重路徑長度則可以使用本文最終改進R-RRT的算法處理。P-RRT則是對最終路徑用B樣條曲線進行平滑處理，對路徑長度和總體時間影響較小，故不進行相關(guān)對比。算法比較結(jié)果如表1所示。

表1 算法比較結(jié)果

5 總結(jié)

本文針對其他學(xué)者基于RRT算法做出的改進中的不足，以三維空間關(guān)節(jié)機械臂為研究基礎(chǔ)，對傳統(tǒng)RRT算法做出改進。首先建立關(guān)節(jié)機械臂的運動模型，模擬相關(guān)障礙物進行路徑規(guī)劃，再通過仿真讓機械臂以此路徑運動，最終得到結(jié)論如下：

(1)本文的M-RRT可以有效地引導(dǎo)隨機樹朝著目標(biāo)運動，運動時間有比較明顯的縮短；

(2)R-RRT最終處理后的路徑有相應(yīng)的縮短，但是提升相對M-RRT來說較??；

(3)利用B樣條曲線對縮短后路徑進行平滑處理，更好地符合6自由度機械臂的運動。