張 振，李新宇，董昊臻，周 林，高 亮+

(1.華中科技大學(xué) 數(shù)字制造裝備與技術(shù)國家重點實驗室，湖北 武漢 430074； 2.航天科工智能運籌與信息安全研究院(武漢)有限公司 體系設(shè)計中心，湖北 武漢 430040)

0 引言

工業(yè)機器人廣泛應(yīng)用于汽車、航空、船舶等工業(yè)生產(chǎn)領(lǐng)域[1]。隨著機器人化智能制造的提出和發(fā)展，傳統(tǒng)示教編程的方式無法滿足作業(yè)場景日漸增加的生產(chǎn)需求，因此應(yīng)用穩(wěn)定快速的運動規(guī)劃算法使機器人自主規(guī)劃運動軌跡，進行高效、靈活地作業(yè)，成為當前的研究熱點，國內(nèi)外學(xué)者對此展開了大量研究[2]。

根據(jù)環(huán)境建模方法和搜索策略的不同，機器人運動規(guī)劃分為圖搜索算法[3-6]和基于隨機采樣的規(guī)劃方法兩類。在復(fù)雜環(huán)境和高維空間下，圖搜索算法難以對障礙物進行建模和描述，而基于隨機采樣的概率路線圖(Probabilistic Road Map，PRM)[7]和快速搜索隨機樹(Rapidly-exploring Random Tree，RRT)[8]能較好地解決這一問題。相比前者，RRT算法采用單查詢、增量式擴展方法，無需構(gòu)建路線圖[9]，然而傳統(tǒng)RRT算法在搜索過程中存在規(guī)劃效率低[10]、導(dǎo)向性較差、路徑平滑較差等問題，近年來學(xué)者們針對機械臂具體應(yīng)用場景和性能要求，不斷改進RRT算法，力求解決以上問題。

KALISIAK等[11]提出RRT-blossom算法，利用回歸約束函數(shù)產(chǎn)生新節(jié)點，降低隨機樹重復(fù)區(qū)域的搜索概率，但限制了節(jié)點的自適應(yīng)拓展，路徑平均長度較長；為解決上述問題，KARAMAN等[12]提出漸進最優(yōu)RRT(RRT*)算法，通過重選父節(jié)點及布線迭代找到最優(yōu)路徑，但其搜索效率較低；GAMMELL等[13]提出Informed RRT*算法，通過限制搜索區(qū)域更快地接近最優(yōu)解，但無法限制隨機樹的規(guī)模；MASHAYEKHI等[14]在RRT*中引入雙樹拓展策略，通過同時擴展兩樹來提高搜索速度并不斷優(yōu)化路徑；針對RRT獲得的路徑質(zhì)量差和探索存在盲目性的問題，PALMIERI等[15]提出Theta*-RRT算法，該算法速度更快、路徑質(zhì)量更優(yōu)，但存在重復(fù)搜索的問題；ZHANG等[16]引入回歸機制和自適應(yīng)擴展機制，防止對配置空間過度搜索；LIU等[17]將目標引力變量加入RRT，獲得不同長度的步長以提高搜索效率，但易使搜索陷入局部最小值。

基于上述成果，針對RRT算法拓展效率低、收斂速度慢和路徑粗糙的問題，本文提出一種基于約束采樣的RRT算法。通過舍棄已探索區(qū)域的采樣點來減少冗余節(jié)點，使隨機樹朝向未探索區(qū)域生長，并采用動態(tài)采樣域和貪婪策略提高收斂速度，剔除路徑的冗余節(jié)點來縮短路徑長度，然后結(jié)合三次B樣條曲線來平滑軌跡。最后通過MATLAB和機器人操作系統(tǒng)(Robot Operating System, ROS)平臺分別進行仿真和真機實驗，驗證該算法有效可行。

1 改進的快速搜索隨機樹算法

1.1 快速搜索隨機樹算法基本原理

RRT是一種基于隨機采樣的路徑探索方法[18]。首先，算法以路徑起點xinit作為隨機樹T的根節(jié)點，通過隨機采樣得到采樣點xrand；然后，在已有隨機樹中找到距離xrand最近的節(jié)點xnear；從xnear指向xrand的方向上，以一定步長生長得到新節(jié)點xnew，若拓展到xnew的路徑上沒有碰到障礙物，則將xnew節(jié)點加入隨機搜索樹，否則重新進行采樣和隨機樹生長；隨著迭代次數(shù)的增加，當隨機樹上新添加的葉子節(jié)點xnew等于目標點xgoal時，搜索完成，找到一條自xinit到xgoal的可行路徑。

通過分析隨機樹的生長過程可知，RRT算法傾向于拓展到開闊的未探索空間。設(shè)隨機樹的已探索空間在整個探索空間占比為P，表示為

(1)

圖1所示為RRT生長過程?？梢?，當?shù)螖?shù)較少時，用隨機采樣不會影響隨機樹探索未拓展空間的效率；隨著迭代次數(shù)的增加，未拓展區(qū)域減少，在已探索過的區(qū)域內(nèi)仍會產(chǎn)生xrand，導(dǎo)致大量的重復(fù)生長。在該過程中，遍歷隨機樹節(jié)點找尋xnear并進行碰撞檢測占用了大量計算資源，對于已探索過的區(qū)域，這樣的消耗是不必要的。

標準RRT將固定空間作為采樣域，意味著每次產(chǎn)生采樣點xrand的空間是相同的，無指向性的隨機采樣具有探索未拓展空間的傾向，但在簡單地圖中搜尋可以到達xgoal的可行路徑時，這種采樣方式缺乏引導(dǎo)，會隨機向各個方向拓展，生成大量無用節(jié)點，收斂速度慢。

PPT規(guī)劃出的路徑花費較長且彎折多，直接使用會使機械臂在運行中產(chǎn)生速度、加速度突變，造成機械臂振動，因此需要對規(guī)劃路徑進行優(yōu)化才能應(yīng)用于機械臂避障任務(wù)。

1.2 稀疏節(jié)點產(chǎn)生機制

為減少標準RRT算法對已探索區(qū)域的重復(fù)搜索，本文提出一種稀疏節(jié)點產(chǎn)生機制，該機制產(chǎn)生稀疏節(jié)點的流程圖如圖2所示。稀疏節(jié)點的生長過程如圖3所示，假設(shè)探索空間被劃分為6×5個獨立方格，h為單位方格長度，稱每個單位方格為一個域。

標準RRT在整個自由空間中采樣，因此xgoal與樹中最接近它的葉子節(jié)點之間的距離終將收斂到0。因此無論ε有多小，最終總會有新的葉子節(jié)點xnew滿足條件

|xnew-xgoal|<ε。

(2)

|xgoal-xnew|≤d;

OBS_NOT_FREE(xgoal,xnew)。

(3)

函數(shù)Create_Sparse_Node(T,h,u,NumOfNode)為本文所提稀疏節(jié)點生成機制，偽代碼描述如下：

函數(shù)1Create_Sparse_Node(T,h,u,NumOfNode)。

1.xrand←RANDOM();

3.for i=1:NumOfNode

5. return xrand;

6. end if

7. i=i+1

8.end for

11.if OBS_ FREE(xnear,xnew)

12.T←T.add_vertex(xnew);

13.end if

1.3 動態(tài)采樣域策略

在標準RRT算法中，隨機樹因擴展方向隨機性過強而在開闊區(qū)域進行大量無用搜索，為加快算法收斂速度，通常將引力函數(shù)加入RRT算法，引導(dǎo)隨機樹向目標點方向生長，但當目標點被障礙物阻擋時，隨機樹拓展將陷入局部最小值。一個良好的采樣策略不僅應(yīng)具有全局性的特點，還要有局部差異性，本文提出一種動態(tài)變化的采樣域策略，使生成的采樣點在以目標點為圓心、goal_r為半徑的圓內(nèi)，

(4)

式中：L為探索區(qū)域邊界的長度；V為控制goal_r半徑變化的權(quán)值，

(5)

式中：N1為能夠生成葉子節(jié)點的采樣點數(shù)量；N2為未通過碰撞檢測的采樣點數(shù)量；λ決定權(quán)值變化的快慢。當N1>N2時采樣半徑縮小，反之采樣半徑增大，但采樣范圍不應(yīng)超出探索空間的邊界。若權(quán)值V較大，則說明探索空間開闊，采樣范圍縮小到目標點附近，從而減少隨機樹在開闊空間生長的盲目性，加快隨機樹向目標點收斂的速度；若探索空間有障礙阻擋，則N2增大使權(quán)值V減小，從而擴大以目標點為圓心的采樣半徑，使隨機樹跳出局部極小值。

采用遺忘式采樣點計數(shù)方法生成S個采樣點后，將權(quán)值V初始化，以及時反映隨機樹拓展邊界附近障礙物的分布情況，提高本策略的魯棒性。函數(shù)Dynamic_sampling_domain(N1,N2,S,M,L)即為動態(tài)采樣域策略，其偽代碼描述如下:

函數(shù)2Dynamic_sampling_domain(N1,N2,S,M,L)。

1.if N1+N2

2. V=(N1/N2)^λ;

3. if V>M

4. goal_r=L/V;

5. if goal_r>L

6. goal_r=L;

7. end if

8. else

9. goal_r=L;

10. end if

11.end if

1.4 貪婪策略

L=V×u。

(6)

貪婪策略能夠以大步長產(chǎn)生新拓展點，從而減少空白區(qū)域的無用搜索，提高局部收斂速度。

1.5 路徑處理

假設(shè)采用DSSP-RRT算法規(guī)劃所得路徑節(jié)點為P[P1,P2,P3,…,Pn]，如圖6所示，若節(jié)點Pi與Pk之間的直線路徑無碰撞，則Pi與Pk間的節(jié)點均為冗余節(jié)點，將其從路徑節(jié)點序列中剔除。從起點開始，遍歷整個路徑節(jié)點序列，去除所有冗余節(jié)點，所得路徑長度較短且曲率小、彎折少。

準均勻B樣條曲線可對彎折處的局部路徑進行平滑處理，且不對整條路徑的基本形狀造成影響[19]，故本文采用三次B樣條曲線對算法所規(guī)劃的路徑進行平滑處理。

三次B樣條基函數(shù)為：

(7)

三次B樣條曲線段

c0,3(u)=c0×N0,3(u)+c1×N1,3(u)+

c2×N2,3(u)+c3×N3,3(u)。

(8)

當給定控制點后，可利用式(8)求出一系列滿足三次B樣條的曲線點。

如圖7所示，以DSSP-RRT算法為例，在一個存在隨機障礙物的環(huán)境中，左上角為起點，右下角為目標點，生成隨機樹，樹的主干線條為初步規(guī)劃所得路徑，應(yīng)用上文所述方法除去冗余節(jié)點，得到彎折的較短路徑，經(jīng)過B樣條平滑處理后得到最終路徑?？梢?，優(yōu)化后的路徑較短且平滑，基本保持原有形狀走勢。

DSSP-RRT算法的詳細過程如圖8所示。改進算法能夠減少對已探索區(qū)域的重復(fù)探索，提高采樣點的目標導(dǎo)向性，加快隨機樹的收斂速度，減少路徑彎折，縮短路徑長度。

2 算法仿真實驗和分析

2.1 仿真實驗

為驗證改進算法DSSP-RRT的性能優(yōu)勢，設(shè)計兩種不同復(fù)雜度的場景進行仿真對比，實驗將標準RRT，PB-RRT，GB-RRT和本文提出的DSSP-RRT進行對比，4種算法中對應(yīng)的參數(shù)相同。其中GB-RRT步長u1=16，u2=8，其余實驗算法步長u=20。PB-RRT算法的概率值P=0.8，DSSP-RRT算法的域長度h=10，權(quán)值V中的冪值λ=3，M=3，S=300。

兩個仿真地圖分別為障礙散亂分布場景和迷宮場景，大小均為400×400，起始點為(20，20)，目標點為(380，380)。如圖9和圖10所示，場景中設(shè)有四邊形隨機障礙物，隨機樹的主干線段為規(guī)劃路徑。表1和表2所示為4種算法在到達目標點過程中的搜索時間、所產(chǎn)生節(jié)點數(shù)量和路徑長度的對比。

表1 散亂障礙環(huán)境中4種算法的實驗結(jié)果

表2 迷宮環(huán)境中4種算法的實驗結(jié)果

仿真實驗在MATLAB R2021b軟件中進行，計算機配置為64位Windows 10操作系統(tǒng)、Intel Core i7-8750H處理器。為保證仿真的真實有效性，各場景地圖均進行20次重復(fù)實驗，并取數(shù)據(jù)平均值作為結(jié)果。

2.2 性能分析

由表1可知，在障礙物散亂分布的場景中，GB-RRT算法和PB-RRT算法的表現(xiàn)明顯優(yōu)于標準RRT，而DSSP-RRT在搜索時間和迭代次數(shù)上比RRT算法分別減少88%和94%，路徑長度減少10%，其各項指標均優(yōu)于GB-RRT算法和PB-RRT算法。這是因為引入的動態(tài)采樣域策略，通過將拓展成功的采樣點與未通過碰撞檢測的采樣點數(shù)量之比的動態(tài)變化作為權(quán)值影響采樣域的范圍，使散亂障礙物環(huán)境中的采樣范圍迅速收縮到目標點附近，減少了隨機樹在開闊空間盲目生長所產(chǎn)生的節(jié)點數(shù)，同時貪婪策略的引入進一步減少了拓展中無用節(jié)點的數(shù)量。兩種策略大大減少了算法無效迭代浪費的時間，加快了算法的收斂速度。

由表2可知，在迷宮環(huán)境中，GB-RRT算法和PB-RRT算法比標準RRT表現(xiàn)差，而DSSP-RRT算法的搜索時間、迭代次數(shù)、路徑長度比RRT算法分別減少79%，78%，7%。這是因為GB-RRT算法和PB-RRT算法在狹窄通道或局部受限區(qū)域中搜索時易陷入局部最小，標準RRT算法在已探索區(qū)域重復(fù)采樣搜索產(chǎn)生大量步長極短的拓展點，導(dǎo)致大量計算資源浪費在尋找xnear和碰撞檢測上。而DSSP-RRT算法引入的稀疏節(jié)點產(chǎn)生機制僅保留未探索區(qū)域的稀疏采樣，減少了已搜索區(qū)域的重復(fù)采樣，從而減少遍歷隨機樹節(jié)點尋找最近節(jié)點的次數(shù)，進而減少產(chǎn)生新節(jié)點時碰撞檢測的次數(shù)，保證復(fù)雜環(huán)境中的隨機樹稀疏而整齊。

在兩個地圖中，DSSP-RRT算法相比RRT算法在路徑長度上分別減少10%和7%，且均優(yōu)于GB-RRT算法和PB-RRT算法，這是由于引入去除路徑中冗余節(jié)點的路徑處理方法和B樣條曲線的平滑方法而獲得了較優(yōu)路徑。

3 機械臂運動規(guī)劃實驗

3.1 機械臂運動學(xué)建模與碰撞檢測

本文研究對象為UR5串聯(lián)機械臂，其D-H坐標如圖11所示，相關(guān)D-H參數(shù)如表3所示。

表3 機械臂D-H參數(shù)

根據(jù)D-H模型及參數(shù)，可得該機械臂相鄰兩關(guān)節(jié)之間的齊次變換矩陣

(9)

式中：cθi表示cosθi，sθi表示sinθi。末端坐標系到基坐標系的變換矩陣

(10)

式中：n，o，a3個列向量表示機械臂的姿態(tài)；px,py,pz表示末端執(zhí)行器坐標系原點在世界坐標系中的位置。

碰撞檢測在運動規(guī)劃過程中所用時間的占比達90%[20]，因此選取合適的碰撞檢測方法可有效提高規(guī)劃效率。本文根據(jù)機械臂結(jié)構(gòu)形態(tài)和障礙物幾何形狀選用軸向平行包圍盒(Aixe Align Bounding Box, AABB)包絡(luò)法。AABB包絡(luò)法指用最小體積的六面體將機械臂各連桿和障礙物分別進行包絡(luò)[21]，使機械臂與障礙物間的碰撞檢測問題轉(zhuǎn)化為對空間平面之間位置關(guān)系的判斷[22]。為進一步簡化，以機械臂連桿的徑向最大半徑ri對障礙物進行膨脹處理，從而將判斷空間平面間的碰撞轉(zhuǎn)化為判斷空間直線與平面間的位置關(guān)系。如圖12所示，包絡(luò)UR5機械臂各連桿的空間線段方程為{R1,R2,…,R6}，包絡(luò)障礙物的六面體為Oi。若{R1,R2,…,R6}與Oi各面存在交點，則機械臂與障礙物發(fā)生碰撞，需重新規(guī)劃路徑，否則機械臂按搜索軌跡運動。

3.2 UR5靜態(tài)避障實驗

為驗證本文所提DSSP-RRT算法有效可行，在MATLAB 2021b版本Robotic System Toolbox工具箱搭建UR5機械臂仿真場景并進行運動規(guī)劃實驗。在關(guān)節(jié)空間采樣規(guī)劃，采用正運動學(xué)在Work-Space進行碰撞檢測，將初步規(guī)劃的無碰撞路徑用回溯去除冗余節(jié)點策略進行剪枝，然后采用三次B樣條曲線得到最終的平滑軌跡。

圖13所示為DSSP-RRT算法與標準RRT算法應(yīng)用于UR5機械臂避障運動規(guī)劃的末端軌跡對比效果。機械臂由初始姿態(tài)(單位：rad)(1.016，-0.339，-1.712，0.158，2.066，-0.570 9)，避開長0.6 m、寬0.03 m、高0.35 m的長方體障礙物及3個圓柱體障礙物到達目標姿態(tài)(1.342，-1.987，0.893，1.041，2.296，-0.852)。路點組成的線段為末端軌跡，從圖中可見RRT算法規(guī)劃的軌跡點散亂，DSSP-RRT算法規(guī)劃的軌跡較平滑且能夠成功避障。

圖14所示為機械臂各關(guān)節(jié)角度及角速度變化曲線，可見DSSP-RRT算法由于剔除了冗余路徑點，有效減小了關(guān)節(jié)角度極值；而且由于結(jié)合B樣條曲線得到平滑的關(guān)節(jié)軌跡，降低了角速度變換范圍，減少了機械臂關(guān)節(jié)抖振情況，從而保證機械臂平穩(wěn)避障。兩種算法的實驗結(jié)果如表4所示，可見DSSP-RRT算法以產(chǎn)生稀疏節(jié)點的機制和變化的采樣域進行采樣，提高了節(jié)點利用率和收斂速度，使節(jié)點數(shù)量減少76.19%，進而使時間成本降低74.81%，有效提高了軌跡規(guī)劃效率。

表4 算法實驗結(jié)果對比

為進一步驗證本文所提DSSP-RRT算法應(yīng)用于機械臂運動規(guī)劃的實用性，在ROS平臺下的MoveIt！搭建UR5機械臂障礙場景并進行運動規(guī)劃測試。將DSSP-RRT算法開發(fā)成規(guī)劃器編寫進OMPL(open motion planning library)庫并作為插件與MoveIt！交互，在關(guān)節(jié)空間中采用標準RRT算法和DSSP-RRT算法進行采樣規(guī)劃，結(jié)合MoveIt！自有運動學(xué)求解器，對機械臂各個采樣位姿進行碰撞檢測，進而規(guī)劃出運動軌跡。

如圖15所示，在Rviz可視化場景中，帶隔板桌面為障礙物，深色不透明機械臂為始末位姿，半透明機械臂為按所規(guī)劃路徑運動過程中的位姿變化。可見機械臂采用DSSP-RRT算法能夠順利避障并到達目標位姿。

圖16所示為真實環(huán)境下的UR5機械臂運動規(guī)劃驗證實驗。采用Intel RealSense Depth Camera D435i相機獲取障礙環(huán)境信息，白色長方體為障礙物，機械臂自左側(cè)起始位姿規(guī)劃出了一條到達右側(cè)目標位姿的無碰撞路徑，能夠在真實環(huán)境中有效完成避障任務(wù)，證明了本文所提算法的實用性。

4 結(jié)束語

本文提出基于約束采樣RRT的機械臂運動規(guī)劃方法，首先分析了標準RRT算法存在的問題，針對其在已探索空間內(nèi)重復(fù)采樣并拓展的問題提出稀疏節(jié)點產(chǎn)生機制，通過控制隨機采樣點產(chǎn)生的位置和數(shù)量減少在已探索區(qū)域?qū)ふ腋腹?jié)點的次數(shù)，從而減少隨機樹冗余節(jié)點數(shù)量和碰撞檢測次數(shù)，加快探索未知區(qū)域；針對標準RRT算法拓展方向隨機性過強的問題，提出動態(tài)采樣域策略，通過約束采樣區(qū)域的范圍使采樣域大小根據(jù)采樣情況動態(tài)變化，確保采樣兼具全局性和局部差異性的特點，進一步減少開闊區(qū)域的盲目搜索；同時，為避免局部拓展緩慢，引入貪婪策略，使步長動態(tài)變化，減少了開闊區(qū)域冗余節(jié)點的數(shù)量，提高了收斂速度；最后通過剔除規(guī)劃路徑中的冗余節(jié)點來縮短路徑長度，用B樣條曲線進行軌跡平滑，提高軌跡規(guī)劃質(zhì)量。通過在二維地圖環(huán)境下的仿真實驗和Robotic System Toolbox仿真實驗以及ROS平臺中機械臂的運動規(guī)劃實驗、真機避障實驗，驗證了所提算法的可行性和實用性。然而，DSSP-RRT算法仍保留了RRT算法的部分缺陷，例如規(guī)劃路徑的長度不是最優(yōu)，隨機樹拓展因具有隨機性而導(dǎo)致重復(fù)規(guī)劃時軌跡不同，這些問題將是今后研究的重點。