六軸工業機器人先驗引導RRT*避障軌跡算法研究

徐文浩，陳東生

(中國工程物理研究院 機械制造工藝研究所，四川 綿陽 621900)

0 引言

隨著科學技術的不斷發展，機器人技術在社會各個領域得到了廣泛應用[1]。六軸串聯機器人具有自由度高、自動化程度高等特點，在非標自動化等運動受限、復雜危險的環境下具有高效的裝配能力，故被越來越多地應用在自動化設備中，作為零件搬運、裝配、焊接等工作的主體。但在復雜工作環境下，自動化機器人的避障要求也越來越突出，必須對避障軌跡進行規劃。末端避障軌跡規劃主要任務是根據作業任務的要求，計算出機器人一條從起始點到目標點的連續無碰撞路徑，傳統人工示教避障軌跡存在弊端，首先所得避障軌跡無法保障為最優軌跡，此外當現場設備環境發生變化時，需人工重新示教，工作量大且耗費時間。自動路徑規劃問題已吸引大量學者研究，提出了大量路徑規劃算法：基于搜索的A*算法[2-3]雖然搜索能力強，但需要對搜索范圍進行柵格劃分，對于高維狀態空間的復雜路徑規劃計算開銷大。基于建立引力斥力場的人工勢場法[4-5]在局部避障中具有較好的性能，可生成光滑的軌跡，但是人工勢場法存在局部極小值、無效避障、斥力過大等問題；基于智能算法的遺傳算法[6-7]能夠保證軌跡的搜索能力，但計算量大收斂速度慢，且存在容易陷入局部最優、搜索效率低等問題；基于隨機采樣路徑規劃方法[8]的提出，為解決高維度、復雜環境路徑規劃問題提供出了一種很好的解決思路。其中快速探索隨機樹(RRT，rapidly exploring random tree)算法[9]不需要空間柵格化，搜索能力強，十分適合應用在高維環境下，在機器人和許多其他領域得到廣泛應用。但RRT算法搜索效率低、路徑不穩定、無法得到最優路徑。為此，Karaman等人[10]將漸進最優的思想引入RRT算法中，提出了RRT*算法。RRT*算法在RRT算法的基礎上增加了父節點重選與剪枝重生成兩個環節，利用這個過程實現漸進最優。但標準RRT*算法仍存在路徑曲折、路徑搜索慢等諸多方面的不足。國內外學者對標準RRT*算法進行了改進，D.Jonathan等人[11]針對上述問題提出Informed RRT*算法，通過縮小狀態子集來簡化路徑搜索過程，提高了標準RRT*算法的收斂速度、減小了最終解決方案的成本、提高了找到困難通道的能力；趙盼等人[12]在路徑規劃層面上對標準 RRT*算法進行了優化，采用改進路徑生成方式、引入轉角約束、碰撞后調整等策略，改進了標準RRT*算法隨機路徑轉角大、路徑復雜等缺點，最終得到適用于超冗余度機器人路徑跟隨運動的末端避障軌跡規劃算法；楊也等人[13]提出一種改進的RRT*算法，采用有目的設置采樣點代替隨機高斯采樣和引進人工勢場與避障策略結合的思想，有效地平衡了室內機器人路徑規劃算法收斂時間與最佳路徑的可靠性；易馳等人[14]針對標準RRT*算法收斂速度慢和目標導向性差的問題，提出基于目標區域偏置擴展的RRT*算法，有效減少了隨機樹節點數量，加快了算法收斂速度；張玨等人[15]提出了一個高斯擬合平滑快速探索隨機樹智能算法(GFS RRT*-smart)，用于全局路徑規劃，提高了機器人在彎道急轉彎時的障礙回避性能；劉沖等人[16]提出一種基于RRT*的母線布線路徑規劃算法，通過引入中間點的方式改變已生成路徑到隨機點的擴展方式，同時在初始路徑生成過程中采取貪心的優化策略，獲得彎頭數量最少且滿足約束的路徑，很好地滿足母線的各項布線要求；崔春雷等人[17]提出了一種改進Bi-RRT的移動機器人路徑規劃算法，引入啟發式搜索策略使采樣過程，既可以利用目標點的位置信息又保證了算法的概率完備性，降低了搜索的盲目性，提高了搜索的效率；此外，楊瑩等人[18]在RRT算法的基礎上提出引入人工勢場法引力分量使節點的擴展更具方向性的混合式算法，在快遞分揀路徑規劃問題上得到突破。

本文針對六軸工業機器人裝配應用場景，采用優化的RRT*算法規劃自動避障路徑，上述算法相較于標準RRT*算法均有一定程度的改進優化效果，但針對本文的應用場景仍然存在兩點不足：1)上述算法尚缺少針對六軸工業機器人手臂避障的軌跡規劃。六軸工業機器人除了要求末端路徑與障礙物不發生碰撞之外，機器人各手臂軸也不能與障礙物發生碰撞；2)標準RRT*算法的隨機性導致其軌跡優化能力不足，生成的路徑曲折；軌跡搜素能力不足，面對條件較為苛刻的障礙物(如障礙物尺寸較大時)會出現繞不開障礙物、不能找到最終軌跡的情況，無法滿足本文應用場景下的路徑優化需求。

針對以上兩個問題，本文提出一種先驗引導RRT*算法，以標準RRT*算法為基礎，融合包圍盒算法思想以及先驗引導思想，使算法能生成同時滿足末端軌跡與機器人手臂避障需求的漸進最優末端軌跡，提高了算法的路徑優化能力與路徑搜索能力。

1 算法原理

本文提出的先驗引導RRT*算法思想如下：1)在標準RRT*算法的基礎上融入包圍盒算法思想：(1)針對機器人手臂軸，采用包圍盒算法計算手臂與障礙物的距離，進而判斷是否發生碰撞；(2)針對機器人回轉死區，為避免RRT*算法隨機樹生長到機器人回轉死區內導致機器人末端無法到達，將回轉死區設置為圓柱體包圍盒區域進行碰撞檢測。2)對RRT*算法隨機點生成方向以及隨機樹生長方向進行先計算最優生長方向后引導其生長的先驗引導機制，整體讓樹枝的生長方向更快速地避開障礙物、趨向目標點，使軌跡滿足漸進最優需求。

1.1 RRT*算法原理

標準RRT*算法的具體流程如下：

1)生成隨機樹Tree：以起始點作為樹的第一個節點Xstart，以目標點作為樹的最終節點Xgoal；

2)隨機樹Tree的生長：在機器人的工作空間Q內，按照目標偏向思想隨機生成采樣點Xrand，之后搜索隨機樹Tree上距離Xrand最近的節點Xnear，以Xnear到Xrand的方向為方向向量按一個固定步長Step拓展長生出新的樹枝節點Xnew，檢測Xnear與Xnew兩節點連線軌跡是否會與障礙物發生碰撞，不會則節點Xnew生長成功，否則廢棄繼續搜尋新的Xnew，生長成功后將其添加到隨機樹Tree上，同時計算其路徑價值函數Cost(即從起始點Xstart到當前節點Xnew，依次累加節點間的距離而得到的路徑距離)；

3)父節點重選：在新節點Xnew的鄰域r范圍內搜索隨機樹Tree上的節點集合Xnear_coor，r為鄰域的半徑，其計算公式見式(1)。計算以節點Xnear_coor(i)作為Xnew父節點的路徑價值Cost，將其與以Xnear作為父節點的路徑價值進行比較。選擇Cost小且無碰撞的路徑作為新樹枝更新Xnew的父節點與路徑價值Cost；

4)剪枝重生成：遍歷樹節點集合Xnear_coor，若Xnear_coor(i)到Xnew的價值函數加上Xnew累計到Xstart的價值函數小于Xnear_coor(i)累計到Xstart的原價值函數，且Xnear_coor(i)與Xnew間的軌跡與障礙物不發生碰撞，則將Xnew作為Xnear_coor(i)的新父節點，同時更新價值函數Cost；

5)搜索過程迭代：重復上述過程，經過有限次的迭代，至隨機樹Tree生長出一個節點與目標點Xgoal之間的距離小于設定的閾值(閾值范圍內無障礙物)，之后下一次生長隨機樹Tree直接生長到Xgoal并停止搜索；

6)末端軌跡檢索生成：隨機樹Tree生長結束后從其目標點Xgoal開始向起始點Xstar檢索當前路徑節點的父節點，之后父節點成為路徑節點繼續檢索，依次迭代最終生成完整的末端避障軌跡。

(1)

式中，Step是樹枝節點的固定生長步長，k是當前隨機樹Tree上所有節點的個數，該公式的作用是縮小后期節點Xnew周圍的檢索范圍，避免后期出現因隨機樹上的節點數量過多而導致計算過程緩慢甚至卡死的情況。

1.2 RRT*算法改進

1.2.1 六軸工業機器人手臂碰撞檢測

由1.1小節的標準RRT*算法原理介紹可知，標準RRT*算法在其第2)～4)步生長新樹枝的過程中會進行末端軌跡與障礙物的碰撞檢測，當滿足無碰撞條件時新的樹枝才會生長成功。本文在此基礎上加入對機器人各手臂軸與障礙物的碰撞檢測，對Xnear與Xnew兩節點生成的末端軌跡利用機器人的逆向運動學，通過其DH參數求逆解[19]得到機器人的關節旋轉角矩陣，再通過機器人的正向運動學求出其手臂各關節的齊次位姿矩陣，之后采用包圍盒算法計算并判斷機器人手臂在運行該段軌跡時是否會與障礙物發生碰撞，在滿足機器人的末端與手臂均不會發生碰撞的情況下才會生長成新的樹枝節點，否則廢棄。以此過程不斷迭代最終得到機器人各手臂軸與末端均不會與障礙物發生碰撞的軌跡。

機器人手臂避障：由于機器人手臂與障礙物結構復雜，難以用數學模型表達。故采用包圍盒算法對其模型進行簡化[20]：用體積稍大于原來模型但是自身特性簡單的幾何體(即包圍盒)來近似地代替原復雜的幾何模型對象。進而將碰撞問題抽象為簡單的幾何體的位置關系問題進行判斷。

本文的研究對象為庫卡KR600r2380六軸串聯工業機器人，為了方便算法設計，需要對其模型進行合理簡化：1)機器人的第一連桿只有轉動，沒有位移，所以不需要考慮其碰撞風險；2)機器人的第四關節軸和軸線平行，故其第三連桿與第四連桿位置關系始終保持不變，將二者視為一體；3)同2)，機器人的第五連桿與機器人末端視為一體；4)機器人各連桿可由圓柱體近似包圍，故采用圓柱體包圍盒進行表示；5)在本文的應用場景中，機器人的第三連桿后端動力部位為安全位置，可不計入包圍盒范圍。最終簡化后的示意圖如圖1所示。圖中L1、L2、L3為簡化后需考慮碰撞部分的機器人連桿中心線，設此部分機器人連桿的最大徑向半徑為RL1、RL2、RL3，根據其模型參數可得：RL1=285 mm、RL2=207 mm、RL3=125 mm。除此之外，根據機器人模型還可以得到機器人的DH參數表，詳細如表1所示。

表1 KR600r2380機器人DH參數表

除了機器人的手臂外，機器人的回轉死區也采用圓柱體包圍盒描述，圓柱體徑向半徑為700 mm。

圖1 機器人圓柱體包圍盒模型

針對障礙物(本文涉及的障礙物為工位上的工裝夾具)，因為包圍球可以較好地包圍障礙物，故采用包圍球表示障礙物。如圖2所示，工裝夾具為障礙物，以O為球心，Ro為半徑的球面為包圍球球面。

最終構成的障礙物空間如圖3所示。

圖2 障礙物包圍球模型

圖3 障礙物空間

最終將障礙物與機器人各手臂軸的碰撞檢測轉化為包圍球中心到圓柱體包圍盒中心線線段的距離計算。如圖4所示，設包圍球中心到圓柱體包圍盒中心線距離為d，s為安全距離，分析可知安全距離即為包圍球與圓柱體包圍盒半徑之和。檢測機器人各手臂軸與障礙物是否發生碰撞需滿足對應的計算判定結果。具體計算過程如下。

障礙物與機器人手臂的位置關系可分為兩種情況，以圖4中L2軸與障礙物的位置關系為例，情況a是包圍球球心向圓柱體包圍盒中心線的投影落在圓柱體包圍盒中心線線段范圍內；情況b則是其投影落在圓柱體包圍盒中心線線段范圍外，經歸納得，要滿足機器人手臂軸與障礙物不發生碰撞僅存在兩種狀態，狀態一是滿足包圍球球心到圓柱體包圍盒中心線的距離d大于安全距離s，這種狀態下無論是情況a還是情況b，均滿足無碰撞需求。狀態二是兩者的距離d小于安全距離s的時候，此時需要滿足兩個條件才能滿足無碰撞需求：條件1是包圍球與圓柱體包圍盒的位置關系為情況b；條件2是包圍球球心到圓柱體包圍盒中心線線段兩端點的距離中較小者大于安全距離s。除此之外的狀態機器人手臂與障礙物均會發生碰撞。故判斷是否發生碰撞需分情況進行討論。

首先計算包圍球球心到圓柱體包圍盒中心線的垂直距離d。通過機器人逆向運動學，根據機器人末端坐標可計算得到機器人關節角矩陣q，再由機器人正向運動學得到機器人各個關節的齊次位姿矩陣T，其中T(1，4)、T(2，4)、T(3，4)分別為機器人各關節位置的x、y、z坐標，設Ta、Tb、Tc、Td分別為機器人關節二、三、五、六的齊次位姿矩陣，由式(2)可求出對應的齊次矩陣：

(2)

進而由式(3)可求出各碰撞軸中心線的向量：

(3)

(4)

當條件d1>s、d2>s、d3>s均滿足時，機器人手臂與障礙物之間距離均大于安全距離，不會發生碰撞。

當存在di≤s的情況時，則需要判斷p到Li線段兩端點是否滿足狀態二的兩個條件才能判斷機器人手臂與障礙物是否發生碰撞：

1)判斷條件1的方法(以軸L2為例)：L2兩端點為L2b和L2c，∠pbc和∠pcb是p與線段的兩個夾角，由余弦定理可得：

(5)

當且僅當夾角∠pbc和∠pcb二者為一個鈍角、一個銳角的情況時，p點向L2的投影為情況b；

2)條件2的判斷：通過兩點間的距離計算公式可求得p到端點L2b和L2c的距離，并將其中的較小者min(L2b，L2c)與s比較，當min(L2b，L2c)>s時，該段路徑的手臂軸與障礙物不會發生碰撞。

當以上情況均不滿足時則會發生機器人手臂與障礙物的碰撞。

1.2.2 RRT*算法先驗引導

標準RRT*算法在隨機采樣點的生成過程中有一個簡單的生成引導機制：設置一個簡單概率(一般情況為50%)，生成隨機采樣點時基于這個概率以目標點作為生成的采樣點，即每次生成采樣點時有50%概率以目標點為生成點，進而對隨機樹的生長起到一定的引導作用，目的是為了避免因隨機采樣點的生成過于分散，產生過多的不必要樹枝節點，進而影響算法軌跡結果的質量以及算法整體的運算速度，不過這種引導機制過于簡單，雖然可以起到一定的引導作用，但是對軌跡結果的質量提高效果還達不到應用要求。以這種引導思想為基礎，本文提出了優化的引導機制：先驗引導機制。

先驗引導算法思想：1)當隨機樹樹枝生長至障礙物附近時，先計算能繞過障礙物、趨向目標點的最短方向，再控制生成隨機采樣點的三坐標方向向量大小，讓其生成更傾向于計算出的方向；2)同1)，在隨機樹樹枝生長擴展時控制其生長方向三坐標方向向量大小以趨向計算出的方向；3)為了更快速找到終點，每成功生成一個新樹枝節點都進行一次新樹枝節點與目標點之間的軌跡障礙物碰撞判斷，若兩節點間的直線軌跡滿足無碰撞需求，則下一次生長直接連接目標點以找到優化的避障軌跡。

具體算法原理如圖5所示，以圖5(a)圓柱體包圍盒情況為例：1)當路徑無障礙物碰撞時隨機樹正常生長樹枝；2)當發生碰撞時，以碰撞位置前樹節點與目標點的連線向量為方向向量對該樹節點進行投影，將該樹節點投影至過目標點且與X-Y平面垂直的平面上(此處該平面與X-Y平面垂直的處理是為了簡化投影算法的計算，將3維空間中的投影簡化為2維投影，如圖5(a)主視圖所示)，投影后可以計算目前樹節點距離障礙物邊緣的最小z軸距離與最小y軸距離(此處為具體例子，實際應用中需要繞開的方向也可能是x軸與y軸的方向，或者其他可能的方向，原理相同不再枚舉)，對比可知此時樹枝往y軸負方向生長最容易繞開障礙物、靠近目標點，故此時控制隨機點向y軸方向以-1.5倍長度生成，向z軸方向以0.5倍長度生成，樹枝生長方向也做相同處理，重復此過程至繞開障礙物，后重復1)、2)兩步過程；3)在每次生長成功新的樹枝節點后，判斷生成的新樹枝節點Xnew與目標點Xgoal之間的直連軌跡是否滿足無碰撞需求，滿足則下一次樹枝生長直接以Xgoal為節點并完成生長，進而得到優化完成的避障軌跡。

圖5 先驗引導原理

包圍球障礙物的處理原理與之相同。

2 試驗結果與分析

為檢驗先驗引導RRT*算法的性能，本文在MATLAB2021a上進行了仿真試驗。試驗軟件環境為：Windows11，硬件配置：12th Gen Intel(R)Core(TM)i7-12700H2.30 GHz，內存(RAM)16 GB。三維仿真地圖尺寸為：6 000×6 000×4 000(mm3)，障礙物設置為球體(工裝夾具)及圓柱體(機器人回轉死區)，具體障礙物信息如表2所示。包括8個球體障礙物和一個圓柱體障礙物。為了驗證算法的魯棒性，本文進行了兩組試驗，試驗參數詳見表3。表中需要說明的是：因為本文的目標點為工裝夾具的裝夾位置，第一組試驗的目標點為工裝夾具8的裝夾位置，即為障礙球8所在位置，所以在第一組試驗時表2中的障礙球8需要去除，否則會導致算法軌跡無法找到目標點。同理，在第2組試驗中，目標點為工裝夾具7的裝夾位置，此時表2中的障礙球7需要去除。設置初始固定步長step為400 mm。

表3 起點與目標點信息表 mm

因為本文是以標準RRT*算法為基礎進行的優化設計算法，故同時對標準RRT*算法與先驗引導RRT*算法進行了對比仿真試驗，試驗1的結果如圖6和圖7所示，圖6是兩種算法得到的末端避障軌跡結果對比，其中圖6(a)、(b)是標準RRT*算法的軌跡結果，圖6(c)、(d)是先驗引導RRT*算法的軌跡結果，由圖可看出，先驗引導RRT*算法所得末端軌跡更優于標準RRT*算法，其中由圖6(b)、(d)主視圖可看出，標準RRT*算法軌跡在中期為了能夠繞過障礙物有明顯的先下降后上升趨勢，導致軌跡冗余，不滿足軌跡優化需求。分析其原因是以固定步長繞過障礙物時，每一步都會前移較多距離，會導致軌跡在向y軸方向繞過障礙物時，向z軸方向進行較大的冗余位移，進而不斷向下傾斜，當繞過障礙物后才能再往上趨向目標點。加入先驗引導機制后軌跡下降/上升趨勢消除，正是因為算法針對此現象有目的性地放大了y軸方向(目標位移方向)的擴展距離，同時減小了z軸方向(冗余位移方向)的擴展距離，進而軌跡得到優化。

圖7是兩種算法機器人運行末端避障軌跡的結果圖，圖中機器人均按實際現場運行要求姿態運行。圖7(a)～(c)是標準RRT*算法結果，圖7(d)～(f)是先驗引導RRT*算法結果，由圖可看出，兩者均滿足六軸機器人手臂與末端軌跡避障要求，機器人運行正常，包圍盒算法有效。

試驗1的最終末端軌跡長度分別為：先驗引導RRT*算法3 483.13 mm，標準RRT*算法4 037.82 mm，軌跡優化14%左右。

試驗2結果如圖8和圖9所示：圖8是兩種算法得到的末端避障軌跡結果對比，其中圖8(a)、(b)是標準RRT*算法的軌跡結果，圖8(c)、(d)是先驗引導RRT*算法的軌跡結果，由圖可看出，此時標準RRT*算法無法找到末端避障軌跡，隨機樹會卡死在機器人回轉死區前，分析原因是碰撞節點與目標點連線方向在障礙物的中心面附近，生成的隨機點不容易出現在繞過障礙物的部位，導致算法達到迭代次數上限，軌跡搜索失敗停止。相反先驗引導RRT*算法能夠通過更大的y軸偏移量繞過障礙物，找到符合要求的末端避障軌跡。

圖9(a)～(c)是先驗引導RRT*算法機器人運行末端避障軌跡圖，因標準RRT*算法未能找到完整末端軌跡，故此處沒有標準RRT*算法的機器人運行圖。由圖可看出，機器人仍能夠順利運行所得的末端避障軌跡，所得結果有效。

綜合對比試驗1與試驗2的結果可知，本文提出的先驗引導RRT*算法在標準RRT*算法的基礎上通過引入包圍盒算法思想和先驗引導思想，使算法能夠生成滿足機器人手臂與末端同時避障的軌跡；且相較于標準RRT*算法有更強的軌跡搜索能力，能夠繞開條件更苛刻的障礙物；有更強的軌跡優化能力，最終軌跡縮短14%左右。

圖7 目標點1機器人避障軌跡運行結果

圖8 目標點2仿真試驗末端避障軌跡結果

圖9 目標點2機器人避障軌跡運行結果

3 結束語

本文針對六軸工業機器人實際裝配中的避障需求，以標準RRT*算法為基礎進行優化算法設計，提出了先驗引導RRT*算法：1)針對標準RRT*算法只生成末端避障軌跡，不能滿足機器人手臂避障需求的問題，本文融入包圍盒算法，加入機器人手臂碰撞檢測，進而使生成的軌跡同時滿足機器人末端與手臂均不發生碰撞的需求；2)針對標準RRT*算法的軌跡優化能力、軌跡搜索能力不能滿足現場裝配需求的問題，參考其隨機點生成引導機制，提出了先驗引導機制。通過先計算最優繞開障礙物、趨向目標點的方向，后對隨機點生成方向與隨機樹生長方向進行計算方向引導的機制以優化標準RRT*算法的軌跡優化能力、軌跡搜索能力來滿足現場裝配的要求。

相較于標準RRT*算法，本文提出的先驗引導RRT*算法有更強的軌跡搜索能力與軌跡優化能力，且所得軌跡滿足機器人末端與手臂同時避障的需求。能夠繞開標準RRT*算法無法繞開的苛刻條件障礙物，能縮短算法軌跡14%左右，達到了現場裝配的避障軌跡需求。