基于自適應(yīng)目標(biāo)偏置系數(shù)的機(jī)械臂路徑規(guī)劃算法

吳文迎， 蔡錦達(dá)， 高朋帥

(上海理工大學(xué) 出版印刷與藝術(shù)設(shè)計(jì)學(xué)院， 上海 200093)

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

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

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

1 機(jī)械臂的運(yùn)動(dòng)學(xué)分析

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

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

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

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

由于課題組對(duì)機(jī)械臂所做的路徑規(guī)劃基于給定的初始點(diǎn)及目標(biāo)點(diǎn)在坐標(biāo)系中的關(guān)節(jié)坐標(biāo)，因此不對(duì)機(jī)械臂做逆運(yùn)動(dòng)學(xué)求解。

2 路徑規(guī)劃方法

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

2.1 基礎(chǔ)RRT算法

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

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

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

圖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ù)在六維空間中完成隨機(jī)點(diǎn)的采樣。Extend( )函數(shù)完成節(jié)點(diǎn)的擴(kuò)展，Collectionfree( )函數(shù)完成采樣過程中的碰撞檢測(cè)。

圖4所示為RRT算法在二維障礙空間和三維障礙空間中的路徑規(guī)劃結(jié)果。RRT算法適用于多障礙物情況下6自由度機(jī)械臂的運(yùn)動(dòng)規(guī)劃，但該算法雖然降低了路徑規(guī)劃的計(jì)算成本，卻因?yàn)椴蓸狱c(diǎn)的隨機(jī)性導(dǎo)致收斂速度慢，路徑生成時(shí)間長(zhǎng)，且未對(duì)生成的路徑做后處理，因此不是路徑規(guī)劃的最優(yōu)解。

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

2.2 RRT-connect算法

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

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

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

2.3 RRT*算法

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

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

2.4 改進(jìn)RRT算法

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

2.4.1 自適應(yīng)偏置算法

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

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

圖8 應(yīng)用目標(biāo)偏置系數(shù)的擴(kuò)展方法Figure 8 Eextended method of applying target bias coefficient

(1)

式中0

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

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

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

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

xrand,xnear, G

d←dist(xnear, world)

P←Bias(d)

xnew←f(P)

returnxnew

2.4.2 重置父節(jié)點(diǎn)

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

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

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

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

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

2.4.3 改進(jìn)的RRT算法偽代碼

得到改進(jìn)的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所示為改進(jìn)后的RRT算法分別在二維障礙空間和三維障礙空間中搜索路徑的結(jié)果，可以看出改進(jìn)RRT算法路徑曲率較小，長(zhǎng)度較短，質(zhì)量較高。

圖11 改進(jìn)的RRT算法搜索路徑的結(jié)果Figure 11 Improved RRT algorithm for searching path results

3 路徑后處理

改進(jìn)后的RRT算法雖然能夠有效提高地圖空間中的RRT搜索效率，但因其搜索能力更強(qiáng)，也會(huì)導(dǎo)致路徑的擴(kuò)展方向變化較大，優(yōu)化后路徑仍有許多冗余點(diǎn)。在有障礙物的復(fù)雜地圖空間中，冗余點(diǎn)過多會(huì)導(dǎo)致無法有效跟蹤機(jī)械臂的運(yùn)動(dòng)[15]。因此對(duì)得到的路徑做剪枝后處理是必要操作。

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

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

4 實(shí)驗(yàn)結(jié)果與分析

4.1 對(duì)比實(shí)驗(yàn)

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

在二維的確定障礙空間內(nèi)進(jìn)行對(duì)比實(shí)驗(yàn)，將搜索時(shí)間作為判斷路徑搜索速度的指標(biāo)；將路徑長(zhǎng)度和最大曲率作為判斷路徑質(zhì)量的指標(biāo)?？紤]到其他因素對(duì)實(shí)驗(yàn)的影響，取各算法運(yùn)行100次的平均值作為結(jié)果參數(shù)，設(shè)置搜索步長(zhǎng)SS=0.5。由于RRT*算法具有漸進(jìn)最優(yōu)的特性，因此實(shí)驗(yàn)中發(fā)現(xiàn)RRT*算法搜索20 000次后生成的路徑基本一致，課題組將RRT*算法搜索20 000次后生成的路徑長(zhǎng)度為最短路徑。實(shí)驗(yàn)采用Intel Xeon(至強(qiáng))W-2255@3.70 GHz的10核處理器，實(shí)驗(yàn)結(jié)果如表2所示。

表2 實(shí)驗(yàn)結(jié)果對(duì)比Table 2 Comparison of experimental results

對(duì)比實(shí)驗(yàn)結(jié)果顯示，RRT算法搜索速度相對(duì)較慢，路徑質(zhì)量也較差；RRT-connect算法因?yàn)閼?yīng)用了貪婪算法，因此每一次迭代都會(huì)生成多步搜索，路徑規(guī)劃的單次時(shí)間最短，搜索速度最快，但RRT-connect算法生成的路徑最大曲率相對(duì)另外2種RRT改進(jìn)算法要大得多，且生成的路徑長(zhǎng)度最大，超過RRT*算法生成的路徑長(zhǎng)度37.5%，路徑質(zhì)量較差；RRT*算法產(chǎn)生的路徑長(zhǎng)度最小，最大曲率也僅僅略高于本研究中改進(jìn)的RRT算法，路徑質(zhì)量較好，但單次搜索時(shí)間遠(yuǎn)高于其他3種算法，搜索速度太慢。

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

4.2 基于ROS的仿真模擬

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

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

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

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

5 結(jié)語

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