（趙 盼，杜兆才，劉 江，王明陽(yáng)

（1.中國(guó)航空制造技術(shù)研究院，北京 100024；2.北京科技大學(xué)，北京 100083）

隨著科學(xué)技術(shù)的不斷發(fā)展，機(jī)器人技術(shù)在社會(huì)各個(gè)領(lǐng)域得到了廣泛應(yīng)用。超冗余度機(jī)器人具有長(zhǎng)徑比大、自由度高等特點(diǎn)，因此在空間狹窄、運(yùn)動(dòng)受限、復(fù)雜危險(xiǎn)環(huán)境下具有超高的避障能力。在航空航天領(lǐng)域，航天器、飛機(jī)等設(shè)施內(nèi)部結(jié)構(gòu)復(fù)雜，可供作業(yè)的區(qū)域非常狹小，因此在進(jìn)行監(jiān)測(cè)、維護(hù)、清理、檢測(cè)等任務(wù)時(shí)十分困難。超冗余度機(jī)器人的靈活性決定了其十分適合這類工作，如代替人工進(jìn)行飛機(jī)油箱的檢測(cè)[1]、狹小空間飛機(jī)部件的裝配工作等。但由于超冗余度機(jī)器人關(guān)節(jié)數(shù)量較多，其逆運(yùn)動(dòng)學(xué)求解也變得十分困難。為了避免或簡(jiǎn)化求解過(guò)程，目前很多學(xué)者專家采用路徑跟隨運(yùn)動(dòng)的方式對(duì)超冗余度機(jī)器人進(jìn)行運(yùn)動(dòng)規(guī)劃。路徑跟隨運(yùn)動(dòng)一般指使整個(gè)超冗余度機(jī)器人機(jī)械臂部分能夠跟隨某個(gè)指定的路徑，其特點(diǎn)是在運(yùn)動(dòng)受限的區(qū)域內(nèi)，超冗余度機(jī)器人的連桿和關(guān)節(jié)沿路徑移動(dòng)，機(jī)械臂看起來(lái)始終跟隨已有路徑動(dòng)[2]。要實(shí)現(xiàn)路徑跟隨運(yùn)動(dòng)，則首先要對(duì)機(jī)器人末端避障路徑進(jìn)行規(guī)劃。

末端避障路徑規(guī)劃的主要任務(wù)是找到一條從起始點(diǎn)到目標(biāo)點(diǎn)連續(xù)的無(wú)碰撞路徑[3]。路徑規(guī)劃問(wèn)題廣泛存在于各個(gè)領(lǐng)域，如工業(yè)機(jī)器人、移動(dòng)機(jī)器人、無(wú)人機(jī)等，吸引了國(guó)內(nèi)外學(xué)者的廣泛研究，大量的路徑規(guī)劃算法也因此被提出：基于建立引力斥力場(chǎng)的人工勢(shì)場(chǎng)法[4]在局部避障中具有較好的性能，但在障礙物復(fù)雜的場(chǎng)景中容易陷入局部最優(yōu)；基于計(jì)算智能的蟻群算法[5]具有較強(qiáng)的魯棒性，但計(jì)算量大，收斂速度慢；基于搜索的A*算法[6]雖然搜索能力強(qiáng)，但不適合高維狀態(tài)空間的路徑規(guī)劃。

基于隨機(jī)采樣的路徑規(guī)劃方法[7]的提出，為解決高維度、復(fù)雜環(huán)境的路徑規(guī)劃問(wèn)題提供出了一種很好的解決思路?？焖偬剿麟S機(jī)樹（Rapidly exploring random tree，RRT）算法[8]搜索能力強(qiáng)，且不需要將搜索空間柵格化，十分適合應(yīng)用在高維環(huán)境下。但RRT 算法也具有搜索效率低、路徑曲折、產(chǎn)生路徑不穩(wěn)定等缺點(diǎn)。并且RRT 算法為單純的隨機(jī)采樣，沒(méi)有對(duì)所求得解的成本進(jìn)行考慮，因此不能得到最優(yōu)路徑。為解決這個(gè)問(wèn)題，Karaman 等[9]將漸進(jìn)最優(yōu)的思想引入標(biāo)準(zhǔn)RRT 算法中，提出了RRT*算法。RRT*算法在RRT 的基礎(chǔ)上增加了父節(jié)點(diǎn)重新選擇與剪枝重新布線的操作，利用這個(gè)過(guò)程來(lái)實(shí)現(xiàn)漸進(jìn)最優(yōu)。雖然RRT*引入了漸進(jìn)最優(yōu)思想，但其仍然具有很多缺點(diǎn)，如收斂速度慢、路徑較為曲折等。因此國(guó)內(nèi)外學(xué)者在RRT*算法的基礎(chǔ)上，提出了很多改進(jìn)措施。Jordan 等[10]根據(jù)RRT 的改進(jìn)算法RRT–Connect 的雙向搜索優(yōu)勢(shì)，提出了B–RRT*算法，利用RRT*中的父節(jié)點(diǎn)重選與重新布線操作來(lái)優(yōu)化兩棵樹的搜索方式，同時(shí)引入了貪婪搜索策略，得到了最優(yōu)解；張偉民等[11]通過(guò)在采樣上引入目標(biāo)偏向思想、新點(diǎn)擴(kuò)展上給采樣點(diǎn)和目標(biāo)點(diǎn)分配權(quán)重等策略對(duì)RRT*算法進(jìn)行改進(jìn)，加快了收斂速度，提升了規(guī)劃效率；曹凱等[12]將人工勢(shì)場(chǎng)法與RRT*算法結(jié)合，提出了一種由人工勢(shì)場(chǎng)（APF）引導(dǎo)RRT*進(jìn)行路徑規(guī)劃的方法，利用人工勢(shì)場(chǎng)引導(dǎo)采樣節(jié)點(diǎn)進(jìn)行采樣，減少了算法運(yùn)行時(shí)間，加快收斂速度。

上述方法對(duì)比標(biāo)準(zhǔn)RRT*算法而言取得了一定的優(yōu)化效果，但由于RRT*算法的隨機(jī)性，生成的路徑仍然存在一定的曲折，路徑轉(zhuǎn)角大小隨機(jī)性較大，因此生成的路徑不一定每次都適合超冗余度機(jī)器人的路徑跟隨運(yùn)動(dòng)。本文對(duì)超冗余度機(jī)器人結(jié)構(gòu)及相鄰關(guān)節(jié)跟隨運(yùn)動(dòng)過(guò)程進(jìn)行分析，對(duì)障礙物進(jìn)行了膨脹化處理，通過(guò)引入超冗余度機(jī)器人相關(guān)約束，對(duì)RRT*算法進(jìn)行改進(jìn)。利用改進(jìn)后的RRT*算法進(jìn)行超冗余度機(jī)器人的路徑規(guī)劃，能夠在三維空間中規(guī)劃出更加適合超冗余度機(jī)器人跟隨運(yùn)動(dòng)的路徑，且能保證一次成功率。

1 超冗余度機(jī)器人分析及障礙物處理

1.1 超冗余度機(jī)器人分析

超冗余度機(jī)器人結(jié)構(gòu)如圖1 所示，主要由進(jìn)給底座、驅(qū)動(dòng)底座、機(jī)械臂3 部分構(gòu)成。機(jī)械臂部分主要由連桿構(gòu)成，兩個(gè)相鄰連桿由萬(wàn)向關(guān)節(jié)進(jìn)行連接，因此每個(gè)關(guān)節(jié)具有兩個(gè)旋轉(zhuǎn)自由度。

圖1 超冗余度機(jī)器人結(jié)構(gòu)Fig.1 Super redundant robot structure

關(guān)節(jié)轉(zhuǎn)角的定義，如圖2 中θ所示。關(guān)節(jié)的彎曲角度θ受臂段直徑D和間距h0限制，滿足tan （θ/2≤h0/D）。由于關(guān)節(jié)角度存在限制，因此在進(jìn)行路徑跟隨運(yùn)動(dòng)時(shí)，末端路徑的角度也應(yīng)滿足一定限制條件，即最終生成的路徑，轉(zhuǎn)角應(yīng)盡量小于該角度。

圖2 關(guān)節(jié)轉(zhuǎn)角定義Fig.2 Joint angle definition

針對(duì)超冗余度機(jī)器人跟隨末端路徑運(yùn)動(dòng)，對(duì)兩個(gè)相鄰連桿的路徑跟隨運(yùn)動(dòng)進(jìn)行分析。由于關(guān)節(jié)轉(zhuǎn)角受到關(guān)節(jié)結(jié)構(gòu)的限制，轉(zhuǎn)角具有極限大小θmax，因此考慮路徑轉(zhuǎn)角為極限轉(zhuǎn)角時(shí)兩臂段的跟隨運(yùn)動(dòng)。如圖3 所示，在第1 節(jié)連桿頭部通過(guò)路徑拐點(diǎn)后，在前進(jìn)過(guò)程中，第1 節(jié)連桿會(huì)掃掠過(guò)一個(gè)區(qū)域。這種情況下，若障礙物在這個(gè)掃掠區(qū)域內(nèi)，從路徑角度來(lái)看沒(méi)有發(fā)生碰撞，但在機(jī)械臂路徑跟隨過(guò)程中，由于連桿為剛性，所以在運(yùn)動(dòng)過(guò)程中，臂段會(huì)與路徑外的障礙物發(fā)生碰撞。因此，在設(shè)計(jì)避障算法時(shí)，需預(yù)留安全距離。

圖3 路徑跟隨運(yùn)動(dòng)Fig.3 Path following motion

由于該掃掠區(qū)域在計(jì)算時(shí)較為復(fù)雜，且與路徑轉(zhuǎn)角有關(guān)，因此將此區(qū)域擴(kuò)大，利用該掃掠區(qū)域的最大寬度來(lái)設(shè)計(jì)避障算法，使障礙物距離路徑至少為該最大值。對(duì)于該值，如圖4 所示：路徑為FOH，選取第1 段連桿AB運(yùn)動(dòng)過(guò)程中3 個(gè)階段進(jìn)行分析，分別為A′B′到AB再到A″B″。其中，當(dāng)連桿AB運(yùn)動(dòng)至路徑拐點(diǎn)O與連桿AB的數(shù)學(xué)關(guān)系為OA=OB時(shí)，即圖4 中AB處，此時(shí)為整個(gè)路徑跟隨運(yùn)動(dòng)過(guò)程的中間時(shí)刻，由A′B′和A″B″可知，在此中間時(shí)刻，之前和之后的運(yùn)動(dòng)過(guò)程掃掠區(qū)域?qū)ΨQ，若將路徑擴(kuò)展至F′O′H′處后再進(jìn)行路徑規(guī)劃，則在原路徑FOH上進(jìn)行路徑跟隨運(yùn)動(dòng)時(shí)，障礙物在任何位置均不會(huì)發(fā)生碰撞。將路徑進(jìn)行移動(dòng)擴(kuò)展，擴(kuò)展的距離為O′N。該距離可擴(kuò)展至障礙物上，再進(jìn)行路徑規(guī)劃。該距離求解方式為

圖4 掃掠區(qū)域分析Fig.4 Analysis of swept area

式中，l為連桿長(zhǎng)度；h0為連桿間關(guān)節(jié)間距；θmax為極限轉(zhuǎn)角大小。

1.2 障礙物模型處理及碰撞檢測(cè)

在本研究路徑規(guī)劃算法設(shè)計(jì)中，障礙物用球體來(lái)代替。由于該路徑用于超冗余度機(jī)器人路徑跟隨運(yùn)動(dòng)，因此在規(guī)劃路徑時(shí)需考慮超冗余度機(jī)器人機(jī)械臂部分在該路徑上運(yùn)動(dòng)時(shí)的碰撞問(wèn)題。

首先由于機(jī)器人臂段為圓柱形，對(duì)于機(jī)器人連桿與障礙物兩個(gè)實(shí)體之間的空間位置關(guān)系，可以簡(jiǎn)化為直線段和實(shí)體之間的相對(duì)位置關(guān)系，將原有球形障礙物沿半徑方向擴(kuò)大與臂段圓柱半徑等長(zhǎng)距離，在此基礎(chǔ)上進(jìn)行路徑規(guī)劃。

同時(shí)，通過(guò)上文相鄰關(guān)節(jié)的路徑跟隨運(yùn)動(dòng)分析，在路徑跟隨運(yùn)動(dòng)時(shí)會(huì)掃掠過(guò)一個(gè)區(qū)域。對(duì)于為避免該區(qū)域的碰撞而預(yù)留的安全距離，同樣可以類比至障礙物上，將障礙物沿半徑方向擴(kuò)大該最大值的距離。

針對(duì)碰撞檢測(cè)算法，在進(jìn)行設(shè)計(jì)時(shí)，不僅要考慮障礙物球心到直線的最小距離，還要考慮線段與球形障礙物的位置關(guān)系。設(shè)障礙物球心O到連桿AB的垂直距離為dmin，根據(jù)連桿AB與球形障礙物的位置關(guān)系，可分為以下兩種情況。

（1）障礙物球心O到連桿AB的距離大于障礙物的半徑，此時(shí)機(jī)械臂與障礙物不發(fā)生碰撞，即dmin≥r，如圖5（a）所示。

（2）障礙物球心O到連桿AB的距離小于障礙物的半徑，此時(shí)分為以下幾種情況進(jìn)行討論。

a. 連接障礙物球心與連桿兩端點(diǎn)AB，若||OA||

圖5 桿與球形障礙物的位置關(guān)系Fig.5 Position relationship between rod and spherical obstacle

b. 連接障礙物球心O與連桿兩端點(diǎn)A、B，若||OA||>r且||OB||>r，則繼續(xù)分為以下情況進(jìn)行討論。

如圖6 所示，判斷A、B兩點(diǎn)在同側(cè)還是異側(cè)，只需判斷兩個(gè)角α、β是否全部小于90°。若其中有任一角度大于90°，則A、B兩點(diǎn)在O點(diǎn)同側(cè)；若均小于90°，則A、B兩點(diǎn)分布于O點(diǎn)異側(cè)。根據(jù)余弦定理

圖6 桿端點(diǎn)相對(duì)位置Fig.6 Relative position of rod end point

判斷兩角是否均小于90°，則只需判斷f是否大于0即可。

若f>0，則A、B兩點(diǎn)位于障礙物球心O兩側(cè)，發(fā)生碰撞，如圖7（a）所示；若f≤0，則A、B兩點(diǎn)位于障礙物球心O同側(cè)，不發(fā)生碰撞，如圖7（b）所示。

圖7 同側(cè)和異側(cè)碰撞檢測(cè)結(jié)果Fig.7 Same side and different side collision detection results

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

2.1 RRT*算法

定義整個(gè)搜索空間為Q，搜索空間Q由障礙物空間Qobs和自由空間Qfree組成。在搜索空間中，xstart為起始點(diǎn)，xgoal為目標(biāo)點(diǎn)。路徑規(guī)劃的定義為，在自由空間Qfree內(nèi)尋找出一條連接xstart與xgoal的路徑，使得路徑無(wú)碰撞且滿足以下約束條件：規(guī)劃出的路徑應(yīng)盡可能短，且不與障礙物發(fā)生碰撞；考慮到超冗余度機(jī)器人后續(xù)路徑跟隨問(wèn)題，規(guī)劃出的路徑不應(yīng)包含過(guò)大轉(zhuǎn)角。

基于隨機(jī)采樣原理的快速搜索隨機(jī)樹方法（RRT）具有概率完備性、計(jì)算量小、計(jì)算難度低等特點(diǎn)，它對(duì)解決高維狀態(tài)規(guī)劃問(wèn)題有著較強(qiáng)的適應(yīng)性，這一能力是目前很多規(guī)劃算法所不具備的，因此非常適合高維運(yùn)動(dòng)狀態(tài)規(guī)劃的實(shí)際情況。鑒于 RRT*算法不僅加快了原RRT 算法的收斂速度，還增強(qiáng)了算法的最優(yōu)性能，因此選取 RRT*作為路徑規(guī)劃的基礎(chǔ)方法。

RRT*算法的具體工作流程如下。

（1）將系統(tǒng)的初始狀態(tài)（末端位置）作為樹的第1個(gè)頂點(diǎn)xstart，將目標(biāo)點(diǎn)作為樹的最終節(jié)點(diǎn)xgoal。

（2）在搜索空間Q內(nèi)，按照目標(biāo)偏向思想，隨機(jī)生成一個(gè)采樣點(diǎn)xrand，搜索隨機(jī)樹T上距離隨機(jī)點(diǎn)xrand距離最近的節(jié)點(diǎn)xnearest，并拓展一個(gè)固定步長(zhǎng)生成新的節(jié)點(diǎn)xnew，檢測(cè)節(jié)點(diǎn)xnearest與節(jié)點(diǎn)xnew的連線間是否有障礙物，如沒(méi)有障礙物則將節(jié)點(diǎn)xnew添加到搜索樹T上，并計(jì)算其價(jià)值函數(shù)。

（3）重選父節(jié)點(diǎn)：在生成的新節(jié)點(diǎn)規(guī)定的鄰域R范圍內(nèi)搜索節(jié)點(diǎn)集合xnear，計(jì)算將集合內(nèi)點(diǎn)xnear–i作為xnew父節(jié)點(diǎn)xparent的價(jià)值函數(shù)，并與之前以xnearest作為父節(jié)點(diǎn)xparent的價(jià)值函數(shù)相比較。如果以xnear–i作為xnew父節(jié)點(diǎn)xparent的價(jià)值函數(shù)小于原價(jià)值函數(shù)cost，并且檢測(cè)連線后無(wú)碰撞，則將xnearest與節(jié)點(diǎn)xnew的連線斷開，連接xnear–i與xnew，并更新價(jià)值函數(shù)。

（4）剪枝重新布線：遍歷集合xnear內(nèi)所有節(jié)點(diǎn)xnear–i，若xnear–i與xnew連線不與障礙物碰撞，且xnear–i與xnew的價(jià)值函數(shù)加上xnew到起始節(jié)點(diǎn)xstart的價(jià)值函數(shù)小于xnear–i到xstart的原價(jià)值函數(shù)，即斷開xnear–i與原父節(jié)點(diǎn)的連接，并將xnew作為xnear–i的父節(jié)點(diǎn)，連接xnear–i與xnew，并更新價(jià)值函數(shù)。

（5）重復(fù)以上過(guò)程，經(jīng)過(guò)有限次迭代優(yōu)化求解，直至搜索樹T存在一點(diǎn)與目標(biāo)點(diǎn)xgoal之間的距離小于所規(guī)定的閾值，停止搜索。

（6）由目標(biāo)點(diǎn)向起始點(diǎn)檢索，生成完整的 RRT*路徑。

2.2 改進(jìn)RRT＊算法

標(biāo)準(zhǔn)RRT*算法為隨機(jī)采樣搜索算法，所以較大概率產(chǎn)生的路徑太過(guò)曲折、無(wú)用節(jié)點(diǎn)數(shù)過(guò)多。因此本文提出以下4 種改進(jìn)策略。

（1）路徑生成方式改進(jìn)。

傳統(tǒng)的RRT*算法中引入了目標(biāo)偏向思想，具體流程為隨機(jī)樹在隨機(jī)獲取采樣點(diǎn)時(shí)，首先按照均勻概率分布獲取一個(gè)概率值p，若p小于設(shè)定的閾值，則xrand=xgoal；若p大于閾值，則按照原有的隨機(jī)采樣方式獲取隨機(jī)點(diǎn)。目標(biāo)偏向思想的引入，可以減少隨機(jī)樹在擴(kuò)展時(shí)的隨機(jī)性，減少損耗。但隨機(jī)點(diǎn)生成后路徑的產(chǎn)生方式上并沒(méi)有任何改變，直接按照固定步長(zhǎng)向隨機(jī)點(diǎn)方向擴(kuò)展。因此本文對(duì)路徑產(chǎn)生方式進(jìn)行改進(jìn)，將目標(biāo)點(diǎn)信息進(jìn)行再次引入：首先，在采樣空間內(nèi)隨機(jī)生成一個(gè)點(diǎn)xrand0，連接xnear與xrand0確定單位向量a，連接xnear與xgoal確定單位向量b，隨機(jī)生成兩個(gè)0 ～ 1 之間的隨機(jī)數(shù)p1、p2，隨機(jī)點(diǎn)的長(zhǎng)度與方向按照式（4）確定。

式中，σ為拓展步長(zhǎng)。

（2）路徑角度約束。

根據(jù)前文分析得知，由于超冗余度機(jī)器人關(guān)節(jié)結(jié)構(gòu)特點(diǎn)，生成路徑中的轉(zhuǎn)角應(yīng)小于θmax，因此在生成新節(jié)點(diǎn)時(shí)引入角度約束。對(duì)于圖8 所示路徑中連續(xù)的3 個(gè)節(jié)點(diǎn)x1、x2、x3及夾角角度φ，計(jì)算公式為

圖8 路徑轉(zhuǎn)角Fig.8 Path corner

根據(jù)式（5）判斷擴(kuò)展的新節(jié)點(diǎn)xnew和xnearest的連線與xnearest和其父節(jié)點(diǎn)xnear–parent的連線之間的夾角是否超過(guò)限定值。若大于限定角度，則放棄節(jié)點(diǎn)xnew；同樣的，在重選父節(jié)點(diǎn)和剪枝過(guò)程中也應(yīng)遵守此規(guī)則。對(duì)于重選父節(jié)點(diǎn)過(guò)程，當(dāng)選定父節(jié)點(diǎn)xnear–i和xnew的連線與xnear–i和其父節(jié)點(diǎn)的連線夾角大于限定角度時(shí)，放棄xnear–i作為xnew的新父節(jié)點(diǎn)。在剪枝過(guò)程中，判斷xnear–i和xnew連線與xnew和其父節(jié)點(diǎn)連線的夾角是否大于限定角度。若大于限定角度，則放棄本次剪枝過(guò)程。

（3）重選父節(jié)點(diǎn)及剪枝過(guò)程優(yōu)化。

在重選父節(jié)點(diǎn)及剪枝過(guò)程中，對(duì)于傳統(tǒng)的RRT*算法，需在鄰域R的范圍內(nèi)遍歷所有節(jié)點(diǎn)，對(duì)路徑進(jìn)行優(yōu)化。但該過(guò)程隨著鄰域范圍的增大，計(jì)算量也會(huì)增大。對(duì)于該過(guò)程，在進(jìn)行遍歷之前，增加一個(gè)與起始點(diǎn)xstart連接的過(guò)程，然后進(jìn)行碰撞檢測(cè)，若無(wú)碰撞，則直接將xstart作為新的父節(jié)點(diǎn)。若滿足無(wú)碰撞的條件，則不僅可以大大減少因遍歷所有節(jié)點(diǎn)而產(chǎn)生的計(jì)算量，而且在一定程度上對(duì)路徑起到優(yōu)化作用。

（4）碰撞檢測(cè)后調(diào)整。

在傳統(tǒng)RRT*算法中，拓展步長(zhǎng)為一個(gè)定值σ，這個(gè)定值需在算法中作為初始量給出。若σ過(guò)大，則在擴(kuò)展過(guò)程中容易產(chǎn)生碰撞，生成大量無(wú)效節(jié)點(diǎn)，擴(kuò)展效率低；若σ過(guò)小，則擴(kuò)展次數(shù)將增大，增大了算法收斂的時(shí)間。

針對(duì)這一問(wèn)題，本文采用拓展步長(zhǎng)調(diào)整策略，即隨機(jī)樹通過(guò)拓展新節(jié)點(diǎn)，當(dāng)拓展路徑與障礙物發(fā)生碰撞時(shí)，利用二分法進(jìn)行調(diào)整，具體方式為：取新節(jié)點(diǎn)xnew與其父節(jié)點(diǎn)xparent連線的中點(diǎn)xmid，將xmid作為新的xnew進(jìn)行碰撞檢測(cè)，若仍發(fā)生碰撞，則重復(fù)進(jìn)行該過(guò)程，重復(fù)兩次后若仍有碰撞，則放棄該節(jié)點(diǎn)。

改進(jìn)RRT*算法的大體流程如圖9 所示。

圖9 改進(jìn)RRT*算法流程圖Fig.9 Flow chart of improved RRT* algorithm

3 試驗(yàn)與分析

為檢驗(yàn)改進(jìn)RRT*算法的性能，在MATLAB2018a上進(jìn)行仿真試驗(yàn)。試驗(yàn)軟件環(huán)境為：Windows10，硬件配置為：Inter（R）Core（TM）i5–6200UCPU@2.30GHz，內(nèi)存（RAM）8GB。三維仿真地圖尺寸設(shè)置為2000×2000×2000，起始點(diǎn)為（10，10，10），終點(diǎn)為（2000，2000，2000）。障礙物設(shè)置為球形，具體坐標(biāo)及大小見(jiàn)表1。設(shè)置初始固定步長(zhǎng)為400mm，在改進(jìn)算法中，路徑角度約束為20°。

表1 障礙物坐標(biāo)及大小Table 1 Obstacle coordinates and size

本文算法是在RRT*算法的基礎(chǔ)上進(jìn)行改進(jìn)后提出的，因此設(shè)計(jì)試驗(yàn)時(shí)，采用標(biāo)準(zhǔn)RRT*作為對(duì)比算法進(jìn)行對(duì)比試驗(yàn)。由于擴(kuò)展隨機(jī)樹算法的隨機(jī)性，因此在進(jìn)行試驗(yàn)時(shí)，每種對(duì)比算法進(jìn)行50 次試驗(yàn)，將50 次試驗(yàn)的數(shù)據(jù)取平均值作為對(duì)比數(shù)據(jù)，對(duì)比結(jié)果數(shù)據(jù)見(jiàn)表2。

表2 對(duì)比試驗(yàn)數(shù)據(jù)Table 2 Comparison of experimental data

如圖10 所示，通過(guò)對(duì)比試驗(yàn)可以看出，改進(jìn)后的RRT*算法具有以下特點(diǎn)。

圖10 路徑規(guī)劃結(jié)果對(duì)比Fig.10 Comparison of path planning results

（1）搜索效率方面。改進(jìn)后的RRT*算法，搜索平均時(shí)間為0.702 s，標(biāo)準(zhǔn)RRT*算法的搜索時(shí)長(zhǎng)為0.534 s，在搜索時(shí)長(zhǎng)上雖有所增加，但該算法應(yīng)用于離線軌跡規(guī)劃，搜索時(shí)長(zhǎng)為次要因素，且每次規(guī)劃出的路徑均能符合超冗余度機(jī)器人的路徑跟隨運(yùn)動(dòng)，無(wú)需人工再次進(jìn)行篩選；標(biāo)準(zhǔn)RRT*算法的平均節(jié)點(diǎn)數(shù)為101，改進(jìn)后算法的節(jié)點(diǎn)數(shù)為75，在搜索效率上與原算法相比，平均迭代次數(shù)減小，平均規(guī)劃時(shí)間相差不大。因此可以看出，改進(jìn)后的算法在增加多種約束后，雖然算法的計(jì)算量有所增加，但節(jié)點(diǎn)數(shù)變少，內(nèi)存消耗減小。

（2）路徑長(zhǎng)度方面。改進(jìn)后的RRT*算法，平均路徑長(zhǎng)度為3610.2771 mm，相比于原算法的平均路徑長(zhǎng)度3951.8089 mm 減少了近9%，由該數(shù)據(jù)可知，改進(jìn)RRT*算法規(guī)劃出的路徑長(zhǎng)度較小，提高了路徑的質(zhì)量。

（3）路徑優(yōu)化方面。由于改進(jìn)RRT*算法中路徑轉(zhuǎn)角約束的引入，所有路徑轉(zhuǎn)角均小于20°，且由于碰撞后調(diào)整策略的引入，使得路徑在空曠區(qū)域中以固定步長(zhǎng)直接通過(guò)；在障礙物較多區(qū)域，碰撞后進(jìn)行調(diào)整，路徑得到明顯優(yōu)化。

4 結(jié)論

本文通過(guò)對(duì)超冗余度機(jī)器人的結(jié)構(gòu)和相鄰關(guān)節(jié)跟隨運(yùn)動(dòng)時(shí)臂桿的跟隨運(yùn)動(dòng)狀態(tài)進(jìn)行分析，一方面對(duì)障礙物進(jìn)行了兩次膨脹化處理，避免了機(jī)器人運(yùn)動(dòng)過(guò)程中潛在的干涉碰撞問(wèn)題；另一方面對(duì)RRT*算法進(jìn)行了改進(jìn)，提出了4 種改進(jìn)策略，縮短了規(guī)劃路徑的長(zhǎng)度，同時(shí)避免了過(guò)大關(guān)節(jié)轉(zhuǎn)角，符合超冗余度機(jī)器人在后續(xù)路徑跟隨運(yùn)動(dòng)的路徑要求，實(shí)現(xiàn)了在三維空間內(nèi)的避障路徑優(yōu)化。