基于稀疏節(jié)點(diǎn)快速擴(kuò)展隨機(jī)樹的移動機(jī)械臂運(yùn)動規(guī)劃

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

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

0 引言

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

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

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

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

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

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

(a)移動機(jī)器臂平臺 (b)誤差模型

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

(1)

C=[0 0 0]

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

(2)

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

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

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

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

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

(3)遍歷隨機(jī)樹已生成的所有節(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ā)，以步長ε生成新節(jié)點(diǎn)xnew；

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

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

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

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

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

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

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

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

(a)RRT (b)RRT-RE

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

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

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

根據(jù)邊界節(jié)點(diǎn)在擴(kuò)展過程中與邊界發(fā)生碰撞的概率PC(邊界節(jié)點(diǎn)中發(fā)生碰撞的子節(jié)點(diǎn)數(shù)k與邊界節(jié)點(diǎn)全部子節(jié)點(diǎn)數(shù)的比)可以評估節(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ā)生碰撞時，當(dāng)前節(jié)點(diǎn)的狀態(tài)值δ將發(fā)生變化，該節(jié)點(diǎn)將被視為邊界節(jié)點(diǎn)。因此，節(jié)點(diǎn)的狀態(tài)信息是持續(xù)回溯更新的。通過δ的實(shí)時更新來判斷該節(jié)點(diǎn)是否為邊界節(jié)點(diǎn)，并且設(shè)計(jì)δ的更新規(guī)則：假定有m條邊與邊界節(jié)點(diǎn)x連接；節(jié)點(diǎn)x為最近節(jié)點(diǎn)xnear時，若擴(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ī)制保證不會無限制地?cái)U(kuò)展邊界節(jié)點(diǎn)的子節(jié)點(diǎn)。對邊界節(jié)點(diǎn)進(jìn)行擴(kuò)展時，要綜合考慮節(jié)點(diǎn)的碰撞概率。節(jié)點(diǎn)的碰撞概率大意味著擴(kuò)展的節(jié)點(diǎn)更可能與障礙物發(fā)生碰撞，選擇碰撞概率小的邊界節(jié)點(diǎn)擴(kuò)展子節(jié)點(diǎn)有助于隨機(jī)樹遠(yuǎn)離障礙物邊界。將引入隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制的改進(jìn)RRT算法記為RRT-BE，其偽代碼如下(偽代碼中的REFRESH(m,xnear)用于更新隨機(jī)樹中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場景下運(yùn)行10次的統(tǒng)計(jì)結(jié)果平均值。由圖7可以看出，邊界節(jié)點(diǎn)的分布可以反饋障礙物的輪廓軌跡。邊界節(jié)點(diǎn)的顏色越暖，擴(kuò)展節(jié)點(diǎn)發(fā)生碰撞越多。對比可得，RRT算法隨機(jī)樹中邊界節(jié)點(diǎn)擴(kuò)展的隨機(jī)點(diǎn)與障礙物邊界發(fā)生碰撞最高可達(dá)近350次，而引入邊界擴(kuò)展機(jī)制后，隨機(jī)樹中邊界節(jié)點(diǎn)擴(kuò)展的隨機(jī)點(diǎn)與障礙物邊界發(fā)生碰撞的次數(shù)最大約為9，碰撞明顯減少。由表2可以看出，引入隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制將減少隨機(jī)樹邊界節(jié)點(diǎn)，進(jìn)而減少了邊界節(jié)點(diǎn)的判斷次數(shù)；RRT算法下發(fā)生碰撞4614.4次，RRT-BE算法下發(fā)生碰撞2473.7次，這意味著RRT-BE算法所消耗的碰撞檢測時間更少。因此，引入隨機(jī)點(diǎn)邊界擴(kuò)展機(jī)制可極大地減少邊界節(jié)點(diǎn)擴(kuò)展的無效節(jié)點(diǎn)，對整個規(guī)劃過程的節(jié)點(diǎn)進(jìn)行了稀疏化，進(jìn)而減少碰撞檢測，提高移動機(jī)械臂運(yùn)動規(guī)劃的效率。

(a)RRT

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

3 測試驗(yàn)證

3.1 迷宮地圖下的測試分析

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

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

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

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

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

3.2 移動機(jī)械臂運(yùn)動規(guī)劃測試

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

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

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

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

4 結(jié)論

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