基于改進RRT*算法的超冗余度機器人末端避障路徑規(guī)劃*

（趙 盼，杜兆才，劉 江，王明陽

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

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

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

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

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

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

1.1 超冗余度機器人分析

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

圖1 超冗余度機器人結(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é)角度存在限制，因此在進行路徑跟隨運動時，末端路徑的角度也應(yīng)滿足一定限制條件，即最終生成的路徑，轉(zhuǎn)角應(yīng)盡量小于該角度。

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

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

圖3 路徑跟隨運動Fig.3 Path following motion

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

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

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

1.2 障礙物模型處理及碰撞檢測

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

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

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

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

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

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

a. 連接障礙物球心與連桿兩端點AB，若||OA||

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

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

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

圖6 桿端點相對位置Fig.6 Relative position of rod end point

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

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

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

2 RRT* 算法改進

2.1 RRT*算法

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

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

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

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

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

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

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

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

（6）由目標點向起始點檢索，生成完整的 RRT*路徑。

2.2 改進RRT＊算法

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

（1）路徑生成方式改進。

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

式中，σ為拓展步長。

（2）路徑角度約束。

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

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

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

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

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

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

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

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

改進RRT*算法的大體流程如圖9 所示。

圖9 改進RRT*算法流程圖Fig.9 Flow chart of improved RRT* algorithm

3 試驗與分析

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

表1 障礙物坐標及大小Table 1 Obstacle coordinates and size

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

表2 對比試驗數(shù)據(jù)Table 2 Comparison of experimental data

如圖10 所示，通過對比試驗可以看出，改進后的RRT*算法具有以下特點。

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

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

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

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

4 結(jié)論

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