李耀仲 王書(shū)亭 蔣立泉 孟 杰 謝遠(yuǎn)龍,2

1.華中科技大學(xué)機(jī)械科學(xué)與工程學(xué)院，武漢，430074 2.廣東省智能機(jī)器人研究院，東莞，523808

0 引言

移動(dòng)機(jī)械臂繼承了移動(dòng)平臺(tái)工作空間的廣闊性和機(jī)械臂操作空間的靈活性，具有靈活性好、承載能力強(qiáng)等優(yōu)勢(shì)[1-2]。為實(shí)現(xiàn)高效高精作業(yè)，需在滿足約束條件的構(gòu)形空間中搜索從起始位置到目標(biāo)位置的無(wú)碰撞路徑[3-5]。但對(duì)于復(fù)雜應(yīng)用場(chǎng)景，傳統(tǒng)在線示教、離線編程的運(yùn)動(dòng)規(guī)劃耗時(shí)長(zhǎng)、效率低，難以滿足實(shí)際應(yīng)用需求[6-7]。

機(jī)械臂的運(yùn)動(dòng)規(guī)劃目前有以下幾種方法：①圖搜索算法[8]需對(duì)構(gòu)形空間進(jìn)行離散化處理，以保證搜索的完備性和最優(yōu)性，這在一定程度上限制了算法的靈活性；②人工勢(shì)場(chǎng)法[9]對(duì)計(jì)算要求不高，但在搜索時(shí)容易陷入局部最小值，難以適應(yīng)復(fù)雜的環(huán)境地圖；③基于隨機(jī)采樣的概率圖法[10]能較好地解決高維空間和復(fù)雜約束下的路徑規(guī)劃問(wèn)題，但對(duì)場(chǎng)景分布和隨機(jī)點(diǎn)的選擇有較高的要求，復(fù)雜空間下的規(guī)劃效率較低；④快速搜索隨機(jī)樹(shù)(rapidly-exploring random tree，RRT)算法具有易于實(shí)現(xiàn)、概率完備等優(yōu)點(diǎn)，可用于解決復(fù)雜約束的高維空間路徑規(guī)劃問(wèn)題，但由于空間的過(guò)度搜索，該方法在復(fù)雜場(chǎng)景下存在計(jì)算量大、收斂慢等問(wèn)題[11]。

目前，RRT算法在工業(yè)機(jī)械臂領(lǐng)域取得了較好的應(yīng)用效果，許多學(xué)者從算法效率、全局最優(yōu)等多個(gè)角度對(duì)其進(jìn)行改進(jìn)，以滿足高效運(yùn)動(dòng)規(guī)劃的需求[12-13]。一方面，為縮短算法收斂時(shí)間，RRT-Biased算法在隨機(jī)樹(shù)迭代擴(kuò)展過(guò)程中將目標(biāo)點(diǎn)設(shè)為具有一定偏置概率的隨機(jī)點(diǎn)[14]，達(dá)到提高搜索效率的目的；RRT-Connect算法從起點(diǎn)和目標(biāo)點(diǎn)同時(shí)建立并擴(kuò)展隨機(jī)樹(shù)，極大地提高了收斂速度[15-16]。另一方面，為避免陷入局部最小，RRT*算法引入成本函數(shù)來(lái)挑選出擴(kuò)展節(jié)點(diǎn)中代價(jià)最低的節(jié)點(diǎn)，保證了全局最優(yōu)解的搜索，但迭代搜索會(huì)導(dǎo)致收斂的速度較慢；李洋等[17]提出了考慮自適應(yīng)步長(zhǎng)的改進(jìn)RRT算法，提高了機(jī)械臂運(yùn)動(dòng)規(guī)劃速度。此外，對(duì)于無(wú)障礙路徑規(guī)劃，作為鉸接機(jī)構(gòu)的機(jī)械臂在運(yùn)動(dòng)規(guī)劃時(shí)不能視為質(zhì)點(diǎn)，因而選擇使用RRT算法在構(gòu)形空間中進(jìn)行機(jī)械臂運(yùn)動(dòng)規(guī)劃時(shí)，需通過(guò)構(gòu)建避障約束實(shí)現(xiàn)對(duì)采樣點(diǎn)的碰撞檢測(cè)[18-22]。

為提高算法的效率和收斂速度，當(dāng)前對(duì)RRT的修改主要關(guān)注算法本身，而沒(méi)有密切關(guān)注復(fù)雜地圖環(huán)境和隨機(jī)樹(shù)擴(kuò)展過(guò)程特征。對(duì)于狹窄隧道和多個(gè)局部受限區(qū)域(多障礙物約束、移動(dòng)平臺(tái)約束)等復(fù)雜環(huán)境中的移動(dòng)機(jī)械臂，基于RRT的運(yùn)動(dòng)規(guī)劃將產(chǎn)生局部位置的大量重復(fù)搜索和碰撞檢測(cè)。同時(shí)，移動(dòng)平臺(tái)?？空`差也影響機(jī)械臂的運(yùn)動(dòng)規(guī)劃。為解決移動(dòng)平臺(tái)?？空`差對(duì)運(yùn)動(dòng)規(guī)劃的影響并避免局部空間的過(guò)度搜索，減少無(wú)效節(jié)點(diǎn)，提高收斂速度，筆者提出基于稀疏節(jié)點(diǎn)RRT的機(jī)械臂運(yùn)動(dòng)規(guī)劃方法，并通過(guò)測(cè)試驗(yàn)證了所提算法的可行性和優(yōu)越性。

1 基于變換矩陣的?？空`差補(bǔ)償

對(duì)移動(dòng)機(jī)械臂而言，多障礙物約束以及與機(jī)械臂耦合的移動(dòng)平臺(tái)約束導(dǎo)致的?？空`差直接影響運(yùn)動(dòng)規(guī)劃的精確度。圖1所示為運(yùn)載操作一體化的移動(dòng)機(jī)械臂，執(zhí)行運(yùn)動(dòng)規(guī)劃前，需將其按照規(guī)定路徑要求運(yùn)行到指定位置?？?，并由定位算法計(jì)算獲取?？空`差。圖1中，o為移動(dòng)平臺(tái)的理論?？课恢茫琽′為移動(dòng)平臺(tái)的實(shí)際?？课恢?。機(jī)械臂運(yùn)動(dòng)規(guī)劃中，機(jī)械臂的關(guān)節(jié)運(yùn)動(dòng)以固定在移動(dòng)平臺(tái)上的機(jī)械臂基座中心為參考原點(diǎn)。機(jī)械臂基座中心與移動(dòng)平臺(tái)的實(shí)際?？课恢妹芮邢嚓P(guān)，而機(jī)械臂運(yùn)動(dòng)規(guī)劃中的運(yùn)動(dòng)學(xué)正解與逆解均以機(jī)械臂理論停靠的基座中心為參考原點(diǎn)。因此，移動(dòng)平臺(tái)?？空`差直接影響移動(dòng)機(jī)械臂的運(yùn)動(dòng)規(guī)劃準(zhǔn)確度。為保證機(jī)械臂能精準(zhǔn)運(yùn)動(dòng)到目標(biāo)位置并執(zhí)行抓取任務(wù)，需要根據(jù)移動(dòng)平臺(tái)的?？空`差對(duì)機(jī)械臂的運(yùn)動(dòng)規(guī)劃進(jìn)行補(bǔ)償。

(a)移動(dòng)機(jī)器臂平臺(tái) (b)誤差模型

用位置誤差(Δx,Δy)和姿態(tài)誤差θ表征?？空`差(Δx,Δy,θ)。采用D-H方法對(duì)該機(jī)械臂進(jìn)行正向運(yùn)動(dòng)學(xué)分析，在考慮?？空`差的情況下，連桿坐標(biāo)系{i}相對(duì)于坐標(biāo)系{i-1}的連桿變換通式為

(1)

C=[0 0 0]

式中，φ為繞z軸的旋轉(zhuǎn)角度；α為機(jī)械臂相鄰坐標(biāo)系z(mì)軸之間的扭角；a為公垂線長(zhǎng)度；d為相鄰公垂線之間的距離。

(2)

得到移動(dòng)平臺(tái)的?？空`差后，將誤差引入到變換矩陣，利用補(bǔ)償消除?？空`差對(duì)運(yùn)動(dòng)規(guī)劃的影響。

2 基于改進(jìn)RRT算法的運(yùn)動(dòng)規(guī)劃

2.1 標(biāo)準(zhǔn)RRT運(yùn)動(dòng)規(guī)劃問(wèn)題描述

RRT算法是一種采用增量方式的隨機(jī)采樣算法，以給定的起始點(diǎn)為根節(jié)點(diǎn)，通過(guò)在自由空間中不斷隨機(jī)采集并以固定步長(zhǎng)擴(kuò)展子節(jié)點(diǎn)的方式生成擴(kuò)展樹(shù)。當(dāng)子節(jié)點(diǎn)到達(dá)目標(biāo)點(diǎn)或目標(biāo)區(qū)域時(shí)，隨機(jī)樹(shù)停止生長(zhǎng)，通過(guò)從目標(biāo)點(diǎn)回溯來(lái)獲取最終規(guī)劃路徑。算法1偽代碼的函數(shù)說(shuō)明如下：RANDOM_CONFIG(Cfree)表示在配置空間可行區(qū)域Cfree內(nèi)產(chǎn)生隨機(jī)點(diǎn)NEAREST_NEIGHBOR(T,xrand)來(lái)重復(fù)迭代遍歷隨機(jī)樹(shù)所有子節(jié)點(diǎn)(其中，T為迭代的隨機(jī)樹(shù)，xrand為隨機(jī)節(jié)點(diǎn))，并搜索與隨機(jī)點(diǎn)最近的子節(jié)點(diǎn)；xnew為NEW_CONFIG(xnear,ε,xrand)生成的新節(jié)點(diǎn)，xnear為迭代隨機(jī)樹(shù)T中離xrand最近的節(jié)點(diǎn)，ε為擴(kuò)展節(jié)點(diǎn)步長(zhǎng)；BOUNDARY_COLLISION(xnear,xnew)判斷新生成的節(jié)點(diǎn)是否滿足非完整約束；GROWN_TREE(T,xnew)根據(jù)xnew擴(kuò)展隨機(jī)樹(shù)。算法1中，xinit為初始節(jié)點(diǎn)，K為迭代次數(shù)。

算法1 RRT

CENERATE_RRT(xinit,K,ε)

1:T=tree(xinit);

2: fork=1 toK

3:xrand=RANDOM_CONFIG(Cfree);

4:xnear=NEAREST_NEIBOR(T,xrand);

5:xnew=NEW_CONFIG(xnear,ε,xrand);

6: ifBOUNDARY_COLLISION(xnear,xnew)

7:GROWN_TREE(T,xnew);

8: end if

9: if ‖xnew-xgoal‖

10: returnT;

11: end if

12: end for

RRT算法基本流程可描述如下：

(1)根據(jù)輸入的xinit生成第一個(gè)節(jié)點(diǎn)并將其作為隨機(jī)樹(shù)根節(jié)點(diǎn)；

(2)在自由空間內(nèi)生成隨機(jī)節(jié)點(diǎn)xrand；

(3)遍歷隨機(jī)樹(shù)已生成的所有節(jié)點(diǎn)，計(jì)算所有節(jié)點(diǎn)與當(dāng)前隨機(jī)節(jié)點(diǎn)的距離，定義距離最短的節(jié)點(diǎn)為xnear；

(4)根據(jù)給定的誤差，在xnear和xrand連線方向上，從xnear點(diǎn)出發(fā)，以步長(zhǎng)ε生成新節(jié)點(diǎn)xnew；

(5)判斷節(jié)點(diǎn)xnew是否滿足碰撞檢測(cè)條件即xnear和xnew之間無(wú)障礙物。若滿足，則將xnew加入隨機(jī)樹(shù)；若不滿足，則將xnew還原為上一步中的值；

(6)判斷xnew與目標(biāo)節(jié)點(diǎn)xgoal的距離，若距離小于給定的距離要求d，則表示搜索到對(duì)應(yīng)的規(guī)劃路徑，結(jié)束循環(huán)，否則繼續(xù)當(dāng)前循環(huán)。

分析RRT算法的基本流程可知，該算法依靠隨機(jī)采樣的方式來(lái)擴(kuò)展隨機(jī)樹(shù)，具有概率完備性。隨機(jī)樹(shù)子節(jié)點(diǎn)擴(kuò)展的隨機(jī)性使得自由空間中的任意位置均有一定概率被子節(jié)點(diǎn)占據(jù)，這將導(dǎo)致一定區(qū)域內(nèi)的子節(jié)點(diǎn)被重復(fù)擴(kuò)展，過(guò)多節(jié)點(diǎn)的堆積使局部區(qū)域節(jié)點(diǎn)密度過(guò)大。不加限制地隨機(jī)擴(kuò)展子節(jié)點(diǎn)將導(dǎo)致以下問(wèn)題：①隨機(jī)擴(kuò)展通常需要較多的迭代完成路徑規(guī)劃，區(qū)域節(jié)點(diǎn)密度過(guò)大將導(dǎo)致迭代進(jìn)一步增多，極端情況下導(dǎo)致路徑規(guī)劃失?。虎诖罅康碾S機(jī)點(diǎn)導(dǎo)致每次迭代搜索最近節(jié)點(diǎn)時(shí)的迭代時(shí)間延長(zhǎng)。圖2給出了RRT算法對(duì)同一環(huán)境的兩次路徑規(guī)劃結(jié)果，可以看出，圖2b的隨機(jī)樹(shù)節(jié)點(diǎn)更少、路徑和規(guī)劃時(shí)間更短，明顯優(yōu)于圖2a的搜索結(jié)果，由此可知搜索結(jié)果存在一定的隨機(jī)性。兩次建立的隨機(jī)樹(shù)都遍布了整個(gè)搜索空間，并在局部空間存在過(guò)度盲目搜索，導(dǎo)致算法效率低，無(wú)法保證路徑搜索的最優(yōu)性。另外，隨著隨機(jī)樹(shù)的不斷生長(zhǎng)，節(jié)點(diǎn)不斷增多，在已有節(jié)點(diǎn)中搜索xnear的成本增大，導(dǎo)致每輪擴(kuò)展隨機(jī)節(jié)點(diǎn)的時(shí)間隨擴(kuò)展節(jié)點(diǎn)的迭代次數(shù)急劇延長(zhǎng)。

(a)規(guī)劃路徑1 (b)規(guī)劃路徑2

針對(duì)采用均勻概率對(duì)整個(gè)空間進(jìn)行搜索導(dǎo)致收斂慢的問(wèn)題，URMSON等[22]提出了RRT- Biased算法，將目標(biāo)點(diǎn)以一定概率作為隨機(jī)樹(shù)隨機(jī)點(diǎn)的方式即讓隨機(jī)樹(shù)以一定概率朝著目標(biāo)點(diǎn)生長(zhǎng)，顯著提高了算法的收斂速度，規(guī)劃效果如圖3所示。該算法在復(fù)雜環(huán)境或局部環(huán)境受限的地圖中容易陷入局部最小值，導(dǎo)致難以在規(guī)定的迭代次數(shù)完成規(guī)劃。如圖4所示，在僅存一個(gè)狹縫的半封閉空間中，隨機(jī)樹(shù)擴(kuò)展將充滿整個(gè)半封閉空間，直至逃逸出狹縫，但點(diǎn)產(chǎn)生的隨機(jī)性可能導(dǎo)致在局部受限區(qū)域內(nèi)耗盡迭代次數(shù)而不能成功規(guī)劃出路徑。另外，靠近邊界的節(jié)點(diǎn)在擴(kuò)展隨機(jī)節(jié)點(diǎn)時(shí)更易與障礙物發(fā)生碰撞，重復(fù)的碰撞檢測(cè)將導(dǎo)致路徑規(guī)劃時(shí)間的延長(zhǎng)。針對(duì)上述問(wèn)題，本文提出稀疏節(jié)點(diǎn)RRT算法，設(shè)計(jì)回歸機(jī)制和隨機(jī)點(diǎn)擴(kuò)展機(jī)制，稀疏搜索節(jié)點(diǎn)，從而達(dá)到加快收斂速度的目的。

(a)RRT算法 (b)10%RRT-Baised算法

圖4 RRT-Baised在狹縫空間中的規(guī)劃

2.2 回歸過(guò)濾機(jī)制

隨機(jī)樹(shù)節(jié)點(diǎn)的激增將導(dǎo)致算法計(jì)算效率下降，移動(dòng)機(jī)械臂碰撞檢測(cè)次數(shù)明顯增大將導(dǎo)致機(jī)械臂路徑規(guī)劃時(shí)間的延長(zhǎng)，降低路徑規(guī)劃的執(zhí)行效率。碰撞檢測(cè)是RRT算法運(yùn)動(dòng)規(guī)劃的重要環(huán)節(jié)，也是最耗時(shí)的部分，耗時(shí)可達(dá)總規(guī)劃時(shí)間的90%。

本文在RRT算法的基礎(chǔ)上引入回歸過(guò)濾機(jī)制(記為RRT-RE)，避免了隨機(jī)節(jié)點(diǎn)擴(kuò)展時(shí)在局部區(qū)域的重復(fù)擴(kuò)展。如圖5所示，對(duì)新節(jié)點(diǎn)xnew采用回歸過(guò)濾機(jī)制，判斷xnew是否可用于節(jié)點(diǎn)樹(shù)擴(kuò)展。將與新擴(kuò)展的xnew節(jié)點(diǎn)直接連接的節(jié)點(diǎn)定義為節(jié)點(diǎn)xnew的父節(jié)點(diǎn)xparent，兩節(jié)點(diǎn)間距為固定步長(zhǎng)ε。當(dāng)xnew節(jié)點(diǎn)與隨機(jī)樹(shù)T中所有子節(jié)點(diǎn)的最短距離小于ε時(shí)，認(rèn)為當(dāng)前節(jié)點(diǎn)xnew是回歸的。區(qū)域內(nèi)過(guò)多的回歸節(jié)點(diǎn)將嚴(yán)重影響算法的執(zhí)行效率，為此，減少區(qū)域內(nèi)的回歸節(jié)點(diǎn)為提高收斂速度提供了可行途徑。圖5中的紅色虛線節(jié)點(diǎn)被認(rèn)為是回歸的，該節(jié)點(diǎn)在隨機(jī)樹(shù)上存在比父節(jié)點(diǎn)xparent近的節(jié)點(diǎn)。若子節(jié)點(diǎn)是回歸的，則將會(huì)被過(guò)濾掉，不直接插入到隨機(jī)樹(shù)中。圖5中的綠色節(jié)點(diǎn)xnew1、xnew2的最近節(jié)點(diǎn)均是xnew1、xnew2的父節(jié)點(diǎn)，故被擴(kuò)展到隨機(jī)樹(shù)中。該機(jī)制可有效防止過(guò)度搜索已搜索的空間，提高算法效率。本文通過(guò)POINT_FILTER(T,xnew,xparent)函數(shù)實(shí)現(xiàn)回歸過(guò)濾機(jī)制，偽代碼描述如下：

圖5 回歸過(guò)濾機(jī)制

函數(shù)2POINT_FILTER(T,xnew,xparent)

1:xnear←NEAREST_NEIBOR(T,xnew);

2: if ‖xnew-xparent‖<‖xnew-xnear1‖

3: returnT;

4: else

5: return failure;

6: end if

為驗(yàn)證回歸過(guò)濾機(jī)制的合理性，以局部受限的環(huán)境(見(jiàn)圖6)為例進(jìn)行仿真分析。該地圖規(guī)模為6769×6769像素點(diǎn)，RRT步長(zhǎng)ε=100。表1給出了2種算法在該地圖場(chǎng)景下運(yùn)行10次的統(tǒng)計(jì)結(jié)果平均值。觀察圖6中隨機(jī)樹(shù)的分布密度可知，RRT算法得到的隨機(jī)樹(shù)節(jié)點(diǎn)在局部受限區(qū)域的密度極大，引入回歸過(guò)濾機(jī)制后的隨機(jī)樹(shù)節(jié)點(diǎn)在局部受限區(qū)域較為稀疏，表明所提算法可避免局部區(qū)域重復(fù)搜索，達(dá)到節(jié)點(diǎn)過(guò)濾的目的。表1所示的仿真結(jié)果表明，引入回歸過(guò)濾機(jī)制后，算法規(guī)劃的隨機(jī)樹(shù)節(jié)點(diǎn)明顯減少，提高了算法規(guī)劃效率?；貧w過(guò)濾機(jī)制通過(guò)過(guò)濾回歸節(jié)點(diǎn)，極大地減少了碰撞檢測(cè)次數(shù)，因此，在RRT算法中引入回歸過(guò)濾機(jī)制可提高算法計(jì)算效率、減少碰撞檢測(cè)。

(a)RRT (b)RRT-RE

表1 回歸機(jī)制下算法仿真對(duì)比結(jié)果

2.3 隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制

在具有空間約束的場(chǎng)景下，對(duì)邊界節(jié)點(diǎn)的處理是RRT算法優(yōu)化的重點(diǎn)工作之一。通常，邊界節(jié)點(diǎn)上擴(kuò)展的隨機(jī)點(diǎn)更易與障礙物發(fā)生碰撞，且擴(kuò)展過(guò)程是耗時(shí)的。為進(jìn)一步提高運(yùn)動(dòng)規(guī)劃的計(jì)算效率，避免在邊界節(jié)點(diǎn)上過(guò)度擴(kuò)展隨機(jī)節(jié)點(diǎn)，提出預(yù)估節(jié)點(diǎn)的隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制，對(duì)每一個(gè)節(jié)點(diǎn)設(shè)置狀態(tài)信息，以提前判斷該節(jié)點(diǎn)是否為邊界點(diǎn)。對(duì)于始終導(dǎo)致移動(dòng)機(jī)械臂與環(huán)境發(fā)生碰撞的節(jié)點(diǎn)，應(yīng)配置相應(yīng)的節(jié)點(diǎn)狀態(tài)信息并將其從隨機(jī)樹(shù)搜索空間中排除。這種機(jī)制避免了在隨后的節(jié)點(diǎn)擴(kuò)展中對(duì)該邊界節(jié)點(diǎn)的重復(fù)碰撞檢測(cè)。隨著隨機(jī)樹(shù)的不斷擴(kuò)展，持續(xù)更新隨機(jī)樹(shù)中節(jié)點(diǎn)的狀態(tài)信息，使移動(dòng)機(jī)械臂的可達(dá)工作空間逐漸變得清晰。

根據(jù)邊界節(jié)點(diǎn)在擴(kuò)展過(guò)程中與邊界發(fā)生碰撞的概率PC(邊界節(jié)點(diǎn)中發(fā)生碰撞的子節(jié)點(diǎn)數(shù)k與邊界節(jié)點(diǎn)全部子節(jié)點(diǎn)數(shù)的比)可以評(píng)估節(jié)點(diǎn)的擴(kuò)展能力。PC越小，節(jié)點(diǎn)成功擴(kuò)展子節(jié)點(diǎn)的概率越大。節(jié)點(diǎn)配置的狀態(tài)信息用狀態(tài)值δ表示，所有節(jié)點(diǎn)的初始狀態(tài)值δ=0表示可以自由擴(kuò)展。當(dāng)節(jié)點(diǎn)擴(kuò)展的子節(jié)點(diǎn)與邊界發(fā)生碰撞時(shí)，當(dāng)前節(jié)點(diǎn)的狀態(tài)值δ將發(fā)生變化，該節(jié)點(diǎn)將被視為邊界節(jié)點(diǎn)。因此，節(jié)點(diǎn)的狀態(tài)信息是持續(xù)回溯更新的。通過(guò)δ的實(shí)時(shí)更新來(lái)判斷該節(jié)點(diǎn)是否為邊界節(jié)點(diǎn)，并且設(shè)計(jì)δ的更新規(guī)則：假定有m條邊與邊界節(jié)點(diǎn)x連接；節(jié)點(diǎn)x為最近節(jié)點(diǎn)xnear時(shí)，若擴(kuò)展的子節(jié)點(diǎn)與障礙物邊界碰撞，則該節(jié)點(diǎn)的狀態(tài)值δ將增加1/(m+2)；若擴(kuò)展的子節(jié)點(diǎn)不發(fā)生碰撞，則該節(jié)點(diǎn)的狀態(tài)值將減少1/(m+1)。如果狀態(tài)值δ>PC，則該邊界節(jié)點(diǎn)將不再擴(kuò)展任何子節(jié)點(diǎn)。因此，該機(jī)制保證不會(huì)無(wú)限制地?cái)U(kuò)展邊界節(jié)點(diǎn)的子節(jié)點(diǎn)。對(duì)邊界節(jié)點(diǎn)進(jìn)行擴(kuò)展時(shí)，要綜合考慮節(jié)點(diǎn)的碰撞概率。節(jié)點(diǎn)的碰撞概率大意味著擴(kuò)展的節(jié)點(diǎn)更可能與障礙物發(fā)生碰撞，選擇碰撞概率小的邊界節(jié)點(diǎn)擴(kuò)展子節(jié)點(diǎn)有助于隨機(jī)樹(shù)遠(yuǎn)離障礙物邊界。將引入隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制的改進(jìn)RRT算法記為RRT-BE，其偽代碼如下(偽代碼中的REFRESH(m,xnear)用于更新隨機(jī)樹(shù)中xnear的狀態(tài)信息m，以便判斷邊界節(jié)點(diǎn))：

函數(shù)3 BOUNDARY -LIMIT (T, xnew, xparent)

1: ifPOINT_FILTER(T,xnew,xparent)

2: ifδ(xnear)>PC

3: return failure;

4: else

5: ifBOUNDARY_COLLISION(xnear,xnew)

6: ifδ(xnear)-1/(m+1)> 0

7:δ(xnear)=δ(xnear)- 1/(m+1)

8: else

9:δ(xnear)=0

10: end if

11: else

12: ifδ(xnear)==0

13:δ(xnear)=1/2;

14: else

15:δ(xnear)+= 1/(m+2);

16: end if

17: end if

18: end if

19:REFRESH(m,xnear);

20: else

21: return failure;

22: end if

為驗(yàn)證隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制的有效性，以圖7為例，分別采用RRT算法和RRT-BE算法進(jìn)行路徑規(guī)劃仿真實(shí)驗(yàn)。表2列出了兩種算法在圖7場(chǎng)景下運(yùn)行10次的統(tǒng)計(jì)結(jié)果平均值。由圖7可以看出，邊界節(jié)點(diǎn)的分布可以反饋障礙物的輪廓軌跡。邊界節(jié)點(diǎn)的顏色越暖，擴(kuò)展節(jié)點(diǎn)發(fā)生碰撞越多。對(duì)比可得，RRT算法隨機(jī)樹(shù)中邊界節(jié)點(diǎn)擴(kuò)展的隨機(jī)點(diǎn)與障礙物邊界發(fā)生碰撞最高可達(dá)近350次，而引入邊界擴(kuò)展機(jī)制后，隨機(jī)樹(shù)中邊界節(jié)點(diǎn)擴(kuò)展的隨機(jī)點(diǎn)與障礙物邊界發(fā)生碰撞的次數(shù)最大約為9，碰撞明顯減少。由表2可以看出，引入隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制將減少隨機(jī)樹(shù)邊界節(jié)點(diǎn)，進(jìn)而減少了邊界節(jié)點(diǎn)的判斷次數(shù)；RRT算法下發(fā)生碰撞4614.4次，RRT-BE算法下發(fā)生碰撞2473.7次，這意味著RRT-BE算法所消耗的碰撞檢測(cè)時(shí)間更少。因此，引入隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制可極大地減少邊界節(jié)點(diǎn)擴(kuò)展的無(wú)效節(jié)點(diǎn)，對(duì)整個(gè)規(guī)劃過(guò)程的節(jié)點(diǎn)進(jìn)行了稀疏化，進(jìn)而減少碰撞檢測(cè)，提高移動(dòng)機(jī)械臂運(yùn)動(dòng)規(guī)劃的效率。

(a)RRT

表2 兩種算法的仿真結(jié)果

3 測(cè)試驗(yàn)證

3.1 迷宮地圖下的測(cè)試分析

為了進(jìn)一步改進(jìn)RRT算法，在RRT算法的基礎(chǔ)上同時(shí)引入回歸過(guò)濾機(jī)制和隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制，即在RRT算法每次迭代生成新節(jié)點(diǎn)xnew后執(zhí)行回歸過(guò)濾，對(duì)未回收的xnew執(zhí)行隨機(jī)點(diǎn)邊界擴(kuò)展，從而保證節(jié)點(diǎn)稀疏下的搜索效率，將該改進(jìn)算法記為RRT-RB。為了驗(yàn)證RRT-RB算法的性能，在有多障礙物的狹縫環(huán)境、帶迷惑特性的二維迷宮環(huán)境來(lái)驗(yàn)證所提算法在復(fù)雜地圖環(huán)境下的全局優(yōu)化、收斂特性。圖8所示的地圖規(guī)模為6856×6769像素點(diǎn)，RRT步長(zhǎng)ε=150。

(a)RRT路徑規(guī)劃 (b)RRT邊界分

圖8的地圖環(huán)境包含多個(gè)狹窄通道和局部受限區(qū)域。兩種方法在隨機(jī)點(diǎn)的擴(kuò)展中均引入目標(biāo)點(diǎn)偏置概率γ=10%。觀察圖8a可以發(fā)現(xiàn)，RRT算法擴(kuò)展的隨機(jī)樹(shù)節(jié)點(diǎn)在局部受限空間下的分布密度較其他區(qū)域大，且隨機(jī)樹(shù)節(jié)點(diǎn)堆疊嚴(yán)重。由圖8c可知，RRT-RB建立的隨機(jī)樹(shù)均勻分布在迷宮地圖自由空間。RRT算法構(gòu)建隨機(jī)樹(shù)的邊界節(jié)點(diǎn)(圖8b)中，某個(gè)邊界節(jié)點(diǎn)的重復(fù)搜索最高可達(dá)近220次，表明RRT算法在該地圖下會(huì)陷入局部最小值，生成許多不必要的節(jié)點(diǎn)，消耗大量計(jì)算時(shí)間，降低算法執(zhí)行效率。圖8d中，RRT-RB算法構(gòu)建的隨機(jī)樹(shù)邊界節(jié)點(diǎn)的重復(fù)搜索得到有效遏制，單個(gè)邊界節(jié)點(diǎn)的最大重復(fù)搜索次數(shù)降低至9。因此，RRT-RB算法擴(kuò)展的隨機(jī)樹(shù)節(jié)點(diǎn)密度明顯降低，邊界節(jié)點(diǎn)擴(kuò)展隨機(jī)點(diǎn)時(shí)發(fā)生碰撞檢測(cè)的次數(shù)也明顯減小。

表3所示為不同算法下對(duì)迷宮地圖進(jìn)行路徑規(guī)劃的實(shí)驗(yàn)結(jié)果(運(yùn)行10次的平均值)。由表3可以看出，RRT-RE、RRT-BE、RRT-RB較RRT在分析指標(biāo)方面均有不同程度的優(yōu)化。對(duì)比隨機(jī)樹(shù)節(jié)點(diǎn)總數(shù)，RRT-RB(γ=0)和RRT-RB(γ=10%)均能減少隨機(jī)樹(shù)節(jié)點(diǎn)，提高算法收斂速度；就運(yùn)行時(shí)間而言， RRT-RB較RRT有大幅的提高，γ=0時(shí)提高91.0%，γ=10%時(shí)提高63.8%；就碰撞檢測(cè)次數(shù)而言，RRT-RE、RRT-BE和RRT-RB較RRT大幅下降，γ=0時(shí)分別降低了147.0%、60.1%和372.4%，引入回歸過(guò)濾機(jī)制和隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制對(duì)減少碰撞檢測(cè)效果顯著。需要指出的是，RRT-RE、RRT-BE、RRT-RB引入目標(biāo)點(diǎn)偏置概率10%對(duì)運(yùn)行時(shí)間和碰撞檢測(cè)次數(shù)的影響不大，這是由于在迷宮環(huán)境下，目標(biāo)節(jié)點(diǎn)的引入和碰撞節(jié)點(diǎn)的檢測(cè)使得規(guī)劃無(wú)法快速收斂?？偨Y(jié)來(lái)看，具有稀疏節(jié)點(diǎn)的RRT-RB算法在同時(shí)引入回歸過(guò)濾機(jī)制和隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制后，一方面有效減少了回歸節(jié)點(diǎn)，縮短了算法迭代時(shí)間，達(dá)到提高算法計(jì)算收斂速度的目的，另一方面通過(guò)考慮障礙物約束，極大地減少了碰撞檢測(cè)，提高了運(yùn)動(dòng)規(guī)劃效率。

表3 迷宮地圖下各種算法的實(shí)驗(yàn)結(jié)果

3.2 移動(dòng)機(jī)械臂運(yùn)動(dòng)規(guī)劃測(cè)試

機(jī)械臂是一種鉸接機(jī)構(gòu)，不能簡(jiǎn)單地視為工作空間中的一個(gè)質(zhì)點(diǎn)，構(gòu)型空間的引入能很好解決這個(gè)問(wèn)題。構(gòu)形空間是一組用來(lái)完全描述機(jī)器人空間狀態(tài)的向量，六軸移動(dòng)機(jī)械臂的構(gòu)形空間為六維空間向量(θ1,θ2,θ3,θ4,θ5,θ6)，θi(i=1，2，…，6)為關(guān)節(jié)i的角度，六維空間中的點(diǎn)為機(jī)械臂的位姿點(diǎn)。移動(dòng)機(jī)械臂構(gòu)形空間和工作空間的映射通過(guò)運(yùn)動(dòng)學(xué)正解和逆解實(shí)現(xiàn)。

為驗(yàn)證所提機(jī)械臂運(yùn)動(dòng)規(guī)劃方法的可行性，在MoveIt！軟件下搭建了移動(dòng)機(jī)械臂的運(yùn)動(dòng)規(guī)劃場(chǎng)景，分析所提運(yùn)動(dòng)規(guī)劃算法的性能。該測(cè)試場(chǎng)景為受到障礙物約束、耦合平臺(tái)約束的局部區(qū)域受限復(fù)雜環(huán)境。機(jī)械臂運(yùn)動(dòng)規(guī)劃前，采用誤差變換矩陣對(duì)移動(dòng)機(jī)械臂的?？空`差進(jìn)行補(bǔ)償，設(shè)定MoveIt！中的起始位姿和目標(biāo)位姿均為誤差補(bǔ)償后的位姿。將基于RRT-RB算法的運(yùn)動(dòng)規(guī)劃方法開(kāi)發(fā)成規(guī)劃器并集成到OMPL庫(kù)中，修改相應(yīng)的ymal文件，使該運(yùn)動(dòng)規(guī)劃方法可在Rviz的路徑規(guī)劃模塊中被調(diào)用。規(guī)劃軌跡的不平滑會(huì)造成機(jī)械臂運(yùn)行過(guò)程中的角速度和角加速度的突變，因此路徑規(guī)劃模塊在工作空間內(nèi)實(shí)現(xiàn)了對(duì)運(yùn)動(dòng)曲線進(jìn)行插值或樣條曲線擬合，提高了轉(zhuǎn)角處的運(yùn)動(dòng)平滑性。機(jī)械臂抓取姿態(tài)的運(yùn)動(dòng)學(xué)逆解求解直接采用MoveIt！運(yùn)動(dòng)學(xué)求解器，分別采用RRT算法和改進(jìn)RRT算法對(duì)UR5機(jī)械臂進(jìn)行測(cè)試。如圖9所示，綠色方塊為障礙物，灰色塊為移動(dòng)平臺(tái)，其中灰色的機(jī)械臂為目標(biāo)點(diǎn)位姿，標(biāo)記為橙黃色的機(jī)械臂為初始點(diǎn)位姿。圖10反映了按照規(guī)劃的路徑從初始位置到目標(biāo)位置的運(yùn)動(dòng),兩種算法均能為機(jī)械臂規(guī)劃出一條從起始位姿到目標(biāo)位姿的運(yùn)動(dòng)軌跡，且不與障礙物發(fā)生碰撞，但規(guī)劃的運(yùn)動(dòng)路徑明顯不同。為對(duì)兩種算法的規(guī)劃效果進(jìn)行定量分析，采用兩種算法對(duì)UR5機(jī)械臂的運(yùn)動(dòng)規(guī)劃問(wèn)題進(jìn)行10次重復(fù)求解，統(tǒng)計(jì)運(yùn)動(dòng)規(guī)劃時(shí)間，RRT算法規(guī)劃成功9次，失敗1次，平均規(guī)劃時(shí)間為2.011s；RRT-RB算法規(guī)劃成功10次，平均規(guī)劃時(shí)間為0.772s。因此，相較于RRT算法，利用具有稀疏節(jié)點(diǎn)的RRT-RB算法進(jìn)行機(jī)械臂運(yùn)動(dòng)規(guī)劃可顯著提高成功率和規(guī)劃效率。

圖9 機(jī)械臂初始及目標(biāo)點(diǎn)位置

(a)RRT算法 (d)RRT-RB算法

4 結(jié)論

本文提出了基于稀疏節(jié)點(diǎn)RRT的移動(dòng)機(jī)械臂運(yùn)動(dòng)規(guī)劃規(guī)劃方法。首先研究了移動(dòng)平臺(tái)停靠誤差對(duì)機(jī)械臂運(yùn)動(dòng)規(guī)劃的影響，制定了基于坐標(biāo)變換的誤差補(bǔ)償措施。然后分析了RRT算法在復(fù)雜地圖環(huán)境下存在的局部空間中重復(fù)擴(kuò)展節(jié)點(diǎn)和易陷入局部最小值的問(wèn)題，提出了回歸過(guò)濾機(jī)制，通過(guò)過(guò)濾重復(fù)擴(kuò)展節(jié)點(diǎn)的方式，減少隨機(jī)樹(shù)節(jié)點(diǎn)和碰撞檢測(cè)；針對(duì)RRT算法對(duì)邊界節(jié)點(diǎn)重復(fù)擴(kuò)展無(wú)效節(jié)點(diǎn)的問(wèn)題，提出隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制，通過(guò)對(duì)邊界節(jié)點(diǎn)擴(kuò)展節(jié)點(diǎn)的控制，進(jìn)一步減少碰撞檢測(cè)。上述兩種改進(jìn)機(jī)制顯著減少了隨機(jī)樹(shù)節(jié)點(diǎn)和碰撞檢測(cè)，可有效縮短機(jī)械臂運(yùn)動(dòng)規(guī)劃時(shí)間，仿真分析驗(yàn)證了改進(jìn)措施的有效性。在迷宮地圖下進(jìn)行了仿真測(cè)試，運(yùn)行時(shí)間、碰撞檢測(cè)次數(shù)和重復(fù)搜索次數(shù)驗(yàn)證了考慮稀疏節(jié)點(diǎn)的RRT-RB算法的可行性和有效性。機(jī)械臂運(yùn)動(dòng)規(guī)劃測(cè)試結(jié)果表明，與RRT算法相比，RRT-RB算法在提高算法收斂速度的基礎(chǔ)上，考慮了工作空間中的障礙物約束，提高了機(jī)械臂運(yùn)動(dòng)規(guī)劃效率和成功率。