徐文浩，陳東生

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

0 引言

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

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

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

1 算法原理

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

1.1 RRT*算法原理

標(biāo)準(zhǔn)RRT*算法的具體流程如下：

1)生成隨機(jī)樹Tree：以起始點(diǎn)作為樹的第一個節(jié)點(diǎn)Xstart，以目標(biāo)點(diǎn)作為樹的最終節(jié)點(diǎn)Xgoal；

2)隨機(jī)樹Tree的生長：在機(jī)器人的工作空間Q內(nèi)，按照目標(biāo)偏向思想隨機(jī)生成采樣點(diǎn)Xrand，之后搜索隨機(jī)樹Tree上距離Xrand最近的節(jié)點(diǎn)Xnear，以Xnear到Xrand的方向?yàn)榉较蛳蛄堪匆粋€固定步長Step拓展長生出新的樹枝節(jié)點(diǎn)Xnew，檢測Xnear與Xnew兩節(jié)點(diǎn)連線軌跡是否會與障礙物發(fā)生碰撞，不會則節(jié)點(diǎn)Xnew生長成功，否則廢棄繼續(xù)搜尋新的Xnew，生長成功后將其添加到隨機(jī)樹Tree上，同時計(jì)算其路徑價值函數(shù)Cost(即從起始點(diǎn)Xstart到當(dāng)前節(jié)點(diǎn)Xnew，依次累加節(jié)點(diǎn)間的距離而得到的路徑距離)；

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

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

5)搜索過程迭代：重復(fù)上述過程，經(jīng)過有限次的迭代，至隨機(jī)樹Tree生長出一個節(jié)點(diǎn)與目標(biāo)點(diǎn)Xgoal之間的距離小于設(shè)定的閾值(閾值范圍內(nèi)無障礙物)，之后下一次生長隨機(jī)樹Tree直接生長到Xgoal并停止搜索；

6)末端軌跡檢索生成：隨機(jī)樹Tree生長結(jié)束后從其目標(biāo)點(diǎn)Xgoal開始向起始點(diǎn)Xstar檢索當(dāng)前路徑節(jié)點(diǎn)的父節(jié)點(diǎn)，之后父節(jié)點(diǎn)成為路徑節(jié)點(diǎn)繼續(xù)檢索，依次迭代最終生成完整的末端避障軌跡。

(1)

式中，Step是樹枝節(jié)點(diǎn)的固定生長步長，k是當(dāng)前隨機(jī)樹Tree上所有節(jié)點(diǎn)的個數(shù)，該公式的作用是縮小后期節(jié)點(diǎn)Xnew周圍的檢索范圍，避免后期出現(xiàn)因隨機(jī)樹上的節(jié)點(diǎn)數(shù)量過多而導(dǎo)致計(jì)算過程緩慢甚至卡死的情況。

1.2 RRT*算法改進(jìn)

1.2.1 六軸工業(yè)機(jī)器人手臂碰撞檢測

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

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

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

表1 KR600r2380機(jī)器人DH參數(shù)表

除了機(jī)器人的手臂外，機(jī)器人的回轉(zhuǎn)死區(qū)也采用圓柱體包圍盒描述，圓柱體徑向半徑為700 mm。

圖1 機(jī)器人圓柱體包圍盒模型

針對障礙物(本文涉及的障礙物為工位上的工裝夾具)，因?yàn)榘鼑蚩梢暂^好地包圍障礙物，故采用包圍球表示障礙物。如圖2所示，工裝夾具為障礙物，以O(shè)為球心，Ro為半徑的球面為包圍球球面。

最終構(gòu)成的障礙物空間如圖3所示。

圖2 障礙物包圍球模型

圖3 障礙物空間

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

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

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

(2)

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

(3)

(4)

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

當(dāng)存在di≤s的情況時，則需要判斷p到Li線段兩端點(diǎn)是否滿足狀態(tài)二的兩個條件才能判斷機(jī)器人手臂與障礙物是否發(fā)生碰撞：

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

(5)

當(dāng)且僅當(dāng)夾角∠pbc和∠pcb二者為一個鈍角、一個銳角的情況時，p點(diǎn)向L2的投影為情況b；

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

當(dāng)以上情況均不滿足時則會發(fā)生機(jī)器人手臂與障礙物的碰撞。

1.2.2 RRT*算法先驗(yàn)引導(dǎo)

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

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

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

圖5 先驗(yàn)引導(dǎo)原理

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

2 試驗(yàn)結(jié)果與分析

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

表3 起點(diǎn)與目標(biāo)點(diǎn)信息表 mm

因?yàn)楸疚氖且詷?biāo)準(zhǔn)RRT*算法為基礎(chǔ)進(jìn)行的優(yōu)化設(shè)計(jì)算法，故同時對標(biāo)準(zhǔn)RRT*算法與先驗(yàn)引導(dǎo)RRT*算法進(jìn)行了對比仿真試驗(yàn)，試驗(yàn)1的結(jié)果如圖6和圖7所示，圖6是兩種算法得到的末端避障軌跡結(jié)果對比，其中圖6(a)、(b)是標(biāo)準(zhǔn)RRT*算法的軌跡結(jié)果，圖6(c)、(d)是先驗(yàn)引導(dǎo)RRT*算法的軌跡結(jié)果，由圖可看出，先驗(yàn)引導(dǎo)RRT*算法所得末端軌跡更優(yōu)于標(biāo)準(zhǔn)RRT*算法，其中由圖6(b)、(d)主視圖可看出，標(biāo)準(zhǔn)RRT*算法軌跡在中期為了能夠繞過障礙物有明顯的先下降后上升趨勢，導(dǎo)致軌跡冗余，不滿足軌跡優(yōu)化需求。分析其原因是以固定步長繞過障礙物時，每一步都會前移較多距離，會導(dǎo)致軌跡在向y軸方向繞過障礙物時，向z軸方向進(jìn)行較大的冗余位移，進(jìn)而不斷向下傾斜，當(dāng)繞過障礙物后才能再往上趨向目標(biāo)點(diǎn)。加入先驗(yàn)引導(dǎo)機(jī)制后軌跡下降/上升趨勢消除，正是因?yàn)樗惴ㄡ槍Υ爽F(xiàn)象有目的性地放大了y軸方向(目標(biāo)位移方向)的擴(kuò)展距離，同時減小了z軸方向(冗余位移方向)的擴(kuò)展距離，進(jìn)而軌跡得到優(yōu)化。

圖7是兩種算法機(jī)器人運(yùn)行末端避障軌跡的結(jié)果圖，圖中機(jī)器人均按實(shí)際現(xiàn)場運(yùn)行要求姿態(tài)運(yùn)行。圖7(a)～(c)是標(biāo)準(zhǔn)RRT*算法結(jié)果，圖7(d)～(f)是先驗(yàn)引導(dǎo)RRT*算法結(jié)果，由圖可看出，兩者均滿足六軸機(jī)器人手臂與末端軌跡避障要求，機(jī)器人運(yùn)行正常，包圍盒算法有效。

試驗(yàn)1的最終末端軌跡長度分別為：先驗(yàn)引導(dǎo)RRT*算法3 483.13 mm，標(biāo)準(zhǔn)RRT*算法4 037.82 mm，軌跡優(yōu)化14%左右。

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

圖9(a)～(c)是先驗(yàn)引導(dǎo)RRT*算法機(jī)器人運(yùn)行末端避障軌跡圖，因標(biāo)準(zhǔn)RRT*算法未能找到完整末端軌跡，故此處沒有標(biāo)準(zhǔn)RRT*算法的機(jī)器人運(yùn)行圖。由圖可看出，機(jī)器人仍能夠順利運(yùn)行所得的末端避障軌跡，所得結(jié)果有效。

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

圖7 目標(biāo)點(diǎn)1機(jī)器人避障軌跡運(yùn)行結(jié)果

圖8 目標(biāo)點(diǎn)2仿真試驗(yàn)?zāi)┒吮苷宪壽E結(jié)果

圖9 目標(biāo)點(diǎn)2機(jī)器人避障軌跡運(yùn)行結(jié)果

3 結(jié)束語

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

相較于標(biāo)準(zhǔn)RRT*算法，本文提出的先驗(yàn)引導(dǎo)RRT*算法有更強(qiáng)的軌跡搜索能力與軌跡優(yōu)化能力，且所得軌跡滿足機(jī)器人末端與手臂同時避障的需求。能夠繞開標(biāo)準(zhǔn)RRT*算法無法繞開的苛刻條件障礙物，能縮短算法軌跡14%左右，達(dá)到了現(xiàn)場裝配的避障軌跡需求。